Custom costmap converter #35
Merged
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Why this custom costmap_converter ? In fact the oscillations of the robot near edges weren't due to a teb problem. The problem is from the costmap converter CostmapToPolygonsDBSMCCH (or all the others costmap converters of the package), in a edge, instead of considering 2 obstacle lines, it considers a triangle ! So there is an imaginary lethal obstacle that prevent the robot to cross it, creating "oscillations". The goal of this custom costmap converter is to get rid of theses triangle. To to it, i'll just decompose the map into a rectangle and 3 lines and set them as lethal obstacles.
Must work direclty on the costmap_converter package for better results, incoming...
The problem of the thickness comes from teb_local_planner that i'll modify a bit.
PhileasL
commented
Mar 3, 2021
|
Updated CI branch for teb_local_planner foxy-devel |
3wnbr1
requested changes
Mar 3, 2021
| @@ -6,4 +6,4 @@ repositories: | |||
| teb_local_planner: | |||
| type: git | |||
| url: https://github.com/robotique-ecam/teb_local_planner.git | |||
| version: foxy | |||
There was a problem hiding this comment.
key must be change to invalidate ros_ws cache and rebuild
cdfr/.github/workflows/main.yml
Line 37 in 7838232
There was a problem hiding this comment.
Otherwise, CI will use its cache and ignore your entry
There was a problem hiding this comment.
Done, CI cache key update
3wnbr1
approved these changes
Mar 3, 2021
|
Nice work there, just waiting for the CI Status 👍 |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Purpose
The purpose of this PR is to create a custom costmap_converter that is suitable for our use : more efficient, more accurate. The way we are using costmap_converter package is just to simplify the static costmap as a costmap described by polygons and lines.
Why a custom plugin ?
In fact the main problem of costmap_converter package in our case is that in edges of the map, the package assumes that it can simplifies the 2 intercepted lines by a triangle. WRONG ! In that way, it creates a virtual lethal obstacle on the game area, we don't want this kind of behavior. Plus, it uses a heavy algorithm that computes and tries to decompose the static costmap into polygons several times per second.
Considering the static costmap as a layer that will never change, we want to reduce the computational time by decomposes the map only one time at launching, and avoid the apparition of virtual lethal obstacles on the game area.
How does it works ?
In the initialization sequence of costmap_converter_custom, the plugin gets the data of "static_map_lines" in the configuration yaml file. This parameter is a string of a vector of vectors of floats that is a description of costmap lines described as following :
static_map_lines: "[ [pt1_start_x, pt1_start_y, pt1_end_x, pt1_end_y],[pt2_start_x, pt2_start_y, pt2_end_x, pt2_end_y], ... ]"Then it performs a naive algorithm that creates the lines from theses coordinates and store them into a global variable that will never change as a PolygonContainerPtr type.
When needed (parameter
costmap_converter_ratein yaml), the costmap_converter package get this PolygonContainerPtr, converts it as obstacles and publish them in order to communicate the teb_local_planner the lethal obstacles.Problem
By default in teb_local_planner, the obstacles are 10 cm thick and in reality, they are just 2 cm thick. And that is a constant value in the teb_local_planner obstacle, plus the lifetime of the obstacle isn't long enough (2 seconds).
In order to make the plugin working as expected, I modify teb_local_planner to set the obstacles as 2 cm thick. See those changes here.. So, in order to make it fully functional, you have to update your teb_local_planner.
Observations and extras
Observations
The plugin works fine, but sometimes teb_local_planner can still find a way through a wall, I don't really know why for the moment, but I've never seen the robot following a plan that is going through a wall.
Extras