1+ #!/usr/bin/env python3
2+ import rclpy
3+ from rclpy .node import Node
4+ from rclpy .callback_groups import ReentrantCallbackGroup , MutuallyExclusiveCallbackGroup
5+ from rclpy .executors import MultiThreadedExecutor
6+ from rclpy .duration import Duration
7+ import threading
8+
9+ from msgs .srv import SetController
10+ from controller_manager_msgs .srv import SwitchController , ListControllers
11+
12+
13+ class ControllerSwitcher (Node ):
14+ def __init__ (self ):
15+ super ().__init__ ('controller_switcher' )
16+
17+ # Create separate callback groups
18+ self .service_cb_group = ReentrantCallbackGroup ()
19+ self .client_cb_group = MutuallyExclusiveCallbackGroup ()
20+
21+ # Controllers to always keep active or ignore
22+ self .always_active = ["joint_state_broadcaster" ]
23+ self .ignore_controllers = []
24+
25+ # Lock to prevent concurrent service processing
26+ self .service_lock = threading .Lock ()
27+
28+ # Create service with callback group
29+ self .srv = self .create_service (
30+ SetController ,
31+ 'set_controller' ,
32+ self .set_controller_callback ,
33+ callback_group = self .service_cb_group
34+ )
35+
36+ # Create clients with separate callback group
37+ self .switch_client = self .create_client (
38+ SwitchController ,
39+ '/controller_manager/switch_controller' ,
40+ callback_group = self .client_cb_group
41+ )
42+ self .list_client = self .create_client (
43+ ListControllers ,
44+ '/controller_manager/list_controllers' ,
45+ callback_group = self .client_cb_group
46+ )
47+
48+ # Wait for services with timeout
49+ self .get_logger ().info ('Waiting for controller_manager services...' )
50+ services_ready = False
51+ for _ in range (30 ): # Try for 30 seconds
52+ if (self .switch_client .wait_for_service (timeout_sec = 1.0 ) and
53+ self .list_client .wait_for_service (timeout_sec = 1.0 )):
54+ services_ready = True
55+ break
56+ self .get_logger ().warn ('Still waiting for controller_manager services...' )
57+
58+ if not services_ready :
59+ self .get_logger ().error ('Controller manager services not available!' )
60+ raise RuntimeError ('Controller manager services timeout' )
61+
62+ self .get_logger ().info ('ControllerSwitcher ready!' )
63+
64+ def set_controller_callback (self , request , response ):
65+ # Use lock to ensure only one service call is processed at a time
66+ with self .service_lock :
67+ return self ._handle_set_controller (request , response )
68+
69+ def _handle_set_controller (self , request , response ):
70+ try :
71+ requested_controllers = list (request .controller_names ) if request .controller_names else []
72+
73+ # Empty list means deactivate all (except always_active and ignored)
74+ if not requested_controllers :
75+ self .get_logger ().info ("Empty controller list - will deactivate all except protected controllers" )
76+ else :
77+ self .get_logger ().info (f"Received request to switch to: { requested_controllers } )
78+
79+ # Get list of all controllers
80+ list_req = ListControllers .Request ()
81+ self .get_logger ().debug ("Calling list_controllers service..." )
82+
83+ # Call and wait for response
84+ try :
85+ list_result = self .list_client .call (list_req )
86+ except Exception as e :
87+ response .success = False
88+ response .message = f"Failed to call list_controllers: { str (e )}
89+ return response
90+
91+ if list_result is None :
92+ response .success = False
93+ response .message = "Failed to get controller list"
94+ return response
95+
96+ # Extract controller names and their states
97+ all_controllers = []
98+ active_controllers = []
99+ for c in list_result .controller :
100+ all_controllers .append (c .name )
101+ if c .state == 'active' :
102+ active_controllers .append (c .name )
103+
104+ self .get_logger ().info (f"Available controllers: { all_controllers } )
105+ self .get_logger ().info (f"Currently active: { active_controllers } )
106+
107+ # Check if all requested controllers exist (only if not empty)
108+ if requested_controllers :
109+ missing_controllers = []
110+ for controller in requested_controllers :
111+ if controller not in all_controllers :
112+ missing_controllers .append (controller )
113+
114+ if missing_controllers :
115+ response .success = False
116+ response .message = f"Controllers not found: { missing_controllers } { all_controllers }
117+ return response
118+
119+ # Build lists for activation/deactivation
120+ controllers_to_activate = []
121+
122+ # If not empty, add requested controllers that aren't already active
123+ if requested_controllers :
124+ for controller in requested_controllers :
125+ if controller not in active_controllers :
126+ controllers_to_activate .append (controller )
127+
128+ # Always ensure joint_state_broadcaster is active
129+ if "joint_state_broadcaster" not in active_controllers and "joint_state_broadcaster" not in controllers_to_activate :
130+ controllers_to_activate .append ("joint_state_broadcaster" )
131+
132+ # Deactivate logic:
133+ # - If empty list: deactivate all except always_active and ignored
134+ # - If controllers specified: deactivate all except requested, always_active, and ignored
135+ controllers_to_deactivate = []
136+ for controller in active_controllers :
137+ if controller in self .always_active or controller in self .ignore_controllers :
138+ continue # Skip protected controllers
139+
140+ if not requested_controllers or controller not in requested_controllers :
141+ controllers_to_deactivate .append (controller )
142+
143+ # Check if already in desired state
144+ if requested_controllers :
145+ all_requested_active = all (c in active_controllers for c in requested_controllers )
146+ if all_requested_active and len (controllers_to_deactivate ) == 0 :
147+ response .success = True
148+ response .message = f"Controllers { requested_controllers }
149+ self .get_logger ().info ("Already in desired state" )
150+ return response
151+ else :
152+ # For empty list, check if only protected controllers are active
153+ if len (controllers_to_deactivate ) == 0 and len (controllers_to_activate ) == 0 :
154+ response .success = True
155+ response .message = "Only protected controllers are active"
156+ self .get_logger ().info ("Already in desired state" )
157+ return response
158+
159+ self .get_logger ().info (f"Will activate: { controllers_to_activate } )
160+ self .get_logger ().info (f"Will deactivate: { controllers_to_deactivate } )
161+
162+ # Skip if nothing to do
163+ if not controllers_to_activate and not controllers_to_deactivate :
164+ response .success = True
165+ response .message = "No changes needed"
166+ return response
167+
168+ # Perform the switch
169+ switch_req = SwitchController .Request ()
170+ switch_req .activate_controllers = controllers_to_activate
171+ switch_req .deactivate_controllers = controllers_to_deactivate
172+ switch_req .strictness = SwitchController .Request .BEST_EFFORT
173+ switch_req .start_asap = True
174+ switch_req .timeout = Duration (seconds = 10 ).to_msg ()
175+
176+ self .get_logger ().debug ("Calling switch_controller service..." )
177+
178+ try :
179+ switch_result = self .switch_client .call (switch_req )
180+ except Exception as e :
181+ response .success = False
182+ response .message = f"Failed to call switch_controller: { str (e )}
183+ return response
184+
185+ if switch_result and switch_result .ok :
186+ response .success = True
187+ if requested_controllers :
188+ active_list = requested_controllers + self .always_active
189+ response .message = f"Successfully activated controllers: { requested_controllers } { active_list }
190+ else :
191+ response .message = f"Successfully deactivated all controllers except: { self .always_active + self .ignore_controllers }
192+ self .get_logger ().info (f"Success! { response .message } )
193+ else :
194+ response .success = False
195+ response .message = "Controller switch failed"
196+ self .get_logger ().error ("Switch failed" )
197+
198+ except Exception as e :
199+ self .get_logger ().error (f"Error in set_controller_callback: { str (e )} )
200+ import traceback
201+ self .get_logger ().error (traceback .format_exc ())
202+ response .success = False
203+ response .message = f"Exception: { str (e )}
204+
205+ self .get_logger ().info (f"Returning response: success={ response .success } { response .message } )
206+ return response
207+
208+
209+ def main (args = None ):
210+ rclpy .init (args = args )
211+
212+ try :
213+ node = ControllerSwitcher ()
214+ # Use MultiThreadedExecutor with more threads
215+ executor = MultiThreadedExecutor (num_threads = 4 )
216+ executor .add_node (node )
217+
218+ try :
219+ executor .spin ()
220+ finally :
221+ executor .shutdown ()
222+ node .destroy_node ()
223+
224+ except Exception as e :
225+ print (f"Failed to start ControllerSwitcher: { e } )
226+ finally :
227+ rclpy .shutdown ()
228+
229+
230+ if __name__ == '__main__' :
231+ main ()
0 commit comments