-
Notifications
You must be signed in to change notification settings - Fork 1.5k
Populate error_msg for all action result messages (#4341) #4460
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
Populate error_msg for all action result messages (#4341) #4460
Conversation
d3be8c2
to
da5e15a
Compare
Let me know if you have any questions - but seems like we're covering that in the issue thread instead |
756982f
to
cdccbdd
Compare
bd49b55
to
54f0402
Compare
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.
Sorry for the delay here - it is still draft so it was lower on my priority list while getting Jazzy out. It seems generally complete, but there seems to be a couple of things missing. When might this be ready for a full review?
See below for an initial review of things that jumped out
nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp
Show resolved
Hide resolved
I can rip through these fixups today. |
2fdbaea
to
19c5b10
Compare
@aosmw, your PR has failed to build. Please check CI outputs and resolve issues. |
Builds seem to fail from your additions of the error messages:
Sounds good! In that case, I can give it a full review, I got through about ~half of it yesterday and I can do the other half right now. |
nav2_docking/opennav_docking_bt/include/opennav_docking_bt/undock_robot.hpp
Outdated
Show resolved
Hide resolved
nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py
Outdated
Show resolved
Hide resolved
5ace62c
to
135bbb5
Compare
5a1e7aa
to
0461cfc
Compare
Status Report: I am still dog fooding this in my own backport to humble. I am successfully propagating useful error messages to our navigator action clients. Tip for any zenoh users (zenoh-bridge-ros2dds 0.11.0-stable), default actions timeout after 5 seconds and you won't get a result, here is a snippet of zenoh config that may help.
I still need to hunt down the exception handling triggering
|
That's a classic decoding issue in the BT. I'm guessing you have the wrong type set for the input/output port. All the blackboard and port objects gets stringified to store, so its having a problem with that process. I'd look into the types and make sure that (A) they're types that have conversion functions implemented (or basic types) and (B) you're both setting the port correctly and setting its value to the right type in the BT node. I think we ran into an issue with a couple being wrong, so maybe you just missed one more. Exciting update! Let me know how the dogfooding goes! |
0461cfc
to
bcb0638
Compare
Finally found the "basic_string: construction from null is not valid" - details of how in the commit message bcb0638 |
Woof, I see that error, that's an annoying one to find. I'm glad you got that unblocked - copy paste errors can be the worst! Let me know if I'm blocking you :-) |
You are not blocking me. I am STILL hunting down what I think is a cancellation that gets reported by the navigator action server as an abort. The result error_code = 0 and error_msg is empty. |
Signed-off-by: Mike Wake <[email protected]>
Signed-off-by: Mike Wake <[email protected]>
Signed-off-by: Mike Wake <[email protected]>
This allows setting a default failure error_code and message in nav2_system_tests behavior_tree dummy_action_server Signed-off-by: Mike Wake <[email protected]>
…navigation#4341) Signed-off-by: Mike Wake <[email protected]>
Signed-off-by: Mike Wake <[email protected]>
…gation#4341) NOTE: This stuff does not really test the error_msg/error_code handling as its all dummy servers. Signed-off-by: Mike Wake <[email protected]>
…gh_poses_w_replanning_and_recovery.xml (ros-navigation#4341) To facilitate error_msg system tests allow progress and goal checkers to be changed via the behaviour tree mechanism. Signed-off-by: Mike Wake <[email protected]>
…os-navigation#4341) Signed-off-by: Mike Wake <[email protected]>
Signed-off-by: Mike Wake <[email protected]>
…-navigation#4341) Signed-off-by: Mike Wake <[email protected]>
…ation#4341) Signed-off-by: Mike Wake <[email protected]>
…os-navigation#4341) Signed-off-by: Mike Wake <[email protected]>
Signed-off-by: Mike Wake <[email protected]>
…os-navigation#4341) Signed-off-by: Mike Wake <[email protected]>
Signed-off-by: Mike Wake <[email protected]>
…#4341) Signed-off-by: Mike Wake <[email protected]>
Signed-off-by: Mike Wake <[email protected]>
Signed-off-by: Mike Wake <[email protected]>
8d0288c
to
addf0dc
Compare
Rebased on 5c763e2 to fix merge conflict due to spelling fixes. |
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.
Tiny pendantic item and this is approved by me.
Next steps:
- I'm going to ask a colleague to give this a review, but if he doesn't within 2 weeks, I'll merge anyway. I've gone through this again with a fine toothed comb and I don't see any more reason to block it
- If you can think of some test cases to exercise on a package-level unit, system-level integration test, or abstracting code into methods to reduce the coverage misses -- that would be appreciated
…ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]>
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.
Otherwise, I had this reviewed and no other issues were found. If you can fix this and the one issue above, I think we are ready to merge this!
I did start wrapping a the error message creation and logging in some functions but you need one for each of INFO, WARN and ERROR OR a switch statement and I don't see a lot of value there. The coverage mechanism flagging that those kind of changes as untested seems weird and not very useful. The new error message handling functions in nav2_behavior_tree::BtActionServer could have tests and coverage of those does make sense. So I started to try to create a TestBtActionServer with a new message nav2_msgs::action::DummyNavigate and added some Result enums to nav2_msgs::action::DummyNavigate that created an essentially cut down navigator that would allow more direct unit tests of nav2_behavior_tree::BtActionServer (ie without the many moving parts of the full launch machinery used in system tests). I ran out of steam on it and it remains unfinished. |
OK - I'm not going to block for that, but if you had time / energy to complete it, I would appreciate it in a follow up PR. Nav2's been flirting with dropping below the 90% test coverage number for a year now that I've been trying to stay above, so maybe this'll be a good excuse for me to spend a few weeks later this year to cover some of the gaps that have built up This is really great to have in! I'm going to post on Discourse / LinkedIn about it shortly 😄 |
… (ros-navigation#4460) * Populate error_msg in action result messages (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * FollowWaypoints - Add usage of error_code and error_msg (ros-navigation#4341) Introduces error codes:- - NO_WAYPOINTS_GIVEN - STOP_ON_MISSED_WAYPOINT Signed-off-by: Mike Wake <[email protected]> * nav2_simple_commander: use error_code and error_msg (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * Create and populate BT::OutputPort(s) for error_msg (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * Add error_msg handling to nav2_bt_navigators (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * nav2_simple_command track LastActionError (ros-navigation#4341) Only actions provide error_code and error_msg in their result msg. Signed-off-by: Mike Wake <[email protected]> * Restore output format of planner server message (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * Fix opennav_docking error_msg/code usage (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * Fix error_msg type -> BT::OutputPort<std::string> (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * report index of failed initializeGoalPoses (ros-navigation#4341) Add the index of the pose that cannot be transformed in NavigateThroughPosesNavigator::initializeGoalPoses to the error_msg Signed-off-by: Mike Wake <[email protected]> * Add error_msg to (un)dock_robot bt (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * fix dock/undock use of error_msg (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * Improve comment to populate error_msg if goal not accepted (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * change robot_navigator api to (get|set|clear)TaskError() (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * remove exceptions from navigator onLoop (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * change error code to GOAL_TRANSFORMATION_ERROR (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * check getCurrentPose in initialiseGoalPoses (ros-navigation#4341) This mirrors the check done in NavigateToPose::initializeGoalPose checking that we can get a current pose in order to not accept the goal request. Signed-off-by: Mike Wake <[email protected]> * change error_code_names_ to error_code_name_prefixes_ (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * add error_msg port to default behavior xml files (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * Fix 'basic_string::_M_construct null not valid' exception(ros-navigation#4341) This was tough to spot. The nav2_system_tests waypoint follower provided a reproducable test Found out how to run just the waypoint_follower test colcon test --packages-select nav2_system_tests --ctest-args -N --event-handler console_direct+ | grep waypoint_follower changed src/navigation2/nav2_bringup/launch/navigation_launch.py to start nav2_bt_navigator Node with + prefix=['gnome-terminal -- gdb -ex run --args'], + respawn=False, Trying to use gdb to catch where the exception was thrown is too slow and the test gives up on itself. I found the relevant throw std::logic_error on my system by writing tiny program and breaking on std::logic_error int main() { const char *a = nullptr; try { std::string b(a); } catch (const std::logic_error &ex) { std::cout << "logic_error:" << ex.what() << std::endl; } catch (const std::exception &ex) { std::cout << "some other type:" << ex.what() << std::endl; } } Turns out it get thrown here on my system /usr/include/c++/11/bits/basic_string.tcc:212 Created ~/.gdbinit set breakpoint pending on break /usr/include/c++/11/bits/basic_string.tcc:212 This specific breakpoint on a line works fast enough for the integration test to throw the 'basic_string::_M_construct null not valid' and the silly initialisation mistake to be found. Signed-off-by: Mike Wake <[email protected]> * fix error_code_name_prefixes: in nav2_system_param.yml (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * Add error_msg handling to builtin nav2_behaviors plugins (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * simple_action_server improve "Aborting handle" log (ros-navigation#4341) Use SFINAE to log get_error_details_if_availble() in ActionT::Result Ideally all Action messages would have error_code and error_msg fields in their Result. BUT the tests use standard messages that do not and will not. i.e. test_msgs/action/Fibonacci.action Some Action messages only have error_code and not error_msg Signed-off-by: Mike Wake <[email protected]> * Add vim temporary files to .gitignore Signed-off-by: Mike Wake <[email protected]> * Fix include what you use (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * Fix cpplint and uncrustify (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * nav2_waypoint_follower do is_premppt_requested() before is_cancel_requested() (4341)(4602) Signed-off-by: Mike Wake <[email protected]> * fix behavior tree xml Backup backup_code_error_msg (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * Fix internal error tracking of navigators (ros-navigation#4341) Key point is only use the internal_error_(code|msg)_ if the action result error_code is 0. Signed-off-by: Mike Wake <[email protected]> * fix halt() in ComputePathToPose ComputePathThroughPoses (ros-navigation#4341) Clear output path for ComputePathThroughPoses similar to ComputePathToPose Explicitly note NOT to clear error_code_id and error_msg behavior tree output ports. Otherwise we cannot read them when the navigator reports errors. Signed-off-by: Mike Wake <[email protected]> * fix NavigateToPose onLoop current_path blackboard existance logic (ros-navigation#4341) Make it consistent with NavigateThroughPoses. This was introduced when changing logic to remove internal exception. Signed-off-by: Mike Wake <[email protected]> * nav2_msgs MissedWaypoint interface change, add error_msg (ros-navigation#4341) Allow of reason for missed waypoint as error_msg. Signed-off-by: Mike Wake <[email protected]> * Fix waypoint_follower error_msg handling (ros-navigation#4341) Include error_msg in reason for missed waypoint Signed-off-by: Mike Wake <[email protected]> * Fix cpplint uncrustify (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * Add tool to help check for error_code and error_msg in behavior trees (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * controller_server: Remove redundant logging (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * timed_behaviour: pass through on_run_result.error_msg (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * nav2_waypoint_follower - revert logic swap experiment (ros-navigation#4341) It was a red herring in search for error_msg clearing bug on abort Signed-off-by: Mike Wake <[email protected]> * nav2_waypoint_follower: clear error_msg when clearing error_code (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * Remove tool to check behaviour tree error_msg compliance (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * nav2_simple_commander: improve goal request rejection error reporting (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * nav2_system_tests: ensure error_msg not empty (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * nav2_system_tests: behaviour_tree error_code, error_msg checks (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * nav2_system_tests: behaviour_tree error_code, error_msg checks (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * doc: fix hint on how to run bt_navigator tests with ctest. Use -R to invoke the regular expresion. Signed-off-by: Mike Wake <[email protected]> * nav2_system_tests: log error_code and error_msg. (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * lint: remove tab (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * add result->error_msg for new CONTROLLER_TIMED_OUT (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * Add error codes NONE and UNKNOWN to Wait.action (ros-navigation#4341) This allows setting a default failure error_code and message in nav2_system_tests behavior_tree dummy_action_server Signed-off-by: Mike Wake <[email protected]> * move internal error tracking into bt_action_server.hpp and test (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * uncrustify and flake8 (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * nav2_system_tests behavior_tree result handling improvement (ros-navigation#4341) NOTE: This stuff does not really test the error_msg/error_code handling as its all dummy servers. Signed-off-by: Mike Wake <[email protected]> * Add ProgressCheckerSelector and GoalCheckerSelector to navigate_through_poses_w_replanning_and_recovery.xml (ros-navigation#4341) To facilitate error_msg system tests allow progress and goal checkers to be changed via the behaviour tree mechanism. Signed-off-by: Mike Wake <[email protected]> * Add nav2_system_tests for controller_server/behaviour tree error_msg (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * Improve README hint on how to run error_msg tests (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * fix PostStampedArray usage in nav_through_poses_tester_error_msg (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * nav_through_poses_tester_error_msg - towards clean pyright (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * SmoothPath fix error_(code_id|msg) output ports nav2_tree_nodes.xml (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * Wait action add error_(code_id|msg) output ports (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * Add error_code_id and error_msg ports to example behavior_tree xml (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * Add more default error_code_name_prefixes (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * Add error_(code_id|msg) to opennav_docking_bt example (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * error_msg review fixes (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * Adjust Navigator error code ranges (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> * Fix Navigate(ToPose|ThroughPoses)Navigator::goalCompleted warning log (ros-navigation#4341) Signed-off-by: Mike Wake <[email protected]> --------- Signed-off-by: Mike Wake <[email protected]> Signed-off-by: Mike Wake <[email protected]> Co-authored-by: Mike Wake <[email protected]> Signed-off-by: stevedanomodolor <[email protected]>
Basic Info
Description of contribution in a few bullet points
First pass at populating error_msg for all action result messages.
Description of documentation updates required from your changes
Adds some new error codes to FollowWaypoints + FollowGPSWaypoints
Docs PR - ros-navigation/docs.nav2.org#587
Tutorial PR - ros-navigation/navigation2_tutorials#104
Future work that may be required in bullet points
For Maintainers: