diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp index 0b74a044629..ae26a0c50ec 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp @@ -60,17 +60,18 @@ class DriveOnHeading : public TimedBehavior */ ResultStatus onRun(const std::shared_ptr 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; @@ -83,11 +84,12 @@ class DriveOnHeading : public TimedBehavior 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, ""}; } /** @@ -99,10 +101,10 @@ class DriveOnHeading : public TimedBehavior 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; @@ -110,8 +112,9 @@ class DriveOnHeading : public TimedBehavior 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; @@ -123,7 +126,7 @@ class DriveOnHeading : public TimedBehavior 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(); @@ -140,13 +143,14 @@ class DriveOnHeading : public TimedBehavior 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, ""}; } /** diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp index a11291b947c..f563045a2eb 100644 --- a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp +++ b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp @@ -53,6 +53,7 @@ struct ResultStatus { Status status; uint16_t error_code{0}; + std::string error_msg; }; using namespace std::chrono_literals; //NOLINT @@ -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; } @@ -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; diff --git a/nav2_behaviors/plugins/assisted_teleop.cpp b/nav2_behaviors/plugins/assisted_teleop.cpp index 9e85229a3c1..6976b0cd686 100644 --- a/nav2_behaviors/plugins/assisted_teleop.cpp +++ b/nav2_behaviors/plugins/assisted_teleop.cpp @@ -71,7 +71,7 @@ ResultStatus AssistedTeleop::onRun(const std::shared_ptrtime_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/*result*/) @@ -88,17 +88,16 @@ 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; @@ -106,11 +105,9 @@ ResultStatus AssistedTeleop::onCycleUpdate() 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; @@ -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( diff --git a/nav2_behaviors/plugins/back_up.cpp b/nav2_behaviors/plugins/back_up.cpp index 7a194874c7e..1e78c6a4247 100644 --- a/nav2_behaviors/plugins/back_up.cpp +++ b/nav2_behaviors/plugins/back_up.cpp @@ -20,10 +20,9 @@ namespace nav2_behaviors ResultStatus BackUp::onRun(const std::shared_ptr 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. @@ -37,11 +36,12 @@ ResultStatus BackUp::onRun(const std::shared_ptr 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 diff --git a/nav2_behaviors/plugins/spin.cpp b/nav2_behaviors/plugins/spin.cpp index 5965675f643..ea7bd7af2e0 100644 --- a/nav2_behaviors/plugins/spin.cpp +++ b/nav2_behaviors/plugins/spin.cpp @@ -78,8 +78,9 @@ ResultStatus Spin::onRun(const std::shared_ptr 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); @@ -93,7 +94,7 @@ ResultStatus Spin::onRun(const std::shared_ptr 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() @@ -101,10 +102,9 @@ 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; @@ -112,8 +112,9 @@ ResultStatus Spin::onCycleUpdate() 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); @@ -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); @@ -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( diff --git a/nav2_behaviors/plugins/wait.cpp b/nav2_behaviors/plugins/wait.cpp index 91e95d6c97a..823aa793ff5 100644 --- a/nav2_behaviors/plugins/wait.cpp +++ b/nav2_behaviors/plugins/wait.cpp @@ -31,7 +31,7 @@ Wait::~Wait() = default; ResultStatus Wait::onRun(const std::shared_ptr command) { wait_end_ = node_.lock()->now() + rclcpp::Duration(command->time); - return ResultStatus{Status::SUCCEEDED}; + return ResultStatus{Status::SUCCEEDED, 0, ""}; } ResultStatus Wait::onCycleUpdate() @@ -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, ""}; } } diff --git a/nav2_behaviors/test/test_behaviors.cpp b/nav2_behaviors/test/test_behaviors.cpp index 2d65ba80b23..ac76d82dea1 100644 --- a/nav2_behaviors/test/test_behaviors.cpp +++ b/nav2_behaviors/test/test_behaviors.cpp @@ -57,10 +57,10 @@ class DummyBehavior : public TimedBehavior // 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 @@ -70,7 +70,7 @@ class DummyBehavior : public TimedBehavior // 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 @@ -80,10 +80,10 @@ class DummyBehavior : public TimedBehavior 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, ""}; } /**