Ask Your Question
0

How to read the x,y,z coordinates of point cloud from the LIDAR SICK LMS 200.

asked 2012-08-13 17:31:41 -0500

metal gravatar image

Goal: To employ obstacle avoidance by interpreting the data from the LIDAR using PCL

Description: Hi everyone, I am working with a LIDAR device for the first time. I used the sick toolbox ros wrapper for reading the values from the LIDAR. I converted the "scan" topic given out by the LIDAR device to point-cloud 2 format. here is the program:

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl_ros/point_cloud.h>
#include <boost/foreach.hpp>
#include <pcl/io/pcd_io.h>
#include <tf/transform_listener.h>
#include <laser_geometry/laser_geometry.h>

 ros::Publisher pub;

class My_Filter {
     public:
        My_Filter();
        void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan);
     private:
        ros::NodeHandle node_;
        laser_geometry::LaserProjection projector_;
        tf::TransformListener tfListener_;

        ros::Publisher point_cloud_publisher_;
        ros::Subscriber scan_sub_;
         };

My_Filter::My_Filter(){
        scan_sub_ = node_.subscribe<sensor_msgs::LaserScan> ("/scan", 100, &My_Filter::scanCallback, this);
        point_cloud_publisher_ = node_.advertise<sensor_msgs::PointCloud2> ("/cloud", 100, false);
        tfListener_.setExtrapolationLimit(ros::Duration(0.1));
}

void My_Filter::scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan){
    sensor_msgs::PointCloud2 cloud;

    projector_.transformLaserScanToPointCloud("/laser", *scan, cloud, tfListener_);
    point_cloud_publisher_.publish(cloud);
}



int main(int argc, char** argv)
{
    ros::init(argc, argv, "my_filter");

    My_Filter filter;

    ros::spin();

    return 0;
}

This above node gives out the point cloud 2 topic "cloud". I subscribed to this topic in the following program. I was successful in reading the widht and height of the point clouds,but I need some help reading the X,Y,Z coordinates. I also need to know how to access the other objects which might be useful in data processing.

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl_ros/point_cloud.h>
#include <boost/foreach.hpp>
#include <pcl/io/pcd_io.h>
#include <tf/transform_listener.h>
#include <laser_geometry/laser_geometry.h>
#include <pcl/ros/conversions.h> 
ros::Publisher pub;
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
  // ... do data processing
 printf ("Cloud: width = %d, height = %d\n", input->width,input->height);

  BOOST_FOREACH (const pcl::PointXYZ& pt, input->points)
  printf ("\t(%f, %f, %f)\n", pt.x, pt.y, pt.z);
  // Publish the data
 // pub.publish (output);
}

int main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "pcs200");
  ros::NodeHandle nh;

  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = nh.subscribe ("/cloud", 100, cloud_cb);

  // Create a ROS publisher for the output point cloud
  pub = nh.advertise<sensor_msgs::PointCloud2> ("output", 100,false);

  // Spin
  ros::spin ();
}

Here is the output for that:

[  0%] Built target rospack_genmsg_libexe
  [  0%] Built target rosbuild_precompile
  Scanning dependencies of target pcs200
  [100%] Building CXX object CMakeFiles/pcs200.dir/src/pcs200.o
  /home/karthik/ros_workspace/pcs200/src/pcs200.cpp: In function ‘void cloud_cb(const sensor_msgs::PointCloud2ConstPtr&)’:
  /home/karthik/ros_workspace/pcs200/src/pcs200.cpp:17: error: ‘const struct sensor_msgs::PointCloud2_<std::allocator<void> >’ has no member named ‘points’
  /home/karthik/ros_workspace/pcs200/src/pcs200.cpp:17: error: ‘const struct sensor_msgs::PointCloud2_<std::allocator<void> >’ has no member named ‘points’
  /home/karthik/ros_workspace/pcs200/src/pcs200.cpp:17: error: ‘const struct sensor_msgs::PointCloud2_<std::allocator<void> >’ has ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
1

answered 2012-08-13 17:58:07 -0500

updated 2012-08-13 18:00:44 -0500

Hi Metal!

The error message

`const struct sensor_msgs::PointCloud2_<std::allocator<void> >’ has no member named ‘points’

is telling the truth, the variable input has type sensor_msgs::PointCloud2 and has no member called points, you can see its definition here.

You can use input->data or transform input to a pcl::PointCloud<pcl::pointxyz> which is somewhat easier to use.

You can do it like this

pcl::PointCloud<pcl::PointXYZ> input_;
pcl::fromROSMsg(*input, input_);

I hope this is helpful for you. Martin.

edit flag offensive delete link more

Comments

It did rectify the error,thank you. But the z is always showing as zero. Also could you please recommend the approach to implement obstacle avoidance using sick lidar?. Is PCL a good approach ?

metal gravatar imagemetal ( 2012-08-15 09:21:34 -0500 )edit

I might be mistaken, I have never worked with LIDAR, but as far as I know the readings of a LIDAR sensor are in 2D, usually in the plane X,Y, unless you are artificially tilting it with a motor. So to me it makes sense that the Z is always zero.

Martin Peris gravatar imageMartin Peris ( 2012-08-15 15:05:15 -0500 )edit
1

As for the best approach to implement obstacle avoidance using sick lidar, I would start a new question, so people with more experience than me can guide you. Also, you should check out the navigation stack http://ros.org/wiki/navigation/ lots of stuff already implemented there

Martin Peris gravatar imageMartin Peris ( 2012-08-15 15:07:12 -0500 )edit

Is there a way to use fromROSMsg in pcl_conversions or pcl_ros with Python? I have a Python node that subscribes to LaserScan data, converts the LaserScan to a PointCloud2 message, and need to extract the x,y data from this point cloud.

Ajay Jain gravatar imageAjay Jain ( 2015-02-09 19:46:22 -0500 )edit

I made a separate question: http://answers.ros.org/question/20278...

Ajay Jain gravatar imageAjay Jain ( 2015-02-09 19:57:28 -0500 )edit

What are the CmakeLists.txt and package codes for this project?

Tooght gravatar imageTooght ( 2016-12-02 16:47:36 -0500 )edit

https://pastebin.com/i71RQcU2 this is my full code,,

please have a look

the image view is crashing but i am getting values though,, also,, there is nothing in x y and z !!

zubair gravatar imagezubair ( 2017-04-20 05:15:35 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2012-08-13 17:31:41 -0500

Seen: 5,448 times

Last updated: Aug 13 '12