Skip to content

Commit

Permalink
move internal error tracking into bt_action_server.hpp and test (ros-…
Browse files Browse the repository at this point in the history 
…navigation#4341)

Signed-off-by: Mike Wake <[email protected]>
  • Loading branch information
@ewak
ewak committed Feb 11, 2025
1 parent 62dc8ac commit 74a1943
Show file tree
Hide file tree
Showing 7 changed files with 436 additions and 47 deletions.
23 changes: 23 additions & 0 deletions nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -188,6 +188,25 @@ class BtActionServer
tree_.haltTree();
}

/**
* @brief Set internal error code and message
* @param error_code the internal error code
* @param error_msg the internal error message
*/
void setInternalError(uint16_t error_code, const std::string & error_msg);

/**
* @brief reset internal error code and message
*/
void resetInternalError(void);

/**
* @brief set result to internal error code and message if not NONE
* @param result the action server result to be updated
* @return bool action server result was changed
*/
bool setResultToInternalError(typename std::shared_ptr<typename ActionT::Result> result);

protected:
/**
* @brief Action server callback
Expand Down Expand Up @@ -263,6 +282,10 @@ class BtActionServer
OnLoopCallback on_loop_callback_;
OnPreemptCallback on_preempt_callback_;
OnCompletionCallback on_completion_callback_;

// internal error tracking (IOW not behaviorTree blackboard errors)
uint16_t internal_error_code_;
std::string internal_error_msg_;
};

} // namespace nav2_behavior_tree
Expand Down
47 changes: 44 additions & 3 deletions nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,9 @@ BtActionServer<ActionT>::BtActionServer(
on_goal_received_callback_(on_goal_received_callback),
on_loop_callback_(on_loop_callback),
on_preempt_callback_(on_preempt_callback),
on_completion_callback_(on_completion_callback)
on_completion_callback_(on_completion_callback),
internal_error_code_(0),
internal_error_msg_()
{
auto node = node_.lock();
logger_ = node->get_logger();
Expand Down Expand Up @@ -195,6 +197,8 @@ bool BtActionServer<ActionT>::on_configure()
"wait_for_service_timeout",
wait_for_service_timeout_);

resetInternalError();

return true;
}

Expand Down Expand Up @@ -246,7 +250,8 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
std::ifstream xml_file(filename);

if (!xml_file.good()) {
RCLCPP_ERROR(logger_, "Couldn't open input XML file: %s", filename.c_str());
setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
"Couldn't open input XML file: " + filename);
return false;
}

Expand All @@ -263,7 +268,8 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena
wait_for_service_timeout_);
}
} catch (const std::exception & e) {
RCLCPP_ERROR(logger_, "Exception when loading BT: %s", e.what());
setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
std::string("Exception when loading BT: ") + e.what());
return false;
}

Expand Down Expand Up @@ -342,13 +348,47 @@ void BtActionServer<ActionT>::executeCallback()
cleanErrorCodes();
}

template<class ActionT>
void BtActionServer<ActionT>::setInternalError(uint16_t error_code, const std::string & error_msg)
{
internal_error_code_ = error_code;
internal_error_msg_ = error_msg;
RCLCPP_ERROR(logger_, "Setting internal error error_code:%d, error_msg:%s",
internal_error_code_, internal_error_msg_.c_str());
}

template<class ActionT>
void BtActionServer<ActionT>::resetInternalError(void)
{
internal_error_code_ = ActionT::Result::NONE;
internal_error_msg_ = "";
}

template<class ActionT>
bool BtActionServer<ActionT>::setResultToInternalError(
typename std::shared_ptr<typename ActionT::Result> result)
{
if (internal_error_code_ != ActionT::Result::NONE) {
result->error_code = internal_error_code_;
result->error_msg = internal_error_msg_;
return true;
}
return false;
}

template<class ActionT>
void BtActionServer<ActionT>::populateErrorCode(
typename std::shared_ptr<typename ActionT::Result> result)
{
int highest_priority_error_code = std::numeric_limits<int>::max();
std::string highest_priority_error_msg = "";
std::string name;

if (internal_error_code_ != 0) {
highest_priority_error_code = internal_error_code_;
highest_priority_error_msg = internal_error_msg_;
}

for (const auto & error_code_name_prefix : error_code_name_prefixes_) {
try {
name = error_code_name_prefix + "_error_code";
Expand Down Expand Up @@ -382,6 +422,7 @@ void BtActionServer<ActionT>::cleanErrorCodes()
name = error_code_name_prefix + "_error_msg";
blackboard_->set<std::string>(name, "");
}
resetInternalError();
}

} // namespace nav2_behavior_tree
Expand Down
28 changes: 10 additions & 18 deletions nav2_bt_navigator/src/navigators/navigate_through_poses.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,8 +45,7 @@ NavigateThroughPosesNavigator::configure(
// Odometry smoother object for getting current speed
odom_smoother_ = odom_smoother;

internal_error_code_ = ActionT::Result::NONE;
internal_error_msg_ = "";
bt_action_server_->resetInternalError();
return true;
}

Expand Down Expand Up @@ -77,10 +76,8 @@ NavigateThroughPosesNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal)
auto bt_xml_filename = goal->behavior_tree;

if (!bt_action_server_->loadBehaviorTree(bt_xml_filename)) {
internal_error_code_ = ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE;
internal_error_msg_ = "Error loading XML file: " + bt_xml_filename +
". Navigation canceled.";
RCLCPP_ERROR(logger_, internal_error_msg_.c_str());
bt_action_server_->setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
"Error loading XML file: " + bt_xml_filename + ". Navigation canceled.");
return false;
}

Expand All @@ -92,12 +89,11 @@ NavigateThroughPosesNavigator::goalCompleted(
typename ActionT::Result::SharedPtr result,
const nav2_behavior_tree::BtStatus /*final_bt_status*/)
{
if (result->error_code == 0 && internal_error_code_ != 0) {
if (result->error_code == 0 && bt_action_server_->setResultToInternalError(result)) {
RCLCPP_WARN(logger_,
"NavigateThroughPosesNavigator::goalCompleted, set result to internal error %d:'%s'.",
internal_error_code_, internal_error_msg_.c_str());
result->error_code = internal_error_code_;
result->error_msg = internal_error_msg_;
result->error_code,
result->error_msg.c_str());
} else {
RCLCPP_INFO(logger_, "NavigateThroughPosesNavigator::goalCompleted result %d:'%s'.",
result->error_code,
Expand Down Expand Up @@ -226,9 +222,8 @@ NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr
feedback_utils_.global_frame, feedback_utils_.robot_frame,
feedback_utils_.transform_tolerance))
{
internal_error_code_ = ActionT::Result::POSE_NOT_AVAILABLE;
internal_error_msg_ = "Initial robot pose is not available.";
RCLCPP_ERROR(logger_, internal_error_msg_.c_str());
bt_action_server_->setInternalError(ActionT::Result::POSE_NOT_AVAILABLE,
"Initial robot pose is not available.");
return false;
}

Expand All @@ -239,15 +234,12 @@ NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr
goal_pose, goal_pose, *feedback_utils_.tf, feedback_utils_.global_frame,
feedback_utils_.transform_tolerance))
{
internal_error_code_ = ActionT::Result::GOAL_TRANSFORMATION_ERROR;
internal_error_msg_ =
bt_action_server_->setInternalError(ActionT::Result::GOAL_TRANSFORMATION_ERROR,
"Failed to transform a goal pose (" + std::to_string(i) + ") provided with frame_id '" +
goal_pose.header.frame_id +
"' to the global frame '" +
feedback_utils_.global_frame +
"'.";

RCLCPP_ERROR(logger_, internal_error_msg_.c_str());
"'.");
return false;
}
i++;
Expand Down
30 changes: 11 additions & 19 deletions nav2_bt_navigator/src/navigators/navigate_to_pose.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,8 +44,7 @@ NavigateToPoseNavigator::configure(
// Odometry smoother object for getting current speed
odom_smoother_ = odom_smoother;

internal_error_code_ = ActionT::Result::NONE;
internal_error_msg_ = "";
bt_action_server_->resetInternalError();

self_client_ = rclcpp_action::create_client<ActionT>(node, getName());

Expand Down Expand Up @@ -91,10 +90,8 @@ NavigateToPoseNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal)
auto bt_xml_filename = goal->behavior_tree;

if (!bt_action_server_->loadBehaviorTree(bt_xml_filename)) {
internal_error_code_ = ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE;
internal_error_msg_ = "Error loading XML file: " + bt_xml_filename +
". Navigation canceled.";
RCLCPP_ERROR(logger_, internal_error_msg_.c_str());
bt_action_server_->setInternalError(ActionT::Result::FAILED_TO_LOAD_BEHAVIOR_TREE,
std::string("Error loading XML file: ") + bt_xml_filename + ". Navigation canceled.");
return false;
}

Expand All @@ -106,12 +103,11 @@ NavigateToPoseNavigator::goalCompleted(
typename ActionT::Result::SharedPtr result,
const nav2_behavior_tree::BtStatus /*final_bt_status*/)
{
if (result->error_code == 0 && !internal_error_code_ == 0) {
if (result->error_code == 0 && bt_action_server_->setResultToInternalError(result)) {
RCLCPP_WARN(logger_,
"NavigateThroughPosesNavigator::goalCompleted, set result to internal error %d:%s.",
internal_error_code_, internal_error_msg_.c_str());
result->error_code = internal_error_code_;
result->error_msg = internal_error_msg_;
"NavigateToPoseNavigator::goalCompleted, set result to internal error %d:%s.",
result->error_code,
result->error_msg.c_str());
} else {
RCLCPP_INFO(logger_, "NavigateThroughPosesNavigator::goalCompleted result %d:%s.",
result->error_code,
Expand Down Expand Up @@ -229,9 +225,8 @@ NavigateToPoseNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal)
feedback_utils_.global_frame, feedback_utils_.robot_frame,
feedback_utils_.transform_tolerance))
{
internal_error_code_ = ActionT::Result::POSE_NOT_AVAILABLE;
internal_error_msg_ = "Initial robot pose is not available.";
RCLCPP_ERROR(logger_, internal_error_msg_.c_str());
bt_action_server_->setInternalError(ActionT::Result::POSE_NOT_AVAILABLE,
"Initial robot pose is not available.");
return false;
}

Expand All @@ -240,15 +235,12 @@ NavigateToPoseNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal)
goal->pose, goal_pose, *feedback_utils_.tf, feedback_utils_.global_frame,
feedback_utils_.transform_tolerance))
{
internal_error_code_ = ActionT::Result::GOAL_TRANSFORMATION_ERROR;
internal_error_msg_ =
bt_action_server_->setInternalError(ActionT::Result::GOAL_TRANSFORMATION_ERROR,
"Failed to transform a goal pose provided with frame_id '" +
goal->pose.header.frame_id +
"' to the global frame '" +
feedback_utils_.global_frame +
"'.";

RCLCPP_ERROR(logger_, internal_error_msg_.c_str());
"'.");
return false;
}

Expand Down
7 changes: 0 additions & 7 deletions nav2_core/include/nav2_core/behavior_tree_navigator.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -225,9 +225,6 @@ class BehaviorTreeNavigator : public NavigatorBase
blackboard->set("number_recoveries", 0); // NOLINT
blackboard->set("odom_smoother", odom_smoother); // NOLINT

internal_error_code_ = 0;
internal_error_msg_ = "";

return configure(parent_node, odom_smoother) && ok;
}

Expand Down Expand Up @@ -374,10 +371,6 @@ class BehaviorTreeNavigator : public NavigatorBase
rclcpp::Clock::SharedPtr clock_;
FeedbackUtils feedback_utils_;
NavigatorMuxer * plugin_muxer_;

// internal error tracking (IOW not behaviorTree blackboard errors)
uint16_t internal_error_code_;
std::string internal_error_msg_;
};

} // namespace nav2_core
Expand Down
15 changes: 15 additions & 0 deletions nav2_system_tests/src/system/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -84,6 +84,21 @@ ament_add_test(test_nav_through_poses
PLANNER=nav2_navfn_planner::NavfnPlanner
)

ament_add_test(test_nav_through_poses_error_msg
GENERATE_RESULT_FOR_RETURN_CODE_ZERO
COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_with_obstacle_launch.py"
WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
TIMEOUT 180
ENV
TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
ADD_OBSTACLE=True
BT_NAVIGATOR_XML=navigate_through_poses_w_replanning_and_recovery.xml
TESTER=nav_through_poses_tester_error_msg_node.py
ASTAR=False
CONTROLLER=dwb_core::DWBLocalPlanner
PLANNER=nav2_navfn_planner::NavfnPlanner
)

# ament_add_test(test_multi_robot
# GENERATE_RESULT_FOR_RETURN_CODE_ZERO
# COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_multi_robot_launch.py"
Expand Down
Loading

0 comments on commit 74a1943

Please sign in to comment.