-
Notifications
You must be signed in to change notification settings - Fork 1.3k
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Fix velocities comparison for rotation at place case #3177
Fix velocities comparison for rotation at place case #3177
Conversation
@AlexeyMerzlyakov, please properly fill in PR template in the future. @SteveMacenski, use this instead.
|
a3ecd3a
to
6e54086
Compare
nav2_collision_monitor/include/nav2_collision_monitor/types.hpp
Outdated
Show resolved
Hide resolved
The other option would be to compare the total components, e.g. not just translation and rotation separately but together
But it kind of depends on what you're trying to do and where exactly this code is being invoked |
I thought about it: the latest comparison will work fine for Collision Monitor (only), since it is being used in two places: in "approach" and "slowdown" models comparisons during decision making of selected behavior. Here the velocity is being multiplied on some slowdown coefficient, all of components including twists that will be changed proportionally. So, adding this term in the velocities comparison won't brake anything, and moreover, this is better solution. Thank you for the suggestion! I'll update the patch and re-run the testing to verify updated solution works fine. |
const double second_vel = second.x * second.x + second.y * second.y; | ||
const double first_vel = x * x + y * y + tw * tw; | ||
const double second_vel = second.x * second.x + second.y * second.y + second.tw * second.tw; | ||
// In Collision Monitor twists will change proportionally to linear velocities, |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I'm not sure you need the comment to justify it - if you add a comment at all it should just be explaining what its doing / the intention (and the fact that this resolves rotation in place issues)
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Okay, I've updated the comment intention
* Fix velocities comparison for rotation at place case * Meet review item * Remove unnecessary header * Change the comment
* standalone assisted teleop (#2904) * standalone assisted teleop * added in action message * code review * moved to behavior server * added assisted teleop bt node * revert * added bt node for assisted teleop * lint fix * added cancel assisted teleop node * code review * working * cleanup * updated feeback * code review * update compute velocity * cleanup * lint fixes * cleanup * test fix * starting to add tests for assisted teleop * fixed tests * undo * fixed test * is_recovery * adjust abort result based on recovery or not * code review * added preempt velocity * working preempt assisted teleop test * completed assisted teleop tests * code review * undo * code review * remove sleep * topic rename * missing comma * added comma :( * added comma Co-authored-by: Steve Macenski <[email protected]> * Add the support of range sensors to Collision Monitor (#3099) * Support range sensors in Collision Monitor * Adjust README.md * Meet review fixes * Fix #3152: Costmap extend did not include Y component (#3153) * missing nodes added to nav2_tree_nodes.xml (#3155) * Change deprecated ceres function (#3158) * Change deprecated function * Update smoother_cost_function.hpp * remove camera_rgb_joint since child frame does not exist (#3162) * bugfix (#3109) deadlock when costmap receives new map (#3145) * bugfix (#3109) deadlock when costmap receives new map Signed-off-by: Daisuke Sato <[email protected]> * introduce map_received_in_update_bounds_ flag to make sure processMap will not be called between updateBounds and updateCosts Signed-off-by: Daisuke Sato <[email protected]> Signed-off-by: Daisuke Sato <[email protected]> * simple command costmap api - first few functions (#3159) * initial commit costmap_2d template Signed-off-by: Stevedan Omodolor <[email protected]> * finish task A and tested * lint * Update nav2_simple_commander/nav2_simple_commander/costmap_2d.py Co-authored-by: Steve Macenski <[email protected]> * fix trailing underscores Signed-off-by: Stevedan Omodolor <[email protected]> Co-authored-by: Steve Macenski <[email protected]> * Fix missing dependency on nav2_collision_monitor (#3175) * fixed start (#3168) * fixed start * return true * fix tests * Fix velocities comparison for rotation at place case (#3177) * Fix velocities comparison for rotation at place case * Meet review item * Remove unnecessary header * Change the comment * set a empty path on halt (#3178) * set a empty path on halt * fixed issues * remove path reset * fixing * reverting * revert * revert * fixed lint * test fix * uncrusify fix * simple command costmap api - update few functions (#3169) * * add aditional function to costmap_2d.py Signed-off-by: Stevedan Omodolor <[email protected]> Updated-by: Jaehun Kim <[email protected]> * finish task B * Update nav2_simple_commander/nav2_simple_commander/costmap_2d.py * Update method docs * Remove underscores at parameters and split getCost into getCostXY and getCostIdx * Update method docstrings * lint code & update docstring, remove default value of getCostXY * lint code with pep257 & flake8 * clear names for bt nodes (#3183) * [Smac] check if a node exists before creating (#3195) * check if a node exists before creating * invert logic to group like with like * Update a_star.cpp * fixing benchmarkign for planners (#3202) * [Smac] Robin hood data structure improves performance by 10-15%! (#3201) * adding robin_hood unordered_map * using robin_hood node map * ignore robin_hood file * linting * linting cont. for triple pointers * linting cont. for uncrustify * [RPP] Add parameter to enable/disable collision detection (#3204) * [RPP] Add parameter to enable/disable collision detection * [RPP] Update README * Update waffle.model * add benchmark launch file + instructions (#3218) * removing hypotf from smac planner heuristic computation (#3217) * removing hypotf * swapping to node2d sqrt * complete smac planner tolerances (#3219) * Disable Output Buffering (#3220) To ensure await asyncio prints [Processing: %s]' every 30s as expected * fix majority of python linting errors introduced in python costmap API additions to get CI turning over again (#3223) * fix majority of python linting errors * finish linting * Assisted teleop simple commander (#3198) * add assisted teleop to python api * cleanup * assisted teleop demo * rename * lint * code review * trigger build * flake8 fix * break cashe * moved all v11 to v12 * lint fix * remove package dep * change default time allowance * Costmap Filter enabling service (#3229) * Add enabling service to costmap filters * Add service testcase * Fix comment * Use toggle_filter service name * Add binary flip costmap filter (#3228) * Add binary flip costmap filter * Move transformPose, worldToMask, getMaskData to CostmapFilter * Added default parametrized binary filter state * Switched to std_msgs/msg/Bool.msg * Use arbitrary filter values * Update waffle.model * Update waffle.model * Update test_actions.cpp * odom alpha restriction to avoid overflow caused by user-misconfiguration (#3238) * odom alpha restriction * odom alpha code style * odom alpha code style * odom alpha code style * Update controller server goal checker (#3240) * [FIX] Update controller server goal checker * [FIX] Autoformat code * [FIX] Misplaced tabs. Co-authored-by: Pedro Alejandro González <[email protected]> * map-size restriction to avoid overflow and nullptr caused by user-misconfiguration (#3242) * odom alpha restriction * odom alpha code style * odom alpha code style * odom alpha code style * map-size restriction * map-size code style * map-size rejection * map-size codestyle * map-size return false * Add Path Smoothers Benchmarking suite (#3236) * Add Path Smoothers Benchmarking suite * Meet review items * Update tools/smoother_benchmarking/README.md Co-authored-by: Steve Macenski <[email protected]> * Move optional performance patch to the end of README * Fix README Co-authored-by: Steve Macenski <[email protected]> * Fix typo (#3262) * Adding new Nav2 Smoother: Savitzky-Golay Smoother (#3264) * initial prototype of the Savitzky Golay Filter Path Smoother * fixed indexing issue - tested working * updates for filter * adding unit tests for SG-filter smoother * adding lifecycle transitions * Added Line Iterator (#3197) * Added Line Iterator * Updated Line Iterator to a new iteration method * Added the resolution as a parameter/ fixed linting * Added the resolution as a parameter/ fixed linting * Added unittests for the line iterator * Added unittests based on "unittest" package * Fixed __init__.py and rephrased some docstrings * Fixed linting errors * Fixed Linting Errors * Added some unittests and removed some methods * Dummy commit for CircleCI Issue Co-authored-by: Afif Swaidan <[email protected]> * bumping to 1.1.3 for release Signed-off-by: Daisuke Sato <[email protected]> Signed-off-by: Stevedan Omodolor <[email protected]> Co-authored-by: Joshua Wallace <[email protected]> Co-authored-by: Alexey Merzlyakov <[email protected]> Co-authored-by: Abdullah Enes BEDİR <[email protected]> Co-authored-by: Tobias Fischer <[email protected]> Co-authored-by: Tejas Kumar Shastha <[email protected]> Co-authored-by: Daisuke Sato <[email protected]> Co-authored-by: Stevedan Ogochukwu Omodolor <[email protected]> Co-authored-by: Lukas Fanta <[email protected]> Co-authored-by: Jackson9 <[email protected]> Co-authored-by: Ruffin <[email protected]> Co-authored-by: Hao-Xuan Song <[email protected]> Co-authored-by: Nicolas Rocha Pacheco <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: jaeminSHIN <[email protected]> Co-authored-by: Afif Swaidan <[email protected]> Co-authored-by: Afif Swaidan <[email protected]>
* standalone assisted teleop (ros-navigation#2904) * standalone assisted teleop * added in action message * code review * moved to behavior server * added assisted teleop bt node * revert * added bt node for assisted teleop * lint fix * added cancel assisted teleop node * code review * working * cleanup * updated feeback * code review * update compute velocity * cleanup * lint fixes * cleanup * test fix * starting to add tests for assisted teleop * fixed tests * undo * fixed test * is_recovery * adjust abort result based on recovery or not * code review * added preempt velocity * working preempt assisted teleop test * completed assisted teleop tests * code review * undo * code review * remove sleep * topic rename * missing comma * added comma :( * added comma Co-authored-by: Steve Macenski <[email protected]> * Add the support of range sensors to Collision Monitor (ros-navigation#3099) * Support range sensors in Collision Monitor * Adjust README.md * Meet review fixes * Fix ros-navigation#3152: Costmap extend did not include Y component (ros-navigation#3153) * missing nodes added to nav2_tree_nodes.xml (ros-navigation#3155) * Change deprecated ceres function (ros-navigation#3158) * Change deprecated function * Update smoother_cost_function.hpp * remove camera_rgb_joint since child frame does not exist (ros-navigation#3162) * bugfix (ros-navigation#3109) deadlock when costmap receives new map (ros-navigation#3145) * bugfix (ros-navigation#3109) deadlock when costmap receives new map Signed-off-by: Daisuke Sato <[email protected]> * introduce map_received_in_update_bounds_ flag to make sure processMap will not be called between updateBounds and updateCosts Signed-off-by: Daisuke Sato <[email protected]> Signed-off-by: Daisuke Sato <[email protected]> * simple command costmap api - first few functions (ros-navigation#3159) * initial commit costmap_2d template Signed-off-by: Stevedan Omodolor <[email protected]> * finish task A and tested * lint * Update nav2_simple_commander/nav2_simple_commander/costmap_2d.py Co-authored-by: Steve Macenski <[email protected]> * fix trailing underscores Signed-off-by: Stevedan Omodolor <[email protected]> Co-authored-by: Steve Macenski <[email protected]> * Fix missing dependency on nav2_collision_monitor (ros-navigation#3175) * fixed start (ros-navigation#3168) * fixed start * return true * fix tests * Fix velocities comparison for rotation at place case (ros-navigation#3177) * Fix velocities comparison for rotation at place case * Meet review item * Remove unnecessary header * Change the comment * set a empty path on halt (ros-navigation#3178) * set a empty path on halt * fixed issues * remove path reset * fixing * reverting * revert * revert * fixed lint * test fix * uncrusify fix * simple command costmap api - update few functions (ros-navigation#3169) * * add aditional function to costmap_2d.py Signed-off-by: Stevedan Omodolor <[email protected]> Updated-by: Jaehun Kim <[email protected]> * finish task B * Update nav2_simple_commander/nav2_simple_commander/costmap_2d.py * Update method docs * Remove underscores at parameters and split getCost into getCostXY and getCostIdx * Update method docstrings * lint code & update docstring, remove default value of getCostXY * lint code with pep257 & flake8 * clear names for bt nodes (ros-navigation#3183) * [Smac] check if a node exists before creating (ros-navigation#3195) * check if a node exists before creating * invert logic to group like with like * Update a_star.cpp * fixing benchmarkign for planners (ros-navigation#3202) * [Smac] Robin hood data structure improves performance by 10-15%! (ros-navigation#3201) * adding robin_hood unordered_map * using robin_hood node map * ignore robin_hood file * linting * linting cont. for triple pointers * linting cont. for uncrustify * [RPP] Add parameter to enable/disable collision detection (ros-navigation#3204) * [RPP] Add parameter to enable/disable collision detection * [RPP] Update README * Update waffle.model * add benchmark launch file + instructions (ros-navigation#3218) * removing hypotf from smac planner heuristic computation (ros-navigation#3217) * removing hypotf * swapping to node2d sqrt * complete smac planner tolerances (ros-navigation#3219) * Disable Output Buffering (ros-navigation#3220) To ensure await asyncio prints [Processing: %s]' every 30s as expected * fix majority of python linting errors introduced in python costmap API additions to get CI turning over again (ros-navigation#3223) * fix majority of python linting errors * finish linting * Assisted teleop simple commander (ros-navigation#3198) * add assisted teleop to python api * cleanup * assisted teleop demo * rename * lint * code review * trigger build * flake8 fix * break cashe * moved all v11 to v12 * lint fix * remove package dep * change default time allowance * Costmap Filter enabling service (ros-navigation#3229) * Add enabling service to costmap filters * Add service testcase * Fix comment * Use toggle_filter service name * Add binary flip costmap filter (ros-navigation#3228) * Add binary flip costmap filter * Move transformPose, worldToMask, getMaskData to CostmapFilter * Added default parametrized binary filter state * Switched to std_msgs/msg/Bool.msg * Use arbitrary filter values * Update waffle.model * Update waffle.model * Update test_actions.cpp * odom alpha restriction to avoid overflow caused by user-misconfiguration (ros-navigation#3238) * odom alpha restriction * odom alpha code style * odom alpha code style * odom alpha code style * Update controller server goal checker (ros-navigation#3240) * [FIX] Update controller server goal checker * [FIX] Autoformat code * [FIX] Misplaced tabs. Co-authored-by: Pedro Alejandro González <[email protected]> * map-size restriction to avoid overflow and nullptr caused by user-misconfiguration (ros-navigation#3242) * odom alpha restriction * odom alpha code style * odom alpha code style * odom alpha code style * map-size restriction * map-size code style * map-size rejection * map-size codestyle * map-size return false * Add Path Smoothers Benchmarking suite (ros-navigation#3236) * Add Path Smoothers Benchmarking suite * Meet review items * Update tools/smoother_benchmarking/README.md Co-authored-by: Steve Macenski <[email protected]> * Move optional performance patch to the end of README * Fix README Co-authored-by: Steve Macenski <[email protected]> * Fix typo (ros-navigation#3262) * Adding new Nav2 Smoother: Savitzky-Golay Smoother (ros-navigation#3264) * initial prototype of the Savitzky Golay Filter Path Smoother * fixed indexing issue - tested working * updates for filter * adding unit tests for SG-filter smoother * adding lifecycle transitions * Added Line Iterator (ros-navigation#3197) * Added Line Iterator * Updated Line Iterator to a new iteration method * Added the resolution as a parameter/ fixed linting * Added the resolution as a parameter/ fixed linting * Added unittests for the line iterator * Added unittests based on "unittest" package * Fixed __init__.py and rephrased some docstrings * Fixed linting errors * Fixed Linting Errors * Added some unittests and removed some methods * Dummy commit for CircleCI Issue Co-authored-by: Afif Swaidan <[email protected]> * bumping to 1.1.3 for release Signed-off-by: Daisuke Sato <[email protected]> Signed-off-by: Stevedan Omodolor <[email protected]> Co-authored-by: Joshua Wallace <[email protected]> Co-authored-by: Alexey Merzlyakov <[email protected]> Co-authored-by: Abdullah Enes BEDİR <[email protected]> Co-authored-by: Tobias Fischer <[email protected]> Co-authored-by: Tejas Kumar Shastha <[email protected]> Co-authored-by: Daisuke Sato <[email protected]> Co-authored-by: Stevedan Ogochukwu Omodolor <[email protected]> Co-authored-by: Lukas Fanta <[email protected]> Co-authored-by: Jackson9 <[email protected]> Co-authored-by: Ruffin <[email protected]> Co-authored-by: Hao-Xuan Song <[email protected]> Co-authored-by: Nicolas Rocha Pacheco <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: jaeminSHIN <[email protected]> Co-authored-by: Afif Swaidan <[email protected]> Co-authored-by: Afif Swaidan <[email protected]>
…3177) * Fix velocities comparison for rotation at place case * Meet review item * Remove unnecessary header * Change the comment
* standalone assisted teleop (ros-navigation#2904) * standalone assisted teleop * added in action message * code review * moved to behavior server * added assisted teleop bt node * revert * added bt node for assisted teleop * lint fix * added cancel assisted teleop node * code review * working * cleanup * updated feeback * code review * update compute velocity * cleanup * lint fixes * cleanup * test fix * starting to add tests for assisted teleop * fixed tests * undo * fixed test * is_recovery * adjust abort result based on recovery or not * code review * added preempt velocity * working preempt assisted teleop test * completed assisted teleop tests * code review * undo * code review * remove sleep * topic rename * missing comma * added comma :( * added comma Co-authored-by: Steve Macenski <[email protected]> * Add the support of range sensors to Collision Monitor (ros-navigation#3099) * Support range sensors in Collision Monitor * Adjust README.md * Meet review fixes * Fix ros-navigation#3152: Costmap extend did not include Y component (ros-navigation#3153) * missing nodes added to nav2_tree_nodes.xml (ros-navigation#3155) * Change deprecated ceres function (ros-navigation#3158) * Change deprecated function * Update smoother_cost_function.hpp * remove camera_rgb_joint since child frame does not exist (ros-navigation#3162) * bugfix (ros-navigation#3109) deadlock when costmap receives new map (ros-navigation#3145) * bugfix (ros-navigation#3109) deadlock when costmap receives new map Signed-off-by: Daisuke Sato <[email protected]> * introduce map_received_in_update_bounds_ flag to make sure processMap will not be called between updateBounds and updateCosts Signed-off-by: Daisuke Sato <[email protected]> Signed-off-by: Daisuke Sato <[email protected]> * simple command costmap api - first few functions (ros-navigation#3159) * initial commit costmap_2d template Signed-off-by: Stevedan Omodolor <[email protected]> * finish task A and tested * lint * Update nav2_simple_commander/nav2_simple_commander/costmap_2d.py Co-authored-by: Steve Macenski <[email protected]> * fix trailing underscores Signed-off-by: Stevedan Omodolor <[email protected]> Co-authored-by: Steve Macenski <[email protected]> * Fix missing dependency on nav2_collision_monitor (ros-navigation#3175) * fixed start (ros-navigation#3168) * fixed start * return true * fix tests * Fix velocities comparison for rotation at place case (ros-navigation#3177) * Fix velocities comparison for rotation at place case * Meet review item * Remove unnecessary header * Change the comment * set a empty path on halt (ros-navigation#3178) * set a empty path on halt * fixed issues * remove path reset * fixing * reverting * revert * revert * fixed lint * test fix * uncrusify fix * simple command costmap api - update few functions (ros-navigation#3169) * * add aditional function to costmap_2d.py Signed-off-by: Stevedan Omodolor <[email protected]> Updated-by: Jaehun Kim <[email protected]> * finish task B * Update nav2_simple_commander/nav2_simple_commander/costmap_2d.py * Update method docs * Remove underscores at parameters and split getCost into getCostXY and getCostIdx * Update method docstrings * lint code & update docstring, remove default value of getCostXY * lint code with pep257 & flake8 * clear names for bt nodes (ros-navigation#3183) * [Smac] check if a node exists before creating (ros-navigation#3195) * check if a node exists before creating * invert logic to group like with like * Update a_star.cpp * fixing benchmarkign for planners (ros-navigation#3202) * [Smac] Robin hood data structure improves performance by 10-15%! (ros-navigation#3201) * adding robin_hood unordered_map * using robin_hood node map * ignore robin_hood file * linting * linting cont. for triple pointers * linting cont. for uncrustify * [RPP] Add parameter to enable/disable collision detection (ros-navigation#3204) * [RPP] Add parameter to enable/disable collision detection * [RPP] Update README * Update waffle.model * add benchmark launch file + instructions (ros-navigation#3218) * removing hypotf from smac planner heuristic computation (ros-navigation#3217) * removing hypotf * swapping to node2d sqrt * complete smac planner tolerances (ros-navigation#3219) * Disable Output Buffering (ros-navigation#3220) To ensure await asyncio prints [Processing: %s]' every 30s as expected * fix majority of python linting errors introduced in python costmap API additions to get CI turning over again (ros-navigation#3223) * fix majority of python linting errors * finish linting * Assisted teleop simple commander (ros-navigation#3198) * add assisted teleop to python api * cleanup * assisted teleop demo * rename * lint * code review * trigger build * flake8 fix * break cashe * moved all v11 to v12 * lint fix * remove package dep * change default time allowance * Costmap Filter enabling service (ros-navigation#3229) * Add enabling service to costmap filters * Add service testcase * Fix comment * Use toggle_filter service name * Add binary flip costmap filter (ros-navigation#3228) * Add binary flip costmap filter * Move transformPose, worldToMask, getMaskData to CostmapFilter * Added default parametrized binary filter state * Switched to std_msgs/msg/Bool.msg * Use arbitrary filter values * Update waffle.model * Update waffle.model * Update test_actions.cpp * odom alpha restriction to avoid overflow caused by user-misconfiguration (ros-navigation#3238) * odom alpha restriction * odom alpha code style * odom alpha code style * odom alpha code style * Update controller server goal checker (ros-navigation#3240) * [FIX] Update controller server goal checker * [FIX] Autoformat code * [FIX] Misplaced tabs. Co-authored-by: Pedro Alejandro González <[email protected]> * map-size restriction to avoid overflow and nullptr caused by user-misconfiguration (ros-navigation#3242) * odom alpha restriction * odom alpha code style * odom alpha code style * odom alpha code style * map-size restriction * map-size code style * map-size rejection * map-size codestyle * map-size return false * Add Path Smoothers Benchmarking suite (ros-navigation#3236) * Add Path Smoothers Benchmarking suite * Meet review items * Update tools/smoother_benchmarking/README.md Co-authored-by: Steve Macenski <[email protected]> * Move optional performance patch to the end of README * Fix README Co-authored-by: Steve Macenski <[email protected]> * Fix typo (ros-navigation#3262) * Adding new Nav2 Smoother: Savitzky-Golay Smoother (ros-navigation#3264) * initial prototype of the Savitzky Golay Filter Path Smoother * fixed indexing issue - tested working * updates for filter * adding unit tests for SG-filter smoother * adding lifecycle transitions * Added Line Iterator (ros-navigation#3197) * Added Line Iterator * Updated Line Iterator to a new iteration method * Added the resolution as a parameter/ fixed linting * Added the resolution as a parameter/ fixed linting * Added unittests for the line iterator * Added unittests based on "unittest" package * Fixed __init__.py and rephrased some docstrings * Fixed linting errors * Fixed Linting Errors * Added some unittests and removed some methods * Dummy commit for CircleCI Issue Co-authored-by: Afif Swaidan <[email protected]> * bumping to 1.1.3 for release Signed-off-by: Daisuke Sato <[email protected]> Signed-off-by: Stevedan Omodolor <[email protected]> Co-authored-by: Joshua Wallace <[email protected]> Co-authored-by: Alexey Merzlyakov <[email protected]> Co-authored-by: Abdullah Enes BEDİR <[email protected]> Co-authored-by: Tobias Fischer <[email protected]> Co-authored-by: Tejas Kumar Shastha <[email protected]> Co-authored-by: Daisuke Sato <[email protected]> Co-authored-by: Stevedan Ogochukwu Omodolor <[email protected]> Co-authored-by: Lukas Fanta <[email protected]> Co-authored-by: Jackson9 <[email protected]> Co-authored-by: Ruffin <[email protected]> Co-authored-by: Hao-Xuan Song <[email protected]> Co-authored-by: Nicolas Rocha Pacheco <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: jaeminSHIN <[email protected]> Co-authored-by: Afif Swaidan <[email protected]> Co-authored-by: Afif Swaidan <[email protected]>
* standalone assisted teleop (ros-navigation#2904) * standalone assisted teleop * added in action message * code review * moved to behavior server * added assisted teleop bt node * revert * added bt node for assisted teleop * lint fix * added cancel assisted teleop node * code review * working * cleanup * updated feeback * code review * update compute velocity * cleanup * lint fixes * cleanup * test fix * starting to add tests for assisted teleop * fixed tests * undo * fixed test * is_recovery * adjust abort result based on recovery or not * code review * added preempt velocity * working preempt assisted teleop test * completed assisted teleop tests * code review * undo * code review * remove sleep * topic rename * missing comma * added comma :( * added comma Co-authored-by: Steve Macenski <[email protected]> * Add the support of range sensors to Collision Monitor (ros-navigation#3099) * Support range sensors in Collision Monitor * Adjust README.md * Meet review fixes * Fix ros-navigation#3152: Costmap extend did not include Y component (ros-navigation#3153) * missing nodes added to nav2_tree_nodes.xml (ros-navigation#3155) * Change deprecated ceres function (ros-navigation#3158) * Change deprecated function * Update smoother_cost_function.hpp * remove camera_rgb_joint since child frame does not exist (ros-navigation#3162) * bugfix (ros-navigation#3109) deadlock when costmap receives new map (ros-navigation#3145) * bugfix (ros-navigation#3109) deadlock when costmap receives new map Signed-off-by: Daisuke Sato <[email protected]> * introduce map_received_in_update_bounds_ flag to make sure processMap will not be called between updateBounds and updateCosts Signed-off-by: Daisuke Sato <[email protected]> Signed-off-by: Daisuke Sato <[email protected]> * simple command costmap api - first few functions (ros-navigation#3159) * initial commit costmap_2d template Signed-off-by: Stevedan Omodolor <[email protected]> * finish task A and tested * lint * Update nav2_simple_commander/nav2_simple_commander/costmap_2d.py Co-authored-by: Steve Macenski <[email protected]> * fix trailing underscores Signed-off-by: Stevedan Omodolor <[email protected]> Co-authored-by: Steve Macenski <[email protected]> * Fix missing dependency on nav2_collision_monitor (ros-navigation#3175) * fixed start (ros-navigation#3168) * fixed start * return true * fix tests * Fix velocities comparison for rotation at place case (ros-navigation#3177) * Fix velocities comparison for rotation at place case * Meet review item * Remove unnecessary header * Change the comment * set a empty path on halt (ros-navigation#3178) * set a empty path on halt * fixed issues * remove path reset * fixing * reverting * revert * revert * fixed lint * test fix * uncrusify fix * simple command costmap api - update few functions (ros-navigation#3169) * * add aditional function to costmap_2d.py Signed-off-by: Stevedan Omodolor <[email protected]> Updated-by: Jaehun Kim <[email protected]> * finish task B * Update nav2_simple_commander/nav2_simple_commander/costmap_2d.py * Update method docs * Remove underscores at parameters and split getCost into getCostXY and getCostIdx * Update method docstrings * lint code & update docstring, remove default value of getCostXY * lint code with pep257 & flake8 * clear names for bt nodes (ros-navigation#3183) * [Smac] check if a node exists before creating (ros-navigation#3195) * check if a node exists before creating * invert logic to group like with like * Update a_star.cpp * fixing benchmarkign for planners (ros-navigation#3202) * [Smac] Robin hood data structure improves performance by 10-15%! (ros-navigation#3201) * adding robin_hood unordered_map * using robin_hood node map * ignore robin_hood file * linting * linting cont. for triple pointers * linting cont. for uncrustify * [RPP] Add parameter to enable/disable collision detection (ros-navigation#3204) * [RPP] Add parameter to enable/disable collision detection * [RPP] Update README * Update waffle.model * add benchmark launch file + instructions (ros-navigation#3218) * removing hypotf from smac planner heuristic computation (ros-navigation#3217) * removing hypotf * swapping to node2d sqrt * complete smac planner tolerances (ros-navigation#3219) * Disable Output Buffering (ros-navigation#3220) To ensure await asyncio prints [Processing: %s]' every 30s as expected * fix majority of python linting errors introduced in python costmap API additions to get CI turning over again (ros-navigation#3223) * fix majority of python linting errors * finish linting * Assisted teleop simple commander (ros-navigation#3198) * add assisted teleop to python api * cleanup * assisted teleop demo * rename * lint * code review * trigger build * flake8 fix * break cashe * moved all v11 to v12 * lint fix * remove package dep * change default time allowance * Costmap Filter enabling service (ros-navigation#3229) * Add enabling service to costmap filters * Add service testcase * Fix comment * Use toggle_filter service name * Add binary flip costmap filter (ros-navigation#3228) * Add binary flip costmap filter * Move transformPose, worldToMask, getMaskData to CostmapFilter * Added default parametrized binary filter state * Switched to std_msgs/msg/Bool.msg * Use arbitrary filter values * Update waffle.model * Update waffle.model * Update test_actions.cpp * odom alpha restriction to avoid overflow caused by user-misconfiguration (ros-navigation#3238) * odom alpha restriction * odom alpha code style * odom alpha code style * odom alpha code style * Update controller server goal checker (ros-navigation#3240) * [FIX] Update controller server goal checker * [FIX] Autoformat code * [FIX] Misplaced tabs. Co-authored-by: Pedro Alejandro González <[email protected]> * map-size restriction to avoid overflow and nullptr caused by user-misconfiguration (ros-navigation#3242) * odom alpha restriction * odom alpha code style * odom alpha code style * odom alpha code style * map-size restriction * map-size code style * map-size rejection * map-size codestyle * map-size return false * Add Path Smoothers Benchmarking suite (ros-navigation#3236) * Add Path Smoothers Benchmarking suite * Meet review items * Update tools/smoother_benchmarking/README.md Co-authored-by: Steve Macenski <[email protected]> * Move optional performance patch to the end of README * Fix README Co-authored-by: Steve Macenski <[email protected]> * Fix typo (ros-navigation#3262) * Adding new Nav2 Smoother: Savitzky-Golay Smoother (ros-navigation#3264) * initial prototype of the Savitzky Golay Filter Path Smoother * fixed indexing issue - tested working * updates for filter * adding unit tests for SG-filter smoother * adding lifecycle transitions * Added Line Iterator (ros-navigation#3197) * Added Line Iterator * Updated Line Iterator to a new iteration method * Added the resolution as a parameter/ fixed linting * Added the resolution as a parameter/ fixed linting * Added unittests for the line iterator * Added unittests based on "unittest" package * Fixed __init__.py and rephrased some docstrings * Fixed linting errors * Fixed Linting Errors * Added some unittests and removed some methods * Dummy commit for CircleCI Issue Co-authored-by: Afif Swaidan <[email protected]> * bumping to 1.1.3 for release Signed-off-by: Daisuke Sato <[email protected]> Signed-off-by: Stevedan Omodolor <[email protected]> Co-authored-by: Joshua Wallace <[email protected]> Co-authored-by: Alexey Merzlyakov <[email protected]> Co-authored-by: Abdullah Enes BEDİR <[email protected]> Co-authored-by: Tobias Fischer <[email protected]> Co-authored-by: Tejas Kumar Shastha <[email protected]> Co-authored-by: Daisuke Sato <[email protected]> Co-authored-by: Stevedan Ogochukwu Omodolor <[email protected]> Co-authored-by: Lukas Fanta <[email protected]> Co-authored-by: Jackson9 <[email protected]> Co-authored-by: Ruffin <[email protected]> Co-authored-by: Hao-Xuan Song <[email protected]> Co-authored-by: Nicolas Rocha Pacheco <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: jaeminSHIN <[email protected]> Co-authored-by: Afif Swaidan <[email protected]> Co-authored-by: Afif Swaidan <[email protected]>
Fixes incorrect velocities comparison appeared when robot is rotating on the place. This was leading to failure of
safe_vel < robot_action.req_vel
checks for APPROACH and SLOWDOWN behavioral models, that causes robot to ignoring its intended behavior.Basic Info
For Maintainers: