Tunning UoL people tracking pipeline

MFernandezCarmona commented 4 years ago

Current UoL tracking pipeline is configured to merge two different detection sources:

Velodyne (object3d_detector)
Laserscan (leg_detector): currently uses filtered points from floor laser. They are later merged in the bayes_people tracker. Both sources have an observation model and noise parameters.

Pipeline has some limitations while robot is moving and a lot of false positives. In order to improve this pipeline:

[x] Make sure that /robot5/sensors/laser2d_floor_filtered does not self-collisions (e.g. wheels, handle)
[x] Reduce false positives in leg_detector: check leg detector wiki and configure paramenters to smooth it.
[ ] Modify velodyne nodelet to filter out self collisions. Use parameter min_range from velodyne_pointcloud/launch/VLP16_points.launch included in iliad_metapackage/launch/robots/velodyne.launch
[x] Reduce false positives in object3d_detector. Check parameters: z_limit_min, z_limit_max, cluster_size_min, cluster_size_max, human_probability, human_size_limit. Or SVN model...
[ ] Change noise parameters and/or filters in bayes_people_tracking.

MFernandezCarmona commented 4 years ago

In order to work with the above, it will be better to record a rosbag where the robot is moving and a human is around (I'd say overtaking, as it seems to be one of the most challenging scenarios). Then is just a matter of replaying the rosbag and launching iliad_launch_system/launch/subsystems/perception/people_3d.launch

Here is a list of needed topics:

/robot5/sensors/velodyne_points 
/robot5/sensors/laser2d_floor
/robot5/sensors/laser2d_floor_filtered                                                                                                                                        
/maps/map_laser2d 
/tf 
/tf_static

And topics that should be nice to add as well to have a better track of the robot whereabouts:

/robot5/control/controller/commands
/robot5/control/controller/reports
/robot5/control/controller/trajectories
/robot5/control/encoders
/robot5/control/odom
/robot5/control/report
/robot5/control/state
/robot5/goal
/robot5/status

One of the options I mention implies changing the default behaviour of the velodyne. For that, we record the velodyne packets and change the parameters on the velodyne driver iIn order to filter the packets and generate points only with a greater distance than the default one. For that, record :

/robot5/sensors/velodyne_packets

Finally, a list of topics generated by the tracking pipeline. Recording them is not necessary, as they should be generated from the sensor data, but on a first record, they might be necesary to disect what the robot is doing:

/robot5/leg_tracker_measurements 
/robot5/to_pose_array/leg_detector 
/robot5/object3d_detector/poses                                                                                                                                   
/robot5/people_tracker/people 
/robot5/people_tracker/pose 
/robot5/people_tracker/pose_array 
/robot5/people_tracker/positions

@laurencejbelliott Please edit these lists if you find that something is missing.

MFernandezCarmona commented 4 years ago

Final comment, a log of rosnode info from the relevant nodes:

--------------------------------------------------------------------------------
Node [/robot5/object3d_detector]                                                                                                                                                                            
Publications:                                                                                                                                                                                               
 * /robot5/object3d_detector/markers [visualization_msgs/MarkerArray]                                                                                                                                       
 * /robot5/object3d_detector/poses [geometry_msgs/PoseArray]                                                                                                                                                
 * /rosout [rosgraph_msgs/Log]                                                                                                                                                                              

Subscriptions:                                                                                                                                                                                              
 * /robot5/sensors/velodyne_points [sensor_msgs/PointCloud2]     

--------------------------------------------------------------------------------
Node [/robot5/map_laser_filter]
Publications: 
 * /robot5/sensors/laser2d_floor_filtered [sensor_msgs/LaserScan]
 * /rosout [rosgraph_msgs/Log]

Subscriptions: 
 * /maps/map_laser2d [nav_msgs/OccupancyGrid]
 * /robot5/sensors/laser2d_floor [sensor_msgs/LaserScan]
 * /tf [tf2_msgs/TFMessage]
 * /tf_static [tf2_msgs/TFMessage]

--------------------------------------------------------------------------------
Node [/robot5/leg_detector]
Publications: 
 * /robot5/leg_detector/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
 * /robot5/leg_detector/parameter_updates [dynamic_reconfigure/Config]
 * /robot5/leg_tracker_measurements [people_msgs/PositionMeasurementArray]
 * /robot5/people_tracker_measurements [people_msgs/PositionMeasurementArray]
 * /robot5/visualization_marker [visualization_msgs/Marker]
 * /rosout [rosgraph_msgs/Log]

Subscriptions: 
 * /robot5/people_tracker_filter [unknown type]
 * /robot5/sensors/laser2d_floor_filtered [sensor_msgs/LaserScan]
 * /tf [tf2_msgs/TFMessage]
 * /tf_static [tf2_msgs/TFMessage]

--------------------------------------------------------------------------------
Node [/robot5/to_pose_array]
Publications: 
 * /robot5/to_pose_array/leg_detector [geometry_msgs/PoseArray]
 * /rosout [rosgraph_msgs/Log]

Subscriptions: 
 * /robot5/leg_tracker_measurements [people_msgs/PositionMeasurementArray]

--------------------------------------------------------------------------------
Node [/robot5/bayes_people_tracker]                                                                                                                                                                         Publications:                                                                                                                                                                                                 * /robot5/people_tracker/marker_array [visualization_msgs/MarkerArray]                                                                                                                                      * / * /robot5/people_tracker/people [people_msgs/People]                                                                                                                                                        * / * /robot5/people_tracker/pose [geometry_msgs/PoseStamped]                                                                                                                                                   * / * /robot5/people_tracker/pose_array [geometry_msgs/PoseArray]                                                                                                                                               * / * /robot5/people_tracker/positions [bayes_people_tracker/PeopleTracker]                                                                                                                                     * / * /robot5/people_tracker/trajectory [geometry_msgs/PoseArray]                                                                                                                                               * / * /rosout [rosgraph_msgs/Log]                                                                                                                                                                              

Subscriptions:                                                                                                                                                                                              
 * /robot5/object3d_detector/poses [geometry_msgs/PoseArray]                                                                                                                                                
 * /robot5/to_pose_array/leg_detector [geometry_msgs/PoseArray]                                                                                                                                             
 * /tf [tf2_msgs/TFMessage]                                                                                                                                                                                 
 * /tf_static [tf2_msgs/TFMessage]         

--------------------------------------------------------------------------------
Node [/robot5/people_tracker_filter]
Publications: 
 * /robot5/people_tracker_filtered/marker_array [visualization_msgs/MarkerArray]
 * /robot5/people_tracker_filtered/people [people_msgs/People]
 * /robot5/people_tracker_filtered/pose [geometry_msgs/PoseStamped]
 * /robot5/people_tracker_filtered/pose_array [geometry_msgs/PoseArray]
 * /robot5/people_tracker_filtered/positions [bayes_people_tracker/PeopleTracker]
 * /rosout [rosgraph_msgs/Log]

Subscriptions: 
 * /maps/map_laser2d [nav_msgs/OccupancyGrid]
 * /robot5/people_tracker/people [people_msgs/People]
 * /robot5/people_tracker/pose [geometry_msgs/PoseStamped]
 * /robot5/people_tracker/pose_array [geometry_msgs/PoseArray]
 * /robot5/people_tracker/positions [bayes_people_tracker/PeopleTracker]

MFernandezCarmona commented 4 years ago

Final parameter tunning has been done. We have a reasonably working people tracker fusing leg and velodyne detectors. Configuration parameters are in people_3d.launch at iliad_metapackage.

LCAS / iliad

Tunning UoL people tracking pipeline #33