Tracking error layer #5605
Tracking error layer #5605
Conversation
Signed-off-by: silanus23 <[email protected]>
Signed-off-by: silanus23 <[email protected]>
Signed-off-by: silanus23 <[email protected]>
Signed-off-by: silanus23 <[email protected]>
Signed-off-by: silanus23 <[email protected]>
There was a problem hiding this comment.
I'd recommend spending some time to think about how we can most optimally compute this since this is going to be a little heavy so its worth some performance considerations.
Off the cuff:
- We don't really need to store an internal representation of the layer at all, so this could derive from the
Layerrather thanCostmapLayersince we shouldn't really need the internal costmap structure. - We default assume all points are lethal unless proven otherwise in the
for eachloop in the update window. - Given the path, we shift the path left/right by the left/right tolerances by the base frame. If we enclose the start of the path and the end of the path segment, we should end up with a polygon.
- Using some polygon math, we can ID if a point is inside of that polygon or not. If inside, we set ignore. If outside, we set as lethal. We'd need to make sure this works with concave polygons since there's no promise that this is convex.
Also maybe ways that can improve the method you have now and/or convolved the path by some radius left/right?
We probably want this to mark more than just 1 cell around the boundary as lethal so that small quantization errors don't allow the system to break out. At least 3 cells thick, but also I was thinking perhaps it would be sensible to just default mark everything as occupied unless within the bounds of tracking. But maybe a "thick" line is fine actually if that gives us some computational gains :-)
| this, std::placeholders::_1)); | ||
|
|
||
| path_sub_ = node->create_subscription<nav_msgs::msg::Path>( | ||
| "/plan", |
There was a problem hiding this comment.
Remove forward slash in both subscriptions
| tracking_feedback_sub_ = node->create_subscription<nav2_msgs::msg::TrackingFeedback>( | ||
| "/tracking_feedback", | ||
| std::bind(&TrackingErrorLayer::trackingCallback, this, std::placeholders::_1), | ||
| rclcpp::QoS(10).reliable() |
There was a problem hiding this comment.
For all QoS: Use the established Nav2 policies in nav2_ros_common
| std::bind(&TrackingErrorLayer::trackingCallback, this, std::placeholders::_1), | ||
| rclcpp::QoS(10).reliable() | ||
| ); | ||
| tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node->get_clock()); |
There was a problem hiding this comment.
You should have this already from base classes to this class. You shouldn't need to manually create another (which is very heavy)
| std::mutex path_mutex_; | ||
| std::mutex tracking_error_mutex_; |
There was a problem hiding this comment.
I think you can get away with a single data mutex
| return result; | ||
| } | ||
|
|
||
| TrackingErrorLayer::~TrackingErrorLayer() |
There was a problem hiding this comment.
Move destructor right after the constructor
| void TrackingErrorLayer::reset() {} | ||
| void TrackingErrorLayer::activate() {enabled_ = true;} | ||
| void TrackingErrorLayer::deactivate() {enabled_ = false;} | ||
|
|
||
| void TrackingErrorLayer::onFootprintChanged() {} | ||
| void TrackingErrorLayer::cleanup() {} |
There was a problem hiding this comment.
Dont override if not implementing
| void TrackingErrorLayer::activate() {enabled_ = true;} | ||
| void TrackingErrorLayer::deactivate() {enabled_ = false;} |
There was a problem hiding this comment.
These two should create / destroy the subscriptions
| dyn_params_handler_.reset(); | ||
| } | ||
|
|
||
| void TrackingErrorLayer::reset() {} |
There was a problem hiding this comment.
This should reset data and other states. Please check out other layers for example
2 participants
Basic Info
Description of contribution in a few bullet points
I have added a layer that can draw a corridor around path with obstacles. In addition I added a geometry util that can draw smoother lines.
Description of documentation updates required from your changes
A costmap layer and it's params
Description of how this change was tested
Wrote a unit test and I have seen visually it is working
Future work that may be required in bullet points
Tests definitely need a see throgh.
Note:
I had to copy paste and carry changes to a new branch. Like I did on the prev 2 PR. This going to make me move with less burden. Commits look lesser and rushy but they ain't.
For Maintainers:
backport-*.