├── .gitignore ├── CMakeLists.txt ├── README.md ├── cfg └── lidar_body_tracking.cfg ├── images ├── clustered_markers.gif └── lidar_tracking.gif ├── launch ├── includes │ └── urdf.launch.xml ├── lidar_body_tracking.launch └── rviz_filtered.launch ├── package.xml ├── rviz └── view_cloud_filtered.rviz ├── src └── pcl_body_tracking.cpp └── urdf └── m8.urdf.xacro /.gitignore: -------------------------------------------------------------------------------- 1 | # Temporary Files # 2 | ################### 3 | *.swp 4 | *.swo 5 | *.bak 6 | *.pyo 7 | *.pyc 8 | *~ 9 | *# 10 | 11 | # OS generated files # 12 | ###################### 13 | .DS_Store 14 | .DS_Store? 15 | ._* 16 | .Spotlight-V100 17 | .Trashes 18 | ehthumbs.db 19 | Thumbs.db 20 | 21 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(lidar_body_tracking) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | sensor_msgs 13 | std_msgs 14 | pcl_conversions 15 | pcl_ros 16 | geometry_msgs 17 | nav_msgs 18 | dynamic_reconfigure 19 | ) 20 | 21 | ## System dependencies are found with CMake's conventions 22 | # find_package(Boost REQUIRED COMPONENTS system) 23 | 24 | 25 | ## Uncomment this if the package has a setup.py. This macro ensures 26 | ## modules and global scripts declared therein get installed 27 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 28 | # catkin_python_setup() 29 | 30 | ################################################ 31 | ## Declare ROS messages, services and actions ## 32 | ################################################ 33 | 34 | ## To declare and build messages, services or actions from within this 35 | ## package, follow these steps: 36 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 37 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 38 | ## * In the file package.xml: 39 | ## * add a build_depend tag for "message_generation" 40 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 41 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 42 | ## but can be declared for certainty nonetheless: 43 | ## * add a run_depend tag for "message_runtime" 44 | ## * In this file (CMakeLists.txt): 45 | ## * add "message_generation" and every package in MSG_DEP_SET to 46 | ## find_package(catkin REQUIRED COMPONENTS ...) 47 | ## * add "message_runtime" and every package in MSG_DEP_SET to 48 | ## catkin_package(CATKIN_DEPENDS ...) 49 | ## * uncomment the add_*_files sections below as needed 50 | ## and list every .msg/.srv/.action file to be processed 51 | ## * uncomment the generate_messages entry below 52 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 53 | 54 | ## Generate messages in the 'msg' folder 55 | # add_message_files( 56 | # FILES 57 | # Message1.msg 58 | # Message2.msg 59 | # ) 60 | 61 | ## Generate services in the 'srv' folder 62 | # add_service_files( 63 | # FILES 64 | # Service1.srv 65 | # Service2.srv 66 | # ) 67 | 68 | ## Generate actions in the 'action' folder 69 | # add_action_files( 70 | # FILES 71 | # Action1.action 72 | # Action2.action 73 | # ) 74 | 75 | ## Generate added messages and services with any dependencies listed here 76 | # generate_messages( 77 | # DEPENDENCIES 78 | # std_msgs # Or other packages containing msgs 79 | # ) 80 | 81 | ################################################ 82 | ## Declare ROS dynamic reconfigure parameters ## 83 | ################################################ 84 | 85 | ## To declare and build dynamic reconfigure parameters within this 86 | ## package, follow these steps: 87 | ## * In the file package.xml: 88 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 89 | ## * In this file (CMakeLists.txt): 90 | ## * add "dynamic_reconfigure" to 91 | ## find_package(catkin REQUIRED COMPONENTS ...) 92 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 93 | ## and list every .cfg file to be processed 94 | 95 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 96 | generate_dynamic_reconfigure_options( 97 | cfg/lidar_body_tracking.cfg 98 | ) 99 | 100 | ################################### 101 | ## catkin specific configuration ## 102 | ################################### 103 | ## The catkin_package macro generates cmake config files for your package 104 | ## Declare things to be passed to dependent projects 105 | ## INCLUDE_DIRS: uncomment this if you package contains header files 106 | ## LIBRARIES: libraries you create in this project that dependent projects also need 107 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 108 | ## DEPENDS: system dependencies of this project that dependent projects also need 109 | catkin_package( 110 | INCLUDE_DIRS 111 | CATKIN_DEPENDS roscpp 112 | sensor_msgs 113 | std_msgs 114 | pcl_conversions 115 | pcl_ros 116 | ) 117 | 118 | ########### 119 | ## Build ## 120 | ########### 121 | 122 | ## Specify additional locations of header files 123 | ## Your package locations should be listed before other locations 124 | include_directories( 125 | ${catkin_INCLUDE_DIRS} 126 | ) 127 | 128 | ## Declare a C++ library 129 | # add_library(${PROJECT_NAME} 130 | # src/${PROJECT_NAME}/lidar_body_tracking.cpp 131 | # ) 132 | 133 | ## Add cmake target dependencies of the library 134 | ## as an example, code may need to be generated before libraries 135 | ## either from message generation or dynamic reconfigure 136 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 137 | 138 | ## Declare a C++ executable 139 | ## With catkin_make all packages are built within a single CMake context 140 | ## The recommended prefix ensures that target names across packages don't collide 141 | # add_executable(${PROJECT_NAME}_node src/lidar_body_tracking_node.cpp) 142 | add_executable(pcl_body_tracking src/pcl_body_tracking.cpp) 143 | target_link_libraries(pcl_body_tracking ${catkin_LIBRARIES}) 144 | 145 | ## Rename C++ executable without prefix 146 | ## The above recommended prefix causes long target names, the following renames the 147 | ## target back to the shorter version for ease of user use 148 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 149 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 150 | 151 | ## Add cmake target dependencies of the executable 152 | ## same as for the library above 153 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 154 | add_dependencies(pcl_body_tracking ${PROJECT_NAME}_gencfg) 155 | 156 | ## Specify libraries to link a library or executable target against 157 | # target_link_libraries(${PROJECT_NAME}_node 158 | # ${catkin_LIBRARIES} 159 | # ) 160 | 161 | ############# 162 | ## Install ## 163 | ############# 164 | 165 | # all install targets should use catkin DESTINATION variables 166 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 167 | 168 | ## Mark executable scripts (Python etc.) for installation 169 | ## in contrast to setup.py, you can choose the destination 170 | # install(PROGRAMS 171 | # scripts/my_python_script 172 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 173 | # ) 174 | 175 | ## Mark executables and/or libraries for installation 176 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 177 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 178 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 179 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 180 | # ) 181 | 182 | ## Mark cpp header files for installation 183 | # install(DIRECTORY include/${PROJECT_NAME}/ 184 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 185 | # FILES_MATCHING PATTERN "*.h" 186 | # PATTERN ".svn" EXCLUDE 187 | # ) 188 | 189 | ## Mark other files for installation (e.g. launch and bag files, etc.) 190 | # install(FILES 191 | # # myfile1 192 | # # myfile2 193 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 194 | # ) 195 | 196 | ############# 197 | ## Testing ## 198 | ############# 199 | 200 | ## Add gtest based cpp test target and link libraries 201 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_lidar_body_tracking.cpp) 202 | # if(TARGET ${PROJECT_NAME}-test) 203 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 204 | # endif() 205 | 206 | ## Add folders to be run by python nosetests 207 | # catkin_add_nosetests(test) 208 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # lidar_body_tracking 2 | ROS Catkin package to track people using ortree and cluster extraction from a fixed point. 3 | Sensor Used for testing: Quanergy M8 4 | Written and tested on: Ubuntu 16.04, ROS Kinetic 5 | ![](images/lidar_tracking.gif) 6 | ![](images/clustered_markers.gif) 7 | 8 | ## Dependencies 9 | This package depends on the people_msgs package: 10 | ![kdhansen/people](https://github.com/kdhansen/people) 11 | 1. `sudo apt install ros-kinetic-easy-markers ros-kinetic-kalman-filter` 12 | 2. `git clone https://github.com/kdhansen/people ~/catkin_ws/src` 13 | 14 | ## Installation 15 | 1. Make your catkin workspace: 16 | 1. `mkdir -p ~/catkin_ws/src` 17 | 2. `catkin_init_workspace ~/catkin_ws/src` 18 | 3. `catkin_make -C ~/catkin_ws` 19 | 2. Clone `kdhansen/people` to your workspace: 20 | 1. `git clone https://github.com/kdhansen/people ~/catkin_ws/src` 21 | 3. Clone this repo to your workspace: 22 | 1. `git clone https://github.com/MyNameIsCosmo/lidar_body_tracking ~/catkin_ws/src` 23 | 4. Source your workspace 24 | 1. `source ~/catkin_ws/devel/setup.sh` 25 | 5. Build your workspace 26 | 1. `catkin_make -C ~/catkin_ws --pkg people_msgs` 27 | 2. `catkin_make -C ~/catkin_ws` 28 | 29 | ## Running the tracking 30 | 1. Initialize your LIDAR, or play your ROSBAG 31 | 1. `rosbag play -l bagname.bag` 32 | 2. Launch 33 | 1. `source ~/catkin_ws/devel/setup.sh` 34 | 2. `roslaunch lidar_body_tracking lidar_body_tracking.launch` 35 | 36 | ## Notes 37 | 1. The URDF frame is QP308. You can change this in `/urdf/m8.launch.xacro` 38 | 2. You can change the default topics in the launch file `/launch/lidar_body_tracking.launch` 39 | 40 | ## TODO: 41 | - [x] Dynamic Reconfigure for Link, Leaf size, min cluster, etc 42 | - [x] ROSParam for topics 43 | - [x] Clustering of indicies for person detection 44 | - [ ] Output potential people to a topic 45 | - [ ] Calculate person velocity 46 | - [ ] Calculate certainty of person 47 | - [ ] Estimate person height, size 48 | - [ ] Track person based on previous location 49 | - [ ] people_msgs/Person does not include orientation 50 | - [x] Control an RVIZ marker or something 51 | - [ ] Support body tracking while lidar is moving, loop closure and Odom tracking. 52 | - [ ] Comment and document code 53 | - [ ] Object-oriented 54 | - [ ] Clean-up code 55 | 56 | ## References 57 | [ROS WIKI URDF](http://wiki.ros.org/urdf) 58 | [ROS WIKI Xacro Reference](http://wiki.ros.org/xacro) 59 | -------------------------------------------------------------------------------- /cfg/lidar_body_tracking.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | PACKAGE = "lidar_body_tracking" 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("leaf_size", int_t, 0, "Leaf size", 10, 0, 100) 9 | gen.add("resolution", double_t, 0, "Octree Cloud Resolution", 0.1, 0, 2) 10 | gen.add("min_filtered_cloud_size", int_t, 0, "Min Filtered Cloud Size", 50, 0, 2000) 11 | gen.add("min_clustered_cloud_size", int_t, 0, "Min Clustered Cloud Size", 40, 0, 2000) 12 | gen.add("cluster_tolerance", double_t, 0, "Cluster Tolerance in meters", 0.2, 0, 1) 13 | gen.add("min_cluster_size", int_t, 0, "Minimum Euclidean Cluster Size", 40, 0, 2000) 14 | gen.add("max_cluster_size", int_t, 0, "Maximum Euclidean Cluster Size", 25000, 0, 30000) 15 | 16 | exit(gen.generate(PACKAGE, "lidar_body_tracking", "lidar_body_tracking")) 17 | -------------------------------------------------------------------------------- /images/clustered_markers.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MyNameIsCosmo/lidar_body_tracking/2d15b025f2a47da9108ce654f4e3ccd9ecf80e4f/images/clustered_markers.gif -------------------------------------------------------------------------------- /images/lidar_tracking.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MyNameIsCosmo/lidar_body_tracking/2d15b025f2a47da9108ce654f4e3ccd9ecf80e4f/images/lidar_tracking.gif -------------------------------------------------------------------------------- /launch/includes/urdf.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /launch/lidar_body_tracking.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /launch/rviz_filtered.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lidar_body_tracking 4 | 0.0.0 5 | The lidar_body_tracking package 6 | 7 | 8 | 9 | 10 | user 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | roscpp 44 | sensor_msgs 45 | std_msgs 46 | pcl_conversions 47 | pcl_ros 48 | libpcl-all-dev 49 | roscpp 50 | sensor_msgs 51 | std_msgs 52 | pcl_conversions 53 | pcl_ros 54 | libpcl-all-dev 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | -------------------------------------------------------------------------------- /rviz/view_cloud_filtered.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 81 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | - /Grid1/Offset1 11 | - /PointCloud23 12 | - /Marker1/Status1 13 | Splitter Ratio: 0.497222215 14 | Tree Height: 169 15 | - Class: rviz/Selection 16 | Name: Selection 17 | - Class: rviz/Tool Properties 18 | Expanded: 19 | - /2D Pose Estimate1 20 | - /2D Nav Goal1 21 | - /Publish Point1 22 | Name: Tool Properties 23 | Splitter Ratio: 0.588679016 24 | - Class: rviz/Views 25 | Expanded: 26 | - /Current View1 27 | Name: Views 28 | Splitter Ratio: 0.5 29 | - Class: rviz/Time 30 | Experimental: true 31 | Name: Time 32 | SyncMode: 0 33 | SyncSource: PointCloud2 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: true 42 | Line Style: 43 | Line Width: 0.0299999993 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: -1.20000005 51 | Plane: XY 52 | Plane Cell Count: 10 53 | Reference Frame: 54 | Value: true 55 | - Alpha: 0.5 56 | Autocompute Intensity Bounds: true 57 | Autocompute Value Bounds: 58 | Max Value: 10 59 | Min Value: -10 60 | Value: true 61 | Axis: Z 62 | Channel Name: ring 63 | Class: rviz/PointCloud2 64 | Color: 255; 255; 255 65 | Color Transformer: Intensity 66 | Decay Time: 0 67 | Enabled: true 68 | Invert Rainbow: false 69 | Max Color: 255; 255; 255 70 | Max Intensity: 7 71 | Min Color: 0; 0; 0 72 | Min Intensity: 0 73 | Name: PointCloud2 74 | Position Transformer: XYZ 75 | Queue Size: 10 76 | Selectable: true 77 | Size (Pixels): 2 78 | Size (m): 0.0299999993 79 | Style: Points 80 | Topic: /QP308/pc_QP308 81 | Unreliable: false 82 | Use Fixed Frame: true 83 | Use rainbow: true 84 | Value: true 85 | - Alpha: 0.5 86 | Autocompute Intensity Bounds: true 87 | Autocompute Value Bounds: 88 | Max Value: 10 89 | Min Value: -10 90 | Value: true 91 | Axis: Z 92 | Channel Name: intensity 93 | Class: rviz/PointCloud2 94 | Color: 255; 255; 255 95 | Color Transformer: FlatColor 96 | Decay Time: 1 97 | Enabled: true 98 | Invert Rainbow: false 99 | Max Color: 255; 255; 255 100 | Max Intensity: 4096 101 | Min Color: 0; 0; 0 102 | Min Intensity: 0 103 | Name: PointCloud2 104 | Position Transformer: XYZ 105 | Queue Size: 10 106 | Selectable: true 107 | Size (Pixels): 3 108 | Size (m): 0.400000006 109 | Style: Points 110 | Topic: /pcl_filtered 111 | Unreliable: false 112 | Use Fixed Frame: true 113 | Use rainbow: true 114 | Value: true 115 | - Alpha: 1 116 | Autocompute Intensity Bounds: true 117 | Autocompute Value Bounds: 118 | Max Value: 10 119 | Min Value: -10 120 | Value: true 121 | Axis: Z 122 | Channel Name: intensity 123 | Class: rviz/PointCloud2 124 | Color: 255; 85; 255 125 | Color Transformer: FlatColor 126 | Decay Time: 0 127 | Enabled: false 128 | Invert Rainbow: false 129 | Max Color: 170; 0; 255 130 | Max Intensity: 4096 131 | Min Color: 0; 0; 0 132 | Min Intensity: 0 133 | Name: PointCloud2 134 | Position Transformer: XYZ 135 | Queue Size: 10 136 | Selectable: true 137 | Size (Pixels): 4 138 | Size (m): 0.100000001 139 | Style: Spheres 140 | Topic: /pcl_clustered 141 | Unreliable: false 142 | Use Fixed Frame: true 143 | Use rainbow: true 144 | Value: false 145 | - Alpha: 1 146 | Class: rviz/RobotModel 147 | Collision Enabled: false 148 | Enabled: true 149 | Links: 150 | All Links Enabled: true 151 | Expand Joint Details: false 152 | Expand Link Details: false 153 | Expand Tree: false 154 | Link Tree Style: Links in Alphabetic Order 155 | QP308: 156 | Alpha: 1 157 | Show Axes: false 158 | Show Trail: false 159 | Value: true 160 | base_link: 161 | Alpha: 1 162 | Show Axes: false 163 | Show Trail: false 164 | Name: RobotModel 165 | Robot Description: robot_description 166 | TF Prefix: "" 167 | Update Interval: 0 168 | Value: true 169 | Visual Enabled: true 170 | - Class: rviz/Marker 171 | Enabled: true 172 | Marker Topic: visualization_marker 173 | Name: Marker 174 | Namespaces: 175 | cluster: true 176 | Queue Size: 10 177 | Value: true 178 | Enabled: true 179 | Global Options: 180 | Background Color: 48; 48; 48 181 | Fixed Frame: base_link 182 | Frame Rate: 30 183 | Name: root 184 | Tools: 185 | - Class: rviz/Interact 186 | Hide Inactive Objects: true 187 | - Class: rviz/MoveCamera 188 | - Class: rviz/Select 189 | - Class: rviz/FocusCamera 190 | - Class: rviz/Measure 191 | - Class: rviz/SetInitialPose 192 | Topic: /initialpose 193 | - Class: rviz/SetGoal 194 | Topic: /move_base_simple/goal 195 | - Class: rviz/PublishPoint 196 | Single click: true 197 | Topic: /clicked_point 198 | Value: true 199 | Views: 200 | Current: 201 | Class: rviz/Orbit 202 | Distance: 5.94841766 203 | Enable Stereo Rendering: 204 | Stereo Eye Separation: 0.0599999987 205 | Stereo Focal Distance: 1 206 | Swap Stereo Eyes: false 207 | Value: false 208 | Focal Point: 209 | X: 0 210 | Y: 0 211 | Z: 0 212 | Focal Shape Fixed Size: true 213 | Focal Shape Size: 0.0500000007 214 | Name: Current View 215 | Near Clip Distance: 0.00999999978 216 | Pitch: 0.5 217 | Target Frame: QP308 218 | Value: Orbit (rviz) 219 | Yaw: 0.699999988 220 | Saved: 221 | - Class: rviz/XYOrbit 222 | Distance: 9.99999905 223 | Enable Stereo Rendering: 224 | Stereo Eye Separation: 0.0599999987 225 | Stereo Focal Distance: 1 226 | Swap Stereo Eyes: false 227 | Value: false 228 | Focal Point: 229 | X: 0 230 | Y: 0 231 | Z: 0 232 | Focal Shape Fixed Size: true 233 | Focal Shape Size: 0.0500000007 234 | Name: XYOrbit 235 | Near Clip Distance: 0.00999999978 236 | Pitch: 0.5 237 | Target Frame: QP308 238 | Value: XYOrbit (rviz) 239 | Yaw: 0.75 240 | - Class: rviz/Orbit 241 | Distance: 5.94841766 242 | Enable Stereo Rendering: 243 | Stereo Eye Separation: 0.0599999987 244 | Stereo Focal Distance: 1 245 | Swap Stereo Eyes: false 246 | Value: false 247 | Focal Point: 248 | X: 0 249 | Y: 0 250 | Z: 0 251 | Focal Shape Fixed Size: true 252 | Focal Shape Size: 0.0500000007 253 | Name: Orbit 254 | Near Clip Distance: 0.00999999978 255 | Pitch: 0.5 256 | Target Frame: QP308 257 | Value: Orbit (rviz) 258 | Yaw: 0.699999988 259 | Window Geometry: 260 | Displays: 261 | collapsed: false 262 | Height: 480 263 | Hide Left Dock: false 264 | Hide Right Dock: false 265 | QMainWindow State: 000000ff00000000fd00000004000000000000016a0000013bfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006700fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000430000013b000000e300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000013bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000430000013b000000b300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005560000003efc0100000002fb0000000800540069006d0065010000000000000556000002a900fffffffb0000000800540069006d00650100000000000004500000000000000000000005560000013b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 266 | Selection: 267 | collapsed: false 268 | Time: 269 | collapsed: false 270 | Tool Properties: 271 | collapsed: false 272 | Views: 273 | collapsed: false 274 | Width: 1366 275 | X: 533 276 | Y: 201 277 | -------------------------------------------------------------------------------- /src/pcl_body_tracking.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | // Topics 28 | static const std::string SCAN_TOPIC = "/QP308/pc_QP308"; 29 | static const std::string FILTERED_TOPIC = "/pcl_filtered"; 30 | static const std::string CLUSTERED_TOPIC = "/pcl_clustered"; 31 | static const std::string PERSON_TOPIC = "/person"; 32 | 33 | // Variables 34 | int LEAF_SIZE = 10; 35 | float RESOLUTION = 0.1f; 36 | int MIN_FILTERED_CLOUD_SIZE = 50; 37 | int MIN_CLUSTERED_CLOUD_SIZE = 50; 38 | float CLUSTER_TOLERANCE = 0.2; 39 | int MIN_CLUSTER_SIZE = 50; 40 | int MAX_CLUSTER_SIZE = 25000; 41 | 42 | // ROS Publisher 43 | ros::Publisher pub_filtered, pub_clustered, pub_vis, pub_person; 44 | bool first_msg = true; 45 | sensor_msgs::PointCloud2 msg_clustered, msg_filtered; 46 | pcl::PointCloud cloud, filter; 47 | pcl::PointCloud::Ptr filter_ptr, cloud_ptr; 48 | 49 | void dynrcfg_callback(lidar_body_tracking::lidar_body_trackingConfig &config, uint32_t level) { 50 | ROS_INFO("Reconfigure Request: %d %f %d %d", 51 | config.leaf_size, 52 | config.resolution, 53 | config.min_filtered_cloud_size, 54 | config.min_clustered_cloud_size); 55 | LEAF_SIZE = config.leaf_size; 56 | RESOLUTION = config.resolution; 57 | MIN_FILTERED_CLOUD_SIZE = config.min_filtered_cloud_size; 58 | MIN_CLUSTERED_CLOUD_SIZE = config.min_clustered_cloud_size; 59 | CLUSTER_TOLERANCE = config.cluster_tolerance; 60 | MIN_CLUSTER_SIZE = config.min_cluster_size; 61 | MAX_CLUSTER_SIZE = config.max_cluster_size; 62 | } 63 | 64 | people_msgs::PersonStamped tag_person(std_msgs::Header header, std::string name, geometry_msgs::Point position, geometry_msgs::Point velocity, float reliability) 65 | { 66 | people_msgs::PersonStamped personStamped; 67 | people_msgs::Person person; 68 | 69 | person.name = name; 70 | person.position = position; 71 | person.velocity = velocity; 72 | person.reliability = reliability; 73 | //person.tagnames = tagnames; 74 | //person.tags = tags; 75 | 76 | personStamped.header = header; 77 | personStamped.person = person; 78 | 79 | return personStamped; 80 | } 81 | 82 | visualization_msgs::Marker mark_centroid(std_msgs::Header header, Eigen::Vector4f centroid, Eigen::Vector4f min, Eigen::Vector4f max, std::string ns ,int id, float r, float g, float b) 83 | { 84 | uint32_t shape = visualization_msgs::Marker::CUBE; 85 | visualization_msgs::Marker marker; 86 | marker.header = header; 87 | 88 | marker.ns = ns; 89 | marker.id = id; 90 | marker.type = shape; 91 | marker.action = visualization_msgs::Marker::ADD; 92 | 93 | marker.pose.position.x = centroid[0]; 94 | marker.pose.position.y = centroid[1]; 95 | marker.pose.position.z = centroid[2]; 96 | marker.pose.orientation.x = 0.0; 97 | marker.pose.orientation.y = 0.0; 98 | marker.pose.orientation.z = 0.0; 99 | marker.pose.orientation.w = 1.0; 100 | 101 | marker.scale.x = (max[0]-min[0]); 102 | marker.scale.y = (max[1]-min[1]); 103 | marker.scale.z = (max[2]-min[2]); 104 | 105 | if (marker.scale.x ==0) 106 | marker.scale.x=0.1; 107 | 108 | if (marker.scale.y ==0) 109 | marker.scale.y=0.1; 110 | 111 | if (marker.scale.z ==0) 112 | marker.scale.z=0.1; 113 | 114 | marker.color.r = r; 115 | marker.color.g = g; 116 | marker.color.b = b; 117 | marker.color.a = 0.5; 118 | 119 | marker.lifetime = ros::Duration(0.5); 120 | return marker; 121 | } 122 | 123 | void new_person(pcl::PointCloud::Ptr cloud_cluster, int id){ 124 | Eigen::Vector4f centroid; 125 | Eigen::Vector4f min; 126 | Eigen::Vector4f max; 127 | 128 | pcl::compute3DCentroid (*cloud_cluster, centroid); 129 | pcl::getMinMax3D (*cloud_cluster, min, max); 130 | 131 | geometry_msgs::Point pose, velocity; 132 | pose.x = centroid[0]; 133 | pose.y = centroid[1]; 134 | pose.z = centroid[2]; 135 | 136 | //mark_centroid visualization_msgs::Marker mark_centroid(Eigen::Vector4f centroid, std::string ns ,int id, float r, float g, float b) 137 | pub_vis.publish(mark_centroid(pcl_conversions::fromPCL(cloud_cluster->header), centroid, min, max, "cluster", id, 255, 0, 0)); 138 | 139 | //tag_person people_msgs::PersonStamped tag_person(std_msgs::Header header, std::string name, geometry_msgs::Point position, geometry_msgs::Point velocity, float reliability) 140 | pub_person.publish(tag_person(pcl_conversions::fromPCL(cloud_cluster->header), boost::to_string(id), pose, velocity, 0)); 141 | } 142 | 143 | void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) 144 | { 145 | // Convert to PCL data type 146 | pcl::PCLPointCloud2 pcl_pc; 147 | pcl_conversions::toPCL(*cloud_msg, pcl_pc); 148 | pcl::fromPCLPointCloud2(pcl_pc, cloud); 149 | 150 | cloud.header = pcl_conversions::toPCL(cloud_msg->header); 151 | 152 | pcl::octree::OctreePointCloudChangeDetector octree (RESOLUTION); 153 | // TODO: use pcl::PointCloud::Ptr cloud (new pcl::PointCloud); schema 154 | cloud_ptr = cloud.makeShared(); 155 | 156 | if (first_msg){ 157 | first_msg = false; 158 | filter_ptr = cloud.makeShared(); 159 | } 160 | 161 | octree.setInputCloud(filter_ptr); 162 | octree.addPointsFromInputCloud(); 163 | 164 | octree.switchBuffers(); 165 | 166 | octree.setInputCloud(cloud_ptr); 167 | octree.addPointsFromInputCloud(); 168 | 169 | std::vector newPointVector; 170 | octree.getPointIndicesFromNewVoxels(newPointVector, LEAF_SIZE); 171 | 172 | ROS_INFO("indicies %d", newPointVector.size()); 173 | 174 | pcl::PointCloud::Ptr filtered_cloud; 175 | filtered_cloud = (pcl::PointCloud::Ptr) new pcl::PointCloud(); 176 | 177 | (filtered_cloud)->header = pcl_conversions::toPCL(cloud_msg->header); 178 | 179 | filtered_cloud->points.reserve(newPointVector.size()); 180 | 181 | for (std::vector::iterator it = newPointVector.begin (); it != newPointVector.end (); it++) 182 | filtered_cloud->points.push_back(cloud_ptr->points[*it]); 183 | 184 | std::vector cluster_indices; 185 | if(filtered_cloud->size() > MIN_FILTERED_CLOUD_SIZE){ 186 | // Creating the KdTree object for the search method of the extraction 187 | pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); 188 | tree->setInputCloud (filtered_cloud); 189 | 190 | // ClusterExtraction 191 | pcl::EuclideanClusterExtraction ec; 192 | ec.setClusterTolerance (CLUSTER_TOLERANCE); // 7cm 193 | ec.setMinClusterSize (MIN_CLUSTER_SIZE); 194 | ec.setMaxClusterSize (MAX_CLUSTER_SIZE); 195 | ec.setSearchMethod (tree); 196 | ec.setInputCloud (filtered_cloud); 197 | ec.extract (cluster_indices); 198 | ROS_INFO("cluster size %d", cluster_indices.size()); 199 | } 200 | 201 | 202 | int mark = 0; 203 | for (std::vector::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it) { 204 | pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); 205 | for (std::vector::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++) { 206 | cloud_cluster->points.push_back (filtered_cloud->points[*pit]); //* 207 | } 208 | ROS_INFO("PointCloud representing the Cluster: %d data points.", cloud_cluster->points.size()); 209 | 210 | (cloud_cluster)->header = pcl_conversions::toPCL(cloud_msg->header); 211 | 212 | if(cloud_cluster->points.size() > MIN_CLUSTERED_CLOUD_SIZE){ 213 | pcl::toROSMsg(*cloud_cluster, msg_clustered); 214 | msg_clustered.header = cloud_msg->header; 215 | pub_clustered.publish(msg_clustered); 216 | mark++; 217 | new_person(cloud_cluster, mark); 218 | } 219 | } 220 | 221 | pcl::toROSMsg(*filtered_cloud, msg_filtered); 222 | msg_filtered.header = cloud_msg->header; 223 | pub_filtered.publish(msg_filtered); 224 | 225 | } 226 | 227 | int main (int argc, char** argv) 228 | { 229 | ros::init (argc, argv, "lidar_body_extraction"); 230 | ros::NodeHandle nh; 231 | 232 | dynamic_reconfigure::Server server; 233 | dynamic_reconfigure::Server::CallbackType f; 234 | 235 | f = boost::bind(&dynrcfg_callback, _1, _2); 236 | server.setCallback(f); 237 | 238 | std::string topic_scan, topic_filtered, topic_clustered, topic_person; 239 | nh.param("scan_topic", topic_scan, SCAN_TOPIC); 240 | nh.param("filtered_topic", topic_filtered, FILTERED_TOPIC); 241 | nh.param("clustered_topic", topic_clustered, CLUSTERED_TOPIC); 242 | nh.param("person_topic", topic_person, PERSON_TOPIC); 243 | 244 | ros::Subscriber sub = nh.subscribe(topic_scan, 1, cloud_cb); 245 | 246 | pub_filtered = nh.advertise(topic_filtered, 1); 247 | pub_clustered = nh.advertise(topic_clustered, 1); 248 | pub_vis = nh.advertise ("visualization_marker", 0); 249 | pub_person = nh.advertise (topic_person, 1); 250 | 251 | ROS_INFO_STREAM("Spinning"); 252 | 253 | ros::spin(); 254 | 255 | return 0; 256 | } 257 | 258 | 259 | -------------------------------------------------------------------------------- /urdf/m8.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | --------------------------------------------------------------------------------