Tracking critics dwb and mppi #5603
Conversation
Signed-off-by: silanus23 <[email protected]>
Signed-off-by: silanus23 <[email protected]>
|
Have you tested these critics in some way - in particular for MPPI? The TB4 world with some strategically placed obstacles in the way might be a good test case. The MPPI critic looks pretty complex. I would have intuitively started with something that simply:
I suspect (?) that it may be possible to instead strategically place this logic into the Path Align Critic and/or share some of its computations to make that critic more simple and concise. Is it possible to surgically place this into the Path Align logic and/or some of the computation is shared? Either we can share some functions and/or data, or put the Path Hug scoring inside of that critic as additional weight penalties (if we already have path-trajectory distances computed for its own scoring) |
|
@SteveMacenski I didn't try to get full results. I tried to solve the problem of the Planner can not keep up with the HZ (this part needed optimizations making this PR a lot complex). |
|
I think that makes sense - though I think MPPI is much higher priority than DWB as the default controller. DWB is more for legacy users. |
Signed-off-by: silanus23 <[email protected]>
silanus23
force-pushed
the
tracking-critics-dwb-and-mppi
branch
from
8f77cec to
dd0faed
Compare
SteveMacenski
left a comment
SteveMacenski
left a comment
There was a problem hiding this comment.
Otherwise, I can't comment too too much on the formulation without you/I doing some some simulation testing to see how well functionally it works.
| throw std::runtime_error{"Failed to lock node"}; | ||
| } | ||
|
|
||
| nav2::declare_parameter_if_not_declared( |
There was a problem hiding this comment.
Should we also set a max allowable left/right deviation to score as invalid if any path of the path exceeds?
| namespace dwb_critics | ||
| { | ||
|
|
||
| void PathHugCritic::onInit() |
There was a problem hiding this comment.
I actually rather like this name, but for the description docs / doxygen, can we mention cross tracking error (if not rename the critic to be more 'technical') so that this is clear the itention?
| start_index_, | ||
| search_window_); | ||
| distance += search_result.distance; | ||
| start_index_ = search_result.closest_segment_index; |
There was a problem hiding this comment.
The trajectories can technically go backwards so I don't think we should reset the start index, no?
| double scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj) override; | ||
|
|
||
| protected: | ||
| bool zero_scale_{false}; |
There was a problem hiding this comment.
This isn't exposed as a parameter?
| bool zero_scale_{false}; | ||
| nav_msgs::msg::Path global_path_; | ||
| size_t start_index_{0}; | ||
| geometry_msgs::msg::Pose current_pose_{}; |
There was a problem hiding this comment.
I don't think this needs to be a class member variable
| protected: | ||
| bool zero_scale_{false}; | ||
| nav_msgs::msg::Path global_path_; | ||
| size_t start_index_{0}; |
2 participants
Basic Info
Description of contribution in a few bullet points
Description of documentation updates required from your changes
Critic additons and their parameter additions
Description of how this change was tested
Added util tests. I have succeded to create cases where turtlebot has to create a new path cause it's so out of bounds(done this on dwb).
Future work that may be required in bullet points
These can be added to default yaml
###Note:
I had to create a new branch cause the original branch was sub branch of my first PR. I had to carry it via copy paste couldn't find a better way.
For Maintainers:
backport-*.