Commit 0629e9e1 authored by mastefan's avatar mastefan
Browse files

[SM] Rviz now working. Added images.

parent cf49cbd7
......@@ -463,7 +463,9 @@ ros::Publisher flyingStatePublisher;
visualization_msgs::Marker drone_position_list;
visualization_msgs::Marker setpoint_position_list;
ros::Publisher rviz_points_publisher;
ros::Publisher rviz_points_trajectory_publisher;
ros::Publisher rviz_points_trajectory_generated_publisher;
// RELEVANT NOTES ABOUT THE VARIABLES DECLARE HERE:
// The "CrazyflieData" type used for the "request" variable is a
......
......@@ -1495,7 +1495,7 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
if (ros::ok()){
ROS_INFO("Init RVIZ STUFF");
//ROS_INFO("Init RVIZ STUFF");
geometry_msgs::Point p;
p.x = request.ownCrazyflie.x;
p.y = request.ownCrazyflie.y;
......@@ -1517,14 +1517,17 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
drone_position_list.color.r = 1.0f;
drone_position_list.color.a = 1.0;
drone_position_list.color.g = 0;
drone_position_list.points.push_back(p);
// Only save first 5000 points
if(drone_position_list.points.size()<5000)
//drone_position_list.points.erase(drone_position_list.points.begin());
drone_position_list.points.push_back(p);
rviz_points_publisher.publish(drone_position_list);
rviz_points_trajectory_publisher.publish(drone_position_list);
p.x = dronexSetpoint.x;
p.y = dronexSetpoint.y;
p.z = dronexSetpoint.z;
......@@ -1550,7 +1553,8 @@ void construct_and_publish_debug_message(Controller::Request &request, Controlle
setpoint_position_list.points.push_back(p);
rviz_points_publisher.publish(setpoint_position_list);
rviz_points_trajectory_generated_publisher.publish(setpoint_position_list);
}
......@@ -2336,7 +2340,8 @@ int main(int argc, char* argv[]) {
rviz_points_publisher = nodeHandle.advertise<visualization_msgs::Marker>("rviz_trajectory", 1);
rviz_points_trajectory_publisher = nodeHandle.advertise<visualization_msgs::Marker>("rviz_trajectory", 1);
rviz_points_trajectory_generated_publisher = nodeHandle.advertise<visualization_msgs::Marker>("rviz_trajectory_generated", 1);
/*if (ros::ok()){
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment