Skip to content

Commit bc94198

Browse files
SteveMacenskiKemal Bektas
SteveMacenski
and
Kemal Bektas
committed
fix: handle transition failures in all servers (#4708)
* fix: handle transition failures in planner/controller/smoother servers Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> * adding support for rest of servers + review comments Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Replacing throws with error and failed lifecycle transitions Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fix vel smoother unit tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fixing docking server unit testing Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fixing last bits Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Kemal Bektas <kemal.bektas@node-robotics.com>
1 parent c536b1c commit bc94198

File tree

12 files changed

+117
-33
lines changed

12 files changed

+117
-33
lines changed

‎nav2_behaviors/src/behavior_server.cpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -78,7 +78,7 @@ BehaviorServer::~BehaviorServer()
7878
}
7979

8080
nav2_util::CallbackReturn
81-
BehaviorServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
81+
BehaviorServer::on_configure(const rclcpp_lifecycle::State & state)
8282
{
8383
RCLCPP_INFO(get_logger(), "Configuring");
8484

@@ -91,6 +91,7 @@ BehaviorServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
9191

9292
behavior_types_.resize(behavior_ids_.size());
9393
if (!loadBehaviorPlugins()) {
94+
on_cleanup(state);
9495
return nav2_util::CallbackReturn::FAILURE;
9596
}
9697
setupResourcesForBehaviorPlugins();

‎nav2_bt_navigator/src/bt_navigator.cpp‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -56,7 +56,7 @@ BtNavigator::~BtNavigator()
5656
}
5757

5858
nav2_util::CallbackReturn
59-
BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
59+
BtNavigator::on_configure(const rclcpp_lifecycle::State & state)
6060
{
6161
RCLCPP_INFO(get_logger(), "Configuring");
6262

@@ -133,6 +133,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
133133
RCLCPP_FATAL(
134134
get_logger(), "Failed to create navigator id %s."
135135
" Exception: %s", navigator_ids[i].c_str(), ex.what());
136+
on_cleanup(state);
136137
return nav2_util::CallbackReturn::FAILURE;
137138
}
138139
}
@@ -141,11 +142,12 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
141142
}
142143

143144
nav2_util::CallbackReturn
144-
BtNavigator::on_activate(const rclcpp_lifecycle::State & /*state*/)
145+
BtNavigator::on_activate(const rclcpp_lifecycle::State & state)
145146
{
146147
RCLCPP_INFO(get_logger(), "Activating");
147148
for (size_t i = 0; i != navigators_.size(); i++) {
148149
if (!navigators_[i]->on_activate()) {
150+
on_deactivate(state);
149151
return nav2_util::CallbackReturn::FAILURE;
150152
}
151153
}

‎nav2_collision_monitor/src/collision_detector_node.cpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -40,7 +40,7 @@ CollisionDetector::~CollisionDetector()
4040
}
4141

4242
nav2_util::CallbackReturn
43-
CollisionDetector::on_configure(const rclcpp_lifecycle::State & /*state*/)
43+
CollisionDetector::on_configure(const rclcpp_lifecycle::State & state)
4444
{
4545
RCLCPP_INFO(get_logger(), "Configuring");
4646

@@ -60,6 +60,7 @@ CollisionDetector::on_configure(const rclcpp_lifecycle::State & /*state*/)
6060

6161
// Obtaining ROS parameters
6262
if (!getParameters()) {
63+
on_cleanup(state);
6364
return nav2_util::CallbackReturn::FAILURE;
6465
}
6566

‎nav2_collision_monitor/src/collision_monitor_node.cpp‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -42,7 +42,7 @@ CollisionMonitor::~CollisionMonitor()
4242
}
4343

4444
nav2_util::CallbackReturn
45-
CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/)
45+
CollisionMonitor::on_configure(const rclcpp_lifecycle::State & state)
4646
{
4747
RCLCPP_INFO(get_logger(), "Configuring");
4848

@@ -60,6 +60,7 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/)
6060

6161
// Obtaining ROS parameters
6262
if (!getParameters(cmd_vel_in_topic, cmd_vel_out_topic, state_topic)) {
63+
on_cleanup(state);
6364
return nav2_util::CallbackReturn::FAILURE;
6465
}
6566

@@ -90,6 +91,7 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/)
9091
nav2_util::setSoftRealTimePriority();
9192
} catch (const std::runtime_error & e) {
9293
RCLCPP_ERROR(get_logger(), "%s", e.what());
94+
on_cleanup(state);
9395
return nav2_util::CallbackReturn::FAILURE;
9496
}
9597
}

‎nav2_controller/src/controller_server.cpp‎

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -82,7 +82,7 @@ ControllerServer::~ControllerServer()
8282
}
8383

8484
nav2_util::CallbackReturn
85-
ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
85+
ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
8686
{
8787
auto node = shared_from_this();
8888

@@ -151,6 +151,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
151151
RCLCPP_FATAL(
152152
get_logger(),
153153
"Failed to create progress_checker. Exception: %s", ex.what());
154+
on_cleanup(state);
154155
return nav2_util::CallbackReturn::FAILURE;
155156
}
156157
}
@@ -177,6 +178,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
177178
RCLCPP_FATAL(
178179
get_logger(),
179180
"Failed to create goal checker. Exception: %s", ex.what());
181+
on_cleanup(state);
180182
return nav2_util::CallbackReturn::FAILURE;
181183
}
182184
}
@@ -205,6 +207,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
205207
RCLCPP_FATAL(
206208
get_logger(),
207209
"Failed to create controller. Exception: %s", ex.what());
210+
on_cleanup(state);
208211
return nav2_util::CallbackReturn::FAILURE;
209212
}
210213
}
@@ -241,6 +244,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
241244
true /*spin thread*/, server_options, use_realtime_priority_ /*soft realtime*/);
242245
} catch (const std::runtime_error & e) {
243246
RCLCPP_ERROR(get_logger(), "Error creating action server! %s", e.what());
247+
on_cleanup(state);
244248
return nav2_util::CallbackReturn::FAILURE;
245249
}
246250

‎nav2_docking/opennav_docking/src/docking_server.cpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -43,7 +43,7 @@ DockingServer::DockingServer(const rclcpp::NodeOptions & options)
4343
}
4444

4545
nav2_util::CallbackReturn
46-
DockingServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
46+
DockingServer::on_configure(const rclcpp_lifecycle::State & state)
4747
{
4848
RCLCPP_INFO(get_logger(), "Configuring %s", get_name());
4949
auto node = shared_from_this();
@@ -90,6 +90,7 @@ DockingServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
9090
navigator_ = std::make_unique<Navigator>(node);
9191
dock_db_ = std::make_unique<DockDatabase>(mutex_);
9292
if (!dock_db_->initialize(node, tf2_buffer_)) {
93+
on_cleanup(state);
9394
return nav2_util::CallbackReturn::FAILURE;
9495
}
9596

‎nav2_docking/opennav_docking/test/test_docking_server_unit.cpp‎

Lines changed: 49 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -175,22 +175,65 @@ TEST(DockingServerTests, testErrorExceptions)
175175
EXPECT_TRUE(false);
176176
}
177177
}
178+
node->on_deactivate(rclcpp_lifecycle::State());
179+
node->on_cleanup(rclcpp_lifecycle::State());
180+
node->on_shutdown(rclcpp_lifecycle::State());
181+
node.reset();
178182
}
179183

180184
TEST(DockingServerTests, getateGoalDock)
181185
{
182186
auto node = std::make_shared<opennav_docking::DockingServer>();
187+
188+
// Setup 1 instance of the test failure dock & its plugin instance
189+
node->declare_parameter(
190+
"docks",
191+
rclcpp::ParameterValue(std::vector<std::string>{"test_dock"}));
192+
node->declare_parameter(
193+
"test_dock.type",
194+
rclcpp::ParameterValue(std::string{"dock_plugin"}));
195+
node->declare_parameter(
196+
"test_dock.pose",
197+
rclcpp::ParameterValue(std::vector<double>{0.0, 0.0, 0.0}));
198+
node->declare_parameter(
199+
"dock_plugins",
200+
rclcpp::ParameterValue(std::vector<std::string>{"dock_plugin"}));
201+
node->declare_parameter(
202+
"dock_plugin.plugin",
203+
rclcpp::ParameterValue(std::string{"opennav_docking::TestFailureDock"}));
204+
183205
node->on_configure(rclcpp_lifecycle::State());
184206
std::shared_ptr<const DockRobot::Goal> goal = std::make_shared<const DockRobot::Goal>();
185207
auto dock = node->generateGoalDock(goal);
186-
EXPECT_EQ(dock->plugin, nullptr);
208+
EXPECT_NE(dock->plugin, nullptr);
187209
EXPECT_EQ(dock->frame, std::string());
188210
node->stashDockData(false, dock, true);
211+
node->on_cleanup(rclcpp_lifecycle::State());
212+
node->on_shutdown(rclcpp_lifecycle::State());
213+
node.reset();
189214
}
190215

191216
TEST(DockingServerTests, testDynamicParams)
192217
{
193218
auto node = std::make_shared<opennav_docking::DockingServer>();
219+
220+
// Setup 1 instance of the test failure dock & its plugin instance
221+
node->declare_parameter(
222+
"docks",
223+
rclcpp::ParameterValue(std::vector<std::string>{"test_dock"}));
224+
node->declare_parameter(
225+
"test_dock.type",
226+
rclcpp::ParameterValue(std::string{"dock_plugin"}));
227+
node->declare_parameter(
228+
"test_dock.pose",
229+
rclcpp::ParameterValue(std::vector<double>{0.0, 0.0, 0.0}));
230+
node->declare_parameter(
231+
"dock_plugins",
232+
rclcpp::ParameterValue(std::vector<std::string>{"dock_plugin"}));
233+
node->declare_parameter(
234+
"dock_plugin.plugin",
235+
rclcpp::ParameterValue(std::string{"opennav_docking::TestFailureDock"}));
236+
194237
node->on_configure(rclcpp_lifecycle::State());
195238
node->on_activate(rclcpp_lifecycle::State());
196239

@@ -221,6 +264,11 @@ TEST(DockingServerTests, testDynamicParams)
221264
EXPECT_EQ(node->get_parameter("base_frame").as_string(), std::string("hi"));
222265
EXPECT_EQ(node->get_parameter("fixed_frame").as_string(), std::string("hi"));
223266
EXPECT_EQ(node->get_parameter("max_retries").as_int(), 7);
267+
268+
node->on_deactivate(rclcpp_lifecycle::State());
269+
node->on_cleanup(rclcpp_lifecycle::State());
270+
node->on_shutdown(rclcpp_lifecycle::State());
271+
node.reset();
224272
}
225273

226274
} // namespace opennav_docking

‎nav2_planner/src/planner_server.cpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -80,7 +80,7 @@ PlannerServer::~PlannerServer()
8080
}
8181

8282
nav2_util::CallbackReturn
83-
PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
83+
PlannerServer::on_configure(const rclcpp_lifecycle::State & state)
8484
{
8585
RCLCPP_INFO(get_logger(), "Configuring");
8686

@@ -121,6 +121,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
121121
RCLCPP_FATAL(
122122
get_logger(), "Failed to create global planner. Exception: %s",
123123
ex.what());
124+
on_cleanup(state);
124125
return nav2_util::CallbackReturn::FAILURE;
125126
}
126127
}

‎nav2_smoother/src/nav2_smoother.cpp‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -63,7 +63,7 @@ SmootherServer::~SmootherServer()
6363
}
6464

6565
nav2_util::CallbackReturn
66-
SmootherServer::on_configure(const rclcpp_lifecycle::State &)
66+
SmootherServer::on_configure(const rclcpp_lifecycle::State & state)
6767
{
6868
RCLCPP_INFO(get_logger(), "Configuring smoother server");
6969

@@ -100,6 +100,7 @@ SmootherServer::on_configure(const rclcpp_lifecycle::State &)
100100
*costmap_sub_, *footprint_sub_, this->get_name());
101101

102102
if (!loadSmootherPlugins()) {
103+
on_cleanup(state);
103104
return nav2_util::CallbackReturn::FAILURE;
104105
}
105106

@@ -162,7 +163,7 @@ bool SmootherServer::loadSmootherPlugins()
162163
}
163164

164165
nav2_util::CallbackReturn
165-
SmootherServer::on_activate(const rclcpp_lifecycle::State &)
166+
SmootherServer::on_activate(const rclcpp_lifecycle::State & /*state*/)
166167
{
167168
RCLCPP_INFO(get_logger(), "Activating");
168169

‎nav2_velocity_smoother/src/velocity_smoother.cpp‎

Lines changed: 34 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -44,7 +44,7 @@ VelocitySmoother::~VelocitySmoother()
4444
}
4545

4646
nav2_util::CallbackReturn
47-
VelocitySmoother::on_configure(const rclcpp_lifecycle::State &)
47+
VelocitySmoother::on_configure(const rclcpp_lifecycle::State & state)
4848
{
4949
RCLCPP_INFO(get_logger(), "Configuring velocity smoother");
5050
auto node = shared_from_this();
@@ -76,24 +76,35 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &)
7676

7777
for (unsigned int i = 0; i != 3; i++) {
7878
if (max_decels_[i] > 0.0) {
79-
throw std::runtime_error(
80-
"Positive values set of deceleration! These should be negative to slow down!");
79+
RCLCPP_ERROR(
80+
get_logger(),
81+
"Positive values set of deceleration! These should be negative to slow down!");
82+
on_cleanup(state);
83+
return nav2_util::CallbackReturn::FAILURE;
8184
}
8285
if (max_accels_[i] < 0.0) {
83-
throw std::runtime_error(
84-
"Negative values set of acceleration! These should be positive to speed up!");
86+
RCLCPP_ERROR(
87+
get_logger(),
88+
"Negative values set of acceleration! These should be positive to speed up!");
89+
on_cleanup(state);
90+
return nav2_util::CallbackReturn::FAILURE;
8591
}
8692
if (min_velocities_[i] > 0.0) {
87-
throw std::runtime_error(
88-
"Positive values set of min_velocities! These should be negative!");
93+
RCLCPP_ERROR(
94+
get_logger(), "Positive values set of min_velocities! These should be negative!");
95+
on_cleanup(state);
96+
return nav2_util::CallbackReturn::FAILURE;
8997
}
9098
if (max_velocities_[i] < 0.0) {
91-
throw std::runtime_error(
92-
"Negative values set of max_velocities! These should be positive!");
99+
RCLCPP_ERROR(
100+
get_logger(), "Negative values set of max_velocities! These should be positive!");
101+
on_cleanup(state);
102+
return nav2_util::CallbackReturn::FAILURE;
93103
}
94104
if (min_velocities_[i] > max_velocities_[i]) {
95-
throw std::runtime_error(
96-
"Min velocities are higher than max velocities!");
105+
RCLCPP_ERROR(get_logger(), "Min velocities are higher than max velocities!");
106+
on_cleanup(state);
107+
return nav2_util::CallbackReturn::FAILURE;
97108
}
98109
}
99110

@@ -112,9 +123,12 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &)
112123
if (max_velocities_.size() != 3 || min_velocities_.size() != 3 ||
113124
max_accels_.size() != 3 || max_decels_.size() != 3 || deadband_velocities_.size() != 3)
114125
{
115-
throw std::runtime_error(
116-
"Invalid setting of kinematic and/or deadband limits!"
117-
" All limits must be size of 3 representing (x, y, theta).");
126+
RCLCPP_ERROR(
127+
get_logger(),
128+
"Invalid setting of kinematic and/or deadband limits!"
129+
" All limits must be size of 3 representing (x, y, theta).");
130+
on_cleanup(state);
131+
return nav2_util::CallbackReturn::FAILURE;
118132
}
119133

120134
// Get control type
@@ -124,7 +138,11 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &)
124138
open_loop_ = false;
125139
odom_smoother_ = std::make_unique<nav2_util::OdomSmoother>(node, odom_duration_, odom_topic_);
126140
} else {
127-
throw std::runtime_error("Invalid feedback_type, options are OPEN_LOOP and CLOSED_LOOP.");
141+
RCLCPP_ERROR(
142+
get_logger(),
143+
"Invalid feedback_type, options are OPEN_LOOP and CLOSED_LOOP.");
144+
on_cleanup(state);
145+
return nav2_util::CallbackReturn::FAILURE;
128146
}
129147

130148
// Setup inputs / outputs
@@ -144,6 +162,7 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &)
144162
nav2_util::setSoftRealTimePriority();
145163
} catch (const std::runtime_error & e) {
146164
RCLCPP_ERROR(get_logger(), "%s", e.what());
165+
on_cleanup(state);
147166
return nav2_util::CallbackReturn::FAILURE;
148167
}
149168
}

0 commit comments

Comments
 (0)