copter_visualization.cpp
Go to the documentation of this file.
00001 
00006 /*
00007  * Copyright 2014,2015 M.H.Kabir
00008  *
00009  * This file is part of the mavros package and subject to the license terms
00010  * in the top-level LICENSE file of the mavros repository.
00011  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
00012  */
00013 
00014 #include <vector>
00015 
00016 #include <tf/tf.h>
00017 
00018 #include <ros/ros.h>
00019 #include <ros/console.h>
00020 
00021 #include <geometry_msgs/PoseStamped.h>
00022 #include <visualization_msgs/Marker.h>
00023 #include <visualization_msgs/MarkerArray.h>
00024 
00025 // parameters
00026 static std::string fixed_frame_id;
00027 static std::string child_frame_id;
00028 static double marker_scale;
00029 static int max_track_size = 100;
00030 
00031 // merker publishers
00032 ros::Publisher track_marker_pub;
00033 ros::Publisher vehicle_marker_pub;
00034 ros::Publisher wp_marker_pub;
00035 
00036 boost::shared_ptr<visualization_msgs::MarkerArray> vehicle_marker;
00037 
00041 static void publish_track_marker(const geometry_msgs::PoseStamped::ConstPtr &pose)
00042 {
00043         static boost::shared_ptr<visualization_msgs::Marker> track_marker;
00044 
00045         if ( !track_marker )
00046         {
00047                 track_marker = boost::make_shared<visualization_msgs::Marker>();
00048                 track_marker->type = visualization_msgs::Marker::CUBE_LIST;
00049                 track_marker->ns = "fcu";
00050                 track_marker->action = visualization_msgs::Marker::ADD;
00051                 track_marker->scale.x = marker_scale * 0.015;
00052                 track_marker->scale.y = marker_scale * 0.015;
00053                 track_marker->scale.z = marker_scale * 0.015;
00054                 track_marker->color.a = 1.0;
00055                 track_marker->color.r = 0.0;
00056                 track_marker->color.g = 0.0;
00057                 track_marker->color.b = 0.5;
00058                 track_marker->points.reserve(max_track_size);
00059         }
00060 
00061         static int marker_idx = 0;
00062 
00063         if ( track_marker->points.size() < max_track_size )
00064                 track_marker->points.push_back(pose->pose.position);
00065         else track_marker->points[marker_idx] = pose->pose.position;
00066 
00067         marker_idx = ++marker_idx % max_track_size;
00068 
00069         track_marker->header = pose->header;
00070         track_marker_pub.publish(track_marker);
00071 }
00072 
00073 static void publish_wp_marker(const geometry_msgs::PoseStamped::ConstPtr &wp)
00074 {
00075         static boost::shared_ptr<visualization_msgs::Marker> marker;
00076 
00077         if ( !marker )  // only instantiate marker once
00078         {
00079                 marker = boost::make_shared<visualization_msgs::Marker>();
00080 
00081                 marker->header = wp->header;
00082                 marker->header.frame_id = fixed_frame_id;
00083                 marker->type = visualization_msgs::Marker::ARROW;
00084                 marker->ns = "wp";
00085                 marker->action = visualization_msgs::Marker::ADD;
00086                 marker->scale.x = marker_scale * 1.0;
00087                 marker->scale.y = marker_scale * 0.1;
00088                 marker->scale.z = marker_scale * 0.1;
00089 
00090                 marker->color.a = 1.0;
00091                 marker->color.r = 0.0;
00092                 marker->color.g = 1.0;
00093                 marker->color.b = 0.0;
00094         }
00095 
00096         marker->pose = wp->pose;
00097         wp_marker_pub.publish(marker);
00098 }
00099 
00100 
00104 static void create_vehicle_markers( int num_rotors, float arm_len, float body_width, float body_height )
00105 {
00106         if ( num_rotors <= 0 ) num_rotors = 2;
00107 
00112         if ( vehicle_marker )
00113                 return;
00114 
00115         vehicle_marker = boost::make_shared<visualization_msgs::MarkerArray>();
00116         vehicle_marker->markers.reserve( 2 * num_rotors + 1 );
00117 
00122         // rotor marker template
00123         visualization_msgs::Marker rotor;
00124         rotor.header.stamp = ros::Time();
00125         rotor.header.frame_id = child_frame_id;
00126         rotor.ns = "vehicle_rotor";
00127         rotor.action = visualization_msgs::Marker::ADD;
00128         rotor.type = visualization_msgs::Marker::CYLINDER;
00129         rotor.scale.x = 0.2 * marker_scale;
00130         rotor.scale.y = 0.2 * marker_scale;
00131         rotor.scale.z = 0.01 * marker_scale;
00132         rotor.color.r = 0.4;
00133         rotor.color.g = 0.4;
00134         rotor.color.b = 0.4;
00135         rotor.color.a = 0.8;
00136         rotor.pose.position.z = 0;
00137 
00138         // arm marker template
00139         visualization_msgs::Marker arm;
00140         arm.header.stamp = ros::Time();
00141         arm.header.frame_id = child_frame_id;
00142         arm.ns = "vehicle_arm";
00143         arm.action = visualization_msgs::Marker::ADD;
00144         arm.type = visualization_msgs::Marker::CUBE;
00145         arm.scale.x = arm_len * marker_scale;
00146         arm.scale.y = 0.02 * marker_scale;
00147         arm.scale.z = 0.01 * marker_scale;
00148         arm.color.r = 0.0;
00149         arm.color.g = 0.0;
00150         arm.color.b = 1.0;
00151         arm.color.a = 1.0;
00152         arm.pose.position.z = -0.015 * marker_scale;
00153 
00154         float angle_increment = 2 * M_PI / num_rotors;
00155 
00156         for ( float angle = angle_increment / 2; angle <= (2 * M_PI); angle += angle_increment )
00157         {
00158                 rotor.pose.position.x = arm_len * cos(angle) * marker_scale;
00159                 rotor.pose.position.y = arm_len * sin(angle) * marker_scale;
00160                 rotor.id++;
00161 
00162                 arm.pose.position.x = rotor.pose.position.x / 2;
00163                 arm.pose.position.y = rotor.pose.position.y / 2;
00164                 arm.pose.orientation = tf::createQuaternionMsgFromYaw(angle);
00165                 arm.id++;
00166 
00167                 vehicle_marker->markers.push_back(rotor);
00168                 vehicle_marker->markers.push_back(arm);
00169         }
00170 
00171         // body marker template
00172         visualization_msgs::Marker body;
00173         body.header.stamp = ros::Time();
00174         body.header.frame_id = child_frame_id;
00175         body.ns = "vehicle_body";
00176         body.action = visualization_msgs::Marker::ADD;
00177         body.type = visualization_msgs::Marker::CUBE;
00178         body.scale.x = body_width * marker_scale;
00179         body.scale.y = body_width * marker_scale;
00180         body.scale.z = body_height * marker_scale;
00181         body.color.r = 0.0;
00182         body.color.g = 1.0;
00183         body.color.b = 0.0;
00184         body.color.a = 0.8;
00185 
00186         vehicle_marker->markers.push_back(body);
00187 }
00188 
00189 static void local_position_sub_cb(const geometry_msgs::PoseStamped::ConstPtr &pose)
00190 {
00191         publish_track_marker(pose);
00192         if (vehicle_marker) vehicle_marker_pub.publish(vehicle_marker);
00193 }
00194 
00195 void setpoint_local_pos_sub_cb(const geometry_msgs::PoseStamped::ConstPtr &wp)
00196 {
00197         publish_wp_marker(wp);
00198 }
00199 
00200 int main(int argc, char *argv[])
00201 {
00202         ros::init(argc, argv, "copter_visualization");
00203         ros::NodeHandle nh;
00204         ros::NodeHandle priv_nh("~");
00205 
00206         int num_rotors;
00207         double arm_len, body_width, body_height;
00208 
00209         priv_nh.param<std::string>("fixed_frame_id", fixed_frame_id, "local_origin");
00210         priv_nh.param<std::string>("child_frame_id", child_frame_id, "fcu");
00211 
00212         priv_nh.param("marker_scale", marker_scale, 1.0);
00213         priv_nh.param("num_rotors", num_rotors, 6);
00214         priv_nh.param("arm_len", arm_len, 0.22 );
00215         priv_nh.param("body_width", body_width, 0.15 );
00216         priv_nh.param("body_height", body_height, 0.10 );
00217         priv_nh.param("max_track_size", max_track_size, 1000 );
00218 
00219         create_vehicle_markers( num_rotors, arm_len, body_width, body_height );
00220 
00221         track_marker_pub = nh.advertise<visualization_msgs::Marker>("track_markers", 10);
00222         vehicle_marker_pub = nh.advertise<visualization_msgs::MarkerArray>("vehicle_marker", 10);
00223         wp_marker_pub = nh.advertise<visualization_msgs::Marker>("wp_markers", 10);
00224 
00225         auto pos_sub = nh.subscribe("local_position", 10, local_position_sub_cb);
00226         auto wp_sub = nh.subscribe("local_setpoint", 10, setpoint_local_pos_sub_cb);
00227 
00228         ros::spin();
00229         return 0;
00230 }


mavros_extras
Author(s): Vladimir Ermakov
autogenerated on Thu Feb 9 2017 04:00:23