I have a PMD camboard nano and I'm using this ROS interface to publish cloud data to the ROS network. The publisher inside the nodelet is
ros::Publisher points_publisher_ = nh.advertise<sensor_msgs::PointCloud2>("points_unrectified", 1);
The data is retrieve from a function PMDCamboardNano::getPointCloud() which returns a sensor_msgs::PointCloud2Ptr.
Before I get to the problem I have to mention that the data generated by the node (and nodelet respectively) is valid and I can view it without a problem inside RViz.
Problem: When I use my subscriber I get a segmentation fault:
#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;
}
I get a segmentation fault at the location where I declare my statistical outlier remover. And I have no idea what is causing it. I think the problem is not in the declaration itself but lies somewhere deeper though I cannot figure out where. I cannot even debug because the node crashes instantly even before I can attach my Qt Creator to the externally running process that runs the node (God, I hate how complicated debugging of ROS nodes!!!). Adding a sleep before my subscriber is even declared doesn't help either.
Here is another code from my repository on GitHub. The only difference is that I'm reading from a file (instead of a device), using a publisher from the pcl_ros namespace and not the one from the ros namespace and finally I convert my PCL cloud data straight to a PointCloud2 object and not a PointCloud2Ptr. This still doesn't explain why the segmentation fault is occurring at that exact definition.
This is my CMakeLists.txt for the project:
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)
EDIT:
Ah, Qt Creator can actually launch an external process, hook itself to it using ptrace (hat to enable this since Ubuntu forbids useful things like this for security reasons) and pause at the beginning of main (only problem is that I can only look at the disassembly LOL). Below you can see a single step into after the applications was paused at 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)
It seems that a boost function is causing this (from what I can tell...):
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 ??
EDIT 2: Getting closer. Here is some information about a very similar problem. It seems that this is an issue with PCL 1.7. However this is the PCL that is shipped with ROS Indigo. :-/ I am very unhappy right now...
