Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

fix: handle transition failures in planner/controller/smoother servers #4697

Closed
wants to merge 1 commit into from
+38 −8
Closed

fix: handle transition failures in planner/controller/smoother servers #4697

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 14 additions & 2 deletions nav2_controller/src/controller_server.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -82,7 +82,7 @@
}

nav2_util::CallbackReturn
ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
{
auto node = shared_from_this();

Expand Down Expand Up @@ -152,6 +152,7 @@
RCLCPP_FATAL(
get_logger(),
"Failed to create progress_checker. Exception: %s", ex.what());
on_cleanup(state);

Check warning on line 155 in nav2_controller/src/controller_server.cpp

View check run for this annotation

Codecov / codecov/patch

nav2_controller/src/controller_server.cpp#L155 

Added line #L155 was not covered by tests
return nav2_util::CallbackReturn::FAILURE;
}
}
Expand All @@ -178,6 +179,7 @@
RCLCPP_FATAL(
get_logger(),
"Failed to create goal checker. Exception: %s", ex.what());
on_cleanup(state);

Check warning on line 182 in nav2_controller/src/controller_server.cpp

View check run for this annotation

Codecov / codecov/patch

nav2_controller/src/controller_server.cpp#L182 

Added line #L182 was not covered by tests
return nav2_util::CallbackReturn::FAILURE;
}
}
Expand Down Expand Up @@ -206,6 +208,7 @@
RCLCPP_FATAL(
get_logger(),
"Failed to create controller. Exception: %s", ex.what());
on_cleanup(state);

Check warning on line 211 in nav2_controller/src/controller_server.cpp

View check run for this annotation

Codecov / codecov/patch

nav2_controller/src/controller_server.cpp#L211 

Added line #L211 was not covered by tests
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
return nav2_util::CallbackReturn::FAILURE;
}
}
Expand Down Expand Up @@ -254,7 +257,7 @@
}

nav2_util::CallbackReturn
ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/)
ControllerServer::on_activate(const rclcpp_lifecycle::State & state)
{
RCLCPP_INFO(get_logger(), "Activating");

Expand All @@ -265,6 +268,15 @@
ControllerMap::iterator it;
for (it = controllers_.begin(); it != controllers_.end(); ++it) {
it->second->activate();
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
try {
it->second->activate();
} catch (const std::exception & ex) {
RCLCPP_FATAL(

Check warning on line 274 in nav2_controller/src/controller_server.cpp

View check run for this annotation

Codecov / codecov/patch

nav2_controller/src/controller_server.cpp#L273-L274 

Added lines #L273 - L274 were not covered by tests
get_logger(), "Failed to activate controller. Exception: %s",
ex.what());
on_deactivate(state);

Check warning on line 277 in nav2_controller/src/controller_server.cpp

View check run for this annotation

Codecov / codecov/patch

nav2_controller/src/controller_server.cpp#L277 

Added line #L277 was not covered by tests
return nav2_util::CallbackReturn::FAILURE;
}
}
vel_publisher_->on_activate();
action_server_->activate();
Expand Down
15 changes: 12 additions & 3 deletions nav2_planner/src/planner_server.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -79,7 +79,7 @@
}

nav2_util::CallbackReturn
PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
PlannerServer::on_configure(const rclcpp_lifecycle::State & state)
{
RCLCPP_INFO(get_logger(), "Configuring");

Expand Down Expand Up @@ -120,6 +120,7 @@
RCLCPP_FATAL(
get_logger(), "Failed to create global planner. Exception: %s",
ex.what());
on_cleanup(state);

Check warning on line 123 in nav2_planner/src/planner_server.cpp

View check run for this annotation

Codecov / codecov/patch

nav2_planner/src/planner_server.cpp#L123 

Added line #L123 was not covered by tests
return nav2_util::CallbackReturn::FAILURE;
}
}
Expand Down Expand Up @@ -177,7 +178,7 @@
}

nav2_util::CallbackReturn
PlannerServer::on_activate(const rclcpp_lifecycle::State & /*state*/)
PlannerServer::on_activate(const rclcpp_lifecycle::State & state)
{
RCLCPP_INFO(get_logger(), "Activating");

Expand All @@ -191,7 +192,15 @@

PlannerMap::iterator it;
for (it = planners_.begin(); it != planners_.end(); ++it) {
it->second->activate();
try {
it->second->activate();
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
} catch (const std::exception & ex) {
RCLCPP_FATAL(

Check warning on line 198 in nav2_planner/src/planner_server.cpp

View check run for this annotation

Codecov / codecov/patch

nav2_planner/src/planner_server.cpp#L197-L198 

Added lines #L197 - L198 were not covered by tests
get_logger(), "Failed to activate global planner. Exception: %s",
ex.what());
on_deactivate(state);

Check warning on line 201 in nav2_planner/src/planner_server.cpp

View check run for this annotation

Codecov / codecov/patch

nav2_planner/src/planner_server.cpp#L201 

Added line #L201 was not covered by tests
return nav2_util::CallbackReturn::FAILURE;
}
}

auto node = shared_from_this();
Expand Down
15 changes: 12 additions & 3 deletions nav2_smoother/src/nav2_smoother.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,7 @@
}

nav2_util::CallbackReturn
SmootherServer::on_configure(const rclcpp_lifecycle::State &)
SmootherServer::on_configure(const rclcpp_lifecycle::State & state)
{
RCLCPP_INFO(get_logger(), "Configuring smoother server");

Expand Down Expand Up @@ -100,6 +100,7 @@
*costmap_sub_, *footprint_sub_, this->get_name());

if (!loadSmootherPlugins()) {
on_cleanup(state);
return nav2_util::CallbackReturn::FAILURE;
}

Expand Down Expand Up @@ -162,14 +163,22 @@
}

nav2_util::CallbackReturn
SmootherServer::on_activate(const rclcpp_lifecycle::State &)
SmootherServer::on_activate(const rclcpp_lifecycle::State & state)
{
RCLCPP_INFO(get_logger(), "Activating");

plan_publisher_->on_activate();
SmootherMap::iterator it;
for (it = smoothers_.begin(); it != smoothers_.end(); ++it) {
it->second->activate();
try {
it->second->activate();
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
} catch (const std::exception & ex) {
RCLCPP_FATAL(

Check warning on line 176 in nav2_smoother/src/nav2_smoother.cpp

View check run for this annotation

Codecov / codecov/patch

nav2_smoother/src/nav2_smoother.cpp#L175-L176 

Added lines #L175 - L176 were not covered by tests
get_logger(), "Failed to activate smoother. Exception: %s",
ex.what());
on_deactivate(state);

Check warning on line 179 in nav2_smoother/src/nav2_smoother.cpp

View check run for this annotation

Codecov / codecov/patch

nav2_smoother/src/nav2_smoother.cpp#L179 

Added line #L179 was not covered by tests
return nav2_util::CallbackReturn::FAILURE;
}
}
action_server_->activate();

Expand Down
Loading