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.

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

fix: handle transition failures in all servers #4708

Merged
merged 7 commits into from
Oct 8, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
3 changes: 2 additions & 1 deletion nav2_behaviors/src/behavior_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -78,7 +78,7 @@
}

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

Expand All @@ -91,6 +91,7 @@

behavior_types_.resize(behavior_ids_.size());
if (!loadBehaviorPlugins()) {
on_cleanup(state);

Check warning on line 94 in nav2_behaviors/src/behavior_server.cpp

View check run for this annotation

Codecov / codecov/patch

nav2_behaviors/src/behavior_server.cpp#L94

Added line #L94 was not covered by tests
return nav2_util::CallbackReturn::FAILURE;
}
setupResourcesForBehaviorPlugins();
Expand Down
6 changes: 4 additions & 2 deletions nav2_bt_navigator/src/bt_navigator.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,7 @@
}

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

Expand Down Expand Up @@ -133,6 +133,7 @@
RCLCPP_FATAL(
get_logger(), "Failed to create navigator id %s."
" Exception: %s", navigator_ids[i].c_str(), ex.what());
on_cleanup(state);

Check warning on line 136 in nav2_bt_navigator/src/bt_navigator.cpp

View check run for this annotation

Codecov / codecov/patch

nav2_bt_navigator/src/bt_navigator.cpp#L136

Added line #L136 was not covered by tests
return nav2_util::CallbackReturn::FAILURE;
}
}
Expand All @@ -141,11 +142,12 @@
}

nav2_util::CallbackReturn
BtNavigator::on_activate(const rclcpp_lifecycle::State & /*state*/)
BtNavigator::on_activate(const rclcpp_lifecycle::State & state)
{
RCLCPP_INFO(get_logger(), "Activating");
for (size_t i = 0; i != navigators_.size(); i++) {
if (!navigators_[i]->on_activate()) {
on_deactivate(state);

Check warning on line 150 in nav2_bt_navigator/src/bt_navigator.cpp

View check run for this annotation

Codecov / codecov/patch

nav2_bt_navigator/src/bt_navigator.cpp#L150

Added line #L150 was not covered by tests
return nav2_util::CallbackReturn::FAILURE;
}
}
Expand Down
3 changes: 2 additions & 1 deletion nav2_collision_monitor/src/collision_detector_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@ CollisionDetector::~CollisionDetector()
}

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

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

// Obtaining ROS parameters
if (!getParameters()) {
on_cleanup(state);
return nav2_util::CallbackReturn::FAILURE;
}

Expand Down
4 changes: 3 additions & 1 deletion nav2_collision_monitor/src/collision_monitor_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@
}

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

Expand All @@ -60,6 +60,7 @@

// Obtaining ROS parameters
if (!getParameters(cmd_vel_in_topic, cmd_vel_out_topic, state_topic)) {
on_cleanup(state);
return nav2_util::CallbackReturn::FAILURE;
}

Expand Down Expand Up @@ -90,6 +91,7 @@
nav2_util::setSoftRealTimePriority();
} catch (const std::runtime_error & e) {
RCLCPP_ERROR(get_logger(), "%s", e.what());
on_cleanup(state);

Check warning on line 94 in nav2_collision_monitor/src/collision_monitor_node.cpp

View check run for this annotation

Codecov / codecov/patch

nav2_collision_monitor/src/collision_monitor_node.cpp#L94

Added line #L94 was not covered by tests
return nav2_util::CallbackReturn::FAILURE;
}
}
Expand Down
6 changes: 5 additions & 1 deletion nav2_controller/src/controller_server.cpp
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
return nav2_util::CallbackReturn::FAILURE;
}
}
Expand Down Expand Up @@ -242,6 +245,7 @@
true /*spin thread*/, server_options, use_realtime_priority_ /*soft realtime*/);
} catch (const std::runtime_error & e) {
RCLCPP_ERROR(get_logger(), "Error creating action server! %s", e.what());
on_cleanup(state);

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

View check run for this annotation

Codecov / codecov/patch

nav2_controller/src/controller_server.cpp#L248

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

Expand Down
3 changes: 2 additions & 1 deletion nav2_docking/opennav_docking/src/docking_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ DockingServer::DockingServer(const rclcpp::NodeOptions & options)
}

nav2_util::CallbackReturn
DockingServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
DockingServer::on_configure(const rclcpp_lifecycle::State & state)
{
RCLCPP_INFO(get_logger(), "Configuring %s", get_name());
auto node = shared_from_this();
Expand Down Expand Up @@ -90,6 +90,7 @@ DockingServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
navigator_ = std::make_unique<Navigator>(node);
dock_db_ = std::make_unique<DockDatabase>(mutex_);
if (!dock_db_->initialize(node, tf2_buffer_)) {
on_cleanup(state);
return nav2_util::CallbackReturn::FAILURE;
}

Expand Down
50 changes: 49 additions & 1 deletion nav2_docking/opennav_docking/test/test_docking_server_unit.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -175,22 +175,65 @@ TEST(DockingServerTests, testErrorExceptions)
EXPECT_TRUE(false);
}
}
node->on_deactivate(rclcpp_lifecycle::State());
node->on_cleanup(rclcpp_lifecycle::State());
node->on_shutdown(rclcpp_lifecycle::State());
node.reset();
}

TEST(DockingServerTests, getateGoalDock)
{
auto node = std::make_shared<opennav_docking::DockingServer>();

// Setup 1 instance of the test failure dock & its plugin instance
node->declare_parameter(
"docks",
rclcpp::ParameterValue(std::vector<std::string>{"test_dock"}));
node->declare_parameter(
"test_dock.type",
rclcpp::ParameterValue(std::string{"dock_plugin"}));
node->declare_parameter(
"test_dock.pose",
rclcpp::ParameterValue(std::vector<double>{0.0, 0.0, 0.0}));
node->declare_parameter(
"dock_plugins",
rclcpp::ParameterValue(std::vector<std::string>{"dock_plugin"}));
node->declare_parameter(
"dock_plugin.plugin",
rclcpp::ParameterValue(std::string{"opennav_docking::TestFailureDock"}));

node->on_configure(rclcpp_lifecycle::State());
std::shared_ptr<const DockRobot::Goal> goal = std::make_shared<const DockRobot::Goal>();
auto dock = node->generateGoalDock(goal);
EXPECT_EQ(dock->plugin, nullptr);
EXPECT_NE(dock->plugin, nullptr);
EXPECT_EQ(dock->frame, std::string());
node->stashDockData(false, dock, true);
node->on_cleanup(rclcpp_lifecycle::State());
node->on_shutdown(rclcpp_lifecycle::State());
node.reset();
}

TEST(DockingServerTests, testDynamicParams)
{
auto node = std::make_shared<opennav_docking::DockingServer>();

// Setup 1 instance of the test failure dock & its plugin instance
node->declare_parameter(
"docks",
rclcpp::ParameterValue(std::vector<std::string>{"test_dock"}));
node->declare_parameter(
"test_dock.type",
rclcpp::ParameterValue(std::string{"dock_plugin"}));
node->declare_parameter(
"test_dock.pose",
rclcpp::ParameterValue(std::vector<double>{0.0, 0.0, 0.0}));
node->declare_parameter(
"dock_plugins",
rclcpp::ParameterValue(std::vector<std::string>{"dock_plugin"}));
node->declare_parameter(
"dock_plugin.plugin",
rclcpp::ParameterValue(std::string{"opennav_docking::TestFailureDock"}));

node->on_configure(rclcpp_lifecycle::State());
node->on_activate(rclcpp_lifecycle::State());

Expand Down Expand Up @@ -221,6 +264,11 @@ TEST(DockingServerTests, testDynamicParams)
EXPECT_EQ(node->get_parameter("base_frame").as_string(), std::string("hi"));
EXPECT_EQ(node->get_parameter("fixed_frame").as_string(), std::string("hi"));
EXPECT_EQ(node->get_parameter("max_retries").as_int(), 7);

node->on_deactivate(rclcpp_lifecycle::State());
node->on_cleanup(rclcpp_lifecycle::State());
node->on_shutdown(rclcpp_lifecycle::State());
node.reset();
}

} // namespace opennav_docking
3 changes: 2 additions & 1 deletion nav2_planner/src/planner_server.cpp
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
5 changes: 3 additions & 2 deletions nav2_smoother/src/nav2_smoother.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,7 @@ SmootherServer::~SmootherServer()
}

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 @@ SmootherServer::on_configure(const rclcpp_lifecycle::State &)
*costmap_sub_, *footprint_sub_, this->get_name());

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

Expand Down Expand Up @@ -162,7 +163,7 @@ bool SmootherServer::loadSmootherPlugins()
}

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

Expand Down
49 changes: 34 additions & 15 deletions nav2_velocity_smoother/src/velocity_smoother.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,7 @@
}

nav2_util::CallbackReturn
VelocitySmoother::on_configure(const rclcpp_lifecycle::State &)
VelocitySmoother::on_configure(const rclcpp_lifecycle::State & state)
{
RCLCPP_INFO(get_logger(), "Configuring velocity smoother");
auto node = shared_from_this();
Expand Down Expand Up @@ -76,24 +76,35 @@

for (unsigned int i = 0; i != 3; i++) {
if (max_decels_[i] > 0.0) {
throw std::runtime_error(
"Positive values set of deceleration! These should be negative to slow down!");
RCLCPP_ERROR(
get_logger(),
"Positive values set of deceleration! These should be negative to slow down!");
on_cleanup(state);
return nav2_util::CallbackReturn::FAILURE;
}
if (max_accels_[i] < 0.0) {
throw std::runtime_error(
"Negative values set of acceleration! These should be positive to speed up!");
RCLCPP_ERROR(
get_logger(),
"Negative values set of acceleration! These should be positive to speed up!");
on_cleanup(state);
return nav2_util::CallbackReturn::FAILURE;
}
if (min_velocities_[i] > 0.0) {
throw std::runtime_error(
"Positive values set of min_velocities! These should be negative!");
RCLCPP_ERROR(
get_logger(), "Positive values set of min_velocities! These should be negative!");
on_cleanup(state);
return nav2_util::CallbackReturn::FAILURE;
}
if (max_velocities_[i] < 0.0) {
throw std::runtime_error(
"Negative values set of max_velocities! These should be positive!");
RCLCPP_ERROR(
get_logger(), "Negative values set of max_velocities! These should be positive!");
on_cleanup(state);
return nav2_util::CallbackReturn::FAILURE;
}
if (min_velocities_[i] > max_velocities_[i]) {
throw std::runtime_error(
"Min velocities are higher than max velocities!");
RCLCPP_ERROR(get_logger(), "Min velocities are higher than max velocities!");
on_cleanup(state);
return nav2_util::CallbackReturn::FAILURE;
}
}

Expand All @@ -112,9 +123,12 @@
if (max_velocities_.size() != 3 || min_velocities_.size() != 3 ||
max_accels_.size() != 3 || max_decels_.size() != 3 || deadband_velocities_.size() != 3)
{
throw std::runtime_error(
"Invalid setting of kinematic and/or deadband limits!"
" All limits must be size of 3 representing (x, y, theta).");
RCLCPP_ERROR(
get_logger(),
"Invalid setting of kinematic and/or deadband limits!"
" All limits must be size of 3 representing (x, y, theta).");
on_cleanup(state);
return nav2_util::CallbackReturn::FAILURE;
}

// Get control type
Expand All @@ -124,7 +138,11 @@
open_loop_ = false;
odom_smoother_ = std::make_unique<nav2_util::OdomSmoother>(node, odom_duration_, odom_topic_);
} else {
throw std::runtime_error("Invalid feedback_type, options are OPEN_LOOP and CLOSED_LOOP.");
RCLCPP_ERROR(

Check warning on line 141 in nav2_velocity_smoother/src/velocity_smoother.cpp

View check run for this annotation

Codecov / codecov/patch

nav2_velocity_smoother/src/velocity_smoother.cpp#L141

Added line #L141 was not covered by tests
get_logger(),
"Invalid feedback_type, options are OPEN_LOOP and CLOSED_LOOP.");
on_cleanup(state);

Check warning on line 144 in nav2_velocity_smoother/src/velocity_smoother.cpp

View check run for this annotation

Codecov / codecov/patch

nav2_velocity_smoother/src/velocity_smoother.cpp#L144

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

// Setup inputs / outputs
Expand All @@ -144,6 +162,7 @@
nav2_util::setSoftRealTimePriority();
} catch (const std::runtime_error & e) {
RCLCPP_ERROR(get_logger(), "%s", e.what());
on_cleanup(state);

Check warning on line 165 in nav2_velocity_smoother/src/velocity_smoother.cpp

View check run for this annotation

Codecov / codecov/patch

nav2_velocity_smoother/src/velocity_smoother.cpp#L165

Added line #L165 was not covered by tests
return nav2_util::CallbackReturn::FAILURE;
}
}
Expand Down
Loading
Loading