From c311377b148dfbe1dfef7e617417113124392ba7 Mon Sep 17 00:00:00 2001 From: Arshad Date: Wed, 8 Jun 2022 15:06:55 +0800 Subject: [PATCH] Fix for costmap nodes lifecycle status Lifecycle status for global and local cost nodes not correct. ros2 lifecycle/service commands shows unconfigured for these two. This is due to directly calling on_configure/on_activate/on_cleanup calls in parent node. This PR to replace on_xxxxxx() to configure()/activate()/cleanup() calls of lifecycle base. Signed-off-by: Arshad --- nav2_controller/src/controller_server.cpp | 16 ++++++++-------- nav2_planner/src/planner_server.cpp | 16 ++++++++-------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 711cb96e0c..3f4e71cc1c 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -77,7 +77,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(); @@ -122,7 +122,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) get_parameter("speed_limit_topic", speed_limit_topic); get_parameter("failure_tolerance", failure_tolerance_); - costmap_ros_->on_configure(state); + costmap_ros_->configure(); try { progress_checker_type_ = nav2_util::get_plugin_type_param(node, progress_checker_id_); @@ -213,11 +213,11 @@ 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"); - costmap_ros_->on_activate(state); + costmap_ros_->activate(); ControllerMap::iterator it; for (it = controllers_.begin(); it != controllers_.end(); ++it) { it->second->activate(); @@ -237,7 +237,7 @@ ControllerServer::on_activate(const rclcpp_lifecycle::State & state) } nav2_util::CallbackReturn -ControllerServer::on_deactivate(const rclcpp_lifecycle::State & state) +ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Deactivating"); @@ -246,7 +246,7 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & state) for (it = controllers_.begin(); it != controllers_.end(); ++it) { it->second->deactivate(); } - costmap_ros_->on_deactivate(state); + costmap_ros_->deactivate(); publishZeroVelocity(); vel_publisher_->on_deactivate(); @@ -259,7 +259,7 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & state) } nav2_util::CallbackReturn -ControllerServer::on_cleanup(const rclcpp_lifecycle::State & state) +ControllerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); @@ -271,7 +271,7 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & state) controllers_.clear(); goal_checkers_.clear(); - costmap_ros_->on_cleanup(state); + costmap_ros_->cleanup(); // Release any allocated resources action_server_.reset(); diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index e9efb627a4..9abc461d30 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -74,11 +74,11 @@ 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"); - costmap_ros_->on_configure(state); + costmap_ros_->configure(); costmap_ = costmap_ros_->getCostmap(); RCLCPP_DEBUG( @@ -154,14 +154,14 @@ 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"); plan_publisher_->on_activate(); action_server_pose_->activate(); action_server_poses_->activate(); - costmap_ros_->on_activate(state); + costmap_ros_->activate(); PlannerMap::iterator it; for (it = planners_.begin(); it != planners_.end(); ++it) { @@ -187,14 +187,14 @@ PlannerServer::on_activate(const rclcpp_lifecycle::State & state) } nav2_util::CallbackReturn -PlannerServer::on_deactivate(const rclcpp_lifecycle::State & state) +PlannerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Deactivating"); action_server_pose_->deactivate(); action_server_poses_->deactivate(); plan_publisher_->on_deactivate(); - costmap_ros_->on_deactivate(state); + costmap_ros_->deactivate(); PlannerMap::iterator it; for (it = planners_.begin(); it != planners_.end(); ++it) { @@ -210,7 +210,7 @@ PlannerServer::on_deactivate(const rclcpp_lifecycle::State & state) } nav2_util::CallbackReturn -PlannerServer::on_cleanup(const rclcpp_lifecycle::State & state) +PlannerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); @@ -218,7 +218,7 @@ PlannerServer::on_cleanup(const rclcpp_lifecycle::State & state) action_server_poses_.reset(); plan_publisher_.reset(); tf_.reset(); - costmap_ros_->on_cleanup(state); + costmap_ros_->cleanup(); PlannerMap::iterator it; for (it = planners_.begin(); it != planners_.end(); ++it) {