将 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 后出现奇怪的分段错误的主要内容,如果未能解决你的问题,请参考以下文章