From 5e27266f34ffbe2c01f0f70b070c8a753df83142 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 4 Oct 2024 16:04:47 -0700 Subject: [PATCH] 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;