A very simple (and not very robust) node for tracking dynamic obstacles using lidar.
- Author: Michael Phillips
- License: BSD
- Repository: penn-ros-pkg
- Source: svn https://mediabox.grasp.upenn.edu/svn/penn-ros-pkgs/sbpl_dynamic_env/tags/sbpl_dynamic_env-0.1.1
A very simple (and not robust) node that uses lidar to track moving people. This package does a basic clustering of points from a scan. It then matches the new clusters with clusters from the previous iteration (based on distance). The position, velocity, and size of a cluster is updated using a simple constant velocity motion model and through observations by using a weighted average from cluster matches. Clusters are declared if they are moving above a minimum speed and below a maximum. It can also uses a static costmap_2d to help prune out static obstacles that appear to be moving due to error (this can be a blank map but the tracker will return more false positives). On each iteration, after determining what clusters are considered "moving obstacles" the node runs a basic linear extrapolation to predict where the dynamic obstacles will be over the next few seconds and publishes a message with this information.
The tracker's performance isn't too bad when the robot is standing still. Once the robot is moving, the tracker's performance becomes pretty bad especially if there is localization error. In my experience, this becomes particularly bad when the robot is turning, and so there is an option to disable observation updates when the robot is turning too fast (the motion model still runs).
Subscribed Topicsscan (sensor_msgs/LaserScan)
- This message provides the laser scans used for tracking.
- If the track_when_turning parameter is false, the node doesn't use observations when the robot is primarily turning.
Published Topicsdynamic_obstacles (dynamic_obs_msgs/DynamicObstacles)
- The detected dynamic obstacles and their predicted future trajectories.
- Visualization markers that can be seen using rviz. It shows the clusters (blue), the dynamic obstacles (red), and the predicted future trajectories (fading red).
Parameterslaser_link (string, default: "base_laser_link")
- The frame of the laser scans.
- The frame of the map.
- Whether to use a costmap_2d named tracking_costmap, to help get rid of false positives.
- Whether to use observations while the robot is turning. Setting this to false is useful if the robot's localization is poor while turning.