Skip to content

Commit 7e3ae16

Browse files
aosmwewak
andauthoredMar 4, 2025··
Populate error_msg for all action result messages (#4341) (#4460)
* Populate error_msg in action result messages (#4341) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * FollowWaypoints - Add usage of error_code and error_msg (#4341) Introduces error codes:- - NO_WAYPOINTS_GIVEN - STOP_ON_MISSED_WAYPOINT Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * nav2_simple_commander: use error_code and error_msg (#4341) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * Create and populate BT::OutputPort(s) for error_msg (#4341) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * Add error_msg handling to nav2_bt_navigators (#4341) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * nav2_simple_command track LastActionError (#4341) Only actions provide error_code and error_msg in their result msg. Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * Restore output format of planner server message (#4341) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * Fix opennav_docking error_msg/code usage (#4341) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * Fix error_msg type -> BT::OutputPort<std::string> (#4341) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * report index of failed initializeGoalPoses (#4341) Add the index of the pose that cannot be transformed in NavigateThroughPosesNavigator::initializeGoalPoses to the error_msg Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * Add error_msg to (un)dock_robot bt (#4341) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * fix dock/undock use of error_msg (#4341) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * Improve comment to populate error_msg if goal not accepted (#4341) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * change robot_navigator api to (get|set|clear)TaskError() (#4341) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * remove exceptions from navigator onLoop (#4341) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * change error code to GOAL_TRANSFORMATION_ERROR (#4341) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * check getCurrentPose in initialiseGoalPoses (#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 <michael.wake@aosgrp.com.au> * change error_code_names_ to error_code_name_prefixes_ (#4341) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * add error_msg port to default behavior xml files (#4341) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * Fix 'basic_string::_M_construct null not valid' exception(#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 <michael.wake@aosgrp.com.au> * fix error_code_name_prefixes: in nav2_system_param.yml (#4341) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * Add error_msg handling to builtin nav2_behaviors plugins (#4341) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * simple_action_server improve "Aborting handle" log (#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 <michael.wake@aosgrp.com.au> * Add vim temporary files to .gitignore Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * Fix include what you use (#4341) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * Fix cpplint and uncrustify (#4341) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * nav2_waypoint_follower do is_premppt_requested() before is_cancel_requested() (4341)(4602) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * fix behavior tree xml Backup backup_code_error_msg (#4341) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * Fix internal error tracking of navigators (#4341) Key point is only use the internal_error_(code|msg)_ if the action result error_code is 0. Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * fix halt() in ComputePathToPose ComputePathThroughPoses (#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 <michael.wake@aosgrp.com.au> * fix NavigateToPose onLoop current_path blackboard existance logic (#4341) Make it consistent with NavigateThroughPoses. This was introduced when changing logic to remove internal exception. Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * nav2_msgs MissedWaypoint interface change, add error_msg (#4341) Allow of reason for missed waypoint as error_msg. Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * Fix waypoint_follower error_msg handling (#4341) Include error_msg in reason for missed waypoint Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * Fix cpplint uncrustify (#4341) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * Add tool to help check for error_code and error_msg in behavior trees (#4341) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * controller_server: Remove redundant logging (#4341) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * timed_behaviour: pass through on_run_result.error_msg (#4341) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * nav2_waypoint_follower - revert logic swap experiment (#4341) It was a red herring in search for error_msg clearing bug on abort Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * nav2_waypoint_follower: clear error_msg when clearing error_code (#4341) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * Remove tool to check behaviour tree error_msg compliance (#4341) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * nav2_simple_commander: improve goal request rejection error reporting (#4341) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * nav2_system_tests: ensure error_msg not empty (#4341) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * nav2_system_tests: behaviour_tree error_code, error_msg checks (#4341) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * nav2_system_tests: behaviour_tree error_code, error_msg checks (#4341) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * doc: fix hint on how to run bt_navigator tests with ctest. Use -R to invoke the regular expresion. Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * nav2_system_tests: log error_code and error_msg. (#4341) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * lint: remove tab (#4341) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * add result->error_msg for new CONTROLLER_TIMED_OUT (#4341) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * Add error codes NONE and UNKNOWN to Wait.action (#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 <michael.wake@aosgrp.com.au> * move internal error tracking into bt_action_server.hpp and test (#4341) Signed-off-by: Mike Wake <macwake@gmail.com> * uncrustify and flake8 (#4341) Signed-off-by: Mike Wake <macwake@gmail.com> * nav2_system_tests behavior_tree result handling improvement (#4341) NOTE: This stuff does not really test the error_msg/error_code handling as its all dummy servers. Signed-off-by: Mike Wake <macwake@gmail.com> * Add ProgressCheckerSelector and GoalCheckerSelector to navigate_through_poses_w_replanning_and_recovery.xml (#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 <macwake@gmail.com> * Add nav2_system_tests for controller_server/behaviour tree error_msg (#4341) Signed-off-by: Mike Wake <macwake@gmail.com> * Improve README hint on how to run error_msg tests (#4341) Signed-off-by: Mike Wake <macwake@gmail.com> * fix PostStampedArray usage in nav_through_poses_tester_error_msg (#4341) Signed-off-by: Mike Wake <macwake@gmail.com> * nav_through_poses_tester_error_msg - towards clean pyright (#4341) Signed-off-by: Mike Wake <macwake@gmail.com> * SmoothPath fix error_(code_id|msg) output ports nav2_tree_nodes.xml (#4341) Signed-off-by: Mike Wake <macwake@gmail.com> * Wait action add error_(code_id|msg) output ports (#4341) Signed-off-by: Mike Wake <macwake@gmail.com> * Add error_code_id and error_msg ports to example behavior_tree xml (#4341) Signed-off-by: Mike Wake <macwake@gmail.com> * Add more default error_code_name_prefixes (#4341) Signed-off-by: Mike Wake <macwake@gmail.com> * Add error_(code_id|msg) to opennav_docking_bt example (#4341) Signed-off-by: Mike Wake <macwake@gmail.com> * error_msg review fixes (#4341) Signed-off-by: Mike Wake <macwake@gmail.com> * Adjust Navigator error code ranges (#4341) Signed-off-by: Mike Wake <macwake@gmail.com> * Fix Navigate(ToPose|ThroughPoses)Navigator::goalCompleted warning log (#4341) Signed-off-by: Mike Wake <macwake@gmail.com> --------- Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> Signed-off-by: Mike Wake <macwake@gmail.com> Co-authored-by: Mike Wake <macwake@gmail.com>
1 parent 3ddf03c commit 7e3ae16

File tree

90 files changed

+1370
-370
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

90 files changed

+1370
-370
lines changed
 

‎.gitignore

+16
Original file line numberDiff line numberDiff line change
@@ -54,3 +54,19 @@ cmake-build-debug/
5454

5555
# doxygen docs
5656
doc/html/
57+
# Vim Swap
58+
[._]*.s[a-v][a-z]
59+
[._]*.sw[a-p]
60+
[._]s[a-rt-v][a-z]
61+
[._]ss[a-gi-z]
62+
[._]sw[a-p]
63+
64+
# Vim Persistent undo
65+
[._]*.un~
66+
67+
# Vim Session
68+
Session.vim
69+
70+
# Vim Temporary
71+
.netrwhist
72+

‎nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp

+25-2
Original file line numberDiff line numberDiff line change
@@ -188,6 +188,25 @@ class BtActionServer
188188
tree_.haltTree();
189189
}
190190

191+
/**
192+
* @brief Set internal error code and message
193+
* @param error_code the internal error code
194+
* @param error_msg the internal error message
195+
*/
196+
void setInternalError(uint16_t error_code, const std::string & error_msg);
197+
198+
/**
199+
* @brief reset internal error code and message
200+
*/
201+
void resetInternalError(void);
202+
203+
/**
204+
* @brief populate result with internal error code and error_msg if not NONE
205+
* @param result the action server result to be updated
206+
* @return bool action server result was changed
207+
*/
208+
bool populateInternalError(typename std::shared_ptr<typename ActionT::Result> result);
209+
191210
protected:
192211
/**
193212
* @brief Action server callback
@@ -228,8 +247,8 @@ class BtActionServer
228247
// Libraries to pull plugins (BT Nodes) from
229248
std::vector<std::string> plugin_lib_names_;
230249

231-
// Error code id names
232-
std::vector<std::string> error_code_names_;
250+
// Error code name prefixes
251+
std::vector<std::string> error_code_name_prefixes_;
233252

234253
// A regular, non-spinning ROS node that we can use for calls to the action client
235254
rclcpp::Node::SharedPtr client_node_;
@@ -263,6 +282,10 @@ class BtActionServer
263282
OnLoopCallback on_loop_callback_;
264283
OnPreemptCallback on_preempt_callback_;
265284
OnCompletionCallback on_completion_callback_;
285+
286+
// internal error tracking (IOW not behaviorTree blackboard errors)
287+
uint16_t internal_error_code_;
288+
std::string internal_error_msg_;
266289
};
267290

268291
} // namespace nav2_behavior_tree

0 commit comments

Comments
 (0)
Please sign in to comment.