diff --git a/pc_ws/src/pcl_ros_processing/.gitignore b/pc_ws/src/pcl_ros_processing/.gitignore new file mode 100644 index 000000000..259148fa1 --- /dev/null +++ b/pc_ws/src/pcl_ros_processing/.gitignore @@ -0,0 +1,32 @@ +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app diff --git a/pc_ws/src/pcl_ros_processing/CMakeLists.txt b/pc_ws/src/pcl_ros_processing/CMakeLists.txt new file mode 100644 index 000000000..ba1502e7d --- /dev/null +++ b/pc_ws/src/pcl_ros_processing/CMakeLists.txt @@ -0,0 +1,210 @@ +cmake_minimum_required(VERSION 2.8.3) +project(pcl_ros_processing) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + pcl_conversions + pcl_ros + roscpp + sensor_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# sensor_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES pcl_ros_processing +# CATKIN_DEPENDS pcl_conversions pcl_ros roscpp sensor_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) +include_directories( + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(pcl-ros-processing +# src/${PROJECT_NAME}/pcl-ros-processing.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(pcl-ros-processing ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +add_executable(downsampling src/downsampling.cpp) +add_executable(planar_segmentation src/planar_segmentation.cpp) +add_executable(pcd_write src/pcd_write.cpp) +add_executable(color_filter_rgb src/color_filter_rgb.cpp) +add_executable(color_filter_hsv src/color_filter_hsv.cpp) + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(pcl-ros-processing_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(downsampling + ${catkin_LIBRARIES} +) + +target_link_libraries(planar_segmentation + ${catkin_LIBRARIES} +) + +target_link_libraries(pcd_write + ${catkin_LIBRARIES} +) + +target_link_libraries(color_filter_rgb + ${catkin_LIBRARIES} +) + +target_link_libraries(color_filter_hsv + ${catkin_LIBRARIES} +) + + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS pcl-ros-processing pcl-ros-processing_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_pcl-ros-processing.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/pc_ws/src/pcl_ros_processing/LICENSE b/pc_ws/src/pcl_ros_processing/LICENSE new file mode 100644 index 000000000..b56ab6992 --- /dev/null +++ b/pc_ws/src/pcl_ros_processing/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2017 karaage + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/pc_ws/src/pcl_ros_processing/README.md b/pc_ws/src/pcl_ros_processing/README.md new file mode 100644 index 000000000..2368348c6 --- /dev/null +++ b/pc_ws/src/pcl_ros_processing/README.md @@ -0,0 +1,69 @@ +# pcl_ros_processing +pcl sample program on ROS(Robot Operating System) + +# usage +Please prepare rgbd sensor(ex:Kinect, Realsense) and build this package + +## downsampling +Execute following command: +```sh +$ rosrun pcl_ros_processing downsampling input:=/camera/depth_registered/points +``` + +Execute following command for changing downsampling rate: +```sh +$ rosparam set down_rate 0.01 +``` +`down_rate` means voxel size + + +## planar segmentation +Execute following command: +```sh +$ rosrun pcl_ros_processing planar_segmentation input:=/camera/depth_registered/points +``` + +for rviz visualization +```sh +$ rosrun rviz rviz +``` + +for pcl viewer visualization +```sh +$ pcl_viewer save.pcd +``` +press `5` for color visualization + +## color filter +for rgb +```sh +$ rosrun pcl_ros_processing color_filter_rgb input:=/camera/depth_registered/points +``` + +for hsv +```sh +$ rosrun pcl_ros_processing color_filter_hsv input:=/camera/depth_registered/points +``` + +for rviz visualization +```sh +$ rosrun rviz rviz +``` + +for pcl viewer visualization +```sh +$ pcl_viewer save.pcd +``` +press `5` for color visualization + + +# References + +## planar segmentation +- http://wiki.ros.org/pcl/Overview +- http://pointclouds.org/documentation/tutorials/planar_segmentation.php#planar-segmentation +- https://staff.aist.go.jp/kanezaki.asako/pdf/SSII2016_AsakoKanezaki_tutorial.pdf +- http://ros-robot.blogspot.jp/2011/08/pclapi-point-cloud-library-pcl-pcl-api.html + +## color filter +- http://qiita.com/akachochin/items/47f1470565e76adb1880 diff --git a/pc_ws/src/pcl_ros_processing/launch/pcl_downsampling.launch b/pc_ws/src/pcl_ros_processing/launch/pcl_downsampling.launch new file mode 100644 index 000000000..39865af26 --- /dev/null +++ b/pc_ws/src/pcl_ros_processing/launch/pcl_downsampling.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/pc_ws/src/pcl_ros_processing/package.xml b/pc_ws/src/pcl_ros_processing/package.xml new file mode 100644 index 000000000..5d002c78e --- /dev/null +++ b/pc_ws/src/pcl_ros_processing/package.xml @@ -0,0 +1,60 @@ + + + pcl_ros_processing + 0.0.0 + The pcl_ros_processing package + + + + + karaage0703 + + + + + + MIT + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + pcl_conversions + pcl_ros + roscpp + sensor_msgs + libpcl-all-dev + pcl_conversions + pcl_ros + roscpp + sensor_msgs + libpcl-all + + + + + + + + diff --git a/pc_ws/src/pcl_ros_processing/rosbag_data/kinect_room.bag b/pc_ws/src/pcl_ros_processing/rosbag_data/kinect_room.bag new file mode 100644 index 000000000..c9a4cbad8 Binary files /dev/null and b/pc_ws/src/pcl_ros_processing/rosbag_data/kinect_room.bag differ diff --git a/pc_ws/src/pcl_ros_processing/rviz/rosbag.rviz b/pc_ws/src/pcl_ros_processing/rviz/rosbag.rviz new file mode 100644 index 000000000..92f660286 --- /dev/null +++ b/pc_ws/src/pcl_ros_processing/rviz/rosbag.rviz @@ -0,0 +1,151 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud21 + Splitter Ratio: 0.5 + Tree Height: 571 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /color_filter + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: camera_depth_frame + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 4.027906894683838 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.05775698274374008 + Y: 0.23111726343631744 + Z: -0.09710270911455154 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.08039818704128265 + Target Frame: + Value: Orbit (rviz) + Yaw: 6.223585605621338 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd000000040000000000000156000002c6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000027000002c6000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000027000002c6000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1200 + X: 472 + Y: 92 diff --git a/pc_ws/src/pcl_ros_processing/src/color_filter_hsv.cpp b/pc_ws/src/pcl_ros_processing/src/color_filter_hsv.cpp new file mode 100644 index 000000000..7a9b5b11e --- /dev/null +++ b/pc_ws/src/pcl_ros_processing/src/color_filter_hsv.cpp @@ -0,0 +1,101 @@ +#include +// PCL specific includes +#include +#include +#include +#include +#include +#include +#include +#include +#include + +ros::Publisher pub; + +void +cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input) +{ + // Define PointCloud2 data + pcl::PointCloud in_cloud, out_cloud; + // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud + pcl::fromROSMsg (*input, in_cloud); + + // setting alpha = 1.0 + for (size_t i = 0; i < in_cloud.points.size(); ++i){ + in_cloud.points[i].a = 255; + } + + // set frame id + out_cloud.header.frame_id = in_cloud.header.frame_id; + // std::cerr << "in_cloud frame id=" << in_cloud.header.frame_id << std::endl; + + // convert rgb to hsv + pcl::PointCloud hsv_cloud; + pcl::PointCloudXYZRGBAtoXYZHSV(in_cloud, hsv_cloud); + + // color parameter + int max_h, min_h, max_s, min_s, max_v, min_v; + + ros::param::get("max_h", max_h); + ros::param::get("min_h", min_h); + ros::param::get("max_s", max_s); + ros::param::get("min_s", min_s); + ros::param::get("max_v", max_v); + ros::param::get("min_v", min_v); + + // color filter + if(max_h > min_h){ + for (size_t i = 0; i < in_cloud.points.size(); ++i){ + if (hsv_cloud.points[i].h > min_h && hsv_cloud.points[i].h < max_h && + hsv_cloud.points[i].s > min_s && hsv_cloud.points[i].s < max_s && + hsv_cloud.points[i].v > min_v && hsv_cloud.points[i].v < max_v + ){ + out_cloud.push_back(in_cloud.points[i]); + } + } + } + + if(max_h < min_h){ + for (size_t i = 0; i < in_cloud.points.size(); ++i){ + if ((hsv_cloud.points[i].h > min_h && hsv_cloud.points[i].h < 255) || (hsv_cloud.points[i].h < max_h && hsv_cloud.points[i].h > 0) && + hsv_cloud.points[i].s > min_s && hsv_cloud.points[i].s < max_s && + hsv_cloud.points[i].v > min_v && hsv_cloud.points[i].v < max_v + ){ + out_cloud.push_back(in_cloud.points[i]); + } + } + } + + + // publish ros msg + sensor_msgs::PointCloud2 output; + if(out_cloud.size() > 0){ + toROSMsg(out_cloud, output); + pub.publish(output); + // save pcd file + pcl::io::savePCDFileASCII ("save.pcd", out_cloud); + } +} + +int +main (int argc, char** argv) +{ + // Initialize ROS + ros::init (argc, argv, "color_filter_hsv"); + ros::NodeHandle nh; + + ros::param::set("max_h", 255); + ros::param::set("min_h", 0); + ros::param::set("max_s", 255); + ros::param::set("min_s", 0); + ros::param::set("max_v", 255); + ros::param::set("min_v", 0); + + // Create a ROS subscriber for the input point cloud + ros::Subscriber sub = nh.subscribe ("input", 1, cloud_cb); + + // Create a ROS publisher for the model coefficients + pub = nh.advertise > ("color_filter", 1); + // Spin + ros::spin (); +} diff --git a/pc_ws/src/pcl_ros_processing/src/color_filter_rgb.cpp b/pc_ws/src/pcl_ros_processing/src/color_filter_rgb.cpp new file mode 100644 index 000000000..1fed8230f --- /dev/null +++ b/pc_ws/src/pcl_ros_processing/src/color_filter_rgb.cpp @@ -0,0 +1,82 @@ +#include +// PCL specific includes +#include +#include +#include +#include +#include +#include +#include +#include + +ros::Publisher pub; + +void +cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input) +{ + // Define PointCloud2 data + pcl::PointCloud in_cloud, out_cloud; + // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud + pcl::fromROSMsg (*input, in_cloud); + + // setting alpha = 1.0 + for (size_t i = 0; i < in_cloud.points.size(); ++i){ + in_cloud.points[i].a = 255; + } + + // set frame id + out_cloud.header.frame_id = in_cloud.header.frame_id; + // std::cerr << "in_cloud frame id=" << in_cloud.header.frame_id << std::endl; + + // color parameter + int max_r, min_r, max_g, min_g, max_b, min_b; + + ros::param::get("max_r", max_r); + ros::param::get("min_r", min_r); + ros::param::get("max_g", max_g); + ros::param::get("min_g", min_g); + ros::param::get("max_b", max_b); + ros::param::get("min_b", min_b); + + // color filter + for (size_t i = 0; i < in_cloud.points.size(); ++i){ + if (in_cloud.points[i].r > min_r && in_cloud.points[i].r < max_r && + in_cloud.points[i].g > min_g && in_cloud.points[i].g < max_g && + in_cloud.points[i].b > min_b && in_cloud.points[i].b < max_b + ){ + out_cloud.push_back(in_cloud.points[i]); + } + } + + // publish ros msg + sensor_msgs::PointCloud2 output; + if(out_cloud.size() > 0){ + toROSMsg(out_cloud, output); + pub.publish(output); + // save pcd file + pcl::io::savePCDFileASCII ("save.pcd", out_cloud); + } +} + +int +main (int argc, char** argv) +{ + // Initialize ROS + ros::init (argc, argv, "color_filter_rgb"); + ros::NodeHandle nh; + + ros::param::set("max_r", 255); + ros::param::set("min_r", 0); + ros::param::set("max_g", 255); + ros::param::set("min_g", 0); + ros::param::set("max_b", 255); + ros::param::set("min_b", 0); + + // Create a ROS subscriber for the input point cloud + ros::Subscriber sub = nh.subscribe ("input", 1, cloud_cb); + + // Create a ROS publisher for the model coefficients + pub = nh.advertise > ("color_filter", 1); + // Spin + ros::spin (); +} diff --git a/pc_ws/src/pcl_ros_processing/src/downsampling.cpp b/pc_ws/src/pcl_ros_processing/src/downsampling.cpp new file mode 100644 index 000000000..33af1593a --- /dev/null +++ b/pc_ws/src/pcl_ros_processing/src/downsampling.cpp @@ -0,0 +1,58 @@ +#include +// PCL specific includes +#include +#include +#include +#include +#include + +ros::Publisher pub; + +void +cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg) +{ + // Container for original & filtered data + pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2; + pcl::PCLPointCloud2ConstPtr cloudPtr(cloud); + pcl::PCLPointCloud2 cloud_filtered; + + // Convert to PCL data type + pcl_conversions::toPCL(*cloud_msg, *cloud); + + // Perform the actual filtering + pcl::VoxelGrid sor; + sor.setInputCloud (cloudPtr); + + double down_rate; + ros::param::get("down_rate", down_rate); + // sor.setLeafSize (0.1, 0.1, 0.1); + sor.setLeafSize (down_rate, down_rate, down_rate); + sor.filter (cloud_filtered); + + // Convert to ROS data type + sensor_msgs::PointCloud2 output; + pcl_conversions::fromPCL(cloud_filtered, output); + + // Publish the data + pub.publish (output); +} + +int +main (int argc, char** argv) +{ + // Initialize ROS + ros::init (argc, argv, "downsampling"); + ros::NodeHandle nh; + + // Set ROS param + ros::param::set("down_rate", 0.001); + + // Create a ROS subscriber for the input point cloud + ros::Subscriber sub = nh.subscribe ("input", 1, cloud_cb); + + // Create a ROS publisher for the output point cloud + pub = nh.advertise ("downsampling", 1); + + // Spin + ros::spin (); +} diff --git a/pc_ws/src/pcl_ros_processing/src/pcd_write.cpp b/pc_ws/src/pcl_ros_processing/src/pcd_write.cpp new file mode 100644 index 000000000..911b1d4fd --- /dev/null +++ b/pc_ws/src/pcl_ros_processing/src/pcd_write.cpp @@ -0,0 +1,35 @@ +#include +// PCL specific includes +// #include +#include +// #include +// #include + +ros::Publisher pub; + +void +cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg) +{ + pcl::PointCloud cloud; + pcl::fromROSMsg (*cloud_msg, cloud); + + pcl::io::savePCDFileASCII ("save.pcd", cloud); + std::cerr << "Saved " << cloud.points.size () << " data points to save_pcd.pcd." << std::endl; + + // for (size_t i = 0; i < cloud.points.size (); ++i) + // std::cerr << " " << cloud.points[i].x << " " << cloud.points[i].y << " " << cloud.points[i].z << std::endl; +} + +int +main (int argc, char** argv) +{ + // Initialize ROS + ros::init (argc, argv, "pcd_write"); + ros::NodeHandle nh; + + // Create a ROS subscriber for the input point cloud + ros::Subscriber sub = nh.subscribe ("input", 1, cloud_cb); + + // Spin + ros::spin (); +} diff --git a/pc_ws/src/pcl_ros_processing/src/planar_segmentation.cpp b/pc_ws/src/pcl_ros_processing/src/planar_segmentation.cpp new file mode 100644 index 000000000..ce9d2fb6f --- /dev/null +++ b/pc_ws/src/pcl_ros_processing/src/planar_segmentation.cpp @@ -0,0 +1,86 @@ +#include +// PCL specific includes +#include +#include +#include +#include +#include +#include +#include +#include + +ros::Publisher pub; + +void +cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input) +{ + // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud + pcl::PointCloud cloud; + pcl::fromROSMsg (*input, cloud); + + // setting alpha = 1.0 + for (size_t i = 0; i < cloud.points.size(); ++i){ + cloud.points[i].a = 255; + } + + + pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); + pcl::PointIndices::Ptr inliers (new pcl::PointIndices); + // Create the segmentation object + pcl::SACSegmentation seg; + + + // Optional + seg.setOptimizeCoefficients (true); + // Mandatory + seg.setModelType (pcl::SACMODEL_PLANE); + seg.setMethodType (pcl::SAC_RANSAC); + + double dist_th; + ros::param::get("dist_th", dist_th); + seg.setDistanceThreshold (dist_th); + + seg.setInputCloud (cloud.makeShared()); + seg.segment (*inliers, *coefficients); + + + if(inliers->indices.size() == 0){ + std::cerr << "Could not estimate a planar model for the given data" << std::endl; + }else{ + for (size_t i = 0; i < inliers->indices.size (); ++i) { + // std::cerr << inliers->indices[i] << " " << cloud.points[inliers->indices[i]].x << " " + // << cloud.points[inliers->indices[i]].y << " " + // << cloud.points[inliers->indices[i]].z << std::endl; + cloud.points[inliers->indices[i]].r = 255; + cloud.points[inliers->indices[i]].g = 0; + cloud.points[inliers->indices[i]].b = 0; + cloud.points[inliers->indices[i]].a = 255; + } + } + + + pub.publish(cloud); + + // save pcd file + pcl::io::savePCDFileASCII ("save.pcd", cloud); +} + + +int +main (int argc, char** argv) +{ + // Initialize ROS + ros::init (argc, argv, "planar_segmentation"); + ros::NodeHandle nh; + + // Set ROS param + ros::param::set("dist_th", 0.1); + + // Create a ROS subscriber for the input point cloud + ros::Subscriber sub = nh.subscribe ("input", 1, cloud_cb); + + // Create a ROS publisher for the model coefficients + pub = nh.advertise > ("planar_segmentation", 1); + // Spin + ros::spin (); +}