Hi,
I am using Ubuntu 12.04 with Hydro.
With reference to Points and Lines tutorial and Basic Shapes, I tried to create marker to each goal i send on rviz.
Essentially, I am trying to record and visualize all the goals i have sent and delete the goal i clicked for the second time. The points
are working fine, while the lines
are not showing up. Following is my code:
Class createMarker;
void Class::goalcb(const geometry_msgs::PoseStamped::ConstPtr& waypoint)
{
check = false;
x.push_back (waypoint->pose.position.x);
y.push_back (waypoint->pose.position.y);
for (int count = 0; count < x.size(); count ++)
printf("Starting Count%u X = %f, Y = %f \n", count, x[count], y[count]);
cout << "Sample 1 ="<< x.size() << endl;
for (int count = 0; count < x.size(); count ++)
{
printf("Y count = %f, Y que = %f\n", y[count], y[que]);
if ( count!=que && x[count]<(x[que]+0.05) && x[count]>(x[que]-0.05) && y[count]<(y[que]+0.05) && y[count]>(y[que]-0.05) )
{
Point = count;
printf("X = %f\n", x[count]);
reaction = visualization_msgs::Marker::DELETE;
x.erase(x.begin() + count);
y.erase(y.begin() + count);
//createMarker.makeMarker();
check = true;
/** x.erase(x.begin() + que);
y.erase(y.begin() + que);*/
printf("Value of point %u is erased\n", count);
printf("X after erased = %f\n", x[count]);
}
cout << "Sample 2 ="<< x.size() << endl;
Point = count;
if (check = true && count == x.size())
reaction = visualization_msgs::Marker::DELETE;
else
reaction = visualization_msgs::Marker::ADD;
createMarker.makeMarker();
vector<float>::const_iterator xii, yii;
xii = x.begin() + que;
yii = y.begin() + que;
cout << "X end =" << *xii << endl;
cout << "Y end =" << *yii << endl;
//printf("X end = %d, Y end = %d\n", x.end(), y.end());
}
printf("\n************************************\n");
for (int count = 0; count < x.size(); count ++)
printf("Point%u X = %f, Y = %f \n", count, x[count], y[count]);
printf("************************************\n");
if (check)
que --;
else
que ++;
return;
}
void Class::makeMarker()
{
visualization_msgs::Marker points, line_strip;
// Set the frame ID and timestamp. See the TF tutorials for information on these.
points.header.frame_id = line_strip.header.frame_id = "/map";
points.header.stamp = line_strip.header.stamp = ros::Time::now();
// Set the namespace and id for this marker. This serves to create a unique ID
// Any marker sent with the same namespace and id will overwrite the old one
points.ns = "Points";
line_strip.ns = "Lines";
points.id = line_strip.id = Point; //trying out
// Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
points.type = visualization_msgs::Marker::POINTS;
line_strip.type = visualization_msgs::Marker::LINE_STRIP;
// Set the marker action. Options are ADD and DELETE
points.action = line_strip.action = reaction;//visualization_msgs::Marker::ADD;
if (reaction == visualization_msgs::Marker::DELETE)
printf("Point %u is deleted\n", Point);
else
printf("making marker %u at x = %f, y = %f\n", Point, x[Point], y[Point]);
// Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header
geometry_msgs::Point p;
p.x = x[Point];
p.y = y[Point];
p.z = 0;
points.points.push_back(p);
line_strip.points.push_back(p);
/**points.pose.position.x = x[Point];
points.pose.position.y = y[Point];
line_strip.pose.position.x = x[Point];
line_strip.pose.position.y = y[Point];
points.pose.position.z = line_strip.pose.position.z = 0;*/
points.pose.orientation.w = line_strip.pose.orientation.w = 1.0;
points.pose.orientation.x = points.pose.orientation.y = points.pose.orientation.z = 0;
//marker.text = Point;
// Set the scale of the marker -- 1x1x1 here means 1m on a side
points.scale.x = 0.1;
points.scale.y = 0.1;
line_strip.scale.x = 0.1;
// Set the color -- be sure to set alpha to something non-zero!
points.color.g = 1.0f;
points.color.a = 1.0;
line_strip.color.b = 1.0;
line_strip.color.a = 1.0;
//marker.lifetime = ros::Duration();
// Publish the marker
marker_pub.publish(points);
marker_pub.publish(line_strip);
}
Please help. Thank you.
Originally posted by chao on ROS Answers with karma: 153 on 2014-04-06
Post score: 1
Original comments
Comment by chao on 2014-04-06:
I suppose that it is the push_back function in the makeMaker() causing the problem. Any suggestion on how i can solve it? Thanks.