0
$\begingroup$

Rosanswers logo

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.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

Hi,

The problem can by solved by adding the following lines

for (int count = 0; count < x.size(); count ++)
{
  geometry_msgs::Point tmp_p;

  tmp_p.x=x[count];
  tmp_p.y=y[count];
  line_strip.points.push_back(tmp_p);
}

Thanks


Originally posted by chao with karma: 153 on 2014-04-06

This answer was ACCEPTED on the original site

Post score: 1

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.