Collision monitor

[AlexeyMerzlyakov]

Implementation of Collision Monitor node

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#1899
Primary OS tested on	Ubuntu 20.04 with ROS2 Rolling built from sources
Robotic platform tested on	Gazebo simulation of Turtlebot3

---

Description of contribution in a few bullet points

• New Collision Monitor (formerly Collision Safety) node
• Available 3 modes: Stop, Slowdown and Approach.
  • Stop mode: the robot will be stopped if more than N points are inside given polygon
  • Slowdown mode: the robot will be slow down on S% if more than N points are inside given polygon. Working principle similar to Stop mode
  • Approach mode: the robot reduces its speed to keep constant M seconds before a collision with obstacle. In this model, given polygon is acting as robot's footprint.
• Two types of shapes are supported: polygon and circle.
• Two types of inputs are supported: laser scanner and pointcloud
• Each mode is attached/connected to each shape. Although all 3 conditions are mutually exclusive, nobody restricts to set all modes' polygons simultaneously. The priority in this case will be given to the safest mode when two or more activated (The safest mode is considered as Stop. If there is no stop required, the mode producing the lowest speed will be activated.)
• Collision Monitor is acting as independent node, laying "under" Nav2 stack. There are remapped input and output cmd_vel required for node operation.

Description of documentation updates required from your changes

• Node parameters description page
• Collision Monitor usage tutorial
• New feature in Navigation Concepts ?

---

Future work that may be required in bullet points

• Cover Collision Monitor by tests
• Cover the code by comments / better explaining what is going on
• Performance optimizations for Approach mode
• Increasing the stability of Approach mode when the odom speed changes in steps/making noise

For Maintainers:

• Check that any new parameters added are updated in navigation.ros.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[mergify]

@AlexeyMerzlyakov, please properly fill in PR template in the future. @SteveMacenski, use this instead.

• Check that any new parameters added are updated in navigation.ros.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[AlexeyMerzlyakov]

Initial review by @SteveMacenski (current status):

• Add me as a maintainer
• Looks like tests are commented out of cmakelist
• Also don't think this is a component node, we should do that so it can be used as a component or standalone node like the other servers
• emergency thresh --> num_pts or something similar, I think that's more clear as to what that means
• emergency model --> model (or another word? I guess I don't like 'emergency', I think it sets a level of alarm I don't want to imply)
• PCL2 --> PointCloud, we might be using PCL as a library (but maybe not?) but the datatype is a pointcloud
• In class, all private to protected so someone can override the class and have access. Also, all functions before all class member variables
• getPoly is called in loops, so we should only get the polygon if one doesn't already exist. For instance, the circle polygon will continuously recompute it versus compute it once and store it. If there's something stored, then return that. Issue in the polygon publication thread.
• Does the polygon publication thread update based on robot pose so that its following the robot?
• Why not publish the polygons at the end of a cycle of the main worker thread?
• min/max_z -> min/max_height (z is a bit ambiguous in sensor processing)
• getPoly -> getPolygon or getShape or something along those lines
• dynamics are actually kinematics (dynamics are differential equations). movePose to projectPose
• For the scan type, there's a tool in ros called the laser projector http://wiki.ros.org/laser_geometry that will take in a laser scan and return to you a pointcloud to use so you don't need to do that conversion yourself. I believe it will even do the transformation for you as well to another frame
• From a quick glance, I'm not sure what fixPoint is doing semantically in the code, but maybe its just another rename might be in order
• Remove all the default_ variables in the constructors and just set the defaults in the declare calls. Also, the declare calls should be in the configuration function and use declare if not already declared function in nav2_util so that it can be cycled up multiple times
• I want to say the subscriptions should be in activate , but please check other servers to make sure I'm right about that. The publishers can be in configure because there's an on_activate function to not activate the interface until its up, subscribers don't have that (yet)
• Why not make the main worker thread actually just a timer? Then you don't need mutexes, because it will be added to the same single threaded executor as the data callbacks so they'll be mutually exclusive. That should be running at a fixed rate I think, not just when a  velocity command comes in. -- but actually I'd like your opinion on that. Should this be independent of cmd_vels or done in the same callback? How long does it take to execute that callback as it is now?
  It would just be good to measure some times to process to make sure that we're not blocking up the executor too badly if we're running at 20hz (max probably 50hz)
• If its a timer, you can get rid of setWorkerActive / getWorkerActive because you can just reset and recreate the timer in configure.
• Use unique / shared pointers, don't allocate raw memory (e.g. L334/L341/L377 of the main node file). I actually prefer what you did, but the codebase styling established by OSRF says we should use modern C++ tools so I just fall in line with that
• Huh, should the time before collision (rename to time_before_collision) be a parameter of the node or the data sources? You have it as each polygon, which I had not actually considered.
• Can declaring / getting parameters specific to the specific sensor models happen in the sensor model files (e.g. min/max Z)?
• Change the main worker so that we iterate through the set of data 1 time and we check against it for each of the polygon inside of it. I think that's more efficient, no?
• If you use smart pointer, you can only do polygons_.clear(); in clearNode, you wouldn't need to iterate and manually delete. At that point, I'd remove the function entirely and just add those 2 clear()s to the cleanup / destructor directly
• Also, what if we have multiple sources in a polygon, just last data doesnt make sense because we'd drop measurements from other sensors. I think we need to store all in a window or probably better the latest of each frame_id , since each sensor will have a different frame_id  you can use to differentiate if all coming over the same callback.
• I don't remember seeing any data expiration (e.g. what if the data is too old to use) in a case where a sensor stops giving data. We need to make sure we delete any expired data older than some timeframe so that its not falsely being used for collision avoidance. If we have no new data, we should throttle log a warning about it .
• I think there's some simplification that needs to occur / breaking down long functions and trying to use that nice object hierarchy you established

[AlexeyMerzlyakov]

Remaining items/questions to discuss:

Looks like tests are commented out of cmakelist

Yes, currently testcases are not being developed yet. I suggest to develop test coverage after the main code from the Collision Monitor node will be agreed.

Does the polygon publication thread update based on robot pose so that its following the robot?

Yes, it is being calculated and published in robot base_frame.

Why not publish the polygons at the end of a cycle of the main worker thread?

Since publishPolygons() routine is made only for visualization purposes, I think we do not need to have it is being published in high-priority thread at least on 50Hz. That also is excess for RViz. Therefore, to reduce main cycle and RViz CPU load, I've moved it to separate timer.

For the scan type, there's a tool in ros called the laser projector http://wiki.ros.org/laser_geometry that will take in a laser scan and return to you a pointcloud to use so you don't need to do that conversion yourself. I believe it will even do the transformation for you as well to another frame

Work is in progress: currently evaluation of how to better us laser projector API is being working on.

I want to say the subscriptions should be in activate , but please check other servers to make sure I'm right about that. The publishers can be in configure because there's an on_activate function to not activate the interface until its up, subscribers don't have that (yet)

Currently for AMCL and Costmap2D the subscribers are set in configure state, while for Costmap Filters - are in activate. From my point of view it looks like both lifecycle publishers and classical subscribers are better to be set symmetrically in configure while we are using setWorkerActive()/getWorkerActive() to make sure we do are not starting the processing earlier than active state (yet I do not insist on this solution).

It would just be good to measure some times to process to make sure that we're not blocking up the executor too badly if we're running at 20hz (max probably 50hz)

If its a timer, you can get rid of setWorkerActive / getWorkerActive because you can just reset and recreate the timer in configure.

The results of performance measurement are presented below (I've used PCL data sparse patch, not included in this PR):

Time, ms                               Stop/Slowdown     Approach
LaserScan (source size 360 points)     0.13              6.93
PointCloud (source size ~22K points)   1.21	         258.10
PointCloud / 23 (sparsed source size ~950 points)        17.30
		
Max freq, Hz                           Stop/Slowdown	 Approach
LaserScan (source size 360 points)     7687 Hz           144 Hz
PointCloud (source size ~22K points)   829 Hz            4 Hz
PointCloud / 23 (sparsed source size ~950 points)        58 Hz

The Approach mode with PCL works the slowest, as it is simulates the robot's movement in kinematics module with 0.02 sec(by-default) time step, and re-calculates all points coordinates towards to robot simulated position on each step.
The one of the solution - is PCL data sparsing, which makes appropriate results.
According to this, I agree, we need to think about making main worker in a separate from input cmd_vel thread.

Huh, should the time before collision (rename to time_before_collision) be a parameter of the node or the data sources? You have it as each polygon, which I had not actually considered.

Currently, all parameters, such as number of stop points and slowdown % are connected with each polygon. I thought it is reasonable to make it for Approach model too. Since we might not have an approach polygon at all, why should we handle this parameter at global level?

Change the main worker so that we iterate through the set of data 1 time and we check against it for each of the polygon inside of it. I think that's more efficient, no?

The complexity of algorithm is O(MxN), where M - total number of points in data sources, N - number of polygons iterating on. It should not depend on which dimension we will iterate first: polygons or data points.
Since, each polygon is connected with its own model (Stop, Slowdown or Approach), it is more convenient in the code logic to iterate by shapes first, and then calculate the number of point collisions per each shape instead of have a temporary array of number of collisions per each polygon to store (when iterating by data source points in outer cycle). Please correct me, if I wrong.

Also, what if we have multiple sources in a polygon, just last data doesnt make sense because we'd drop measurements from other sensors. I think we need to store all in a window or probably better the latest of each frame_id , since each sensor will have a different frame_id you can use to differentiate if all coming over the same callback.

I am not sure that understand this point well: all data points from each source are being added to the collision_points array:

void SourceBase::getData(
  std::vector<Point> & data, const rclcpp::Time & curr_time, const Velocity & velocity)
{
  std::lock_guard<mutex_t> lock(data_mutex_);

  fixData(curr_time, velocity);
  data.insert(data.end(), data_fixed_.data.begin(), data_fixed_.data.end());
}

I think there's some simplification that needs to occur / breaking down long functions and trying to use that nice object hierarchy you established

Code/functions simplification and code commenting work is in progress

[SteveMacenski]

Since publishPolygons() routine is made only for visualization purposes, I think we do not need to have it is being published in high-priority thread at least on 50Hz. That also is excess for RViz. Therefore, to reduce main cycle and RViz CPU load, I've moved it to separate timer.

A separate timer has its own computational drawbacks, I think having it in the main cycle makes the most sense so that as a debug tool we know exactly what we see in rviz is what the safety node is seeing in the same timestep. It can be made an optional parameter to only publish when someone wants to visualize but then false by default so there is no overhead for normal use.

0.02 sec

I'd do like 0.05 sec which should help (a little). We should work on that compute runtime for the approach then.

I am not sure that understand this point well: all data points from each source are being added to the collision_points array:

https://github.com/ros-planning/navigation2/pull/2982/files#diff-a8ab77e548af62fe331c646d4df405f23654b0a652d6d63718a4c0c1f6ee1ab7R87 data_.data.clear();, you only store the data from the last message, you clear out all others so if there are N different subscriptions in a polygon (like 2 laser scans) its only storing and evaluating the last one from the perspective of the polygon analysis

Edit: unless each sensor type (Scan/Pointcloud) only has 1 subscription each and you make N of those instead. That works too 👍

[SteveMacenski]

TODO

[SteveMacenski]

Also, missing a readme, but like the tests, I'm fine waiting on that until we hammer out the big items in the review

[SteveMacenski]

@AlexeyMerzlyakov any updates or questions on this? I'm targetting this work to be in by June 30th to stay on schedule for the year so we have plenty of time to work on the Navigation algorithms paper and ROSCon talks

[AlexeyMerzlyakov]

@AlexeyMerzlyakov any updates or questions on this? I'm targetting this work to be in by June 30th to stay on schedule for the year so we have plenty of time to work on the Navigation algorithms paper and ROSCon talks

@SteveMacenski, thank you for the detailed review on this! I am actively working to meet review items from above. Many time took the experiments and switching to using lookupTransform(to_frame, to_time, from_frame, from_time) with time stamps conversion (#2982 (comment)); and publishing each polygon (by PolygonBase class) in a separate topic (#2982 (comment)). These both items required some code structure reorganization.

I got the deadline plans, and will also plan to finish all works for CM before that time. I expect to finish update-after-review (without testcases development) at the beginning of next week and then immediately switch to tests development.

[AlexeyMerzlyakov]

There are some items to comment:

unless each sensor type (Scan/Pointcloud) only has 1 subscription each and you make N of those instead. That works too +1

Yes, this is how currently data sources classes work, so there should not be a problem.

For the scan type, there's a tool in ros called the laser projector http://wiki.ros.org/laser_geometry that will take in a laser scan and return to you a pointcloud to use so you don't need to do that conversion yourself. I believe it will even do the transformation for you as well to another frame

I've made some performance comparison of LaserProjector approach with currently existing one. For the experiments I've used 2 implementations of getData():

• getDataClassic() - previous implementation based on direct std::sin() and std::cos() calculations +
  lookupTransform(base_frame_id, current_time, scan_frame_id, scan_time) interpolated in time points transform.
• getDataLP() based on laser_geometry::LaserProjection::projectLaser(laser_in, cloud_out) +
  lookupTransform(base_frame_id, current_time, scan_frame_id, scan_time) interpolated in time points transform
  (we can't use laser_geometry::LaserProjection::transformLaserScanToPointCloud() with transform into another frame, as it does not have interpolation in time).

The performance esimations shown the average time of getDataClassic()/getDataLP() in microseconds based on ~330 functions calls:

Avg. getDataClassic() time, us: 3927
Avg. getDataLP() time, us: 5401

which is on ~38% slower than current realization.
Therefore, I'd like to remain current getData() implementation for Scan class.

[mergify]

@AlexeyMerzlyakov, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[SteveMacenski]

I didn't get a chance to look over anything else but just verifying changes and asking a few more questions. It look the better part of 2 hours just to do that much 😆 so I'll look at it in more complete detail the next iteration.

The laser projector is doing more than a base transformation, it also uses the scan times to project each point slightly differently in time knowing that the laser scan is taken for each point with some dt apart. But I think for this low-range capability, its fine that we don't go to that extreme

[jwallace42]

I noticed that there is no instance of https://github.com/ros-planning/navigation2/blob/main/nav2_costmap_2d/src/footprint_subscriber.cpp. Do you account for a potential change in the robot footprint?

[SteveMacenski]

This does not require a footprint 👍 The polygons are the safety zones, not the robot footprint. For 'on approach' that is a representation of a footprint but could be inflated / changed for conservative actions so its separated from the footprint object.

But I do see now that perhaps we should just use the actual robot footprint in case a robot platform changes over time. @AlexeyMerzlyakov what do you think about using a footprint subscriber instead of a polygon/circle for the 'on approach' mode?

[SteveMacenski]

Also, missing a readme, but like the tests, I'm fine waiting on that until we hammer out the big items in the review

[SteveMacenski]

Had time to get through the full codebase and resolve the items I felt made the main process function feel a little odd

[SteveMacenski]

I hate to even say this, but we should also add a sensor processing for sonars / IR feeds using the sensor_msgs/Range message type

[AlexeyMerzlyakov]

@AlexeyMerzlyakov what do you think about using a footprint subscriber instead of a polygon/circle for the 'on approach' mode?

Given polygon in this model represents but not exactly repeats robot's footprint. We could intentionally set this shape to be larger than actual robot's footprint. For example, if we want to avoid in 100% collisions with robot in APPROACH model (that will even still appear due to sensors noise, if it is robot footprint), we could make it to be some "inflated" to stop robot if max_points will go inside the polygon on its border, but still will actually prevent from robot collision.

From other side, we could replace it with two polygons operating simultaneously: robot footprint at APPROACH model + safety area at STOP model. However, two polygons operating slower than one, doing the same thing. And getFootprintInRobotFrame() from footprint_subscriber.hpp will return only polygon model, while we might use circles which should operate much more effective due to points finding algorithms (need to measure practically?)
From this point of view it seems to be more justified to use current models instead of robot footprint subscriber.

The only thing we are skipping - is a dynamically changing robot's footprint. If APPROACH model to be used for estimations, it is OK. But if we want to say robot will exactly away on N seconds from a collision, that should be considered (by cost of poorer performance).

[AlexeyMerzlyakov]

I hate to even say this, but we should also add a sensor processing for sonars / IR feeds using the sensor_msgs/Range message type

Hm... not a good news, but I could try to add this after some time. The main question here is - how should we treat sensor_msgs/Range message sensors data? It represents the distance for the object and continuous field of view, where this object could be placed. We get a continuous object, while all intersection algorithms are designed to work with pointy ones:

---

Update: it looks like we have the solution: RangeSensorLayer in Costmaps2D. From first glance, we could utilize RangeSensorLayer::updateCostmap(sensor_msgs::msg::Range, bool) method. We need to make a new class, derived from RangeSensorLayer, initialize it with empty costmap, call this method to convert range sensor data -> to a costmap and then read non-empty and non-unknown values from costmap_ (which will come from grandparent Costmap2D).

[jwallace42]

Given polygon in this model represents but not exactly repeats robot's footprint. We could intentionally set this shape to be larger than actual robot's footprint. For example, if we want to avoid in 100% collisions with robot in APPROACH model (that will even still appear due to sensors noise, if it is robot footprint), we could make it to be some "inflated" to stop robot if max_points will go inside the polygon on its border, but still will actually prevent from robot collision.

Sorry, maybe I am being a little dense or not explaining it well. :) My main concern is that the polygons that are created for the collision monitor do not dynamically change based off of changes to the robot footprint. If a robot picks up a payload of some sort that increases the footprint size the collision monitor polygons must change to satisfy the new footprint.

[SteveMacenski]

The concern with footprints is as @jwallace42 mentions (only for approach models, mind you) -- some robots will change shape over time by picking up carts/tools or having a manipulator on top in a new pose. Anyway, if we do that, it removes a parameter and its the same footprint being used in the costmaps for collision avoidance when can themselves have some padding added to them (even footprint_padding parameter to automate it).

w.r.t. Range, we can just say if any measurement is in that box, since it has only a single return, we only need to evaluate if a single return is in a slowdown / collision box. No need for any costmap things, its just a point like a laser scan has a series of points. Its just another point to add to the sources like from PC2 or scan, just only contains a single value.

[SteveMacenski]

We can always move the sonar to a follow up PR, I think its worth focusing on getting these other items done so we can get something out there that we can refine later with more sensor models

[AlexeyMerzlyakov]

Everything seems to be done.

[SteveMacenski]

Is there a way to appreciably speed up the approach model polygon? Maybe a different algorithm or algorithm optimizations?

[SteveMacenski]

Try to embed the getPointInside inside of getPointsInside?

[SteveMacenski]

Multithreading?

[AlexeyMerzlyakov]

Yes, I thought about ways of optimization.

Here is the work of comparing the performance for different algorithms about point-in-polygon solution. Among basic algorithms, raycrossing algorithm (used in current implementation) is the fastest one. There is an optimization, called as "MacMartin" used for large polygons, which speeds up the raycrossing algorithms on x1.8 times.
The best way of enhanced algorithms: is a grid algorithm, quantizing the map and making "inside", "non-inside" and "indeterminate" cells. According to the work, this algorithm gives best performance, and thus more promising as an optimization point.

Try to embed the getPointInside inside of getPointsInside?

This function was already inlined locally: inline bool Polygon::isPointInside(const Point & point) const, and confirmed by binary disassembling.

The multithreading is another way of optimization. Currently, the motion is being simulated by small steps. But it could being calculated the exact position of robot pose after t time (since we have theta and linear velocity, this making arcs; and then we need to make a transform from robot's velocity frame down to base frame). Knowing the exact robot position in any time frame, we could parallel the whole calculation loop e.g. by giving odd iterations to one thread, and even ones - to another. All iterations will be independent from each other. This is another point of optimization could be made there.

The good notice here, that both grid algorithms and multithreading optimizations could be applied there at the same time, that should give a noticeable increase of the performance.

[AlexeyMerzlyakov]

Update: the multithreading could be parallelized by points, instead of simulation steps. This makes it easiest to implement giving tangible benefits on multi-CPU systems.

[SteveMacenski]

What would a default of 0.1 for simulation timestep yield? I think 0.1s timestep increments are plenty small and are 5x larger than currently set.

[AlexeyMerzlyakov]

That is definitely makes sense. The Footprint Approach time was decreased from ~78 -> ~20 ms. Additionally checked that robot still behaves well in the simulation with footprint approach. Re-measured the peroformance and updated the README accordingly.

[mergify]

@AlexeyMerzlyakov, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[SteveMacenski]

I'm ready to merge this if you are!

[AlexeyMerzlyakov]

Added some strokes: changed default parameters to fit used for README.md picture ones: e8b491e, and fixed "safety area" -> "zone" names in code comments: 7f4c3bf. Looked again though code: everything seems to be OK.
The further code optimization could be the subject of separate ticket: currently performance is on the edge of 50Hz for the most heavy case (footprint approach with PCL).

[padhupradheep]

after yesterdays merge, on rolling, the build is failing, did you guys notice it as well?

--- stderr: nav2_collision_monitor                                                                              
/home/pradheep/ros2_rolling_ws/src/navigation2/nav2_collision_monitor/src/collision_monitor_node.cpp: In constructor ‘nav2_collision_monitor::CollisionMonitor::CollisionMonitor(const rclcpp::NodeOptions&)’:
/home/pradheep/ros2_rolling_ws/src/navigation2/nav2_collision_monitor/src/collision_monitor_node.cpp:33:79: error: no matching function for call to ‘nav2_util::LifecycleNode::LifecycleNode(const char [18], const char [1], bool, const rclcpp::NodeOptions&)’
   33 |   stop_stamp_{0, 0, get_clock()->get_clock_type()}, stop_pub_timeout_(1.0, 0.0)
      |                                                                               ^
In file included from /home/pradheep/ros2_rolling_ws/src/navigation2/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp:29,
                 from /home/pradheep/ros2_rolling_ws/src/navigation2/nav2_collision_monitor/src/collision_monitor_node.cpp:15:
/home/pradheep/ros2_rolling_ws/install/nav2_util/include/nav2_util/lifecycle_node.hpp:46:3: note: candidate: ‘nav2_util::LifecycleNode::LifecycleNode(const string&, const string&, const rclcpp::NodeOptions&)’
   46 |   LifecycleNode(
      |   ^~~~~~~~~~~~~
/home/pradheep/ros2_rolling_ws/install/nav2_util/include/nav2_util/lifecycle_node.hpp:46:3: note:   candidate expects 3 arguments, 4 provided
gmake[2]: *** [CMakeFiles/collision_monitor_core.dir/build.make:76: CMakeFiles/collision_monitor_core.dir/src/collision_monitor_node.cpp.o] Error 1
gmake[1]: *** [CMakeFiles/Makefile2:180: CMakeFiles/collision_monitor_core.dir/all] Error 2
gmake: *** [Makefile:146: all] Error 2

#3084 fixes it!

[AlexeyMerzlyakov]

@padhupradheep, Yes, it looks like during the development the sources were outdated and drifted apart. Thank for the notice!