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

Question

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

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

数据是从返回 sensor_msgs::PointCloud2Ptr 的函数 PMDCamboardNano::getPointCloud() 中检索的。

在解决问题之前，我必须提到节点(分别是 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 节点的调试多么复杂!!!)。在我的订阅者被宣布之前添加 sleep 也无济于事。

这里是 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 将自身 Hook 到它(因为出于安全原因 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。 :-/我现在很不开心...

Answer 1

是的...PCL 1.7 和 C++11 根本不能很好地运行，除非你用 C++11 编译了库:

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

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