From 0ae7c0886fec33da4ed2c26bd54f2358894727f5 Mon Sep 17 00:00:00 2001 From: Kemal Bektas Date: Mon, 30 Sep 2024 15:30:24 +0200 Subject: [PATCH 1/6] fix: handle transition failures in planner/controller/smoother servers Signed-off-by: Kemal Bektas --- nav2_controller/src/controller_server.cpp | 16 ++++++++++++++-- nav2_planner/src/planner_server.cpp | 15 ++++++++++++--- nav2_smoother/src/nav2_smoother.cpp | 15 ++++++++++++--- 3 files changed, 38 insertions(+), 8 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index ec9dc3d81b8..ea9c87fdaca 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -82,7 +82,7 @@ ControllerServer::~ControllerServer() } nav2_util::CallbackReturn -ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +ControllerServer::on_configure(const rclcpp_lifecycle::State & state) { auto node = shared_from_this(); @@ -152,6 +152,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_FATAL( get_logger(), "Failed to create progress_checker. Exception: %s", ex.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } @@ -178,6 +179,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_FATAL( get_logger(), "Failed to create goal checker. Exception: %s", ex.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } @@ -206,6 +208,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_FATAL( get_logger(), "Failed to create controller. Exception: %s", ex.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } @@ -254,7 +257,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) } nav2_util::CallbackReturn -ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) +ControllerServer::on_activate(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Activating"); @@ -265,6 +268,15 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) ControllerMap::iterator it; for (it = controllers_.begin(); it != controllers_.end(); ++it) { it->second->activate(); + try { + it->second->activate(); + } catch (const std::exception & ex) { + RCLCPP_FATAL( + get_logger(), "Failed to activate controller. Exception: %s", + ex.what()); + on_deactivate(state); + return nav2_util::CallbackReturn::FAILURE; + } } vel_publisher_->on_activate(); action_server_->activate(); diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index bb59b0b0235..1afb4131fb2 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -79,7 +79,7 @@ PlannerServer::~PlannerServer() } nav2_util::CallbackReturn -PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +PlannerServer::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring"); @@ -120,6 +120,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_FATAL( get_logger(), "Failed to create global planner. Exception: %s", ex.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } @@ -177,7 +178,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) } nav2_util::CallbackReturn -PlannerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) +PlannerServer::on_activate(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Activating"); @@ -191,7 +192,15 @@ PlannerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) PlannerMap::iterator it; for (it = planners_.begin(); it != planners_.end(); ++it) { - it->second->activate(); + try { + it->second->activate(); + } catch (const std::exception & ex) { + RCLCPP_FATAL( + get_logger(), "Failed to activate global planner. Exception: %s", + ex.what()); + on_deactivate(state); + return nav2_util::CallbackReturn::FAILURE; + } } auto node = shared_from_this(); diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index e7baa5e4839..fac5e296d1d 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -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"); @@ -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; } @@ -162,14 +163,22 @@ 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"); plan_publisher_->on_activate(); SmootherMap::iterator it; for (it = smoothers_.begin(); it != smoothers_.end(); ++it) { - it->second->activate(); + try { + it->second->activate(); + } catch (const std::exception & ex) { + RCLCPP_FATAL( + get_logger(), "Failed to activate smoother. Exception: %s", + ex.what()); + on_deactivate(state); + return nav2_util::CallbackReturn::FAILURE; + } } action_server_->activate(); From 5e27266f34ffbe2c01f0f70b070c8a753df83142 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 4 Oct 2024 16:04:47 -0700 Subject: [PATCH 2/6] adding support for rest of servers + review comments Signed-off-by: Steve Macenski --- nav2_behaviors/src/behavior_server.cpp | 3 ++- nav2_bt_navigator/src/bt_navigator.cpp | 6 ++++-- .../src/collision_detector_node.cpp | 3 ++- .../src/collision_monitor_node.cpp | 4 +++- nav2_controller/src/controller_server.cpp | 12 ++---------- nav2_docking/opennav_docking/src/docking_server.cpp | 3 ++- nav2_planner/src/planner_server.cpp | 12 ++---------- nav2_smoother/src/nav2_smoother.cpp | 12 ++---------- nav2_velocity_smoother/src/velocity_smoother.cpp | 3 ++- nav2_waypoint_follower/src/waypoint_follower.cpp | 3 ++- 10 files changed, 23 insertions(+), 38 deletions(-) diff --git a/nav2_behaviors/src/behavior_server.cpp b/nav2_behaviors/src/behavior_server.cpp index 1017bf595a3..a04aea40315 100644 --- a/nav2_behaviors/src/behavior_server.cpp +++ b/nav2_behaviors/src/behavior_server.cpp @@ -78,7 +78,7 @@ BehaviorServer::~BehaviorServer() } nav2_util::CallbackReturn -BehaviorServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +BehaviorServer::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring"); @@ -91,6 +91,7 @@ BehaviorServer::on_configure(const rclcpp_lifecycle::State & /*state*/) behavior_types_.resize(behavior_ids_.size()); if (!loadBehaviorPlugins()) { + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } setupResourcesForBehaviorPlugins(); diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 9ae64016743..c49fae0e2b9 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -56,7 +56,7 @@ BtNavigator::~BtNavigator() } nav2_util::CallbackReturn -BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) +BtNavigator::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring"); @@ -133,6 +133,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_FATAL( get_logger(), "Failed to create navigator id %s." " Exception: %s", navigator_ids[i].c_str(), ex.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } @@ -141,11 +142,12 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) } 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); return nav2_util::CallbackReturn::FAILURE; } } diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index d8e03e19ed4..7f87bc1bb92 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -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"); @@ -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; } diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index c0bd15cb63a..2b0991c9941 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -42,7 +42,7 @@ CollisionMonitor::~CollisionMonitor() } nav2_util::CallbackReturn -CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) +CollisionMonitor::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring"); @@ -60,6 +60,7 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) // Obtaining ROS parameters if (!getParameters(cmd_vel_in_topic, cmd_vel_out_topic, state_topic)) { + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } @@ -90,6 +91,7 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) nav2_util::setSoftRealTimePriority(); } catch (const std::runtime_error & e) { RCLCPP_ERROR(get_logger(), "%s", e.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index ea9c87fdaca..c221c2db17b 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -245,6 +245,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) 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); return nav2_util::CallbackReturn::FAILURE; } @@ -257,7 +258,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) } nav2_util::CallbackReturn -ControllerServer::on_activate(const rclcpp_lifecycle::State & state) +ControllerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); @@ -268,15 +269,6 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & state) ControllerMap::iterator it; for (it = controllers_.begin(); it != controllers_.end(); ++it) { it->second->activate(); - try { - it->second->activate(); - } catch (const std::exception & ex) { - RCLCPP_FATAL( - get_logger(), "Failed to activate controller. Exception: %s", - ex.what()); - on_deactivate(state); - return nav2_util::CallbackReturn::FAILURE; - } } vel_publisher_->on_activate(); action_server_->activate(); diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp index 0d5034bd60c..b350cb40262 100644 --- a/nav2_docking/opennav_docking/src/docking_server.cpp +++ b/nav2_docking/opennav_docking/src/docking_server.cpp @@ -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(); @@ -90,6 +90,7 @@ DockingServer::on_configure(const rclcpp_lifecycle::State & /*state*/) navigator_ = std::make_unique(node); dock_db_ = std::make_unique(mutex_); if (!dock_db_->initialize(node, tf2_buffer_)) { + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 1afb4131fb2..7233a7ba53d 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -178,7 +178,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state) } nav2_util::CallbackReturn -PlannerServer::on_activate(const rclcpp_lifecycle::State & state) +PlannerServer::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); @@ -192,15 +192,7 @@ PlannerServer::on_activate(const rclcpp_lifecycle::State & state) PlannerMap::iterator it; for (it = planners_.begin(); it != planners_.end(); ++it) { - try { - it->second->activate(); - } catch (const std::exception & ex) { - RCLCPP_FATAL( - get_logger(), "Failed to activate global planner. Exception: %s", - ex.what()); - on_deactivate(state); - return nav2_util::CallbackReturn::FAILURE; - } + it->second->activate(); } auto node = shared_from_this(); diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index fac5e296d1d..fe462cb1c6b 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -163,22 +163,14 @@ bool SmootherServer::loadSmootherPlugins() } nav2_util::CallbackReturn -SmootherServer::on_activate(const rclcpp_lifecycle::State & 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) { - try { - it->second->activate(); - } catch (const std::exception & ex) { - RCLCPP_FATAL( - get_logger(), "Failed to activate smoother. Exception: %s", - ex.what()); - on_deactivate(state); - return nav2_util::CallbackReturn::FAILURE; - } + it->second->activate(); } action_server_->activate(); diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index 535b6b08f21..fc18c41a8e9 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -44,7 +44,7 @@ VelocitySmoother::~VelocitySmoother() } 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(); @@ -144,6 +144,7 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &) nav2_util::setSoftRealTimePriority(); } catch (const std::runtime_error & e) { RCLCPP_ERROR(get_logger(), "%s", e.what()); + on_cleanup(state); return nav2_util::CallbackReturn::FAILURE; } } diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 02a4158d280..0014e79a15d 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -54,7 +54,7 @@ WaypointFollower::~WaypointFollower() } nav2_util::CallbackReturn -WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) +WaypointFollower::on_configure(const rclcpp_lifecycle::State & state) { RCLCPP_INFO(get_logger(), "Configuring"); @@ -123,6 +123,7 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_FATAL( get_logger(), "Failed to create waypoint_task_executor. Exception: %s", e.what()); + on_cleanup(state); } return nav2_util::CallbackReturn::SUCCESS; From 98e2092e1ec1f7afc49bfb73797a6caf89480d01 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 4 Oct 2024 16:18:00 -0700 Subject: [PATCH 3/6] Replacing throws with error and failed lifecycle transitions Signed-off-by: Steve Macenski --- .../src/velocity_smoother.cpp | 46 +++++++++++++------ 1 file changed, 32 insertions(+), 14 deletions(-) diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index fc18c41a8e9..9519ab94b61 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -76,24 +76,35 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State & state) 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; } } @@ -112,9 +123,12 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State & state) 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 @@ -124,7 +138,11 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State & state) open_loop_ = false; odom_smoother_ = std::make_unique(node, odom_duration_, odom_topic_); } else { - throw std::runtime_error("Invalid feedback_type, options are OPEN_LOOP and CLOSED_LOOP."); + RCLCPP_ERROR( + get_logger(), + "Invalid feedback_type, options are OPEN_LOOP and CLOSED_LOOP."); + on_cleanup(state); + return nav2_util::CallbackReturn::FAILURE; } // Setup inputs / outputs From 559334f145ae2bdc6d5428a35966aa206bed5f97 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 7 Oct 2024 12:48:09 -0700 Subject: [PATCH 4/6] fix vel smoother unit tests Signed-off-by: Steve Macenski --- .../test/test_velocity_smoother.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/nav2_velocity_smoother/test/test_velocity_smoother.cpp b/nav2_velocity_smoother/test/test_velocity_smoother.cpp index 25754d0f355..76c5e0498b6 100644 --- a/nav2_velocity_smoother/test/test_velocity_smoother.cpp +++ b/nav2_velocity_smoother/test/test_velocity_smoother.cpp @@ -40,7 +40,10 @@ class VelSmootherShim : public nav2_velocity_smoother::VelocitySmoother public: VelSmootherShim() : VelocitySmoother() {} - void configure(const rclcpp_lifecycle::State & state) {this->on_configure(state);} + nav2_util::CallbackReturn configure(const rclcpp_lifecycle::State & state) + { + return this->on_configure(state); + } void activate(const rclcpp_lifecycle::State & state) {this->on_activate(state);} void deactivate(const rclcpp_lifecycle::State & state) {this->on_deactivate(state);} void cleanup(const rclcpp_lifecycle::State & state) {this->on_cleanup(state);} @@ -594,10 +597,10 @@ TEST(VelocitySmootherTest, testInvalidParams) std::vector max_vels{0.0, 0.0}; // invalid size smoother->declare_parameter("max_velocity", rclcpp::ParameterValue(max_vels)); rclcpp_lifecycle::State state; - EXPECT_THROW(smoother->configure(state), std::runtime_error); + EXPECT_EQ(smoother->configure(state), nav2_util::CallbackReturn::FAILURE); smoother->set_parameter(rclcpp::Parameter("feedback", std::string("LAWLS"))); - EXPECT_THROW(smoother->configure(state), std::runtime_error); + EXPECT_EQ(smoother->configure(state), nav2_util::CallbackReturn::FAILURE); } TEST(VelocitySmootherTest, testInvalidParamsAccelDecel) @@ -613,13 +616,13 @@ TEST(VelocitySmootherTest, testInvalidParamsAccelDecel) smoother->declare_parameter("max_velocity", rclcpp::ParameterValue(bad_test_max_vel)); smoother->declare_parameter("min_velocity", rclcpp::ParameterValue(bad_test_min_vel)); rclcpp_lifecycle::State state; - EXPECT_THROW(smoother->configure(state), std::runtime_error); + EXPECT_EQ(smoother->configure(state), nav2_util::CallbackReturn::FAILURE); smoother->set_parameter(rclcpp::Parameter("max_accel", rclcpp::ParameterValue(bad_test_accel))); - EXPECT_THROW(smoother->configure(state), std::runtime_error); + EXPECT_EQ(smoother->configure(state), nav2_util::CallbackReturn::FAILURE); smoother->set_parameter(rclcpp::Parameter("max_decel", rclcpp::ParameterValue(bad_test_decel))); - EXPECT_THROW(smoother->configure(state), std::runtime_error); + EXPECT_EQ(smoother->configure(state), nav2_util::CallbackReturn::FAILURE); } TEST(VelocitySmootherTest, testDynamicParameter) From 99f7b98e12543a641aebd13f98aacbb426d086fe Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 7 Oct 2024 12:59:50 -0700 Subject: [PATCH 5/6] fixing docking server unit testing Signed-off-by: Steve Macenski --- .../test/test_docking_server_unit.cpp | 42 ++++++++++++++++++- 1 file changed, 41 insertions(+), 1 deletion(-) diff --git a/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp b/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp index 144a612f3bd..8775a4debf4 100644 --- a/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp +++ b/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp @@ -180,10 +180,28 @@ TEST(DockingServerTests, testErrorExceptions) TEST(DockingServerTests, getateGoalDock) { auto node = std::make_shared(); + + // Setup 1 instance of the test failure dock & its plugin instance + node->declare_parameter( + "docks", + rclcpp::ParameterValue(std::vector{"test_dock"})); + node->declare_parameter( + "test_dock.type", + rclcpp::ParameterValue(std::string{"dock_plugin"})); + node->declare_parameter( + "test_dock.pose", + rclcpp::ParameterValue(std::vector{0.0, 0.0, 0.0})); + node->declare_parameter( + "dock_plugins", + rclcpp::ParameterValue(std::vector{"dock_plugin"})); + node->declare_parameter( + "dock_plugin.plugin", + rclcpp::ParameterValue(std::string{"opennav_docking::TestFailureDock"})); + node->on_configure(rclcpp_lifecycle::State()); std::shared_ptr goal = std::make_shared(); 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); } @@ -191,6 +209,24 @@ TEST(DockingServerTests, getateGoalDock) TEST(DockingServerTests, testDynamicParams) { auto node = std::make_shared(); + + // Setup 1 instance of the test failure dock & its plugin instance + node->declare_parameter( + "docks", + rclcpp::ParameterValue(std::vector{"test_dock"})); + node->declare_parameter( + "test_dock.type", + rclcpp::ParameterValue(std::string{"dock_plugin"})); + node->declare_parameter( + "test_dock.pose", + rclcpp::ParameterValue(std::vector{0.0, 0.0, 0.0})); + node->declare_parameter( + "dock_plugins", + rclcpp::ParameterValue(std::vector{"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()); @@ -221,6 +257,10 @@ 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.reset(); } } // namespace opennav_docking From 780e709ece7268037aca79fbf226f6e84ea58375 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 7 Oct 2024 13:16:35 -0700 Subject: [PATCH 6/6] fixing last bits Signed-off-by: Steve Macenski --- .../opennav_docking/test/test_docking_server_unit.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp b/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp index 8775a4debf4..f12741774c8 100644 --- a/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp +++ b/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp @@ -175,6 +175,10 @@ 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) @@ -204,6 +208,9 @@ TEST(DockingServerTests, getateGoalDock) 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) @@ -260,6 +267,7 @@ TEST(DockingServerTests, testDynamicParams) node->on_deactivate(rclcpp_lifecycle::State()); node->on_cleanup(rclcpp_lifecycle::State()); + node->on_shutdown(rclcpp_lifecycle::State()); node.reset(); }