Skip to content

Instantly share code, notes, and snippets.

Show Gist options
  • Save arshadlab/fe6321824ca8a25a4d69cf522f3d094f to your computer and use it in GitHub Desktop.
Save arshadlab/fe6321824ca8a25a4d69cf522f3d094f to your computer and use it in GitHub Desktop.
Fix for lifecycle cli status for global & local costmap nodes
diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp
index dd938c4d..e72ed3ef 100644
--- a/nav2_controller/src/nav2_controller.cpp
+++ b/nav2_controller/src/nav2_controller.cpp
@@ -67,7 +67,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();
@@ -102,7 +102,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
get_parameter("min_theta_velocity_threshold", min_theta_velocity_threshold_);
RCLCPP_INFO(get_logger(), "Controller frequency set to %.4fHz", controller_frequency_);
- costmap_ros_->on_configure(state);
+ costmap_ros_->configure();
try {
progress_checker_type_ = nav2_util::get_plugin_type_param(node, progress_checker_id_);
@@ -171,11 +171,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();
@@ -187,7 +187,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");
@@ -196,7 +196,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();
@@ -205,7 +205,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");
@@ -215,7 +215,7 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & state)
it->second->cleanup();
}
controllers_.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 48eede36..a7165871 100644
--- a/nav2_planner/src/planner_server.cpp
+++ b/nav2_planner/src/planner_server.cpp
@@ -64,11 +64,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(
@@ -139,13 +139,13 @@ 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_->activate();
- costmap_ros_->on_activate(state);
+ costmap_ros_->activate();
PlannerMap::iterator it;
for (it = planners_.begin(); it != planners_.end(); ++it) {
@@ -156,13 +156,13 @@ 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_->deactivate();
plan_publisher_->on_deactivate();
- costmap_ros_->on_deactivate(state);
+ costmap_ros_->deactivate();
PlannerMap::iterator it;
for (it = planners_.begin(); it != planners_.end(); ++it) {
@@ -173,14 +173,14 @@ 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");
action_server_.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) {
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment