THIS REPOSITORY IS ON EARLY STAGE OF DEVELOPMENT

object_spatial_tools_ros

Nodes to work with results of Extended Object Detection node.
All objects should be detected with distance estimation to it.

• Short Object Memory - remembers position of detected objects for some period of time
• Semantic Map Creator - marks detected objects on map, estimating their sizes
• Kalman Filter Tracker for Unoriented Objects - tracks detected objects without orientation, estimating their speed
• Exteneded Kalman Filter for Oriented Objects - tracks detected objects with orientation, estimating their speed

1. robot_short_object_memory_node.py

Remembers objects in moving frame for short period of time.
Simplified algorithm to add new object:

graph LR
    classDef box fill:#FFFFFF, stroke:#000, stroke-width:2px;

    A[get new detected object]:::box --> B{type exists?}:::box
    B --> |NO|C[add to memory as new]:::box
    C --> J[occurance++, forgotten = false]:::box
    B --> |YES|D[calc match scores, calc thresh]:::box
    D --> E{best match score < thresh}:::box
    E --> |NO|C:::box
    E --> |YES|I[append to best match]:::box
    I --> J:::box

Loading

Simplified algorithm to update objects:

graph LR
    classDef box fill:#FFFFFF, stroke:#000, stroke-width:2px;
    
    A{forgotten == true}:::box -->|YES| B{occurance--}:::box
    B --> C[occurance == 0]:::box
    C --> |YES|D[delete obj]:::box
    C --> |NO|E[do nothing]:::box
    A --> |NO|F{now - obj_stamp < forget_time}:::box
    F -->|NO|E:::box
    F -->|YES|I[forgotten = true]:::box

Loading

Params

• ~target_frame (string, default: odom) frame for remembered objects
• ~score_multiplyer (double, default: 2) multiplier for score, to check similarity of objects
• ~update_rate_hz (double, default: 5 [hz]) rate of update algorithm (see below)
• ~forget_time (double, default: 10 [sec]) time to remove object if not seen
• ~update_count_thresh (double, default: 0) limit for previous position used for update, if 0 - no limit

Subscribed topics

• simple_objects (extended_object_detection/SimpleObjectArray) input result of detection
• complex_objects (extended_object_detection/ComplexObjectArray) input result of detection

Published topics

• ~memory_map (visualization_msgs/MarkerArray) visualization of results
• TODO: results itself!

Provided services

• ~get_object (object_spatial_tools_ros/GetObject) returns coordinates of object with specified type and subtype, but only if it represented once in memory
• ~get_closest_object (object_spatial_tools_ros/GetClosestObject) returns coordinates of closest object to given frame
• ~ignore_object (object_spatial_tools_ros/IgnoreObject) sets some object to ignore (removes previous info and futher precessing)

2. robot_semantic_map_processor_node.py

Creates an 'semantic map layer' which contains position, names and sizes of objects.

graph TD
  classDef box fill:#FFFFFF, stroke:#000, stroke-width:2px;

  A[new objects recieved]:::box
  A-->B{type exists}:::box

  B --> b{Is min MH dist to saved cluster centroid less thresh?}:::box

  b -->|NO| C[Append point to container unmerged data]:::box
  b -->|YES| c[Add point to saved cluster]:::box
  B -->|NO| D[Create new object container]:::box

  c --> E
  C --> E
  D --> E
  E[Free temp clusters, and recluster all unmerged data]:::box
  E --> F{Cluster size > min_size}:::box
  F -->|YES| G{Cluster size > max_size}:::box
  F -->|NO| H[Ignore it]:::box

  G -->|YES| J[Add cluster to saved clusters, remove its data from unmerged data]:::box
  G -->|NO| K[Add clusters to temp clusters]:::box
  J --> L:::box
  K --> L:::box
  L[Merge intersecting saved clusters]

Loading

Params

• ~map_frame (string, default: "map") TF frame, used for mapping.
• ~map_file (string, "/tmp/semantic_map.yaml") Path of map to be loaded.
• ~clear_map_on_start (bool, false) If true creates new map, and save_semantic_map serice will overwrite saved map if exists.
• ~update_map (bool, default: False) If clear_map_on_start is false. If true, loaded map will be updated if exist, else will be crated a new one. If false, that loaded map only will be republished to topics is exist, else programm will be terminated.
• ~pub_rate_sec (float, default: 1.0) If update_map is false, it will be published with such rate.
• ~publish_map_as_markers (bool, default: True) If true marker representation of map is published.
• ~publish_cloud (bool, default: False) If true raw object position is published.
• ~mh_thres (float, default: 3.0) Mahalanobis threshold when new points is added to existing cluster.
• ~cluster_dist_thres (float, default: 3.0) Threshold (in meters?) of distancem which is used to form new cluster by hierarhical clustering.
• ~cluster_min_size (int, default: 10) If cluster size is less, it is ignored.
• ~cluster_max_size (int, default: 100) When cluster size overgrown that value, it is saved and only its params are updated.

Subscibed topics

• detected_objects (extended_object_detection/SimpleObjectArray) Detected objects to be mapped. If update_map is false, node doesn't subscribe to it.

Published topics

• ~semantic_map (object_spatial_tools_ros/SematicMap) Full map information for external usage.
• ~semantic_object_map_as_markers (visualiation_msgs/MarkerArray) Map represented for rviz visualization, only published if publish_map_as_markers param is set.

Provided services

• ~save_semantic_map (std_srvs/Empty) Updates map on it's path, or saves new one if not exist.
• ~clear_map (std_srvs/Empty) Fully clears existed map.

3. robot_kf_undirected_object_tracker_node.py

Tracks visually detected objects in 2d space. Works with unoriented objects. Kalman Filter estimates x,y, vx, vy parameters.
Simplified algorithm to add new object:

graph LR
    classDef box fill:#FFFFFF, stroke:#000, stroke-width:2px;
    
    A[get new object]:::box --> B{type exists?}:::box
    
    Z[Reject object]:::box
    C[start new KF]:::box   
    D[calc mahalanobis]:::box
    E{min maxalanobis < thresh}:::box
    e{score > min_score}:::box
    f{score > min_score_soft}:::box

    B -->|NO|e:::box
    B --> |YES|D:::box
    D --> E:::box
    E --> |NO|e:::box
    e --> |YES|C:::box
    E --> |YES|f:::box
    f --> |YES|F[update KF with object]:::box
    f --> |NO|Z:::box
    e --> |NO|Z:::box

Loading

Simplified algorithm to handle existing filters:

graph LR
    classDef box fill:#FFFFFF, stroke:#000, stroke-width:2px;
    
    A{lifetime > now - last_update}:::box --> |YES|B[remove KF]:::box
    A --> |NO|C[predict KF]:::box

Loading

Params

• ~target_frame (string, default: odom) frame for tracking
• ~tf_pub_prefix (string, default: "") is set, prefix will be added to broadcasted tf frames
• ~tracked_objects_type_names (list, default: []) object names from object base to track
• ~Qdiag (list, default: [0.1, 0.1, 0.1, 0.1]) diagonal values of Q matrix
• ~Rdiag (list, default: [0.1, 0.1]) diagonale values of R matrix
• ~k_decay (double, default: 1) track speed reducer, new step speed will be k * speed_prev
• ~lifetime (double, default: 0) how long to perform tracking when objects disappears, if 0 - infinite
• ~mahalanobis_max (double, default: 1) Mahalanobis dist when new object might be added to existing track
• ~update_rate_hz (double, default: 5 [hz]) rate of tracker
• ~min_score (double, default: 0.0) threshold for score of detected objects
• ~min_score_soft (double, default: ~min_score) threshold for soft-mode traking, to disable set >= ~min_score

Subscribed topics

• simple_objects (extended_object_detection/SimpleObjectArray) input result of detection
• complex_objects (extended_object_detection/ComplexObjectArray) input result of detection

Published topics

• ~vis (visualization_msgs/MarkerArray) visualization of results
• ~tracked_objects (object_spatial_tools_ros/TrackedObjectArray) out results, containg poses and speeds