transformLaserScanToPointCloud segfault
I have a laser scan coming in from SICK LMS200. I convert that scan to a point cloud as shown in the below function:
bool LaserScanListener::convertScanToPointCloud(sensor_msgs::LaserScan& scan){
try
{
if(!buffer_.canTransform(base_link_, scan.header.frame_id,
scan.header.stamp + ros::Duration(scan.scan_time),
ros::Duration(TRANSFORM_WAIT_TIME)))
{
ROS_WARN_STREAM_THROTTLE(1.0, "LaserScanListener::convertScanToPointCloud: Failed to lookup tf from '"
<< scan.header.frame_id << "' to '" << base_link_);
return false;
}
lprojector_.transformLaserScanToPointCloud("base_link", scan, cloud, buffer_);
}
catch (const tf::TransformException &e)
{
ROS_WARN_STREAM_THROTTLE(1.0, "LaserScanListener::convertScanToPointCloud: TF returned a transform exception: " << e.what());
return false;
}
try
{
pcl::PCLPointCloud2 pcl_pcl2;
pcl_conversions::toPCL(cloud, pcl_pcl2);
pcl::PointCloud<pcl::PointXYZ> pcl_cloud;
pcl::fromPCLPointCloud2(pcl_pcl2, pcl_cloud);
pcl_cloud_ = boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(pcl_cloud);
}
catch (const pcl::PCLException &e)
{
ROS_WARN_STREAM_THROTTLE( 1.0, "LaserScanListener::convertScanToPointCloud: Failed to convert a message to pcl type." << e.what());
return false;
}
return true;
}
When running on the gazebo simulator this code block works fine, but on a real robot, segfault occurs. Below is the stack trace for the segfault:
#0 0x00007ffff46fc428 in __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:54
#1 0x00007ffff46fe02a in __GI_abort () at abort.c:89
#2 0x00007ffff473e7ea in __libc_message (do_abort=do_abort@entry=2, fmt=fmt@entry=0x7ffff4857ed8 "*** Error in `%s': %s: 0x%s ***\n") at ../sysdeps/posix/libc_fatal.c:175
#3 0x00007ffff474737a in malloc_printerr (ar_ptr=<optimized out>, ptr=<optimized out>, str=0x7ffff4858030 "free(): invalid next size (normal)", action=3) at malloc.c:5006
#4 _int_free (av=<optimized out>, p=<optimized out>, have_lock=0) at malloc.c:3867
#5 0x00007ffff474b53c in __GI___libc_free (mem=<optimized out>) at malloc.c:2968
#6 0x00007ffff79b5cbd in laser_geometry::LaserProjection::projectLaser_(sensor_msgs::LaserScan_<std::allocator<void> > const&, sensor_msgs::PointCloud2_<std::allocator<void> >&, double, int) ()
from /opt/ros/kinetic/lib/liblaser_geometry.so
#7 0x00007ffff79b6f0a in laser_geometry::LaserProjection::transformLaserScanToPointCloud_(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, sensor_msgs::LaserScan_<std::allocator<void> > const&, sensor_msgs::PointCloud2_<std::allocator<void> >&, tf2::Quaternion, tf2::Vector3, tf2::Quaternion, tf2::Vector3, double, int) () from /opt/ros/kinetic/lib/liblaser_geometry.so
#8 0x00007ffff79b8d48 in laser_geometry::LaserProjection::transformLaserScanToPointCloud_(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, sensor_msgs::LaserScan_<std::allocator<void> > const&, sensor_msgs::PointCloud2_<std::allocator<void> >&, tf2::BufferCore&, double, int) () from /opt/ros/kinetic/lib/liblaser_geometry.so
#9 0x000000000042d8d0 in laser_geometry::LaserProjection::transformLaserScanToPointCloud (channel_options=3, range_cutoff=-1, tf=..., cloud_out=..., scan_in=..., target_frame="base_link", this=0x7fffffffc3c0)
at /opt/ros/kinetic/include/laser_geometry/laser_geometry.h:278
#10 LaserScanListener::convertScanToPointCloud (this=this@entry=0x7fffffffc250, scan=...) at /home/ucmrobotics/inter_det_ws/src/intersection_detection/src/laserScanListener.cpp:97
#11 0x000000000042ea25 in LaserScanListener::scanCallback (this=0x7fffffffc250, scan_in=...) at /home/ucmrobotics/inter_det_ws/src/intersection_detection/src/laserScanListener.cpp:40
Could anybody please help out how to solve this error?
Not an answer, but just making sure: can you not use the nodes in laser_assembler?
do you mean the laser projector? i would require that to convert the scan into point clouds. I need the conversion to point clouds and the scan both simultaneously as I would be processing them both at the same time for one time instance. code at [ https://github.com/harsha336/intersec... ]
re: laser projector: no, the
laser_scan_assembler
node. It takes inLaserScan
msgs and outputsPointCloud2
. Which seems to be partly what you're after.@gvdhoorn Thank you for the suggestions. I was able to figure out the issue. when the projector is converting to point cloud in the simulator(used a different PC), space was allocated to the 'z' coordinate as well even if a 2d laser was used. but in the robot with sick laser no space was allocated
i was using the z of the point cloud as a flag to indicate the kind of point. This was causing the segfault. Thanks again for the directions/suggestions