merged symmetric yaw tolerance from humble#5798
merged symmetric yaw tolerance from humble#5798sandeepdutta wants to merge 10 commits intoros-navigation:mainfrom
Conversation
|
@sandeepdutta could you please fix the linting + DCO? |
|
I have taken care of the lint issues & DCO (sign-off). |
Signed-off-by: Revert <revert@local>
Signed-off-by: Revert <revert@local>
Signed-off-by: Revert <revert@local>
|
Not sure what is wrong with the "style divergence", I made the lines < 100 characters. Please let me know. |
|
You can fix the issue with |
|
Update the code to fix the formatting. The test is passing in the latest docker environment that I created from the latest rolling build. |
Signed-off-by: Revert <revert@local>
Codecov Report✅ All modified and coverable lines are covered by tests.
... and 15 files with indirect coverage changes 🚀 New features to boost your workflow:
|
There was a problem hiding this comment.
@sandeepdutta please check my comment, this should hopefully fix the code coverage problem as well
| if (power_ > 1u) { | ||
| data.costs += (((utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs()). | ||
| rowwise().mean()) * weight_).pow(power_).eval(); | ||
| if (symmetric_yaw_tolerance_) { |
There was a problem hiding this comment.
I think we can simplify this part a little bit:
auto dist = utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs();
if (symmetric) {
// add your logic here
}
if (power_ > 1u) {
data.costs += (distance.rowwise().mean() * weight_).pow(power_).eval();
} else {
data.costs += (distance.rowwise().mean() * weight_).eval();
}
There was a problem hiding this comment.
the refactoring is difficult since the types returned by Eigen rowWise.mean is different in the if and else case. I have added an additional test to improve the coverage with power
There was a problem hiding this comment.
Ah, I think I know why. If you add .eval() to the following line, it should work
auto dist = utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw).abs();
distance = distance.cwiseMin(distance_to_flipped).eval();
For readability, I would still request changing this :)
There was a problem hiding this comment.
Thanks Maurice,
I did not know about the "Lazy Evaluation" of Eigen .. (one learns everyday :). I have refactored the code.
Sandeep
mini-1235
left a comment
mini-1235
left a comment
There was a problem hiding this comment.
The DCO is still failing, check https://github.com/ros-navigation/navigation2/pull/5798/checks?check_run_id=58882886823 for more instructions
nav2_mppi_controller/README.md
Outdated
| | cost_weight | double | Default 3.0. Weight to apply to critic term. | | ||
| | cost_power | int | Default 1. Power order to apply to term. | | ||
| | threshold_to_consider | double | Default 0.5. Minimal distance between robot and goal above which angle goal cost considered. | | ||
| | symmetric_yaw_tolerance | bool | Default false. Enable for symmetric robots - allows goal approach from either forward or backward (goal ± 180°) without penalty. See [Symmetric Yaw Tolerance Guide](./symmetric_goal_critic_README.md) for details. | |
There was a problem hiding this comment.
I don't think the link here is valid symmetric_goal_critic_README.md?
| // For symmetric robots: use minimum distance to either goal orientation or goal + 180° | ||
| const double goal_yaw_flipped = angles::normalize_angle(goal_yaw + M_PI); | ||
|
|
||
| auto distance_to_goal = utils::shortest_angular_distance(data.trajectories.yaws, |
There was a problem hiding this comment.
Remove this variable here, we can directly use the min_distance we have computed before
| } | ||
| } | ||
|
|
||
|
|
There was a problem hiding this comment.
Revert the formatting change here
sandeepdutta
force-pushed
the
symmetric_yaw_tolerance
branch
from
f0e39bc to
2ad87bd
Compare
|
Commit sha: cfabf7e, Author: Revert, Committer: Revert; The sign-off is missing. DCO still failing The test is failing as well, I believe it's because you are doing |
sandeepdutta
force-pushed
the
symmetric_yaw_tolerance
branch
from
2ad87bd to
ef0fbac
Compare
Signed-off-by: Sandeep Dutta <quark01@gmail.com>
|
Please see below that all tests are passing in my environment, I have pulled the latest rolling distribution to [==========] Running 27 tests from 2 test suites. [----------] 15 tests from CriticTests [----------] Global test environment tear-down |
SteveMacenski
left a comment
SteveMacenski
left a comment
There was a problem hiding this comment.
Also see the same review comments from the Humble PR that needs to be done here (i.e. tests need to have hand calculated theoretical values, not just non-zero values)
| symmetric_yaw_tolerance_ = node->declare_or_get_parameter(plugin_name + | ||
| ".symmetric_yaw_tolerance", false); |
There was a problem hiding this comment.
| symmetric_yaw_tolerance_ = node->declare_or_get_parameter(plugin_name + | |
| ".symmetric_yaw_tolerance", false); | |
| symmetric_yaw_tolerance_ = node->declare_or_get_parameter( | |
| plugin_name + ".symmetric_yaw_tolerance", false); |
This needs to be properly formatted
Signed-off-by: Revert <revert@local>
|
@sandeepdutta I am going to take over this PR since there are some test failures and DCO issues to fix. I will add you as a co-author. Once it's merged, I'll backport it to humble |
3 participants
Basic Info
Description of contribution in a few bullet points
Description of documentation updates required from your changes
Description of how this change was tested
For Maintainers:
backport-*.