Skip to content

Commit 0461cfc

Browse files
aosmwaosmw
committed
Report error_msg from nav2_behaviors (#4341)
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]>
1 parent 5e4dddc commit 0461cfc

File tree

7 files changed

+70
-64
lines changed

7 files changed

+70
-64
lines changed

‎nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp

+24-19
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
#include <chrono>
2020
#include <memory>
2121
#include <utility>
22+
#include <string>
2223

2324
#include "nav2_behaviors/timed_behavior.hpp"
2425
#include "nav2_msgs/action/drive_on_heading.hpp"
@@ -60,17 +61,18 @@ class DriveOnHeading : public TimedBehavior<ActionT>
6061
*/
6162
ResultStatus onRun(const std::shared_ptr<const typename ActionT::Goal> command) override
6263
{
64+
std::string error_msg;
6365
if (command->target.y != 0.0 || command->target.z != 0.0) {
64-
RCLCPP_INFO(
65-
this->logger_,
66-
"DrivingOnHeading in Y and Z not supported, will only move in X.");
67-
return ResultStatus{Status::FAILED, ActionT::Result::INVALID_INPUT};
66+
error_msg = "DrivingOnHeading in Y and Z not supported, will only move in X.";
67+
RCLCPP_INFO(this->logger_, error_msg.c_str());
68+
return ResultStatus{Status::FAILED, ActionT::Result::INVALID_INPUT, error_msg};
6869
}
6970

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

7678
command_x_ = command->target.x;
@@ -83,11 +85,12 @@ class DriveOnHeading : public TimedBehavior<ActionT>
8385
initial_pose_, *this->tf_, this->local_frame_, this->robot_base_frame_,
8486
this->transform_tolerance_))
8587
{
86-
RCLCPP_ERROR(this->logger_, "Initial robot pose is not available.");
87-
return ResultStatus{Status::FAILED, ActionT::Result::TF_ERROR};
88+
error_msg = "Initial robot pose is not available.";
89+
RCLCPP_ERROR(this->logger_, error_msg.c_str());
90+
return ResultStatus{Status::FAILED, ActionT::Result::TF_ERROR, error_msg};
8891
}
8992

90-
return ResultStatus{Status::SUCCEEDED, ActionT::Result::NONE};
93+
return ResultStatus{Status::SUCCEEDED, ActionT::Result::NONE, ""};
9194
}
9295

9396
/**
@@ -99,19 +102,20 @@ class DriveOnHeading : public TimedBehavior<ActionT>
99102
rclcpp::Duration time_remaining = end_time_ - this->clock_->now();
100103
if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) {
101104
this->stopRobot();
102-
RCLCPP_WARN(
103-
this->logger_,
104-
"Exceeded time allowance before reaching the DriveOnHeading goal - Exiting DriveOnHeading");
105-
return ResultStatus{Status::FAILED, ActionT::Result::NONE};
105+
std::string error_msg =
106+
"Exceeded time allowance before reaching the DriveOnHeading goal - Exiting DriveOnHeading";
107+
RCLCPP_WARN(this->logger_, error_msg.c_str());
108+
return ResultStatus{Status::FAILED, ActionT::Result::NONE, error_msg};
106109
}
107110

108111
geometry_msgs::msg::PoseStamped current_pose;
109112
if (!nav2_util::getCurrentPose(
110113
current_pose, *this->tf_, this->local_frame_, this->robot_base_frame_,
111114
this->transform_tolerance_))
112115
{
113-
RCLCPP_ERROR(this->logger_, "Current robot pose is not available.");
114-
return ResultStatus{Status::FAILED, ActionT::Result::TF_ERROR};
116+
std::string error_msg = "Current robot pose is not available.";
117+
RCLCPP_ERROR(this->logger_, error_msg.c_str());
118+
return ResultStatus{Status::FAILED, ActionT::Result::TF_ERROR, error_msg};
115119
}
116120

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

124128
if (distance >= std::fabs(command_x_)) {
125129
this->stopRobot();
126-
return ResultStatus{Status::SUCCEEDED, ActionT::Result::NONE};
130+
return ResultStatus{Status::SUCCEEDED, ActionT::Result::NONE, ""};
127131
}
128132

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

141145
if (!isCollisionFree(distance, cmd_vel->twist, pose2d)) {
142146
this->stopRobot();
143-
RCLCPP_WARN(this->logger_, "Collision Ahead - Exiting DriveOnHeading");
144-
return ResultStatus{Status::FAILED, ActionT::Result::COLLISION_AHEAD};
147+
std::string error_msg = "Collision Ahead - Exiting DriveOnHeading";
148+
RCLCPP_WARN(this->logger_, error_msg.c_str());
149+
return ResultStatus{Status::FAILED, ActionT::Result::COLLISION_AHEAD, error_msg};
145150
}
146151

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

149-
return ResultStatus{Status::RUNNING, ActionT::Result::NONE};
154+
return ResultStatus{Status::RUNNING, ActionT::Result::NONE, ""};
150155
}
151156

152157
/**

‎nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp

+6-4
Original file line numberDiff line numberDiff line change
@@ -53,6 +53,7 @@ struct ResultStatus
5353
{
5454
Status status;
5555
uint16_t error_code{0};
56+
std::string error_msg;
5657
};
5758

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

225226
ResultStatus on_run_result = onRun(action_server_->get_current_goal());
226227
if (on_run_result.status != Status::SUCCEEDED) {
227-
result->error_msg = "Initial checks failed for " + behavior_name_;
228-
RCLCPP_INFO(logger_, result->error_msg.c_str());
229228
result->error_code = on_run_result.error_code;
229+
result->error_msg = "Initial checks failed for " + behavior_name_ + " - " +
230+
on_run_result.error_msg;
231+
RCLCPP_INFO(logger_, result->error_msg.c_str());
230232
action_server_->terminate_current(result);
231233
return;
232234
}
@@ -270,10 +272,10 @@ class TimedBehavior : public nav2_core::Behavior
270272
return;
271273

272274
case Status::FAILED:
273-
result->error_msg = behavior_name_ + " failed";
275+
result->error_code = on_cycle_update_result.error_code;
276+
result->error_msg = behavior_name_ + " failed:" + on_cycle_update_result.error_msg;
274277
RCLCPP_WARN(logger_, result->error_msg.c_str());
275278
result->total_elapsed_time = clock_->now() - start_time;
276-
result->error_code = on_cycle_update_result.error_code;
277279
onActionCompletion(result);
278280
action_server_->terminate_current(result);
279281
return;

‎nav2_behaviors/plugins/assisted_teleop.cpp

+10-13
Original file line numberDiff line numberDiff line change
@@ -71,7 +71,7 @@ ResultStatus AssistedTeleop::onRun(const std::shared_ptr<const AssistedTeleopAct
7171
preempt_teleop_ = false;
7272
command_time_allowance_ = command->time_allowance;
7373
end_time_ = this->clock_->now() + command_time_allowance_;
74-
return ResultStatus{Status::SUCCEEDED, AssistedTeleopActionResult::NONE};
74+
return ResultStatus{Status::SUCCEEDED, AssistedTeleopActionResult::NONE, ""};
7575
}
7676

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

9897
// user states that teleop was successful
9998
if (preempt_teleop_) {
10099
stopRobot();
101-
return ResultStatus{Status::SUCCEEDED, AssistedTeleopActionResult::NONE};
100+
return ResultStatus{Status::SUCCEEDED, AssistedTeleopActionResult::NONE, ""};
102101
}
103102

104103
geometry_msgs::msg::PoseStamped current_pose;
105104
if (!nav2_util::getCurrentPose(
106105
current_pose, *tf_, local_frame_, robot_base_frame_,
107106
transform_tolerance_))
108107
{
109-
RCLCPP_ERROR_STREAM(
110-
logger_,
111-
"Current robot pose is not available for " <<
112-
behavior_name_.c_str());
113-
return ResultStatus{Status::FAILED, AssistedTeleopActionResult::TF_ERROR};
108+
std::string error_msg = "Current robot pose is not available for " + behavior_name_;
109+
RCLCPP_ERROR_STREAM(logger_, error_msg.c_str());
110+
return ResultStatus{Status::FAILED, AssistedTeleopActionResult::TF_ERROR, error_msg};
114111
}
115112

116113
geometry_msgs::msg::Pose2D projected_pose;
@@ -151,7 +148,7 @@ ResultStatus AssistedTeleop::onCycleUpdate()
151148
}
152149
vel_pub_->publish(std::move(scaled_twist));
153150

154-
return ResultStatus{Status::RUNNING, AssistedTeleopActionResult::NONE};
151+
return ResultStatus{Status::RUNNING, AssistedTeleopActionResult::NONE, ""};
155152
}
156153

157154
geometry_msgs::msg::Pose2D AssistedTeleop::projectPose(

‎nav2_behaviors/plugins/back_up.cpp

+7-7
Original file line numberDiff line numberDiff line change
@@ -20,10 +20,9 @@ namespace nav2_behaviors
2020
ResultStatus BackUp::onRun(const std::shared_ptr<const BackUpAction::Goal> command)
2121
{
2222
if (command->target.y != 0.0 || command->target.z != 0.0) {
23-
RCLCPP_INFO(
24-
logger_,
25-
"Backing up in Y and Z not supported, will only move in X.");
26-
return ResultStatus{Status::FAILED, BackUpActionResult::INVALID_INPUT};
23+
std::string error_msg = "Backing up in Y and Z not supported, will only move in X.";
24+
RCLCPP_INFO(logger_, error_msg.c_str());
25+
return ResultStatus{Status::FAILED, BackUpActionResult::INVALID_INPUT, error_msg};
2726
}
2827

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

44-
return ResultStatus{Status::SUCCEEDED, BackUpActionResult::NONE};
44+
return ResultStatus{Status::SUCCEEDED, BackUpActionResult::NONE, ""};
4545
}
4646

4747
} // namespace nav2_behaviors

‎nav2_behaviors/plugins/spin.cpp

+15-13
Original file line numberDiff line numberDiff line change
@@ -78,8 +78,9 @@ ResultStatus Spin::onRun(const std::shared_ptr<const SpinActionGoal> command)
7878
current_pose, *tf_, local_frame_, robot_base_frame_,
7979
transform_tolerance_))
8080
{
81-
RCLCPP_ERROR(logger_, "Current robot pose is not available.");
82-
return ResultStatus{Status::FAILED, SpinActionResult::TF_ERROR};
81+
std::string error_msg = "Current robot pose is not available.";
82+
RCLCPP_ERROR(logger_, error_msg.c_str());
83+
return ResultStatus{Status::FAILED, SpinActionResult::TF_ERROR, error_msg};
8384
}
8485

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

96-
return ResultStatus{Status::SUCCEEDED, SpinActionResult::NONE};
97+
return ResultStatus{Status::SUCCEEDED, SpinActionResult::NONE, ""};
9798
}
9899

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

110110
geometry_msgs::msg::PoseStamped current_pose;
111111
if (!nav2_util::getCurrentPose(
112112
current_pose, *tf_, local_frame_, robot_base_frame_,
113113
transform_tolerance_))
114114
{
115-
RCLCPP_ERROR(logger_, "Current robot pose is not available.");
116-
return ResultStatus{Status::FAILED, SpinActionResult::TF_ERROR};
115+
std::string error_msg = "Current robot pose is not available.";
116+
RCLCPP_ERROR(logger_, error_msg.c_str());
117+
return ResultStatus{Status::FAILED, SpinActionResult::TF_ERROR, error_msg};
117118
}
118119

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

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

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

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

159-
return ResultStatus{Status::RUNNING, SpinActionResult::NONE};
161+
return ResultStatus{Status::RUNNING, SpinActionResult::NONE, ""};
160162
}
161163

162164
bool Spin::isCollisionFree(

‎nav2_behaviors/plugins/wait.cpp

+3-3
Original file line numberDiff line numberDiff line change
@@ -31,7 +31,7 @@ Wait::~Wait() = default;
3131
ResultStatus Wait::onRun(const std::shared_ptr<const WaitAction::Goal> command)
3232
{
3333
wait_end_ = node_.lock()->now() + rclcpp::Duration(command->time);
34-
return ResultStatus{Status::SUCCEEDED};
34+
return ResultStatus{Status::SUCCEEDED, 0, ""};
3535
}
3636

3737
ResultStatus Wait::onCycleUpdate()
@@ -43,9 +43,9 @@ ResultStatus Wait::onCycleUpdate()
4343
action_server_->publish_feedback(feedback_);
4444

4545
if (time_left.nanoseconds() > 0) {
46-
return ResultStatus{Status::RUNNING};
46+
return ResultStatus{Status::RUNNING, 0, ""};
4747
} else {
48-
return ResultStatus{Status::SUCCEEDED};
48+
return ResultStatus{Status::SUCCEEDED, 0, ""};
4949
}
5050
}
5151

‎nav2_behaviors/test/test_behaviors.cpp

+5-5
Original file line numberDiff line numberDiff line change
@@ -57,10 +57,10 @@ class DummyBehavior : public TimedBehavior<BehaviorAction>
5757
// The output is defined by the tester class on the command string.
5858
if (command_ == "Testing success" || command_ == "Testing failure on run") {
5959
initialized_ = true;
60-
return ResultStatus{Status::SUCCEEDED, 0};
60+
return ResultStatus{Status::SUCCEEDED, 0, ""};
6161
}
6262

63-
return ResultStatus{Status::FAILED, 0};
63+
return ResultStatus{Status::FAILED, 0, "failed"};
6464
}
6565

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

7272
if (command_ != "Testing success" || !initialized_) {
73-
return ResultStatus{Status::FAILED, 0};
73+
return ResultStatus{Status::FAILED, 0, "failed"};
7474
}
7575

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

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

86-
return ResultStatus{Status::RUNNING, 0};
86+
return ResultStatus{Status::RUNNING, 0, ""};
8787
}
8888

8989
/**

0 commit comments

Comments
 (0)