fixed drive controller switcher activation

Author: duttaishan01

.codex

@@ -2,6 +2,7 @@ build/
 install/
 log/
 .vscode/
+.codex
 
 # Byte-compiled / optimized / DLL files
 __pycache__/

.gitignore

@@ -40,7 +40,7 @@ def __init__(self):
         self.client_cb_group = MutuallyExclusiveCallbackGroup()
 
         # Controllers to always keep active or ignore
-        self.always_active = ["joint_state_broadcaster", "motor_status_broadcaster"]
+        self.always_active = ["joint_state_broadcaster", "motor_status_broadcaster", "led_gpio_controller", "killswitch_gpio_controller", "laser_gpio_controller"]
         self.ignore_controllers = []
 
         # Lock to prevent concurrent service processing
@@ -117,11 +117,14 @@ def _handle_set_controller(self, request, response):
             # Extract controller names and their states
             all_controllers = []
             active_controllers = []
+            valid_always_active = []
             for c in list_result.controller:
                 all_controllers.append(c.name)
                 if c.state == 'active':
                     active_controllers.append(c.name)
 
+            valid_always_active = [c for c in self.always_active if c in all_controllers]
+            
             self.get_logger().info(f"Available controllers: {all_controllers}")
             self.get_logger().info(f"Currently active: {active_controllers}")
 
@@ -282,10 +285,10 @@ def controller_handler(controller, subsystem_controllers):
             if switch_result and switch_result.ok:
                 response.success = True
                 if requested_controllers:
-                    active_list = requested_controllers + self.always_active
+                    active_list = requested_controllers + valid_always_active
                     response.message = f"Successfully activated controllers: {requested_controllers}. Active: {active_list}"
                 else:
-                    response.message = f"Successfully deactivated all controllers except: {self.always_active + self.ignore_controllers}"
+                    response.message = f"Successfully deactivated all controllers except: {valid_always_active + self.ignore_controllers}"
                 self.get_logger().info(f"Success! {response.message}")
             else:
                 response.success = False

src/bringup/scripts/controller_switcher.py

@@ -9,6 +9,30 @@
         <param name="can">can0</param>
       </hardware>
 
+      <!-- Doesn't exist right now -->
+      <joint name="steer_fl_joint">
+        <param name="node_id">0</param>
+        <param name="gear_ratio">26</param>
+        <command_interface name="position"/>
+        <command_interface name="velocity"/>
+        <state_interface name="position">
+          <param name="initial_value">0.0</param>
+        </state_interface>
+        <state_interface name="velocity"/>
+      </joint>
+      
+      <!-- Doesn't exist right now -->
+      <joint name="steer_fr_joint">
+        <param name="node_id">1</param>
+        <param name="gear_ratio">26</param>
+        <command_interface name="position"/>
+        <command_interface name="velocity"/>
+        <state_interface name="position">
+          <param name="initial_value">0.0</param>
+        </state_interface>
+        <state_interface name="velocity"/>
+      </joint>
+
       <joint name="steer_bl_joint">
         <param name="node_id">7</param>
         <param name="gear_ratio">26</param>

src/description/ros2_control/drive/drive.odrive.ros2_control.xacro

@@ -8,7 +8,7 @@
   <li><b>athena_arm_controllers</b>: contains athena arm manual controller</li>
   <li><b>athena_arm_description</b>: contains the full urdf file for the arm including ros2_control urdfs</li>
   <li><b>athena_arm_moveit</b>: contains athena arm moveit setup</li>
-  <li><b>athena_arm_msgs</b>: contains messages used for athena mainly for logging</li>
+  <li><b>msgs</b>: contains messages used for athena mainly for logging</li>
   <li><b>manual_control</b>: testing setup to control each motor on the arm with a PS4 controller. We now have a controller in the ros2_control setup that does the same thing</li>
   <li><b>ros_odrive</b>: contains node and ros2_control hardware interface for ODrives.</li>
   <li><b>smc_ros2_control</b>: contains ros2_control hardware interface for SMC motors</li>

src/subsystems/arm/README.md

@@ -90,7 +90,7 @@ def generate_launch_description():
     )
 
     controller_switcher_config = PathJoinSubstitution(
-        [FindPackageShare("bringup"), "config", "controller_switcher.yaml"]
+        [FindPackageShare(runtime_config_package), "config", "controller_switcher.yaml"]
     )
 
     # -- Additional Configuration Setup --

src/subsystems/arm/arm_bringup/launch/athena_arm.jetson.launch.py

@@ -74,6 +74,6 @@ ros2 launch drive_bringup athena_drive.launch.py mode:=base_station
 **Open another terminal, source the workspace, and call the service to set controllers:**
 ```bash
 source install/setup.bash
-ros2 service call /set_controller athena_drive_msgs/srv/SetController "{controller_names: [INCLUDE CONTROLLER(S) YOU WANT WITHIN BRACKETS]}"
+ros2 service call /set_controller msgs/srv/SetController "{controller_names: [INCLUDE CONTROLLER(S) YOU WANT WITHIN BRACKETS]}"
 ```
-Example: `ros2 service call /set_controller athena_drive_msgs/srv/SetController "{controller_names: [drive_velocity_controller]}"`
+Example: `ros2 service call /set_controller msgs/srv/SetController "{controller_names: [drive_velocity_controller]}"`

src/subsystems/drive/README.md

@@ -148,7 +148,7 @@ def launch_setup(context, *args, **kwargs):
     )
 
     controller_switcher_config = PathJoinSubstitution(
-        [FindPackageShare("bringup"), "config", "controller_switcher.yaml"]
+        [FindPackageShare(runtime_config_package), "config", "controller_switcher.yaml"]
     )
 
     # -- Additional Configuration Setup --

src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py

@@ -145,7 +145,7 @@ def generate_launch_description():
     )
 
     controller_switcher_config = PathJoinSubstitution(
-        [FindPackageShare("bringup"), "config", "controller_switcher.yaml"]
+        [FindPackageShare(runtime_config_package), "config", "controller_switcher.yaml"]
     )
 
     joystick_config_file = PathJoinSubstitution(