fix - remove duplication of add_managed_entity from interface_factory

    - test fix for test_dock_database
    - change method name from 'activate'->'on_activate' majorly over nav2_route pkg
    - implement manual ordered service activation policy

Signed-off-by: Prathmesh Atkale <atkaleprathmesh@gmail.com>

Author: Prathmesh2931

nav2_amcl/src/amcl_node.cpp

@@ -167,6 +167,10 @@ AmclNode::on_activate(const rclcpp_lifecycle::State & /*state*/)
   // create bond connection
   createBond();
 
+  global_loc_srv_->on_activate();
+  initial_guess_srv_->on_activate();
+  nomotion_update_srv_->on_activate();
+
   return nav2::CallbackReturn::SUCCESS;
 }
 
@@ -175,6 +179,10 @@ AmclNode::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
 {
   RCLCPP_INFO(get_logger(), "Deactivating");
 
+  global_loc_srv_->on_deactivate();
+  initial_guess_srv_->on_deactivate();
+  nomotion_update_srv_->on_deactivate();
+
   active_ = false;
 
   // Lifecycle publishers must be explicitly deactivated

nav2_behavior_tree/include/nav2_behavior_tree/plugins/control/pause_resume_controller.hpp

@@ -91,6 +91,9 @@ class PauseResumeController : public BT::ControlNode
     const std::string & xml_tag_name,
     const BT::NodeConfiguration & conf);
 
+  //! @brief Destructor
+  ~PauseResumeController() override;
+
   //! @brief Reset state and go to Idle
   void halt() override;
 

nav2_behavior_tree/plugins/control/pause_resume_controller.cpp

@@ -61,6 +61,15 @@ PauseResumeController::PauseResumeController(
   resume_srv_->on_activate();
 }
 
+PauseResumeController::~PauseResumeController()
+{
+  pause_srv_->on_deactivate();
+  resume_srv_->on_deactivate();
+
+  pause_srv_.reset();
+  resume_srv_.reset();
+}
+
 BT::NodeStatus PauseResumeController::tick()
 {
   unsigned int children_count = children_nodes_.size();

nav2_collision_monitor/src/collision_monitor_node.cpp

@@ -114,7 +114,6 @@ nav2::CallbackReturn
 CollisionMonitor::on_activate(const rclcpp_lifecycle::State & /*state*/)
 {
   RCLCPP_INFO(get_logger(), "Activating");
-  toggle_cm_service_->on_activate();
 
   // Activating lifecycle publisher
   cmd_vel_out_pub_->on_activate();
@@ -135,6 +134,8 @@ CollisionMonitor::on_activate(const rclcpp_lifecycle::State & /*state*/)
   // Activating main worker
   process_active_ = true;
 
+  toggle_cm_service_->on_activate();
+
   // Creating bond connection
   createBond();
 

nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp

@@ -56,6 +56,16 @@ class ClearCostmapService
    */
   ~ClearCostmapService();
 
+  /**
+   * @brief Activate the clear costmap services
+   */
+  void on_activate();
+
+  /**
+   * @brief Deactivate the clear costmap services
+   */
+  void on_deactivate();
+
   /**
    * @brief Clears the region outside of a user-specified area reverting to the static map
    */

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp

@@ -95,13 +95,15 @@ class Costmap2DPublisher
     costmap_update_pub_->on_activate();
     costmap_raw_pub_->on_activate();
     costmap_raw_update_pub_->on_activate();
+    costmap_service_->on_activate();
   }
 
   /**
    * @brief deactivate node
    */
   void on_deactivate()
   {
+    costmap_service_->on_deactivate();
     costmap_pub_->on_deactivate();
     costmap_update_pub_->on_deactivate();
     costmap_raw_pub_->on_deactivate();

nav2_costmap_2d/src/clear_costmap_service.cpp

@@ -75,6 +75,22 @@ ClearCostmapService::~ClearCostmapService()
   clear_entire_service_.reset();
 }
 
+void ClearCostmapService::on_activate()
+{
+  clear_except_service_->on_activate();
+  clear_around_service_->on_activate();
+  clear_around_pose_service_->on_activate();
+  clear_entire_service_->on_activate();
+}
+
+void ClearCostmapService::on_deactivate()
+{
+  clear_except_service_->on_deactivate();
+  clear_around_service_->on_deactivate();
+  clear_around_pose_service_->on_deactivate();
+  clear_entire_service_->on_deactivate();
+}
+
 void ClearCostmapService::clearExceptRegionCallback(
   const shared_ptr<rmw_request_id_t>/*request_header*/,
   const shared_ptr<ClearExceptRegion::Request> request,

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -327,6 +327,7 @@ Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/)
     std::bind(&Costmap2DROS::validateParameterUpdatesCallback, this, _1));
 
   get_cost_service_->on_activate();
+  clear_costmap_service_->on_activate();
 
   return nav2::CallbackReturn::SUCCESS;
 }
@@ -336,6 +337,7 @@ Costmap2DROS::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
 {
   RCLCPP_INFO(get_logger(), "Deactivating");
   get_cost_service_->on_deactivate();
+  clear_costmap_service_->on_deactivate();
 
   remove_post_set_parameters_callback(post_set_params_handler_.get());
   post_set_params_handler_.reset();

nav2_docking/opennav_docking/src/dock_database.cpp

@@ -70,10 +70,12 @@ void DockDatabase::activate()
   for (it = dock_plugins_.begin(); it != dock_plugins_.end(); ++it) {
     it->second->activate();
   }
+  reload_db_service_->on_activate();
 }
 
 void DockDatabase::deactivate()
 {
+  reload_db_service_->on_deactivate();
   DockPluginMap::iterator it;
   for (it = dock_plugins_.begin(); it != dock_plugins_.end(); ++it) {
     it->second->deactivate();

nav2_docking/opennav_docking/test/test_dock_database.cpp

@@ -57,9 +57,14 @@ TEST(DatabaseTests, ObjectLifecycle)
   auto node = std::make_shared<nav2::LifecycleNode>("test");
   std::mutex mutex;
   opennav_docking::DockDatabase db(mutex);
-  db.initialize(node, nullptr);
-  db.activate();
-  db.deactivate();
+  bool initialized = db.initialize(node, nullptr);
+  EXPECT_FALSE(initialized);
+
+  //  ONLY activate if initialization was successful
+  if (initialized) {
+    db.activate();
+    db.deactivate();
+  }
 
   EXPECT_EQ(db.plugin_size(), 0u);
   EXPECT_EQ(db.instance_size(), 0u);
@@ -151,6 +156,7 @@ TEST(DatabaseTests, reloadDbService)
   db.initialize(node, nullptr);
   node->configure();
   node->activate();
+  db.activate();
 
   // Call service with a filepath
   auto client =