ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

ROS point cloud draw a shape

asked 2013-12-09 02:58:52 -0600

silgon gravatar image

updated 2014-01-28 17:18:46 -0600

ngrennan gravatar image

I would like to draw some shapes for reference in a PointCloud2 variable. There's a tutorial (http://pointclouds.org/documentation/tutorials/pcl_visualizer.php) where you can draw in this case an ellipse.

In the next code is where the drawing is made.

  pcl::PointCloud<pcl::PointXYZ>::Ptr basic_cloud_ptr (new 
  pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGB>);
  std::cout << "Genarating example point clouds.\n\n";
  // We're going to make an ellipse extruded along the z-axis. The colour for
  // the XYZRGB cloud will gradually go from red to green to blue.
  uint8_t r(255), g(15), b(15);
  for (float z(-1.0); z <= 1.0; z += 0.05)
  {
    for (float angle(0.0); angle <= 360.0; angle += 5.0)
    {
      pcl::PointXYZ basic_point;
      basic_point.x = 0.5 * cosf (pcl::deg2rad(angle));
      basic_point.y = sinf (pcl::deg2rad(angle));
      basic_point.z = z;
      basic_cloud_ptr->points.push_back(basic_point);

      pcl::PointXYZRGB point;
      point.x = basic_point.x;
      point.y = basic_point.y;
      point.z = basic_point.z;
      uint32_t rgb = (static_cast<uint32_t>(r) << 16 |
              static_cast<uint32_t>(g) << 8 | static_cast<uint32_t>(b));
      point.rgb = *reinterpret_cast<float*>(&rgb);
      point_cloud_ptr->points.push_back (point);
    }
    if (z < 0.0)
    {
      r -= 12;
      g += 12;
    }
    else
    {
      g -= 12;
      b += 12;
    }
  }

So let's put the question simple. How could I integrate this to ROS so I can see it in RVIZ? Even this code, I don't know how to publish, I have problems with the pointer (PTR). And I don't find a good, reference.

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

answered 2013-12-10 02:13:09 -0600

silgon gravatar image

I got my solution and it's working with rviz. I solved this issue adding directly to the header the name of my frame_id.

sensor_msgs::PointCloud2 shape;
pcl::toROSMsg(*point_cloud_ptr,shape);
shape.header.frame_id="myshape";

After that I created a transform broadcaster:

tf::TransformBroadcaster br;
tf::Transform transform;
transform.setOrigin( tf::Vector3(0.0, 0, 0.0) );
transform.setRotation( tf::Quaternion(0, 0, 0) );

which I send in every iteration:

while(nh.ok()){
        br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "myshape"));
        pub.publish (shape);
        loop_rate.sleep();
}

Here is my full code: http://pastebin.com/0nWrm14E

edit flag offensive delete link more
0

answered 2013-12-09 07:43:42 -0600

updated 2013-12-09 07:46:17 -0600

The pcl_ros tutorials page has an example of publishing a point cloud.

edit flag offensive delete link more

Comments

Does this tutorial really work? I'm asking because I usually use the sensor_msgs/PointCloud2.h when subscribing for example to a kinect's topic and the pcl_conversions package...

Tirjen gravatar image Tirjen  ( 2013-12-09 20:37:02 -0600 )edit

Yes, it works.

Dan Lazewatsky gravatar image Dan Lazewatsky  ( 2013-12-10 04:03:40 -0600 )edit

Question Tools

2 followers

Stats

Asked: 2013-12-09 02:58:52 -0600

Seen: 1,461 times

Last updated: Dec 10 '13