One by one activation for controller groups (#11)

* One by one activation for controller groups

* reformatting

* Fixed Tests

* Reformatting

* Removed unnecessary debug output

* updated pre-commits

Author: Joschi3

.pre-commit-config.yaml

@@ -15,7 +15,7 @@
 repos:
   # Standard hooks
   - repo: https://github.com/pre-commit/pre-commit-hooks
-    rev: v5.0.0
+    rev: v6.0.0
     hooks:
       - id: check-added-large-files
       - id: check-docstring-first
@@ -31,7 +31,7 @@ repos:
 
   # CPP Checks
   - repo: https://github.com/pre-commit/mirrors-clang-format
-    rev: v20.1.4
+    rev: v21.1.6
     hooks:
       - id: clang-format
         name: Clang Format
@@ -49,7 +49,7 @@ repos:
 
   # Python hooks
   - repo: https://github.com/astral-sh/ruff-pre-commit
-    rev: v0.12.7
+    rev: v0.14.5
     hooks:
       - id: ruff-check
         args: [ --fix ]
@@ -64,19 +64,19 @@ repos:
 
   # Package XML
   - repo: https://github.com/Joschi3/package_xml_validation.git
-    rev: v1.2.0
+    rev: v1.2.8
     hooks:
       - id: format-package-xml
         name: Validate and Format package.xml
 
   # Spellcheck
   - repo: https://github.com/crate-ci/typos
-    rev: v1.34.0
+    rev: v1
     hooks:
       - id: typos
 
   - repo: https://github.com/python-jsonschema/check-jsonschema
-    rev: 0.33.2
+    rev: 0.35.0
     hooks:
       - id: check-github-workflows
         args: ["--verbose"]

hector_controller_spawner/include/hector_controller_spawner/hector_controller_spawner.hpp

@@ -72,6 +72,8 @@ class MultiSpawner final : public rclcpp::Node
                             std::unordered_map<std::string, std::string> &current_state );
   bool ensureControllerState( bool desired_state,
                               const std::unordered_map<std::string, std::string> &current_state );
+  bool loadControllerGroup( const std::vector<std::string> &to_activate,
+                            const std::vector<std::string> &to_deactivate );
   bool switchControllersRequest( const std::vector<std::string> &to_activate,
                                  const std::vector<std::string> &to_deactivate );
 
@@ -80,9 +82,13 @@ class MultiSpawner final : public rclcpp::Node
   std::vector<std::string> controllers_;
   std::unordered_map<std::string, ControllerCfg> controller_cfg_;
   std::vector<ControllerGroup> controller_groups_;
+  std::map<std::string, std::vector<std::string>>
+      chained_connections_; // controller name → controllers that depend on it
   double retry_delay_{ 5.0 };
+  double start_delay_{ 0.0 };
   std::string estop_topic_;
   bool restart_after_estop_deactivation_{ false };
+  bool load_groups_one_by_one_{ true };
 
   std::atomic<bool> in_progress_{ false };
   std::atomic<bool> done_{ false };

hector_controller_spawner/src/hector_controller_spawner.cpp

@@ -15,9 +15,11 @@ void MultiSpawner::initialize()
   controllers_ =
       this->declare_parameter<std::vector<std::string>>( "controllers", std::vector<std::string>() );
   retry_delay_ = this->declare_parameter<double>( "retry_delay", 5.0 );
+  start_delay_ = this->declare_parameter<double>( "start_delay", 0.0 );
   estop_topic_ = this->declare_parameter<std::string>( "estop_topic", "" );
   restart_after_estop_deactivation_ =
       this->declare_parameter<bool>( "restart_after_estop_deactivation", true );
+  load_groups_one_by_one_ = this->declare_parameter<bool>( "load_groups_one_by_one", true );
 
   for ( const auto &ctrl : controllers_ ) {
     ControllerCfg cfg;
@@ -39,6 +41,13 @@ void MultiSpawner::initialize()
   ss << "  estop_topic: '" << estop_topic_ << "'\n";
   RCLCPP_DEBUG( get_logger(), "%s", ss.str().c_str() );
 
+  if ( start_delay_ > 0.0 ) {
+    RCLCPP_INFO( get_logger(), "Delaying start sequence by %.1f seconds...", start_delay_ );
+    const auto delay = std::chrono::duration_cast<std::chrono::nanoseconds>(
+        std::chrono::duration<double>( start_delay_ ) );
+    rclcpp::sleep_for( delay );
+  }
+
   // 2) Create service clients
   set_hw_state_client_ = this->create_client<controller_manager_msgs::srv::SetHardwareComponentState>(
       "controller_manager/set_hardware_component_state" );
@@ -273,8 +282,8 @@ bool MultiSpawner::ensureControllerState(
     }
 
     /* Issue one switch_controller call for this group. */
-    if ( ( group_requested_active && !switchControllersRequest( group, /*deactivate*/ {} ) ) ||
-         ( !group_requested_active && !switchControllersRequest( {}, group ) ) ) {
+    if ( ( group_requested_active && !loadControllerGroup( group, /*deactivate*/ {} ) ) ||
+         ( !group_requested_active && !loadControllerGroup( {}, group ) ) ) {
       RCLCPP_ERROR( get_logger(), "Failed to %s controller group containing '%s'",
                     group_requested_active ? "activate" : "deactivate",
                     vecToString( group ).c_str() );
@@ -289,14 +298,40 @@ bool MultiSpawner::ensureControllerState(
   return success;
 }
 
+bool MultiSpawner::loadControllerGroup( const std::vector<std::string> &to_activate,
+                                        const std::vector<std::string> &to_deactivate )
+{
+  if ( load_groups_one_by_one_ ) {
+    // deactivate in reverse order
+    for ( auto it = to_deactivate.rbegin(); it != to_deactivate.rend(); ++it ) {
+      const auto &ctrl = *it;
+      if ( !switchControllersRequest( {}, { ctrl } ) ) {
+        RCLCPP_ERROR( get_logger(), "Deactivated controller: %s", ctrl.c_str() );
+        return false;
+      }
+      RCLCPP_DEBUG( get_logger(), "Deactivated controller: %s", ctrl.c_str() );
+    }
+    // activate in order (in respect to normal dependencies)
+    for ( const auto &ctrl : to_activate ) {
+      if ( !switchControllersRequest( { ctrl }, {} ) ) {
+        RCLCPP_ERROR( get_logger(), "Failed to activate controller '%s'", ctrl.c_str() );
+        return false;
+      }
+      RCLCPP_DEBUG( get_logger(), "Activated controller: %s", ctrl.c_str() );
+    }
+    return true;
+  }
+  return switchControllersRequest( to_activate, to_deactivate );
+}
+
 bool MultiSpawner::switchControllersRequest( const std::vector<std::string> &to_activate,
                                              const std::vector<std::string> &to_deactivate )
 {
   if ( !switch_ctrl_client_->wait_for_service( 2s ) ) {
     RCLCPP_ERROR( get_logger(), "switch_controller service unavailable" );
     return false;
   }
-  auto req = std::make_shared<controller_manager_msgs::srv::SwitchController::Request>();
+  const auto req = std::make_shared<controller_manager_msgs::srv::SwitchController::Request>();
   req->activate_controllers = to_activate;
   req->deactivate_controllers = to_deactivate;
   req->strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT;
@@ -317,7 +352,9 @@ void MultiSpawner::parseControllerInfo(
 
   // —— auto-detect chained controllers groups ——
   for ( const auto &c : resp.controller ) {
+    chained_connections_.erase( c.name );
     for ( const auto &conn : c.chain_connections ) {
+      chained_connections_[c.name].push_back( conn.name );
       // check if there is a ControllerGroup that includes name or conn.name
       bool found = false;
       for ( auto &group : controller_groups_ ) {
@@ -365,6 +402,30 @@ void MultiSpawner::parseControllerInfo(
                     c.name.c_str(), c.state.c_str() );
     }
   }
+  // add recursive chained connections
+  for ( auto &group : controller_groups_ ) {
+    bool changed = true;
+    while ( changed ) {
+      changed = false;
+      for ( const auto &ctrl : group ) {
+        for ( const auto &dependent_ctrl : chained_connections_[ctrl] ) {
+          for ( const auto &dep_dep_ctr : chained_connections_[dependent_ctrl] ) {
+            if ( std::find( chained_connections_[ctrl].begin(), chained_connections_[ctrl].end(),
+                            dep_dep_ctr ) == chained_connections_[ctrl].end() ) {
+              chained_connections_[ctrl].push_back( dep_dep_ctr );
+              changed = true;
+            }
+          }
+        }
+      }
+    }
+    // sort members in group by number of dependent controllers (descending)
+    std::sort( group.begin(), group.end(), [this]( const std::string &a, const std::string &b ) {
+      const size_t size_a = chained_connections_.count( a ) ? chained_connections_[a].size() : 0;
+      const size_t size_b = chained_connections_.count( b ) ? chained_connections_[b].size() : 0;
+      return size_a < size_b;
+    } );
+  }
 }
 
 bool MultiSpawner::loadAndActivateHardware( const std::string &name )

hector_controller_spawner/test/test_spawner_estop.test.py

@@ -116,7 +116,8 @@ def test_complete_estop_workflow(self):
         self.assertEqual(
             len(active_hardware),
             0,
-            "No hardware interfaces should be active while e-stop is active",
+            "No hardware interfaces should be active while e-stop is active. Active hardware: "
+            + ", ".join(active_hardware),
         )
 
         # Verify no controllers are active

hector_ros_controllers/package.xml

@@ -3,14 +3,11 @@
   <name>hector_ros_controllers</name>
   <version>1.0.0</version>
   <description>Various custom ros2 controllers</description>
-
   <maintainer email="[email protected]">Marek Daniv</maintainer>
-
   <license>Apache License 2.0</license>
-
   <url type="website">https://control.ros.org</url>
-  <url type="bugtracker">https://github.com/ros-controls/ros2_controllers/issues</url>
   <url type="repository">https://github.com/ros-controls/ros2_controllers/</url>
+  <url type="bugtracker">https://github.com/ros-controls/ros2_controllers/issues</url>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 

hector_ros_controllers/src/collision_checker.cpp

@@ -194,7 +194,6 @@ bool CollisionChecker::checkCollisionQ( const Eigen::VectorXd &q )
   for ( std::size_t k = 0; k < geom_model_.collisionPairs.size(); ++k ) {
     auto &dreq = geom_data_.distanceRequests[k];
     dreq.enable_nearest_points = true;
-    dreq.gjk_initial_guess = hpp::fcl::GJKInitialGuess::CachedGuess;
   }
 
   // Distance pass (fills distanceResults + caches)
@@ -275,6 +274,11 @@ void CollisionChecker::publishMarkers() const
     if ( dres.min_distance <= 0.0 ) {
       add_unique( cp.first );
       add_unique( cp.second );
+      RCLCPP_WARN(
+          node_->get_logger(),
+          "Collision detected between objects %zu and %zu (distance=%.6f), names '%s' - '%s'",
+          cp.first, cp.second, dres.min_distance, geom_model_.geometryObjects[cp.first].name.c_str(),
+          geom_model_.geometryObjects[cp.second].name.c_str() );
     }
   }
 
@@ -368,6 +372,18 @@ void CollisionChecker::publishMarkers() const
     for ( std::size_t k = 0; k < geom_model_.collisionPairs.size(); ++k ) {
       const auto &dres = geom_data_.distanceResults[k];
 
+      // check if nearest points are valid (no nans or infs)
+      if ( dres.nearest_points[0].hasNaN() || dres.nearest_points[1].hasNaN() ||
+           !dres.nearest_points[0].allFinite() || !dres.nearest_points[1].allFinite() ) {
+        RCLCPP_WARN_STREAM(
+            node_->get_logger(),
+            "Skipping invalid nearest points for pair "
+                << k << " (distance=" << dres.min_distance << "names "
+                << geom_model_.geometryObjects[geom_model_.collisionPairs[k].first].name << " - "
+                << geom_model_.geometryObjects[geom_model_.collisionPairs[k].second].name << ")" );
+        continue;
+      }
+
       // dres.nearest_points[0] and [1] should be in the base_link frame (after placements)
       geometry_msgs::msg::Point pA, pB;
       pA.x = dres.nearest_points[0][0];

hector_ros_controllers/src/safety_position_controller.cpp

@@ -241,9 +241,14 @@ SafetyPositionController::update_and_write_commands( const rclcpp::Time &, const
       // write commands if no collision detected
       write_position_commands( cmd_positions_ );
     } else {
-      // write current positions to hold position in case of collision
-      RCLCPP_WARN_THROTTLE( get_node()->get_logger(), *get_node()->get_clock(),
-                            throttle_logging_msg, "Collision detected! Holding current positions." );
+      if ( !success_cc_setup )
+        RCLCPP_WARN_THROTTLE( get_node()->get_logger(), *get_node()->get_clock(),
+                              throttle_logging_msg, "Failed to setup collision checking." );
+      else
+        // write current positions to hold position in case of collision
+        RCLCPP_WARN_THROTTLE( get_node()->get_logger(), *get_node()->get_clock(),
+                              throttle_logging_msg,
+                              "Collision detected! Holding current positions." );
       write_position_commands( current_positions_ );
       // success = false; // make sure parent controllers are unloaded
     }