Only released in EOL distros:   kinetic

Overview

This package provides two main nodes; one for finding moving objects in a stream of sensor_msgs/LaserScan messages, and one for finding moving objects in a stream of sensor_msgs/PointCloud2 messages. Two corresponding nodelets are also provided.

There are also two nodes and two nodelets for handling two special message types: find_moving_objects/LaserScanArray and find_moving_objects/PointCloud2Array. These message types contain a header and, respectively, an array of sensor_msgs/LaserScan and sensor_msgs/PointCloud2. The intended use for these message types is for multi-diode Lidars and similar sensors.

Finding Moving Objects

The above-mentioned nodes and nodelets output a stream of messages (see Defined Message Types) containing an array of found moving objects. For each found object, the provided information includes details about its position and velocity in the given sensor frame, as well as in a map, fixed, and base frame. Typically, the latter frames should be set to map, odom and base_link, respectively (see REP 105). A confidence value and information about each object's distance, closest point and width (relative the sensor) are also provided.

Note that the nodes rely on message filtering from tf2 so the map, fixed and base frames must be specified. For testing purposes, these could of course all be set to the frame of the sensor itself.

The nodes/nodelets store a history of received range scans in an internal data structure referred to as the bank. The velocity of an object is calculated based on how it has moved from the oldest range scan stored in the bank to the newest - so the size of the bank (that is, the number of range scans it stores) is a critical property to set for the system. The size of the bank can be automatically calculated based on (measuring the publish rate of the sensor and) a desired time period which it should cover.

There is also a confidence-enhancing node that can be used for cases where several sensors are used. This node can take the output of the moving-object-finding nodes connected to the sensor data streams and increase the confidence of objects that it believes are seen by several sensors.

The overall structure of using the provided nodes is given in overall_structure.pdf.

Visualization Using RViz

All the nodes and nodelets can be told to publish visualization messages so that the properties of the found objects can be presented using RViz. Each node/nodelet can publish a subset of the following message types.

• Arrows (using visualization_msgs/MarkerArray and visualization_msgs/Marker) can be used to display each object's position (base of the arrow), velocity (length and direction of the arrow) and confidence value (grayness of the arrow; by default, 0.0 is black and 1.0 is white).

• Green lines (using visualization_msgs/MarkerArray and visualization_msgs/Marker) can be used to display each object's estimated width.

• Blue lines (using visualization_msgs/MarkerArray and visualization_msgs/Marker) can be used to display how each object has moved from the oldest range scan stored in the bank to the newest.

• The point of each object which is closest to the sensor can be visualized by a point (using sensor_msgs/LaserScan).

• Points (using sensor_msgs/LaserScan) can be used to display the newest stored range scan, adapted using EMA (Exponential Moving Average), in the bank.

Please refer to the "Parameters" section for each node below for more details.

Paper

A paper explaining the algorithm and data structure used by find_moving_objects was presented at CVC 2019 and published by Springer. This is a pre-print fulltext version of the above paper.

Example

The nodelet for interpreting sensor_msgs/PointCloud2 data streams and the node for interpreting sensor_msgs/LaserScan data streams are used in the provided launch file. This launch file can be used to run the interpreters on live or recorded sensor data (an example bag file is provided, but it must be extracted before use). The sensors supported by the launch file are the following (however, note that other ROS packages and software libraries are needed to run these live).

• Intel Realsense D435 depth camera (for sensor_msgs/PointCloud2 data at about 15-30Hz)

• Slamtec rplidar A2 (for sensor_msgs/LaserScan data at about 12Hz)

For the nodelets, there is a launch argument manager which is used to specify which manager the nodelet should connect to. For more information about the available arguments for launching nodes and nodelets, please refer to the respective launch/includes files.

Defined Message Types

The following are the message types defined by this package:

• find_moving_objects/MovingObject

• find_moving_objects/MovingObjectArray

• find_moving_objects/LaserScanArray

• find_moving_objects/PointCloud2Array

Please refer to the message type sources for their documentation.

Nodes and Nodelets

The node and nodelet used for finding objects in sensor_msgs/LaserScan message streams have the same parameters and publish/subscribe to the same topics. The same is true for the other node and nodelet pairs.

laserscan_interpreter_node and LaserScanInterpreterNodelet

The node/nodelet which finds moving objects in a sensor_msgs/LaserScan message stream. There are many parameters which can be specified but the most needed ones are probably:

• subscribe_topic
• topic_objects
• optimize_nr_scans_in_bank
• object_threshold_*

Parameters

subscribe_topic (string, default: "laserscan")

• The topic subscribed to.

subscribe_buffer_size (int, default: 1)

• The size of the subscription buffer.

ema_alpha (double, default: 1.0)

• Exponential Moving Average (EMA) weighting decrease factor. (If set to 1.0, then there is no EMA.)

map_frame (string, default: "map")

• The map frame.

fixed_frame (string, default: "odom")

• The fixed frame.

base_frame (string, default: "base_link")

• The base frame.

nr_scans_in_bank (int, default: 0)

• The size of the bank. For this size to take effect, optimize_nr_scans_in_bank must be set with the value 0.0.

optimize_nr_scans_in_bank (double, default: 0.5)

• If not 0.0, then the bank size is optimized to cover the specified time period (in seconds) and nr_scans_in_bank is ignored.

publish_objects (bool, default: true)

• Whether to publish the found moving objects in find_moving_objects/MovingObjectArray messages.

publish_ema (bool, default: true)

• Whether to publish the newest sensor data (EMA:ed) stored in the bank in a sensor_msgs/LaserScan message.

publish_objects_closest_points_markers (bool, default: true)

• Whether to publish the closest points (relative the sensor) of the found objects in a sensor_msgs/LaserScan message.

publish_objects_velocity_arrows (bool, default: true)

• Whether to publish a visualization_msgs/MarkerArray message with an arrow describing the velocity and confidence of each found object.

publish_objects_delta_position_lines (bool, default: true)

• Whether to publish a visualization_msgs/MarkerArray message with a a line describing the change in position of each found object as seen be the bank.

publish_objects_width_lines (bool, default: true)

• Whether to publish a visualization_msgs/MarkerArray message with a a line describing the width of each found object.

publish_buffer_size (int, default: 1)

• The size of each publish buffer.

topic_objects (string, default: "moving_objects")

• The topic name on which to publish the found moving objects.

topic_ema (string, default: "ema")

• The topic name on which to publish the newest sensor data stored in the bank.

topic_objects_closest_points_markers (string, default: "objects_closest_point_markers")

• The topic name on which to publish the closest points of the found objects.

topic_objects_velocity_arrows (string, default: "objects_velocity_arrows")

• The topic name on which to publish the velocity arrows of the found objects.

topic_objects_delta_position_lines (string, default: "objects_delta_position_lines")

• The topic name on which to publish the delta position lines of the found objects.

topic_objects_width_lines (string, default: "objects_width_lines")

• The topic name on which to publish the width lines of the found objects.

ns_velocity_arrows (string, default: "velocity_arrows")

• The namespace of the velocity arrows.

ns_delta_position_lines (string, default: "delta_position_lines")

• The namespace of the delta position lines.

ns_width_lines (string, default: "width_lines")

• The namespace of the width lines.

velocity_arrows_use_full_gray_scale (bool, default: false)

• Whether the gray-scale of the velocity arrows should be adapted so that the value of object_threshold_min_confidence corresponds to black (the default is that 0.0 corresponds to black and 1.0 to white).

velocity_arrows_use_sensor_frame (bool, default: false)

• Whether velocity arrows should be displayed in the sensor frame (the default is to display them in the map frame).

velocity_arrows_use_base_frame (bool, default: false)

• Whether velocity arrows should be displayed in the base frame (the default is to display them in the map frame).

velocity_arrows_use_fixed_frame (bool, default: false)

• Whether velocity arrows should be displayed in the fixed frame (the default is to display them in the map frame).

object_threshold_edge_max_delta_range (double, default: 0.15)

• Maximum range difference for two consecutive scan points to belong to the same object.

object_threshold_min_nr_points (int, default: 3)

• The minimum number of points an object must consist of.

object_threshold_max_distance (double, default: 6.5)

• The maximum distance of the object.

object_threshold_min_speed (double, default: 0.1)

• The minimum speed of the object.

object_threshold_max_delta_width_in_points (int, default: 15)

• The maximum difference in width of the object between two consecutive sensor scans.

object_threshold_bank_tracking_max_delta_distance (double, default: 0.4)

• The maximum distance the object is allowed to move between two consecutive sensor scans.

object_threshold_min_confidence (double, default: 0.7)

• The minimum confidence needed in order to publish the given object.

base_confidence (double, default: 0.3)

• A value corresponding to how well the sensor as such is trusted.

laserscanarray_interpreter_node and LaserScanArrayInterpreterNodelet

The node/nodelet which finds moving objects in a find_moving_objects/LaserScanArray message stream. The same parameters as specified for the laserscan_interpreter_node and LaserScanInterpreterNodelet above are valid here as well.

pointcloud2_interpreter_node and PointCloud2InterpreterNodelet

The node/nodelet which finds moving objects in a sensor_msgs/PointCloud2 message stream. The same parameters as specified for the laserscan_interpreter_node and LaserScanInterpreterNodelet above are valid here as well, with the addition of some PointCloud2-specific ones listed below. There are many parameters which can be specified but the most needed ones are probably:

• subscribe_topic
• topic_objects
• optimize_nr_scans_in_bank
• sensor_frame_has_z_axis_forward
• object_threshold_*
• threshold_z_min
• threshold_z_max

Parameters

subscribe_topic (string, default: "pointcloud")

• The topic subscribed to.

sensor_frame_has_z_axis_forward (bool, default: true)

• Whether the sensor frame is an optical frame (see REP 103).

bank_view_angle (double, default: 3.141592654)

• The Field of View (FoV) of the internal bank representation. (This should preferably be larger than the sensor FoV.)

nr_points_per_scan_in_bank (int, default: 360)

• The number of scan points in the internal bank representation.

message_x_coordinate_field_name (string, default: "x")

• The name of the field in the sensor_msgs/PointCloud2 message containing the x-coordinate of a point.

message_y_coordinate_field_name (string, default: "y")

• The name of the field in the sensor_msgs/PointCloud2 message containing the y-coordinate of a point.

message_z_coordinate_field_name (string, default: "z")

• The name of the field in the sensor_msgs/PointCloud2 message containing the z-coordinate of a point.

voxel_leaf_size (double, default: 0.01)

• Defines how many bank points a given cloud point should map onto.

threshold_z_min (double, default: 0.0)

• Cloud points with a z-coordinate (negative y-coordinate if sensor_frame_has_z_axis_forward is set to true) smaller than this are discarded.

threshold_z_max (double, default: 1.0)

• Cloud points with a z-coordinate (negative y-coordinate if sensor_frame_has_z_axis_forward is set to true) larger than this are discarded.

pointcloud2array_interpreter_node and PointCloud2ArrayInterpreterNodelet

The node/nodelet which finds moving objects in a find_moving_objects/PointCloud2Array message stream. The same parameters as specified for the pointcloud2_interpreter_node and PointCloud2InterpreterNodelet above are valid here as well.

moving_objects_confidence_enhancer_node

The node which can take information about moving objects as found by several sensors and increase the confidence for those objects which it believes are seen by several sensors. There are many parameters which can be specified but the most needed ones are probably:

• subscribe_topic
• topic_moving_objects_enhanced
• threshold_max_delta_time_for_different_sources
• threshold_max_delta_position
• threshold_max_delta_velocity
• ignore_z_map_coordinate_for_position

Parameters

subscribe_topic (string, default: "moving_objects")

• The topic to subscribe to.

subscribe_buffer_size (int, default: 10)

• The size of the subscription buffer.

verbose (bool, default: false)

• Verbose output.

print_received_objects (bool, default: false)

• Whether to print the objects received from each sensor.

publish_objects (bool, default: true)

• Whether to publish/forward the received objects in find_moving_objects/MovingObjectArray messages.

publish_objects_velocity_arrows (bool, default: true)

• Whether to publish a visualization_msgs/MarkerArray message with an arrow describing the velocity and confidence of each found object.

publish_objects_closest_points_markers (bool, default: true)

• Whether to publish the closest points (relative the sensor) of the found objects in a sensor_msgs/LaserScan message.

publish_buffer_size (int, default: 2)

• The size of each publish buffer.

topic_moving_objects_enhanced (string, default: "moving_objects_enhanced")

• The topic name on which to publish the moving objects with possibly increased confidence values.

topic_objects_velocity_arrows (string, default: "objects_velocity_arrows")

• The topic name on which to publish the velocity arrows of the found objects.

topic_objects_closest_points_markers (string, default: "objects_closest_points_markers")

• The topic name on which to publish the closest points of the found objects.

velocity_arrows_use_full_gray_scale (bool, default: false)

• Whether the gray-scale of the velocity arrows should be adapted so that the value of threshold_min_confidence corresponds to black (the default is that 0.0 corresponds to black and 1.0 to white).

velocity_arrows_use_sensor_frame (bool, default: false)

• Whether velocity arrows should be displayed in the sensor frame (the default is to display them in the map frame).

velocity_arrows_use_base_frame (bool, default: false)

• Whether velocity arrows should be displayed in the base frame (the default is to display them in the map frame).

velocity_arrows_use_fixed_frame (bool, default: false)

• Whether velocity arrows should be displayed in the fixed frame (the default is to display them in the map frame).

threshold_min_confidence (double, default: 0.0)

• The minimum confidence needed in order to publish the given object.

threshold_max_delta_time_for_different_sources (double, default: 0.2)

• The maximum difference between the timestamps of messages received from different sensors in order to compare the objects contained in the respective messages.

threshold_max_delta_position (double, default: 0.1)

• The maximum difference in the derived positions of two objects seen by different sensors in order to consider the objects to actually be the same physical object.

threshold_max_delta_velocity (double, default: 0.1)

• The maximum difference in the derived velocities of two objects seen by different sensors in order to consider the objects to actually be the same physical object.

ignore_z_map_coordinate_for_position (bool, default: true)

• Whether to ignore the difference in vertical position (in the map frame) when comparing the position of two objects seen by different sensors.

Bug Reports and Feature Requests

Use GitHub to report bugs or submit feature requests. [View active issues]