将 ROS PointCloud2 消息转换为 PCL PointCloud 后出现奇怪的分段错误

Posted

技术标签:

【中文标题】将 ROS PointCloud2 消息转换为 PCL PointCloud 后出现奇怪的分段错误【英文标题】:Weird segmentation fault after converting a ROS PointCloud2 message to PCL PointCloud 【发布时间】:2015-08-20 13:21:53 【问题描述】:

我有一个 PMD camboard nano,我正在使用 this ROS interface 将云数据发布到 ROS 网络。 nodelet内部的发布者是

ros::Publisher points_publisher_ = nh.advertise<sensor_msgs::PointCloud2>("points_unrectified", 1);

从函数PMDCamboardNano::getPointCloud() 中检索数据,该函数返回sensor_msgs::PointCloud2Ptr

在我解决这个问题之前,我必须提到节点(分别是 nodelet)生成的数据是有效的,我可以在 RViz 中毫无问题地查看它。

问题:当我使用我的订阅者时,我遇到了分段错误:

#include <ros/ros.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
//#include <pcl_ros/publisher.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <ros/publisher.h>
#include <string>

class CloudSubscriber

protected:
  ros::NodeHandle nh;
  ros::Subscriber sub;

private:
  sensor_msgs::PointCloud2 cloud;

public:
  CloudSubscriber()
  
    sub = nh.subscribe<sensor_msgs::PointCloud2>("/camera/points_unrectified", 5, &CloudSubscriber::subCallback, this);
  

  ~CloudSubscriber()
  
    sub.shutdown();
  

  void subCallback(const sensor_msgs::PointCloud2ConstPtr& msg)
  
    if(msg->data.empty())
    
      ROS_WARN("Received an empty cloud message. Skipping further processing");
      return;
    

    ROS_INFO_STREAM("Received a cloud message with " << msg->height * msg->width << " points");
    ROS_INFO("Converting ROS cloud message to PCL compatible data structure");
    pcl::PointCloud<pcl::PointXYZ> pclCloud;
    pcl::fromROSMsg(*msg, pclCloud);
    pcl::PointCloud<pcl::PointXYZ>::Ptr p(new pcl::PointCloud<pcl::PointXYZ>(pclCloud));
    pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;  // PROBLEM!!!
    sor.setInputCloud(p);
    sor.setMeanK(50);
    sor.setStddevMulThresh(1.0);
    ROS_INFO("Filtering cloud data");
    // Remove the outliers
    sor.filter(*pclCloud_filtered);
    ROS_INFO_STREAM("Filtering completed. a cloud message with " << (msg->height*msg->width - pclCloud_filtered->height*pclCloud_filtered->width) << " points as outliers leaving " << pclCloud_filtered->height * pclCloud_filtered->width << " in total");

    // Do something with the filtered PCL cloud
    //pcl::io::savePCDFileBinaryCompressed("inliers.pcd", *pclCloud_filtered);
  
;

int main(int argc, char* argv[])

  ros::init (argc, argv, "cloud_subscriber");
  ros::NodeHandle nh;

  CloudSubscriber c;

  while(nh.ok())
    ros::spin();

  return 0;

我在声明统计异常值去除器的位置出现分段错误。而且我不知道是什么原因造成的。我认为问题不在于声明本身,而在于更深层次的地方,尽管我不知道在哪里。我什至无法调试,因为即使在我将 Qt Creator 附加到运行该节点的外部运行进程之前,该节点也会立即崩溃(天哪,我讨厌 ROS 节点的调试有多复杂!!!)。在我的订阅者被声明之前添加睡眠也无济于事。

这里是another code from my repository on GitHub。唯一的区别是我从文件(而不是设备)读取,使用 pcl_ros 命名空间 中的发布者,而不是 ros 命名空间中的发布者,最后我将我的 PCL 云数据直接转换为 PointCloud2 对象,而不是 PointCloud2Ptr。这仍然不能解释为什么在该确切定义下会发生分段错误。

这是我的项目的 CMakeLists.txt:

cmake_minimum_required(VERSION 2.8.3)
project(pmd_camboard_nano)

set(CMAKE_CXX_FLAGS "--std=gnu++11 $CMAKE_CXX_FLAGS")

find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs image_transport nodelet dynamic_reconfigure tf)

generate_dynamic_reconfigure_options(cfg/PMD.cfg)

catkin_package(
    INCLUDE_DIRS include
    CATKIN_DEPENDS roscpp sensor_msgs image_transport nodelet dynamic_reconfigure tf pcl_conversions pcl_msgs pcl_ros
    DEPENDS boost_system PCL
)

# Set PMDSDK Requirements
set(PMDSDK_ROOT_DIR $PROJECT_SOURCE_DIR/PMDSDK) # Change this if the path is different
set(CMAKE_MODULE_PATH $CMAKE_MODULE_PATH "$PROJECT_SOURCE_DIR/cmake/Modules/")
# Register PMDSDK
find_package(PMDSDK REQUIRED)

# Register Boost
find_package(Boost REQUIRED COMPONENTS system)
# Register PCL
find_package(PCL REQUIRED COMPONENTS common io filters)

include_directories($PCL_INCLUDE_DIRS)
link_directories($PCL_LIBRARY_DIRS)
add_definitions($PCL_DEFINITIONS)

#########
# BUILD #
#########
add_definitions(-DPMD_PLUGIN_DIR="$PMDSDK_PLUGIN_DIR/")
include_directories(include $catkin_INCLUDE_DIRS $PMDSDK_INCLUDE_DIR $Boost_INCLUDE_DIRS $PCL_INCLUDE_DIRS)

# make sure configure headers are built before any node using them
#set(CMAKE_LIBRARY_OUTPUT_DIRECTORY $CMAKE_BINARY_DIR)
message($CATKIN_DEVEL_PREFIX)
add_library($PROJECT_NAME_nodelet src/driver_nodelet.cpp src/pmd_camboard_nano.cpp)
target_link_libraries($PROJECT_NAME_nodelet $catkin_LIBRARIES $PMDSDK_LIBRARIES)
add_dependencies($PROJECT_NAME_nodelet $PROJECT_NAME_gencfg)

add_executable($PROJECT_NAME_node src/driver_node)
target_link_libraries($PROJECT_NAME_node $catkin_LIBRARIES $PMDSDK_LIBRARIES)
add_dependencies($PROJECT_NAME_node $PROJECT_NAME_gencfg)
add_dependencies($PROJECT_NAME_node $$PROJECT_NAME_EXPORTED_TARGETS)


# The cloud handler receives point clouds either directly from the PMD publishing node (directly controlling
# the PMD device) or from the cloud relay. Once a point cloud message is received, it is further processed
add_executable($PROJECT_NAME_cloud_handler src/pmd_camboard_nano_cloud_handler)
target_link_libraries($PROJECT_NAME_cloud_handler $catkin_LIBRARIES boost_system pthread pcl_common pcl_io pcl_filters)

# TODO
# The cloud relay receives the output from the cloud publishing node (directly controlling the PMD device)
# and re-publish it ONLY when the user triggers it. This is basically used to reduce the amount of data
# and also to regulate which point clouds are passed onto the cloud handler
#add_executable($PROJECT_NAME_cloud_relay src/pmd_camboard_nano_cloud_relay)
#target_link_libraries($PROJECT_NAME_cloud_relay $catkin_LIBRARIES #boost_system pthread pcl_common pcl_io pcl_filters)

编辑:

啊,Qt Creator 实际上可以启动一个外部进程,使用 ptrace 将自己连接到它(帽子启用此功能,因为 Ubuntu 出于安全原因禁止此类有用的东西)并在 main 开头暂停(唯一的问题是我可以只看拆机LOL)。下面你可以看到应用程序在 main() 处暂停后的一个步骤:

        Function: _ZN5boost4math7lanczos19lanczos_initializerINS1_12lanczos17m64EeE4initC2Ev
0x7f7b4feb6c3c  <+0x02cc>         rex.B std
0x7f7b4feb6c3e  <+0x02ce>         (bad)
0x7f7b4feb6c3f  <+0x02cf>         (bad)
0x7f7b4feb6c40  <+0x02d0>         fldt   0x53028a(%rip)        # 0x7f7b503e6ed0
0x7f7b4feb6c46  <+0x02d6>         mov    0x8dcf0b(%rip),%rax        # 0x7f7b50793b58
0x7f7b4feb6c4d  <+0x02dd>         mov    %rbx,%rdi
0x7f7b4feb6c50  <+0x02e0>         fstpt  (%rax) <------ Qt Creator points here
0x7f7b4feb6c52  <+0x02e2>         fldt   0x530288(%rip)        # 0x7f7b503e6ee0
0x7f7b4feb6c58  <+0x02e8>         fstpt  0x10(%rax)
0x7f7b4feb6c5b  <+0x02eb>         fldt   0x53028f(%rip)        # 0x7f7b503e6ef0
0x7f7b4feb6c61  <+0x02f1>         fstpt  0x20(%rax)
0x7f7b4feb6c64  <+0x02f4>         fldt   0x530296(%rip)        # 0x7f7b503e6f00
0x7f7b4feb6c6a  <+0x02fa>         fstpt  0x30(%rax)
0x7f7b4feb6c6d  <+0x02fd>         fldt   0x53029d(%rip)        # 0x7f7b503e6f10
0x7f7b4feb6c73  <+0x0303>         fstpt  0x40(%rax)
0x7f7b4feb6c76  <+0x0306>         fldt   0x5302a4(%rip)        # 0x7f7b503e6f20
0x7f7b4feb6c7c  <+0x030c>         fstpt  0x50(%rax)
0x7f7b4feb6c7f  <+0x030f>         fldt   0x5302ab(%rip)        # 0x7f7b503e6f30
0x7f7b4feb6c85  <+0x0315>         fstpt  0x60(%rax)
0x7f7b4feb6c88  <+0x0318>         fldt   0x5302b2(%rip)        # 0x7f7b503e6f40
0x7f7b4feb6c8e  <+0x031e>         fstpt  0x70(%rax)
0x7f7b4feb6c91  <+0x0321>         fldt   0x5302b9(%rip)        # 0x7f7b503e6f50
0x7f7b4feb6c97  <+0x0327>         fstpt  0x80(%rax)
0x7f7b4feb6c9d  <+0x032d>         fldt   0x5302bd(%rip)        # 0x7f7b503e6f60
0x7f7b4feb6ca3  <+0x0333>         fstpt  0x90(%rax)
0x7f7b4feb6ca9  <+0x0339>         fldt   0x5302c1(%rip)        # 0x7f7b503e6f70
0x7f7b4feb6caf  <+0x033f>         fstpt  0xa0(%rax)

似乎是一个增强功能导致了这个(据我所知......):

0 boost::math::lanczos::lanczos_initializer<boost::math::lanczos::lanczos17m64, long double>::init::init()  /usr/lib/libpcl_sample_consensus.so.1.7     0x7f7b4feb6c50  
1   ??  /usr/lib/libpcl_sample_consensus.so.1.7     0x7f7b4fe8e6de  
2   call_init       78  0x7f7b541b413a  
3   call_init       36  0x7f7b541b4223  
4   _dl_init        126 0x7f7b541b4223  
5   _dl_start_user  /lib64/ld-linux-x86-64.so.2     0x7f7b541a530a  
6   ??          0x1 
7   ??          0x7ffc39abecce  
8   ??              

编辑 2: 越来越近了。以下是有关very similar problem 的一些信息。这似乎是 PCL 1.7 的一个问题。然而,这是随 ROS Indigo 一起提供的 PCL。 :-/ 我现在很不开心...

【问题讨论】:

【参考方案1】:

是的...PCL 1.7 和 C++11 根本无法正常运行,除非您使用 C++11 编译了库:

http://answers.ros.org/question/194699/segmentation-fault-when-using-correspondencerejectorsampleconsensus/

从我的 CMakeLists.txt 中删除标志后,一切运行顺利。我现在很开心……

【讨论】:

以上是关于将 ROS PointCloud2 消息转换为 PCL PointCloud 后出现奇怪的分段错误的主要内容,如果未能解决你的问题,请参考以下文章

Ros发布点云文件

ROS 可视化: 发布PointCloud2点云数据到Rviz

PCL数据类型和ROS数据类型的转换

Ros发布图像消息

Ros发布图像消息

将xtion pro的depthimage转换为LaserScan