c++ - Weird segmentation fault after converting a ROS PointCloud2 message to PCL PointCloud -
i have pmd camboard nano , i'm using this ros interface publish cloud data ros network. publisher inside nodelet is
ros::publisher points_publisher_ = nh.advertise<sensor_msgs::pointcloud2>("points_unrectified", 1); the data retrieve function pmdcamboardnano::getpointcloud() returns sensor_msgs::pointcloud2ptr.
before problem have mention data generated node (and nodelet respectively) valid , can view without problem inside rviz.
problem: when use subscriber 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 empty cloud message. skipping further processing"); return; } ros_info_stream("received cloud message " << msg->height * msg->width << " points"); ros_info("converting ros cloud message 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 outliers sor.filter(*pclcloud_filtered); ros_info_stream("filtering completed. cloud message " << (msg->height*msg->width - pclcloud_filtered->height*pclcloud_filtered->width) << " points outliers leaving " << pclcloud_filtered->height * pclcloud_filtered->width << " in total"); // 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 segmentation fault @ location declare statistical outlier remover. , have no idea causing it. think problem not in declaration lies somewhere deeper though cannot figure out where. cannot debug because node crashes instantly before can attach qt creator externally running process runs node (god, hate how complicated debugging of ros nodes!!!). adding sleep before subscriber declared doesn't either.
here another code repository on github. difference i'm reading file (instead of device), using publisher pcl_ros namespace , not 1 ros namespace , convert pcl cloud data straight pointcloud2 object , not pointcloud2ptr. still doesn't explain why segmentation fault occurring @ exact definition.
this cmakelists.txt 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 if path 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 built before 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}) # cloud handler receives point clouds either directly pmd publishing node (directly controlling # pmd device) or cloud relay. once point cloud message received, 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 # cloud relay receives output cloud publishing node (directly controlling pmd device) # , re-publish when user triggers it. used reduce amount of data # , regulate point clouds passed onto 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 launch external process, hook using ptrace (hat enable since ubuntu forbids useful things security reasons) , pause @ beginning of main (only problem can @ disassembly lol). below can see single step after applications paused @ 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 boost function causing (from 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 information very similar problem. seems issue pcl 1.7. pcl shipped ros indigo. :-/ unhappy right now...
yeap...pcl 1.7 , c++11 don't play @ unless have compiled libs c++11:
after removing flag cmakelists.txt runs smoothly. happy now...
Comments
Post a Comment