Skip to content

Commit

Permalink
Add error_msg handling to builtin nav2_behaviors plugins (ros-navigat…
Browse files Browse the repository at this point in the history 
…ion#4341)

Signed-off-by: Mike Wake <[email protected]>
  • Loading branch information
@aosmw
aosmw committed Sep 13, 2024
1 parent 837375b commit b2f175b
Show file tree
Hide file tree
Showing 10 changed files with 98 additions and 77 deletions.
3 changes: 2 additions & 1 deletion 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 @@ -328,7 +328,8 @@ void BtActionServer<ActionT>::executeCallback()
break;

case nav2_behavior_tree::BtStatus::FAILED:
RCLCPP_ERROR(logger_, "Goal failed");
RCLCPP_ERROR(logger_, "Goal failed error_code:%d error_msg:'%s'", result->error_code,
result->error_msg.c_str());
action_server_->terminate_current(result);
break;

Expand Down
9 changes: 9 additions & 0 deletions nav2_behavior_tree/nav2_tree_nodes.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@
<input_port name="server_name">Server name</input_port>
<input_port name="server_timeout">Server timeout</input_port>
<output_port name="error_code_id">"Back up error code"</output_port>
<output_port name="error_msg">"Back up error message"</output_port>
</Action>

<Action ID="DriveOnHeading">
Expand All @@ -23,6 +24,7 @@
<input_port name="server_name">Server name</input_port>
<input_port name="server_timeout">Server timeout</input_port>
<output_port name="error_code_id">"Drive on heading error code"</output_port>
<output_port name="error_msg">"Drive on heading error message"</output_port>
</Action>

<Action ID="CancelControl">
Expand Down Expand Up @@ -80,6 +82,7 @@
<input_port name="server_timeout">Server timeout</input_port>
<output_port name="path">Path created by ComputePathToPose node</output_port>
<output_port name="error_code_id">"Compute path to pose error code"</output_port>
<output_port name="error_msg">"Compute path to pose error message"</output_port>
</Action>

<Action ID="ComputePathThroughPoses">
Expand All @@ -90,6 +93,7 @@
<input_port name="planner_id">Mapped name to the planner plugin type to use</input_port>
<output_port name="path">Path created by ComputePathToPose node</output_port>
<output_port name="error_code_id">"Compute path through poses error code"</output_port>
<output_port name="error_msg">"Compute path through poses error message"</output_port>
</Action>

<Action ID="RemovePassedGoals">
Expand Down Expand Up @@ -126,6 +130,7 @@
<input_port name="service_name">Service name</input_port>
<input_port name="server_timeout">Server timeout</input_port>
<output_port name="error_code_id">Follow Path error code</output_port>
<output_port name="error_msg">Follow Path error message</output_port>
</Action>

<Action ID="NavigateToPose">
Expand All @@ -134,6 +139,7 @@
<input_port name="server_timeout">Server timeout</input_port>
<input_port name="behavior_tree">Behavior tree to run</input_port>
<output_port name="error_code_id">Navigate to pose error code</output_port>
<output_port name="error_msg">Navigate to pose error message</output_port>
</Action>

<Action ID="NavigateThroughPoses">
Expand All @@ -142,6 +148,7 @@
<input_port name="server_timeout">Server timeout</input_port>
<input_port name="behavior_tree">Behavior tree to run</input_port>
<output_port name="error_code_id">Navigate through poses error code</output_port>
<output_port name="error_msg">Navigate through poses error message</output_port>
</Action>

<Action ID="ReinitializeGlobalLocalization">
Expand Down Expand Up @@ -202,6 +209,7 @@
<input_port name="server_name">Server name</input_port>
<input_port name="server_timeout">Server timeout</input_port>
<output_port name="error_code_id">Spin error code</output_port>
<output_port name="error_msg">Spin error message</output_port>
</Action>

<Action ID="Wait">
Expand All @@ -216,6 +224,7 @@
<input_port name="server_name">Service name</input_port>
<input_port name="server_timeout">Server timeout</input_port>
<output_port name="error_code_id">Assisted teleop error code</output_port>
<output_port name="error_msg">Assisted teleop error message</output_port>
</Action>

<!-- ############################### CONDITION NODES ############################## -->
Expand Down
42 changes: 23 additions & 19 deletions nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,17 +60,18 @@ class DriveOnHeading : public TimedBehavior<ActionT>
*/
ResultStatus onRun(const std::shared_ptr<const typename ActionT::Goal> command) override
{
std::string error_msg;
if (command->target.y != 0.0 || command->target.z != 0.0) {
RCLCPP_INFO(
this->logger_,
"DrivingOnHeading in Y and Z not supported, will only move in X.");
return ResultStatus{Status::FAILED, ActionT::Result::INVALID_INPUT};
error_msg = "DrivingOnHeading in Y and Z not supported, will only move in X.";
RCLCPP_INFO(this->logger_, error_msg.c_str());
return ResultStatus{Status::FAILED, ActionT::Result::INVALID_INPUT, error_msg};
}

// Ensure that both the speed and direction have the same sign
if (!((command->target.x > 0.0) == (command->speed > 0.0)) ) {
RCLCPP_ERROR(this->logger_, "Speed and command sign did not match");
return ResultStatus{Status::FAILED, ActionT::Result::INVALID_INPUT};
error_msg = "Speed and command sign did not match";
RCLCPP_ERROR(this->logger_, error_msg.c_str());
return ResultStatus{Status::FAILED, ActionT::Result::INVALID_INPUT, error_msg};
}

command_x_ = command->target.x;
Expand All @@ -83,11 +84,12 @@ class DriveOnHeading : public TimedBehavior<ActionT>
initial_pose_, *this->tf_, this->local_frame_, this->robot_base_frame_,
this->transform_tolerance_))
{
RCLCPP_ERROR(this->logger_, "Initial robot pose is not available.");
return ResultStatus{Status::FAILED, ActionT::Result::TF_ERROR};
error_msg = "Initial robot pose is not available.";
RCLCPP_ERROR(this->logger_, error_msg.c_str());
return ResultStatus{Status::FAILED, ActionT::Result::TF_ERROR, error_msg};
}

return ResultStatus{Status::SUCCEEDED, ActionT::Result::NONE};
return ResultStatus{Status::SUCCEEDED, ActionT::Result::NONE, ""};
}

/**
Expand All @@ -99,19 +101,20 @@ class DriveOnHeading : public TimedBehavior<ActionT>
rclcpp::Duration time_remaining = end_time_ - this->clock_->now();
if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) {
this->stopRobot();
RCLCPP_WARN(
this->logger_,
"Exceeded time allowance before reaching the DriveOnHeading goal - Exiting DriveOnHeading");
return ResultStatus{Status::FAILED, ActionT::Result::NONE};
std::string error_msg =
"Exceeded time allowance before reaching the DriveOnHeading goal - Exiting DriveOnHeading";
RCLCPP_WARN(this->logger_, error_msg.c_str());
return ResultStatus{Status::FAILED, ActionT::Result::NONE, error_msg};
}

geometry_msgs::msg::PoseStamped current_pose;
if (!nav2_util::getCurrentPose(
current_pose, *this->tf_, this->local_frame_, this->robot_base_frame_,
this->transform_tolerance_))
{
RCLCPP_ERROR(this->logger_, "Current robot pose is not available.");
return ResultStatus{Status::FAILED, ActionT::Result::TF_ERROR};
std::string error_msg = "Current robot pose is not available.";
RCLCPP_ERROR(this->logger_, error_msg.c_str());
return ResultStatus{Status::FAILED, ActionT::Result::TF_ERROR, error_msg};
}

double diff_x = initial_pose_.pose.position.x - current_pose.pose.position.x;
Expand All @@ -123,7 +126,7 @@ class DriveOnHeading : public TimedBehavior<ActionT>

if (distance >= std::fabs(command_x_)) {
this->stopRobot();
return ResultStatus{Status::SUCCEEDED, ActionT::Result::NONE};
return ResultStatus{Status::SUCCEEDED, ActionT::Result::NONE, ""};
}

auto cmd_vel = std::make_unique<geometry_msgs::msg::TwistStamped>();
Expand All @@ -140,13 +143,14 @@ class DriveOnHeading : public TimedBehavior<ActionT>

if (!isCollisionFree(distance, cmd_vel->twist, pose2d)) {
this->stopRobot();
RCLCPP_WARN(this->logger_, "Collision Ahead - Exiting DriveOnHeading");
return ResultStatus{Status::FAILED, ActionT::Result::COLLISION_AHEAD};
std::string error_msg = "Collision Ahead - Exiting DriveOnHeading";
RCLCPP_WARN(this->logger_, error_msg.c_str());
return ResultStatus{Status::FAILED, ActionT::Result::COLLISION_AHEAD, error_msg};
}

this->vel_pub_->publish(std::move(cmd_vel));

return ResultStatus{Status::RUNNING, ActionT::Result::NONE};
return ResultStatus{Status::RUNNING, ActionT::Result::NONE, ""};
}

/**
Expand Down
10 changes: 6 additions & 4 deletions nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,7 @@ struct ResultStatus
{
Status status;
uint16_t error_code{0};
std::string error_msg;
};

using namespace std::chrono_literals; //NOLINT
Expand Down Expand Up @@ -224,9 +225,10 @@ class TimedBehavior : public nav2_core::Behavior

ResultStatus on_run_result = onRun(action_server_->get_current_goal());
if (on_run_result.status != Status::SUCCEEDED) {
result->error_msg = "Initial checks failed for " + behavior_name_;
RCLCPP_INFO(logger_, result->error_msg.c_str());
result->error_code = on_run_result.error_code;
result->error_msg = "Initial checks failed for " + behavior_name_ + " - " +
on_run_result.error_msg;
RCLCPP_INFO(logger_, result->error_msg.c_str());
action_server_->terminate_current(result);
return;
}
Expand Down Expand Up @@ -270,10 +272,10 @@ class TimedBehavior : public nav2_core::Behavior
return;

case Status::FAILED:
result->error_msg = behavior_name_ + " failed";
result->error_code = on_cycle_update_result.error_code;
result->error_msg = behavior_name_ + " failed:" + on_cycle_update_result.error_msg;
RCLCPP_WARN(logger_, result->error_msg.c_str());
result->total_elapsed_time = clock_->now() - start_time;
result->error_code = on_cycle_update_result.error_code;
onActionCompletion(result);
action_server_->terminate_current(result);
return;
Expand Down
23 changes: 10 additions & 13 deletions nav2_behaviors/plugins/assisted_teleop.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,7 +71,7 @@ ResultStatus AssistedTeleop::onRun(const std::shared_ptr<const AssistedTeleopAct
preempt_teleop_ = false;
command_time_allowance_ = command->time_allowance;
end_time_ = this->clock_->now() + command_time_allowance_;
return ResultStatus{Status::SUCCEEDED, AssistedTeleopActionResult::NONE};
return ResultStatus{Status::SUCCEEDED, AssistedTeleopActionResult::NONE, ""};
}

void AssistedTeleop::onActionCompletion(std::shared_ptr<AssistedTeleopActionResult>/*result*/)
Expand All @@ -88,29 +88,26 @@ ResultStatus AssistedTeleop::onCycleUpdate()
rclcpp::Duration time_remaining = end_time_ - this->clock_->now();
if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) {
stopRobot();
RCLCPP_WARN_STREAM(
logger_,
"Exceeded time allowance before reaching the " << behavior_name_.c_str() <<
"goal - Exiting " << behavior_name_.c_str());
return ResultStatus{Status::FAILED, AssistedTeleopActionResult::TIMEOUT};
std::string error_msg = "Exceeded time allowance before reaching the " + behavior_name_ +
"goal - Exiting " + behavior_name_;
RCLCPP_WARN_STREAM(logger_, error_msg.c_str());
return ResultStatus{Status::FAILED, AssistedTeleopActionResult::TIMEOUT, error_msg};
}

// user states that teleop was successful
if (preempt_teleop_) {
stopRobot();
return ResultStatus{Status::SUCCEEDED, AssistedTeleopActionResult::NONE};
return ResultStatus{Status::SUCCEEDED, AssistedTeleopActionResult::NONE, ""};
}

geometry_msgs::msg::PoseStamped current_pose;
if (!nav2_util::getCurrentPose(
current_pose, *tf_, local_frame_, robot_base_frame_,
transform_tolerance_))
{
RCLCPP_ERROR_STREAM(
logger_,
"Current robot pose is not available for " <<
behavior_name_.c_str());
return ResultStatus{Status::FAILED, AssistedTeleopActionResult::TF_ERROR};
std::string error_msg = "Current robot pose is not available for " + behavior_name_;
RCLCPP_ERROR_STREAM(logger_, error_msg.c_str());
return ResultStatus{Status::FAILED, AssistedTeleopActionResult::TF_ERROR, error_msg};
}

geometry_msgs::msg::Pose2D projected_pose;
Expand Down Expand Up @@ -151,7 +148,7 @@ ResultStatus AssistedTeleop::onCycleUpdate()
}
vel_pub_->publish(std::move(scaled_twist));

return ResultStatus{Status::RUNNING, AssistedTeleopActionResult::NONE};
return ResultStatus{Status::RUNNING, AssistedTeleopActionResult::NONE, ""};
}

geometry_msgs::msg::Pose2D AssistedTeleop::projectPose(
Expand Down
14 changes: 7 additions & 7 deletions nav2_behaviors/plugins/back_up.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,10 +20,9 @@ namespace nav2_behaviors
ResultStatus BackUp::onRun(const std::shared_ptr<const BackUpAction::Goal> command)
{
if (command->target.y != 0.0 || command->target.z != 0.0) {
RCLCPP_INFO(
logger_,
"Backing up in Y and Z not supported, will only move in X.");
return ResultStatus{Status::FAILED, BackUpActionResult::INVALID_INPUT};
std::string error_msg = "Backing up in Y and Z not supported, will only move in X.";
RCLCPP_INFO(logger_, error_msg.c_str());
return ResultStatus{Status::FAILED, BackUpActionResult::INVALID_INPUT, error_msg};
}

// Silently ensure that both the speed and direction are negative.
Expand All @@ -37,11 +36,12 @@ ResultStatus BackUp::onRun(const std::shared_ptr<const BackUpAction::Goal> comma
initial_pose_, *tf_, local_frame_, robot_base_frame_,
transform_tolerance_))
{
RCLCPP_ERROR(logger_, "Initial robot pose is not available.");
return ResultStatus{Status::FAILED, BackUpActionResult::TF_ERROR};
std::string error_msg = "Initial robot pose is not available.";
RCLCPP_ERROR(logger_, error_msg.c_str());
return ResultStatus{Status::FAILED, BackUpActionResult::TF_ERROR, error_msg};
}

return ResultStatus{Status::SUCCEEDED, BackUpActionResult::NONE};
return ResultStatus{Status::SUCCEEDED, BackUpActionResult::NONE, ""};
}

} // namespace nav2_behaviors
Expand Down
28 changes: 15 additions & 13 deletions nav2_behaviors/plugins/spin.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,8 +78,9 @@ ResultStatus Spin::onRun(const std::shared_ptr<const SpinActionGoal> command)
current_pose, *tf_, local_frame_, robot_base_frame_,
transform_tolerance_))
{
RCLCPP_ERROR(logger_, "Current robot pose is not available.");
return ResultStatus{Status::FAILED, SpinActionResult::TF_ERROR};
std::string error_msg = "Current robot pose is not available.";
RCLCPP_ERROR(logger_, error_msg.c_str());
return ResultStatus{Status::FAILED, SpinActionResult::TF_ERROR, error_msg};
}

prev_yaw_ = tf2::getYaw(current_pose.pose.orientation);
Expand All @@ -93,27 +94,27 @@ ResultStatus Spin::onRun(const std::shared_ptr<const SpinActionGoal> command)
command_time_allowance_ = command->time_allowance;
end_time_ = this->clock_->now() + command_time_allowance_;

return ResultStatus{Status::SUCCEEDED, SpinActionResult::NONE};
return ResultStatus{Status::SUCCEEDED, SpinActionResult::NONE, ""};
}

ResultStatus Spin::onCycleUpdate()
{
rclcpp::Duration time_remaining = end_time_ - this->clock_->now();
if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) {
stopRobot();
RCLCPP_WARN(
logger_,
"Exceeded time allowance before reaching the Spin goal - Exiting Spin");
return ResultStatus{Status::FAILED, SpinActionResult::TIMEOUT};
std::string error_msg = "Exceeded time allowance before reaching the Spin goal - Exiting Spin";
RCLCPP_WARN(logger_, error_msg.c_str());
return ResultStatus{Status::FAILED, SpinActionResult::TIMEOUT, error_msg};
}

geometry_msgs::msg::PoseStamped current_pose;
if (!nav2_util::getCurrentPose(
current_pose, *tf_, local_frame_, robot_base_frame_,
transform_tolerance_))
{
RCLCPP_ERROR(logger_, "Current robot pose is not available.");
return ResultStatus{Status::FAILED, SpinActionResult::TF_ERROR};
std::string error_msg = "Current robot pose is not available.";
RCLCPP_ERROR(logger_, error_msg.c_str());
return ResultStatus{Status::FAILED, SpinActionResult::TF_ERROR, error_msg};
}

const double current_yaw = tf2::getYaw(current_pose.pose.orientation);
Expand All @@ -132,7 +133,7 @@ ResultStatus Spin::onCycleUpdate()
double remaining_yaw = abs(cmd_yaw_) - abs(relative_yaw_);
if (remaining_yaw < 1e-6) {
stopRobot();
return ResultStatus{Status::SUCCEEDED, SpinActionResult::NONE};
return ResultStatus{Status::SUCCEEDED, SpinActionResult::NONE, ""};
}

double vel = sqrt(2 * rotational_acc_lim_ * remaining_yaw);
Expand All @@ -150,13 +151,14 @@ ResultStatus Spin::onCycleUpdate()

if (!isCollisionFree(relative_yaw_, cmd_vel->twist, pose2d)) {
stopRobot();
RCLCPP_WARN(logger_, "Collision Ahead - Exiting Spin");
return ResultStatus{Status::FAILED, SpinActionResult::COLLISION_AHEAD};
std::string error_msg = "Collision Ahead - Exiting Spin";
RCLCPP_WARN(logger_, error_msg.c_str());
return ResultStatus{Status::FAILED, SpinActionResult::COLLISION_AHEAD, error_msg};
}

vel_pub_->publish(std::move(cmd_vel));

return ResultStatus{Status::RUNNING, SpinActionResult::NONE};
return ResultStatus{Status::RUNNING, SpinActionResult::NONE, ""};
}

bool Spin::isCollisionFree(
Expand Down
6 changes: 3 additions & 3 deletions nav2_behaviors/plugins/wait.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ Wait::~Wait() = default;
ResultStatus Wait::onRun(const std::shared_ptr<const WaitAction::Goal> command)
{
wait_end_ = node_.lock()->now() + rclcpp::Duration(command->time);
return ResultStatus{Status::SUCCEEDED};
return ResultStatus{Status::SUCCEEDED, 0, ""};
}

ResultStatus Wait::onCycleUpdate()
Expand All @@ -43,9 +43,9 @@ ResultStatus Wait::onCycleUpdate()
action_server_->publish_feedback(feedback_);

if (time_left.nanoseconds() > 0) {
return ResultStatus{Status::RUNNING};
return ResultStatus{Status::RUNNING, 0, ""};
} else {
return ResultStatus{Status::SUCCEEDED};
return ResultStatus{Status::SUCCEEDED, 0, ""};
}
}

Expand Down
Loading

0 comments on commit b2f175b

Please sign in to comment.