Skip to content

Commit

Permalink
Report error_msg from nav2_behaviors (#4341)
Browse files Browse the repository at this point in the history 
This adds error_msg reporting to
wait, spin, back_up, assisted_teleop
timed_behavior and drive_on_heading
behaviours.

Signed-off-by: Mike Wake <[email protected]>
  • Loading branch information
@aosmw
aosmw committed Aug 2, 2024
1 parent 5e4dddc commit 0461cfc
Show file tree
Hide file tree
Showing 7 changed files with 70 additions and 64 deletions.
43 changes: 24 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 @@ -19,6 +19,7 @@
#include <chrono>
#include <memory>
#include <utility>
#include <string>

#include "nav2_behaviors/timed_behavior.hpp"
#include "nav2_msgs/action/drive_on_heading.hpp"
Expand Down Expand Up @@ -60,17 +61,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 +85,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 +102,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 +127,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 +144,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
10 changes: 5 additions & 5 deletions nav2_behaviors/test/test_behaviors.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,10 +57,10 @@ class DummyBehavior : public TimedBehavior<BehaviorAction>
// The output is defined by the tester class on the command string.
if (command_ == "Testing success" || command_ == "Testing failure on run") {
initialized_ = true;
return ResultStatus{Status::SUCCEEDED, 0};
return ResultStatus{Status::SUCCEEDED, 0, ""};
}

return ResultStatus{Status::FAILED, 0};
return ResultStatus{Status::FAILED, 0, "failed"};
}

ResultStatus onCycleUpdate() override
Expand All @@ -70,7 +70,7 @@ class DummyBehavior : public TimedBehavior<BehaviorAction>
// was completed.

if (command_ != "Testing success" || !initialized_) {
return ResultStatus{Status::FAILED, 0};
return ResultStatus{Status::FAILED, 0, "failed"};
}

// For testing, pretend the robot takes some fixed
Expand All @@ -80,10 +80,10 @@ class DummyBehavior : public TimedBehavior<BehaviorAction>

if (current_time - start_time_ >= motion_duration) {
// Movement was completed
return ResultStatus{Status::SUCCEEDED, 0};
return ResultStatus{Status::SUCCEEDED, 0, ""};
}

return ResultStatus{Status::RUNNING, 0};
return ResultStatus{Status::RUNNING, 0, ""};
}

/**
Expand Down

0 comments on commit 0461cfc

Please sign in to comment.