├── .gitignore ├── CMakeLists.txt ├── README.org ├── launch ├── nodelet_demo.rviz ├── nodelet_table_extraction.launch ├── table_rotated.pcd └── table_scene_lms400.pcd ├── msg └── PointArray.msg ├── package.xml └── src ├── cluster_extractor.cpp ├── plot_point_array.py ├── point_publisher.cpp └── table_cutoff_settings.py /.gitignore: -------------------------------------------------------------------------------- 1 | # ignore these files 2 | *.pcd 3 | 4 | # ignore these directories and the files in them. 5 | *~ 6 | bin/ 7 | build/ 8 | data/ 9 | *.html 10 | 11 | 12 | # github ignore suggestions... 13 | 14 | # Compiled source # 15 | ################### 16 | *.com 17 | *.class 18 | *.dll 19 | *.exe 20 | *.o 21 | *.so 22 | 23 | # Packages # 24 | ############ 25 | # it's better to unpack these files and commit the raw source 26 | # git has its own built in compression methods 27 | *.7z 28 | *.dmg 29 | *.gz 30 | *.iso 31 | *.jar 32 | *.rar 33 | *.tar 34 | *.zip 35 | 36 | # Logs and databases # 37 | ###################### 38 | *.log 39 | *.sql 40 | *.sqlite 41 | 42 | # OS generated files # 43 | ###################### 44 | .DS_Store? 45 | ehthumbs.db 46 | Icon? 47 | Thumbs.db 48 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(nodelet_pcl_demo) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | message_generation 7 | pcl_ros 8 | tf 9 | tf_conversions 10 | geometry_msgs) 11 | 12 | find_package(PCL 1.3 REQUIRED) 13 | include_directories(${PCL_INCLUDE_DIRS}) 14 | link_directories(${PCL_LIBRARY_DIRS}) 15 | add_definitions(${PCL_DEFINITIONS}) 16 | 17 | add_message_files( 18 | FILES 19 | PointArray.msg 20 | ) 21 | 22 | generate_messages( 23 | DEPENDENCIES 24 | geometry_msgs 25 | sensor_msgs 26 | std_msgs 27 | ) 28 | 29 | 30 | catkin_package() 31 | 32 | 33 | include_directories(include 34 | ${catkin_INCLUDE_DIRS} 35 | ) 36 | 37 | add_executable(cluster_extractor src/cluster_extractor.cpp) 38 | add_dependencies(cluster_extractor ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 39 | target_link_libraries(cluster_extractor ${catkin_LIBRARIES} ${PCL_LIBRARIES}) 40 | 41 | add_executable(point_publisher src/point_publisher.cpp) 42 | add_dependencies(point_publisher ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 43 | target_link_libraries(point_publisher ${catkin_LIBRARIES} ${PCL_LIBRARIES}) 44 | 45 | catkin_install_python(PROGRAMS src/plot_point_array.py src/table_cutoff_settings.py 46 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 47 | -------------------------------------------------------------------------------- /README.org: -------------------------------------------------------------------------------- 1 | * Demo: Using PCL Nodelets 2 | 3 | This is a simple package that uses a few nodelets from the [[http://wiki.ros.org/pcl_ros][pcl_ros]] ROS 4 | package. The basic idea is that we've constructed a pipeline of pointcloud 5 | processing steps where the overall goal is to extract a few objects sitting on 6 | a tabletop into separate point clouds and calculate the centroid of each of 7 | these clouds. Most of the processing steps use nodelets and the nodelets 8 | operate on =sensor_msgs/PointCloud2= messages (that might come from an Intel D435i) 9 | In this demo, you can either use live data from a 10 | device, or the package includes a [[http://pointclouds.org/documentation/tutorials/pcd_file_format.php][PCD file]] that can be used if you don't have 11 | access to a device. 12 | 13 | ** High Level Description 14 | 15 | The majority of the package is implemented in a single launch file called 16 | [[file:launch/nodelet_table_extraction.launch][nodelet_table_extraction.launch]]. At a high level, the pipeline follows these 17 | steps: 18 | 1. Either a depth sensor device or a PCD file is used to publish a 19 | =sensor_msgs/PointCloud2= on the =/camera/depth/points= topic. 20 | 2. A [[https://github.com/ros-perception/perception_pcl/blob/lunar-devel/pcl_ros/src/pcl_ros/filters/crop_box.cpp][pcl/CropBox filter nodelet]] is used to throw away all points that exist 21 | outside of a cubic volume described by maximum and minimum bounds in x,y,z 22 | coordinate axes. 23 | 3. The output of the =pcl/CropBox= filter is fed into a [[https://github.com/ros-perception/perception_pcl/blob/lunar-devel/pcl_ros/src/pcl_ros/filters/voxel_grid.cpp][pcl/VoxelGrid filter nodelet]] 24 | that downsamples the point cloud to a coarser point cloud that uses less data. 25 | 4. The output of the =pcl/VoxelGrid= nodelet is fed into a 26 | [[https://github.com/ros-perception/perception_pcl/blob/lunar-devel/pcl_ros/src/pcl_ros/segmentation/sac_segmentation.cpp][pcl/SACSegmentation nodelet]] nodelet that is used to extract a planar model 27 | from the point cloud. If your =pcl/CropBox= filter parameters have been 28 | tuned correctly, this plane should be the surface of the table top. This 29 | nodelet produces two things (i) a =pcl_msgs/PointIndices= message 30 | containing the indices of the points in the input cloud that belong to the 31 | plane, and (ii) the model parameters of the plane published as a 32 | =pcl_msgs/ModelCoefficients= (the plane is parameterized as 33 | /ax+by+cz+d=0/). These parameters represent a least-squares plane that 34 | best fits the points that belong to the plane. 35 | 5. The indices are fed into two [[https://github.com/ros-perception/perception_pcl/blob/lunar-devel/pcl_ros/src/pcl_ros/filters/extract_indices.cpp][pcl/ExtractIndices filter nodelets]] that are 36 | used to create point clouds (as opposed to lists of indices) of the points 37 | that belong to the plane, and the points that don't belong to the plane. 38 | These point clouds are not strictly necessary, but they do help 39 | visualization. 40 | 6. The [[file:src/table_cutoff_settings.py][table_cutoff_settings.py node]] subscribes to the model coefficients 41 | from the plane extraction. Every time a client calls the =std_srvs/Empty= 42 | service that this node offers (called =/table_cutoff/update_table_model=), 43 | this node updates its internal model of where the table is. It provides 44 | this information to the ROS world by broadcasting a =/tf= transform from 45 | the frame the original cloud is expressed in 46 | (=camera_depth_optical_frame=) to a frame called =table_frame=. 47 | 7. A [[https://github.com/ros-perception/perception_pcl/blob/lunar-devel/pcl_ros/src/pcl_ros/filters/passthrough.cpp][pcl/PassThrough filter nodelet]] is then used to throw away all points 48 | that are at or below the plane extracted during plane extraction. This is 49 | done by setting the =input_frame= private parameter for the nodelet to be 50 | =table_frame=. Thus, if the =table_cutoff_settings.py= node has provided 51 | the transform to the =table_frame=, then the limits of the pass through 52 | filter are easy to calculate. 53 | 8. The output of the =pcl/PassThrough= nodelet is fed into a 54 | [[https://github.com/ros-perception/perception_pcl/blob/lunar-devel/pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp][pcl/StatisticalOutlierRemoval filter nodelet]] that helps to remove random 55 | stray points. 56 | 9. The output of the =pcl/StatisticalOutlierRemoval= filter is fed into the 57 | [[file:src/cluster_extractor.cpp][cluster_extraction node]]. This simple C++ node subscribes to the output of 58 | the =StatisticalOutlierRemoval= filter, it converts this cloud into a PCL 59 | datatype, and then it calls 60 | =pcl::EuclideanClusterExtraction.extract= to produce a 61 | =std::vector=. Each element in this =std_vector= is of 62 | type =pcl::PointIndices=, and the values in the =pcl::PointIndices= 63 | entries represent the points in the original cloud that belong to each 64 | cluster detected during cluster extraction. This data is used to construct 65 | point clouds for up to =MAX_CLUSTERS= and publish them on separate topics 66 | (named =/cluster_X_cloud=). The centroids of each of these cluster clouds 67 | are also computed and published as =geometry_msgs/PointStamped= messages 68 | (on the =/cluster_X_point= topics) and sent as =/tf= data. 69 | 70 | ** Usage 71 | 72 | *** PCD Data 73 | 74 | To run using the [[file:launch/table_rotated.pcd][included PCD file]] simply run 75 | #+BEGIN_SRC html 76 | roslaunch nodelet_pcl_demo nodelet_table_extraction.launch pcd:=true 77 | #+END_SRC 78 | This will use the =pcd_to_pointcloud= node from the =pcl_ros= package to read 79 | the PCD file, and repeatedly publish it on the =/camera/depth/points= topic 80 | (at approx. 20Hz). The included PCD file is a slightly modified version of 81 | the [[https://raw.github.com/PointCloudLibrary/data/master/tutorials/table_scene_lms400.pcd][table_scene_lms400.pcd]] file used in the [[http://pointclouds.org/documentation/tutorials/cluster_extraction.php#cluster-extraction][PCL Euclidean Cluster Extraction Tutorial]]. The only difference is that I've rotated the raw data by π radians about the x axis. This was to ensure that the coordinate systems when using the PCD data agreed with the data one would get when using =openni_launch= or =openni2_launch=. 82 | 83 | Once all of the nodes/nodelets are up-and-running, you can call the 84 | =/table_cutoff/update_table_model= service to update the location of the 85 | table's plane in the ROS world. This can be done by running 86 | #+BEGIN_SRC sh 87 | rosservice call /table_cutoff/update_table_model "{}" 88 | #+END_SRC 89 | 90 | In =rviz= you should be able to see the output of all of the steps of the 91 | pipeline described above (all steps have labeled displays in the =rviz= 92 | config that is automatically loaded). 93 | 94 | *** Live Data 95 | #+BEGIN_SRC sh 96 | roslaunch nodelet_pcl_demo nodelet_table_extraction.launch source:= 97 | #+END_SRC 98 | 99 | The =source= parameter determines where the =sensor_msgs/PointCloud2= data comes from. 100 | 1. Use "openni" for an openni device such as a [[https://www.asus.com/us/3D-Sensor/Xtion_PRO_LIVE/][ASUS Xtion PRO LIVE]]. 101 | 2. Use "realsense" for an Intel RealSense 102 | 3. Use "pcd" to open the test data. 103 | 4. Use "none" to launch your own camera source. For example, 104 | - Kinect for Box 360 using [[http://wiki.ros.org/freenect_launch][freenect_launch]] 105 | - Kinect for Xbox One using [[https://github.com/code-iai/iai_kinect2][iai_kinect2]] 106 | 6. The =min_z= argument sets the minimum distance between table and camera. Default is 0.5m 107 | You need to call the =/table_cutoff/update_table_model= service to update 108 | the geometry of the table. When using live data, it makes sense to look at the =PlanarInlierCloud= in =rviz= to ensure that you are accurately 109 | fitting the desired plane before calling this service. As before, the 110 | service can be called at the command line using 111 | #+BEGIN_SRC sh 112 | rosservice call /table_cutoff/update_table_model "{}" 113 | #+END_SRC 114 | 115 | When using live data, you may have to tune some of the parameters in the 116 | launch file to achieve the desired performance. Note that many of the 117 | parameters can be tuned in real time using [[http://wiki.ros.org/rqt_reconfigure][rqt_reconfigure]]. 118 | 119 | 120 | 121 | 122 | 123 | 124 | -------------------------------------------------------------------------------- /launch/nodelet_demo.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /TF1/Frames1 9 | - /FullCloud1 10 | - /CropBoxOutput1 11 | - /VoxelCloud1 12 | - /PlaneInlierCloud1 13 | - /AboveTableCloudFilt1 14 | - /Cluster11 15 | - /Cluster21 16 | Splitter Ratio: 0.4497816562652588 17 | Tree Height: 845 18 | - Class: rviz/Selection 19 | Name: Selection 20 | - Class: rviz/Tool Properties 21 | Expanded: 22 | - /2D Pose Estimate1 23 | - /2D Nav Goal1 24 | - /Publish Point1 25 | Name: Tool Properties 26 | Splitter Ratio: 0.5886790156364441 27 | - Class: rviz/Views 28 | Expanded: 29 | - /Current View1 30 | Name: Views 31 | Splitter Ratio: 0.5 32 | - Class: rviz/Time 33 | Experimental: false 34 | Name: Time 35 | SyncMode: 0 36 | SyncSource: AboveTableCloudFilt 37 | Preferences: 38 | PromptSaveOnExit: true 39 | Toolbars: 40 | toolButtonStyle: 2 41 | Visualization Manager: 42 | Class: "" 43 | Displays: 44 | - Alpha: 0.5 45 | Cell Size: 1 46 | Class: rviz/Grid 47 | Color: 160; 160; 164 48 | Enabled: true 49 | Line Style: 50 | Line Width: 0.029999999329447746 51 | Value: Lines 52 | Name: Grid 53 | Normal Cell Count: 0 54 | Offset: 55 | X: 0 56 | Y: 0 57 | Z: 0 58 | Plane: XY 59 | Plane Cell Count: 10 60 | Reference Frame: 61 | Value: true 62 | - Class: rviz/TF 63 | Enabled: true 64 | Frame Timeout: 1 65 | Frames: 66 | All Enabled: false 67 | camera_color_frame: 68 | Value: true 69 | camera_color_optical_frame: 70 | Value: true 71 | camera_depth_frame: 72 | Value: false 73 | camera_depth_optical_frame: 74 | Value: true 75 | camera_link: 76 | Value: false 77 | cluster_1_frame: 78 | Value: true 79 | cluster_2_frame: 80 | Value: true 81 | cluster_3_frame: 82 | Value: true 83 | table_frame: 84 | Value: true 85 | Marker Alpha: 1 86 | Marker Scale: 1 87 | Name: TF 88 | Show Arrows: false 89 | Show Axes: true 90 | Show Names: false 91 | Tree: 92 | camera_link: 93 | camera_color_frame: 94 | camera_color_optical_frame: 95 | {} 96 | camera_depth_frame: 97 | camera_depth_optical_frame: 98 | cluster_1_frame: 99 | {} 100 | cluster_2_frame: 101 | {} 102 | cluster_3_frame: 103 | {} 104 | table_frame: 105 | {} 106 | Update Interval: 0 107 | Value: true 108 | - Alpha: 0.20000000298023224 109 | Autocompute Intensity Bounds: true 110 | Autocompute Value Bounds: 111 | Max Value: 0.4520000219345093 112 | Min Value: 0.2850000262260437 113 | Value: true 114 | Axis: Z 115 | Channel Name: z 116 | Class: rviz/PointCloud2 117 | Color: 255; 255; 255 118 | Color Transformer: AxisColor 119 | Decay Time: 0 120 | Enabled: false 121 | Invert Rainbow: false 122 | Max Color: 255; 255; 255 123 | Max Intensity: 2.5980000495910645 124 | Min Color: 0; 0; 0 125 | Min Intensity: 1.2190001010894775 126 | Name: FullCloud 127 | Position Transformer: XYZ 128 | Queue Size: 10 129 | Selectable: true 130 | Size (Pixels): 1 131 | Size (m): 0.009999999776482582 132 | Style: Flat Squares 133 | Topic: /camera/depth/points 134 | Unreliable: false 135 | Use Fixed Frame: true 136 | Use rainbow: true 137 | Value: false 138 | - Alpha: 0.20000000298023224 139 | Autocompute Intensity Bounds: true 140 | Autocompute Value Bounds: 141 | Max Value: 10 142 | Min Value: -10 143 | Value: true 144 | Axis: Z 145 | Channel Name: intensity 146 | Class: rviz/PointCloud2 147 | Color: 0; 170; 255 148 | Color Transformer: FlatColor 149 | Decay Time: 0 150 | Enabled: false 151 | Invert Rainbow: false 152 | Max Color: 255; 255; 255 153 | Max Intensity: 4096 154 | Min Color: 0; 0; 0 155 | Min Intensity: 0 156 | Name: CropBoxOutput 157 | Position Transformer: XYZ 158 | Queue Size: 10 159 | Selectable: true 160 | Size (Pixels): 1 161 | Size (m): 0.009999999776482582 162 | Style: Flat Squares 163 | Topic: /box_filter/cropbox/output 164 | Unreliable: false 165 | Use Fixed Frame: true 166 | Use rainbow: true 167 | Value: false 168 | - Alpha: 0.20000000298023224 169 | Autocompute Intensity Bounds: true 170 | Autocompute Value Bounds: 171 | Max Value: 10 172 | Min Value: -10 173 | Value: true 174 | Axis: Z 175 | Channel Name: intensity 176 | Class: rviz/PointCloud2 177 | Color: 255; 85; 255 178 | Color Transformer: FlatColor 179 | Decay Time: 0 180 | Enabled: true 181 | Invert Rainbow: false 182 | Max Color: 255; 255; 255 183 | Max Intensity: 4096 184 | Min Color: 0; 0; 0 185 | Min Intensity: 0 186 | Name: VoxelCloud 187 | Position Transformer: XYZ 188 | Queue Size: 10 189 | Selectable: true 190 | Size (Pixels): 3 191 | Size (m): 0.009999999776482582 192 | Style: Flat Squares 193 | Topic: /cloud_downsampler/voxel/output 194 | Unreliable: false 195 | Use Fixed Frame: true 196 | Use rainbow: true 197 | Value: true 198 | - Alpha: 1 199 | Autocompute Intensity Bounds: true 200 | Autocompute Value Bounds: 201 | Max Value: 10 202 | Min Value: -10 203 | Value: true 204 | Axis: Z 205 | Channel Name: intensity 206 | Class: rviz/PointCloud2 207 | Color: 255; 0; 0 208 | Color Transformer: FlatColor 209 | Decay Time: 0 210 | Enabled: false 211 | Invert Rainbow: false 212 | Max Color: 255; 255; 255 213 | Max Intensity: 63 214 | Min Color: 0; 0; 0 215 | Min Intensity: 0 216 | Name: PlaneOutlierCloud 217 | Position Transformer: XYZ 218 | Queue Size: 10 219 | Selectable: true 220 | Size (Pixels): 3 221 | Size (m): 0.009999999776482582 222 | Style: Points 223 | Topic: /extract_indices/extract_outliers/output 224 | Unreliable: true 225 | Use Fixed Frame: true 226 | Use rainbow: true 227 | Value: false 228 | - Alpha: 1 229 | Autocompute Intensity Bounds: true 230 | Autocompute Value Bounds: 231 | Max Value: 10 232 | Min Value: -10 233 | Value: true 234 | Axis: Z 235 | Channel Name: intensity 236 | Class: rviz/PointCloud2 237 | Color: 85; 0; 255 238 | Color Transformer: FlatColor 239 | Decay Time: 0 240 | Enabled: true 241 | Invert Rainbow: false 242 | Max Color: 255; 255; 255 243 | Max Intensity: 63 244 | Min Color: 0; 0; 0 245 | Min Intensity: 0 246 | Name: PlaneInlierCloud 247 | Position Transformer: XYZ 248 | Queue Size: 10 249 | Selectable: true 250 | Size (Pixels): 3 251 | Size (m): 0.009999999776482582 252 | Style: Flat Squares 253 | Topic: /extract_indices/extract_inliers/output 254 | Unreliable: true 255 | Use Fixed Frame: true 256 | Use rainbow: true 257 | Value: true 258 | - Alpha: 0.10000000149011612 259 | Autocompute Intensity Bounds: true 260 | Autocompute Value Bounds: 261 | Max Value: 10 262 | Min Value: -10 263 | Value: true 264 | Axis: Z 265 | Channel Name: intensity 266 | Class: rviz/PointCloud2 267 | Color: 85; 170; 0 268 | Color Transformer: FlatColor 269 | Decay Time: 0 270 | Enabled: false 271 | Invert Rainbow: false 272 | Max Color: 255; 255; 255 273 | Max Intensity: 64 274 | Min Color: 0; 0; 0 275 | Min Intensity: 0 276 | Name: AboveTableCloud 277 | Position Transformer: XYZ 278 | Queue Size: 10 279 | Selectable: true 280 | Size (Pixels): 3 281 | Size (m): 0.009999999776482582 282 | Style: Flat Squares 283 | Topic: /table_cutoff/cutoff/output 284 | Unreliable: false 285 | Use Fixed Frame: true 286 | Use rainbow: true 287 | Value: false 288 | - Alpha: 1 289 | Autocompute Intensity Bounds: true 290 | Autocompute Value Bounds: 291 | Max Value: 10 292 | Min Value: -10 293 | Value: true 294 | Axis: Z 295 | Channel Name: intensity 296 | Class: rviz/PointCloud2 297 | Color: 255; 255; 255 298 | Color Transformer: FlatColor 299 | Decay Time: 0 300 | Enabled: true 301 | Invert Rainbow: false 302 | Max Color: 255; 255; 255 303 | Max Intensity: 64 304 | Min Color: 0; 0; 0 305 | Min Intensity: 0 306 | Name: AboveTableCloudFilt 307 | Position Transformer: XYZ 308 | Queue Size: 10 309 | Selectable: true 310 | Size (Pixels): 3 311 | Size (m): 0.009999999776482582 312 | Style: Flat Squares 313 | Topic: /outlier/cutoff/output 314 | Unreliable: false 315 | Use Fixed Frame: true 316 | Use rainbow: true 317 | Value: true 318 | - Alpha: 1 319 | Autocompute Intensity Bounds: true 320 | Autocompute Value Bounds: 321 | Max Value: 10 322 | Min Value: -10 323 | Value: true 324 | Axis: Z 325 | Channel Name: intensity 326 | Class: rviz/PointCloud2 327 | Color: 255; 255; 0 328 | Color Transformer: FlatColor 329 | Decay Time: 0.10000000149011612 330 | Enabled: true 331 | Invert Rainbow: false 332 | Max Color: 255; 255; 0 333 | Max Intensity: 4096 334 | Min Color: 0; 0; 0 335 | Min Intensity: 0 336 | Name: Cluster1 337 | Position Transformer: XYZ 338 | Queue Size: 10 339 | Selectable: true 340 | Size (Pixels): 3 341 | Size (m): 0.009999999776482582 342 | Style: Flat Squares 343 | Topic: /cluster_1_cloud 344 | Unreliable: false 345 | Use Fixed Frame: true 346 | Use rainbow: true 347 | Value: true 348 | - Alpha: 1 349 | Autocompute Intensity Bounds: true 350 | Autocompute Value Bounds: 351 | Max Value: 10 352 | Min Value: -10 353 | Value: true 354 | Axis: Z 355 | Channel Name: intensity 356 | Class: rviz/PointCloud2 357 | Color: 0; 255; 0 358 | Color Transformer: FlatColor 359 | Decay Time: 0.10000000149011612 360 | Enabled: true 361 | Invert Rainbow: false 362 | Max Color: 255; 255; 0 363 | Max Intensity: 4096 364 | Min Color: 0; 0; 0 365 | Min Intensity: 0 366 | Name: Cluster2 367 | Position Transformer: XYZ 368 | Queue Size: 10 369 | Selectable: true 370 | Size (Pixels): 3 371 | Size (m): 0.009999999776482582 372 | Style: Flat Squares 373 | Topic: /cluster_2_cloud 374 | Unreliable: false 375 | Use Fixed Frame: true 376 | Use rainbow: true 377 | Value: true 378 | - Alpha: 1 379 | Autocompute Intensity Bounds: true 380 | Autocompute Value Bounds: 381 | Max Value: 10 382 | Min Value: -10 383 | Value: true 384 | Axis: Z 385 | Channel Name: intensity 386 | Class: rviz/PointCloud2 387 | Color: 255; 0; 127 388 | Color Transformer: FlatColor 389 | Decay Time: 0.10000000149011612 390 | Enabled: true 391 | Invert Rainbow: false 392 | Max Color: 255; 255; 0 393 | Max Intensity: 4096 394 | Min Color: 0; 0; 0 395 | Min Intensity: 0 396 | Name: Cluster3 397 | Position Transformer: XYZ 398 | Queue Size: 10 399 | Selectable: true 400 | Size (Pixels): 3 401 | Size (m): 0.009999999776482582 402 | Style: Flat Squares 403 | Topic: /cluster_3_cloud 404 | Unreliable: false 405 | Use Fixed Frame: true 406 | Use rainbow: true 407 | Value: true 408 | Enabled: true 409 | Global Options: 410 | Background Color: 48; 48; 48 411 | Default Light: true 412 | Fixed Frame: camera_depth_optical_frame 413 | Frame Rate: 30 414 | Name: root 415 | Tools: 416 | - Class: rviz/Interact 417 | Hide Inactive Objects: true 418 | - Class: rviz/MoveCamera 419 | - Class: rviz/Select 420 | - Class: rviz/FocusCamera 421 | - Class: rviz/Measure 422 | - Class: rviz/SetInitialPose 423 | Theta std deviation: 0.2617993950843811 424 | Topic: /initialpose 425 | X std deviation: 0.5 426 | Y std deviation: 0.5 427 | - Class: rviz/SetGoal 428 | Topic: /move_base_simple/goal 429 | - Class: rviz/PublishPoint 430 | Single click: true 431 | Topic: /clicked_point 432 | Value: true 433 | Views: 434 | Current: 435 | Class: rviz/Orbit 436 | Distance: 2.5229530334472656 437 | Enable Stereo Rendering: 438 | Stereo Eye Separation: 0.05999999865889549 439 | Stereo Focal Distance: 1 440 | Swap Stereo Eyes: false 441 | Value: false 442 | Field of View: 0.7853981852531433 443 | Focal Point: 444 | X: 0.07936588674783707 445 | Y: -0.10336504131555557 446 | Z: 0.996982753276825 447 | Focal Shape Fixed Size: true 448 | Focal Shape Size: 0.05000000074505806 449 | Invert Z Axis: false 450 | Name: Current View 451 | Near Clip Distance: 0.009999999776482582 452 | Pitch: -1.5697963237762451 453 | Target Frame: 454 | Yaw: 4.663490295410156 455 | Saved: ~ 456 | Window Geometry: 457 | Displays: 458 | collapsed: false 459 | Height: 1136 460 | Hide Left Dock: false 461 | Hide Right Dock: false 462 | QMainWindow State: 000000ff00000000fd0000000400000000000001e3000003d6fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003d6000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610000000000ffffffff0000000000000000000000010000010f00000349fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000004100000349000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000054f000003d600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 463 | Selection: 464 | collapsed: false 465 | Time: 466 | collapsed: false 467 | Tool Properties: 468 | collapsed: false 469 | Views: 470 | collapsed: false 471 | Width: 1848 472 | X: 72 473 | Y: 27 474 | -------------------------------------------------------------------------------- /launch/nodelet_table_extraction.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 29 | 30 | 32 | 33 | 34 | 35 | 36 | 37 | 40 | 41 | 42 | 43 | 44 | min_x: -0.75 45 | max_x: 0.5 46 | min_y: -0.6 47 | max_y: 0.1 48 | min_z: $(arg 0.1) 49 | max_z: 2.0 50 | 51 | 52 | 53 | 54 | 57 | 58 | 59 | 60 | 61 | leaf_size: 0.01 62 | filter_field_name: z 63 | filter_limit_min: -10.0 64 | filter_limit_max: 10.0 65 | 66 | 67 | 68 | 69 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 122 | 123 | 125 | 126 | 127 | 128 | input_frame: table_frame 129 | filter_field_name: y 130 | filter_limit_min: -3.0 131 | filter_limit_max: 0.0 132 | 133 | 134 | 135 | 136 | 137 | 140 | 141 | 142 | 143 | 144 | mean_k: 10 145 | stddev: 0.01 146 | 147 | 148 | 149 | 150 | 152 | 153 | 154 | 155 | 156 | 157 | 159 | 160 | 161 | -------------------------------------------------------------------------------- /launch/table_rotated.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NU-MSR/nodelet_pcl_demo/c4612cfaff10b9846d1cb437b561cde2ab4fc623/launch/table_rotated.pcd -------------------------------------------------------------------------------- /launch/table_scene_lms400.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NU-MSR/nodelet_pcl_demo/c4612cfaff10b9846d1cb437b561cde2ab4fc623/launch/table_scene_lms400.pcd -------------------------------------------------------------------------------- /msg/PointArray.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Point[] points 3 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | nodelet_pcl_demo 4 | 0.0.0 5 | 6 | Demonstration of how using nodelets can reduce CPU overhead, especially when 7 | working with large messages such as point clouds. 8 | 9 | 10 | jarvis 11 | GPLv3 12 | 13 | catkin 14 | rospy 15 | roscpp 16 | std_msgs 17 | visualization_msgs 18 | sensor_msgs 19 | geometry_msgs 20 | tf 21 | tf_conversions 22 | libpcl1-dev 23 | pcl_ros 24 | message_generation 25 | 26 | rospy 27 | roscpp 28 | std_msgs 29 | visualization_msgs 30 | sensor_msgs 31 | geometry_msgs 32 | tf 33 | tf_conversions 34 | libpcl1 35 | pcl_ros 36 | rgbd_launch 37 | message_runtime 38 | 39 | 40 | -------------------------------------------------------------------------------- /src/cluster_extractor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | 19 | 20 | //--------------------------------------------------------------------------- 21 | // Global Variables 22 | //--------------------------------------------------------------------------- 23 | #define MAX_CLUSTERS 3 24 | typedef pcl::PointXYZ PointT; 25 | std::string filename; 26 | 27 | //--------------------------------------------------------------------------- 28 | // Objects and Functions 29 | //--------------------------------------------------------------------------- 30 | 31 | class ClusterExtractor 32 | { 33 | 34 | private: 35 | ros::NodeHandle n_; 36 | ros::Subscriber cloud_sub; 37 | ros::Publisher cloud_pub[MAX_CLUSTERS]; 38 | ros::Publisher point_pub[MAX_CLUSTERS]; 39 | tf::TransformBroadcaster br; 40 | 41 | 42 | public: 43 | ClusterExtractor() 44 | { 45 | ROS_DEBUG("Creating subscribers and publishers"); 46 | cloud_sub = n_.subscribe("/outlier/cutoff/output", 10, &ClusterExtractor::cloudcb, this); 47 | br = tf::TransformBroadcaster(); 48 | for(int i = 0; i < MAX_CLUSTERS; i++) 49 | { 50 | cloud_pub[i] = n_.advertise("/cluster_" + std::to_string(i + 1) + "_cloud", 1); 51 | point_pub[i] = n_.advertise("/cluster_" + std::to_string(i + 1) + "_point", 1); 52 | } 53 | } 54 | 55 | // this function gets called every time new pcl data comes in 56 | void cloudcb(const sensor_msgs::PointCloud2ConstPtr &scan) 57 | { 58 | ROS_DEBUG("Filtered cloud receieved"); 59 | sensor_msgs::PointCloud2::Ptr ros_cloud(new sensor_msgs::PointCloud2 ()); 60 | pcl::PointCloud::Ptr cloud (new pcl::PointCloud); 61 | 62 | // set time stamp and frame id 63 | ros::Time tstamp = ros::Time::now(); 64 | 65 | // Convert to pcl 66 | ROS_DEBUG("Convert incoming cloud to pcl cloud"); 67 | pcl::fromROSMsg(*scan, *cloud); 68 | 69 | //////////////////////////////////////// 70 | // STARTING CLUSTER EXTRACTION // 71 | //////////////////////////////////////// 72 | ROS_DEBUG("Begin cluster extraction"); 73 | 74 | // create a vector for storing the indices of the clusters 75 | std::vector cluster_indices; 76 | 77 | // setup extraction: 78 | pcl::EuclideanClusterExtraction ec; 79 | ec.setClusterTolerance (0.01); // cm 80 | ec.setMinClusterSize (50); 81 | ec.setMaxClusterSize (5000); 82 | ec.setInputCloud (cloud); 83 | // perform cluster extraction 84 | ec.extract (cluster_indices); 85 | 86 | // run through the indices, create new clouds, and then publish them 87 | int j=0; 88 | int number_clusters=0; 89 | geometry_msgs::PointStamped pt; 90 | Eigen::Vector4f centroid; 91 | 92 | for(const auto & index : cluster_indices) 93 | { 94 | number_clusters = (int) cluster_indices.size(); 95 | ROS_DEBUG("Number of clusters found: %d",number_clusters); 96 | pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); 97 | for (const auto & point : index.indices) 98 | { 99 | cloud_cluster->points.push_back(cloud->points[point]); 100 | } 101 | cloud_cluster->width = cloud_cluster->points.size(); 102 | cloud_cluster->height = 1; 103 | cloud_cluster->is_dense = true; 104 | 105 | // convert to rosmsg and publish: 106 | ROS_DEBUG("Publishing extracted cloud"); 107 | pcl::toROSMsg(*cloud_cluster, *ros_cloud); 108 | ros_cloud->header.frame_id = scan->header.frame_id; 109 | if(j < MAX_CLUSTERS) 110 | { 111 | cloud_pub[j].publish(ros_cloud); 112 | 113 | // compute centroid and publish 114 | pcl::compute3DCentroid(*cloud_cluster, centroid); 115 | pt.point.x = centroid(0); 116 | pt.point.y = centroid(1); 117 | pt.point.z = centroid(2); 118 | pt.header.stamp = scan->header.stamp; 119 | pt.header.frame_id = scan->header.frame_id; 120 | point_pub[j].publish(pt); 121 | 122 | // let's send transforms as well: 123 | tf::Transform transform; 124 | transform.setOrigin( tf::Vector3(centroid(0), centroid(1), centroid(2)) ); 125 | tf::Quaternion q; 126 | q.setRPY(0, 0, 0); 127 | transform.setRotation(q); 128 | br.sendTransform( tf::StampedTransform(transform, ros::Time::now(), scan->header.frame_id, "cluster_" + std::to_string(j + 1) + "_frame")); 129 | } 130 | j++; 131 | } 132 | } 133 | }; 134 | 135 | 136 | int main(int argc, char **argv) 137 | { 138 | ros::init(argc, argv, "cluster_extractor"); 139 | ClusterExtractor extractor; 140 | 141 | ros::spin(); 142 | 143 | return 0; 144 | } 145 | -------------------------------------------------------------------------------- /src/plot_point_array.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import numpy as np 3 | import rospy 4 | from nodelet_pcl_demo.msg import PointArray 5 | import sys 6 | 7 | 8 | def main(): 9 | rospy.init_node("point_array_plotter") 10 | 11 | # let's subscribe to a single message, plot the value, and then quit 12 | received = False 13 | for i in range(10): 14 | rospy.loginfo("Waiting for message") 15 | try: 16 | points = rospy.wait_for_message("cloud_points", PointArray, 1) 17 | received = True 18 | break 19 | except rospy.ROSException: 20 | pass 21 | if not received: 22 | rospy.loginfo("Never received message on 'cloud_points' topic.... exiting") 23 | sys.exit(1) 24 | rospy.loginfo("Received message!") 25 | 26 | # now let's turn the point arrays into numpy arrays: 27 | xarr = np.zeros(len(points.points)) 28 | yarr = np.zeros(len(points.points)) 29 | zarr = np.zeros(len(points.points)) 30 | for i,pt in enumerate(points.points): 31 | xarr[i] = pt.x 32 | yarr[i] = pt.y 33 | zarr[i] = pt.z 34 | 35 | pltmode = rospy.get_param("~use_mayavi", False) 36 | 37 | if pltmode: 38 | from mayavi import mlab 39 | s = zarr 40 | mlab.points3d(xarr, yarr, zarr, s, colormap="RdYlBu", scale_factor=0.005, 41 | scale_mode='none', mode='2dtriangle') 42 | mlab.show() 43 | else: 44 | import matplotlib.pyplot as mpl 45 | from mpl_toolkits.mplot3d import Axes3D 46 | fig = mpl.figure() 47 | ax = Axes3D(fig) 48 | ax.scatter(xarr, yarr, zarr, c='r', marker='.', alpha=0.5) 49 | ax.set_xlabel('X') 50 | ax.set_ylabel('Y') 51 | ax.set_zlabel('Z') 52 | # hack to ensure equal aspect: https://stackoverflow.com/questions/13685386/matplotlib-equal-unit-length-with-equal-aspect-ratio-z-axis-is-not-equal-to 53 | max_range = np.array([xarr.max()-xarr.min(), yarr.max()-yarr.min(), zarr.max()-zarr.min()]).max() 54 | Xb = 0.5*max_range*np.mgrid[-1:2:2,-1:2:2,-1:2:2][0].flatten() + 0.5*(xarr.max()+xarr.min()) 55 | Yb = 0.5*max_range*np.mgrid[-1:2:2,-1:2:2,-1:2:2][1].flatten() + 0.5*(yarr.max()+yarr.min()) 56 | Zb = 0.5*max_range*np.mgrid[-1:2:2,-1:2:2,-1:2:2][2].flatten() + 0.5*(zarr.max()+zarr.min()) 57 | for xb, yb, zb in zip(Xb, Yb, Zb): 58 | ax.plot([xb], [yb], [zb], 'w') 59 | mpl.show() 60 | 61 | return 62 | 63 | 64 | if __name__ == '__main__': 65 | main() 66 | -------------------------------------------------------------------------------- /src/point_publisher.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | 16 | //--------------------------------------------------------------------------- 17 | // Global Variables 18 | //--------------------------------------------------------------------------- 19 | typedef pcl::PointXYZ PointT; 20 | 21 | 22 | //--------------------------------------------------------------------------- 23 | // Objects and Functions 24 | //--------------------------------------------------------------------------- 25 | class PointPublisher 26 | { 27 | 28 | private: 29 | ros::NodeHandle n_; 30 | ros::Subscriber cloud_sub; 31 | ros::Publisher point_pub; 32 | 33 | public: 34 | PointPublisher() 35 | { 36 | ROS_DEBUG("Creating subscribers and publishers"); 37 | point_pub = n_.advertise("cloud_points", 1); 38 | cloud_sub = n_.subscribe("cloud_in", 1, &PointPublisher::cloudcb, this); 39 | return; 40 | } 41 | 42 | // this function gets called every time new pcl data comes in 43 | void cloudcb(const sensor_msgs::PointCloud2ConstPtr &scan) 44 | { 45 | ROS_DEBUG("Cloud receieved"); 46 | pcl::PointCloud::Ptr cloud (new pcl::PointCloud); 47 | 48 | // Convert to pcl 49 | ROS_DEBUG("Convert incoming cloud to pcl cloud"); 50 | pcl::fromROSMsg(*scan, *cloud); 51 | 52 | // Now we want to iterate through the cloud, extract the XYZ points, 53 | // and fill out a PointArray message 54 | nodelet_pcl_demo::PointArray pts; 55 | pts.header = scan->header; 56 | 57 | for (size_t i=0; ipoints.size(); ++i) 58 | { 59 | geometry_msgs::Point point; 60 | point.x = cloud->points[i].x; 61 | point.y = cloud->points[i].y; 62 | point.z = cloud->points[i].z; 63 | pts.points.push_back(point); 64 | } 65 | point_pub.publish(pts); 66 | 67 | return; 68 | } 69 | }; 70 | 71 | 72 | int main(int argc, char **argv) 73 | { 74 | ros::init(argc, argv, "point_publisher"); 75 | PointPublisher publisher; 76 | 77 | ros::spin(); 78 | 79 | return 0; 80 | } 81 | -------------------------------------------------------------------------------- /src/table_cutoff_settings.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import numpy as np 3 | import rospy 4 | import tf 5 | import tf.transformations as tr 6 | from pcl_msgs.msg import ModelCoefficients 7 | from std_srvs.srv import Empty 8 | from std_srvs.srv import EmptyResponse 9 | from math import pi 10 | 11 | INIT_OFFSET = [0,0,0] 12 | INIT_ROLL = 0.0 13 | PARENT_FRAME = "camera_depth_optical_frame" 14 | CHILD_FRAME = "table_frame" 15 | PLANE_OFFSET = 0.01 16 | 17 | 18 | class TableSegmenter( object ): 19 | def __init__(self): 20 | self.br = tf.TransformBroadcaster() 21 | self.trans = INIT_OFFSET 22 | self.rot = tr.quaternion_from_euler(0, 0, INIT_ROLL, 'szyx') 23 | # create a timer that sends the current estimated transform repeatedly: 24 | self.tf_timer = rospy.Timer(rospy.Duration(0.01), self.tf_pub_cb) 25 | 26 | # create a subscriber that subscribes to the table model: 27 | self.model_sub = rospy.Subscriber("/normal_segmentation/segmentation/model", ModelCoefficients, 28 | self.modelcb) 29 | self.model_coeffs = None 30 | # now create a service provider, that, when called, will update the 31 | # transform data based off of the model 32 | self.model_srv = rospy.Service("update_table_model", Empty, self.update_model) 33 | 34 | return 35 | 36 | def tf_pub_cb(self, te): 37 | self.br.sendTransform(self.trans, self.rot, rospy.Time.now(), CHILD_FRAME, PARENT_FRAME) 38 | return 39 | 40 | def modelcb(self, model): 41 | self.model_coeffs = model.values 42 | return 43 | 44 | def update_model(self, srv): 45 | if self.model_coeffs is None: 46 | return EmptyResponse() 47 | # we need to convert the table model into useful transform data. the 48 | # plane coefficients are [a,b,c,d] where the equation of the plane is 49 | # given by `ax + by + cz + d = 0` 50 | # from the plane normal, we can calculate the orientation: 51 | model = np.array(self.model_coeffs) 52 | # we want to make sure this normal is pointed "down" 53 | if np.dot(model[0:3], [0,1,0]) < 0: 54 | model = -1.0*model 55 | normal = np.array(model[0:3]) 56 | # now we can build a rotation matrix from this normal, and assuming the 57 | # table frame's x-axis is aligned with the sensor's x axis: 58 | z = np.cross([1,0,0], normal) 59 | R = np.vstack(([1,0,0], normal, z)).T 60 | self.rot = tr.quaternion_from_matrix(np.vstack((np.hstack((R,np.zeros((3,1)))),[0,0,0,1]))) 61 | # now if we assume that the center of the plane has the following 62 | # coordinates (0, y0, 1), we can solve for the height of the plane (y0) 63 | # http://mathworld.wolfram.com/Plane.html 64 | a,b,c,d = model 65 | x = 0 66 | z = 1 67 | y0 = (-d - c*z - a*x)/b 68 | self.trans = [0, y0 - PLANE_OFFSET, 1] 69 | return EmptyResponse() 70 | 71 | 72 | def main(): 73 | rospy.init_node("table_cutoff_settings", log_level=rospy.INFO) 74 | try: 75 | segmenter = TableSegmenter() 76 | except rospy.ROSInterruptException: 77 | pass 78 | rospy.spin() 79 | 80 | 81 | 82 | if __name__ == '__main__': 83 | main() 84 | --------------------------------------------------------------------------------