Commit bb45d5fd authored by francois's avatar francois
Browse files

fixed typos

parent 1fd44b85
......@@ -29,7 +29,7 @@ static double marker_scale;
static int max_track_size = 100;
// merker publishers
ros::Publisher marker_pub;
ros::Publisher track_marker_pub;
ros::Publisher vehicle_marker_pub;
ros::Publisher wp_marker_pub;
......@@ -38,36 +38,36 @@ boost::shared_ptr<visualization_msgs::MarkerArray> vehicle_marker;
/**
* @brief publish vehicle track
*/
static void publish_marker(const geometry_msgs::PoseStamped::ConstPtr &pose)
static void publish_track_marker(const geometry_msgs::PoseStamped::ConstPtr &pose)
{
static boost::shared_ptr<visualization_msgs::Marker> marker;
static boost::shared_ptr<visualization_msgs::Marker> track_marker;
if ( !marker )
if ( !track_marker )
{
marker = boost::make_shared<visualization_msgs::Marker>();
marker->type = visualization_msgs::Marker::CUBE_LIST; // fast rendering
marker->ns = "fcu";
marker->action = visualization_msgs::Marker::ADD;
marker->scale.x = marker_scale * 0.015;
marker->scale.y = marker_scale * 0.015;
marker->scale.z = marker_scale * 0.015;
marker->color.a = 1.0;
marker->color.r = 0.0;
marker->color.g = 0.0;
marker->color.b = 0.5;
marker->points.reserve(max_track_size);
track_marker = boost::make_shared<visualization_msgs::Marker>();
track_marker->type = visualization_msgs::Marker::CUBE_LIST;
track_marker->ns = "fcu";
track_marker->action = visualization_msgs::Marker::ADD;
track_marker->scale.x = marker_scale * 0.015;
track_marker->scale.y = marker_scale * 0.015;
track_marker->scale.z = marker_scale * 0.015;
track_marker->color.a = 1.0;
track_marker->color.r = 0.0;
track_marker->color.g = 0.0;
track_marker->color.b = 0.5;
track_marker->points.reserve(max_track_size);
}
static int track_idx = 0;
static int marker_idx = 0;
if ( marker->points.size() < max_track_size )
marker->points.push_back(pose->pose.position);
else marker->points[track_idx] = pose->pose.position;
if ( track_marker->points.size() < max_track_size )
track_marker->points.push_back(pose->pose.position);
else track_marker->points[marker_idx] = pose->pose.position;
track_idx = ++track_idx % max_track_size;
marker_idx = ++marker_idx % max_track_size;
marker->header = pose->header;
marker_pub.publish(marker);
track_marker->header = pose->header;
track_marker_pub.publish(track_marker);
}
static void publish_wp_marker(const geometry_msgs::PoseStamped::ConstPtr &wp)
......@@ -188,7 +188,7 @@ static void create_vehicle_markers( int num_rotors, float arm_len, float body_wi
static void local_position_sub_cb(const geometry_msgs::PoseStamped::ConstPtr &pose)
{
publish_marker(pose);
publish_track_marker(pose);
if (vehicle_marker) vehicle_marker_pub.publish(vehicle_marker);
}
......@@ -214,11 +214,11 @@ int main(int argc, char *argv[])
priv_nh.param("arm_len", arm_len, 0.22 );
priv_nh.param("body_width", body_width, 0.15 );
priv_nh.param("body_height", body_height, 0.10 );
priv_nh.param("max_track_size", max_track_size, 100 );
priv_nh.param("max_track_size", max_track_size, 1000 );
create_vehicle_markers( num_rotors, arm_len, body_width, body_height );
marker_pub = nh.advertise<visualization_msgs::Marker>("track_markers", 10);
track_marker_pub = nh.advertise<visualization_msgs::Marker>("track_markers", 10);
vehicle_marker_pub = nh.advertise<visualization_msgs::MarkerArray>("vehicle_marker", 10);
wp_marker_pub = nh.advertise<visualization_msgs::Marker>("wp_markers", 10);
......
Markdown is supported
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