6.1 Python interface usage example
This chapter will guide you through implementing several features shown in the index
Build & Run Instructions
Enter the top-level AimDK directory (~/aimdk on the robot) and run the following commands
source /opt/ros/humble/setup.bash colcon build source install/local_setup.bash ros2 run py_examples '<corresponding feature name e.g.: get_mc_action>'
📝 Code Notes
The complete code implementation includes full error handling, signal handling, timeout handling, and other mechanisms to ensure program robustness. Please view or modify the code in the py_examples directory
Caution
As standard ROS DO NOT handle cross-host service (request-response) well, please refer to SDK examples to use open interfaces in a robust way (with protection mechanisms e.g. exception safety and retransmission)
While the robot is in Stable Standing Mode or Locomotion Mode, DO NOT launch ROS nodes in rapid bulk (no more than 2 nodes per second is recommended), as a large number of nodes joining DDS discovery within a short period causes communication congestion and degrades motion control real-time performance, which may cause the robot to lose balance and fall
6.1.1 Developer Mode
Invoke the 'GetSystemState' service to obtain the robot’s current system status, and use 'MigrateSystemState' to access the corresponding developer mode.
1#!/usr/bin/env python3
2"""
3System State Migrator - Synchronous Python version
4"""
5
6import sys
7import time
8import rclpy
9from rclpy.node import Node
10import aimdk_msgs.srv as aimdk_srv
11import aimdk_msgs.msg as aimdk_msg
12
13
14class SyncSystemStateMigrator(Node):
15 """Synchronous version of system state migrator"""
16
17 def __init__(self, target_state: str):
18 super().__init__('sync_system_state_migrator')
19 self.target_state = target_state
20
21 # Create service clients
22 self.migrate_client = self.create_client(
23 aimdk_srv.MigrateSystemState,
24 '/aimdk_5Fmsgs/srv/MigrateSystemState'
25 )
26
27 self.get_state_client = self.create_client(
28 aimdk_srv.GetSystemState,
29 '/aimdk_5Fmsgs/srv/GetSystemState'
30 )
31
32 self.get_logger().info(f'Target state: {self.target_state}')
33
34 def wait_for_services(self, timeout_sec: float = 30.0) -> bool:
35 """Wait for services to become available"""
36 self.get_logger().info('Waiting for services...')
37
38 start_time = time.time()
39 while time.time() - start_time < timeout_sec and rclpy.ok():
40 if (self.migrate_client.wait_for_service(timeout_sec=1.0) and
41 self.get_state_client.wait_for_service(timeout_sec=1.0)):
42 self.get_logger().info('Services are available')
43 return True
44
45 self.get_logger().info('Services not available, waiting...')
46 time.sleep(0.5)
47
48 self.get_logger().error(
49 f'Timeout waiting for services after {timeout_sec} seconds')
50 return False
51
52 def migrate_state_sync(self, max_retries: int = 5, timeout_sec: float = 30.0) -> bool:
53 """Send migration request synchronously with retry"""
54 request = aimdk_srv.MigrateSystemState.Request()
55 now = self.get_clock().now()
56 request.header.header.stamp.sec = now.nanoseconds // 1_000_000_000
57 request.header.header.stamp.nanosec = now.nanoseconds % 1_000_000_000
58 request.state = self.target_state
59
60 for attempt in range(1, max_retries + 1):
61 self.get_logger().info(
62 f'Sending migration request to {self.target_state} '
63 f'(attempt {attempt}/{max_retries})...'
64 )
65
66 future = None
67 try:
68 future = self.migrate_client.call_async(request)
69
70 start_time = time.time()
71 while rclpy.ok() and time.time() - start_time < timeout_sec:
72 rclpy.spin_once(self, timeout_sec=0.1)
73 if future.done():
74 response = future.result()
75 # SUCCESS (1)
76 if response.header.status.value == aimdk_msg.CommonState.SUCCESS:
77 self.get_logger().info('Migration request accepted')
78 return True
79 else:
80 self.get_logger().error(
81 f'Migration failed: {response.header.message}'
82 )
83 return False
84
85 self.get_logger().warning(
86 f'Timeout waiting for migration response '
87 f'(attempt {attempt}/{max_retries})'
88 )
89
90 except Exception as e:
91 self.get_logger().error(
92 f'Migration service call failed (attempt {attempt}/{max_retries}): {e}'
93 )
94 finally:
95 if future is not None and not future.done():
96 future.cancel()
97
98 if attempt < max_retries:
99 self.get_logger().info('Retrying migration request in 2.0 seconds...')
100 time.sleep(2.0)
101
102 self.get_logger().error(
103 f'Migration request failed after {max_retries} attempts'
104 )
105 return False
106
107 def get_current_state_sync(self, max_retries: int = 3, timeout_sec: float = 10.0):
108 """Get current system state synchronously with retry"""
109 request = aimdk_srv.GetSystemState.Request()
110 now = self.get_clock().now()
111 request.header.header.stamp.sec = now.nanoseconds // 1_000_000_000
112 request.header.header.stamp.nanosec = now.nanoseconds % 1_000_000_000
113
114 for attempt in range(1, max_retries + 1):
115 future = None
116 try:
117 future = self.get_state_client.call_async(request)
118
119 start_time = time.time()
120 while rclpy.ok() and time.time() - start_time < timeout_sec:
121 rclpy.spin_once(self, timeout_sec=0.1)
122 if future.done():
123 return future.result()
124
125 self.get_logger().warning(
126 f'Timeout getting system state (attempt {attempt}/{max_retries})'
127 )
128
129 except Exception as e:
130 self.get_logger().error(
131 f'GetSystemState service call failed (attempt {attempt}/{max_retries}): {e}'
132 )
133 finally:
134 if future is not None and not future.done():
135 future.cancel()
136
137 if attempt < max_retries:
138 time.sleep(1.0)
139
140 self.get_logger().error(
141 f'Failed to get system state after {max_retries} attempts'
142 )
143 return None
144
145 def monitor_migration_sync(self, max_checks: int = 300) -> bool:
146 """Monitor migration progress synchronously"""
147 self.get_logger().info('Monitoring migration progress...')
148
149 check_count = 0
150 start_time = time.time()
151
152 while check_count < max_checks and rclpy.ok():
153 # Wait between checks
154 time.sleep(1.0)
155
156 # Check current state
157 response = self.get_current_state_sync()
158 if response is None:
159 self.get_logger().warning('Failed to get system state, will retry...')
160 continue
161
162 check_count += 1
163
164 # Check if migration is complete
165 # Ready auto-transitions to Business, so also accept Business
166 # when the target is Ready
167 current_state_lower = response.cur_state.lower()
168 target_state_lower = self.target_state.lower()
169
170 state_match = (
171 current_state_lower == target_state_lower or
172 (target_state_lower == 'ready' and
173 current_state_lower == 'business')
174 )
175 ready_auto_transitioned = (
176 target_state_lower == 'ready' and
177 current_state_lower == 'business'
178 )
179 status_match = (
180 ready_auto_transitioned or
181 response.curr_status.value == aimdk_msg.SystemStatus.IN_READY
182 )
183
184 self.get_logger().info(
185 f'Check {check_count}: State="{response.cur_state}" '
186 f'(match={state_match}), Status={response.curr_status.value} (match={status_match})'
187 )
188
189 if state_match and status_match:
190 self.get_logger().info(
191 f'Migration to {self.target_state} completed successfully!')
192 return True
193
194 # Check timeout
195 if time.time() - start_time > 300.0: # 5 minutes total timeout
196 self.get_logger().error('Migration timeout after 5 minutes')
197 return False
198
199 self.get_logger().error(f'Migration failed after {max_checks} checks')
200 return False
201
202 def run_migration(self) -> bool:
203 """Run complete migration process"""
204 # Step 1: Wait for services
205 if not self.wait_for_services():
206 return False
207
208 # Step 2: Send migration request
209 if not self.migrate_state_sync():
210 return False
211
212 # Step 3: Monitor migration progress
213 return self.monitor_migration_sync()
214
215
216def main():
217 """
218 Main function
219 """
220 # Check command line arguments
221 if len(sys.argv) != 2:
222 print(f'Usage: {sys.argv[0]} <target_state>')
223 print('\nAvailable target states: Ready, Develop_Nav, Develop_Audio_Linux, Develop_Audio_ROS, Develop_MC')
224 return 1
225
226 # Get target state from command line argument
227 target_state = sys.argv[1]
228
229 # Validate target state
230 if not target_state:
231 print('Error: Target state cannot be empty', file=sys.stderr)
232 return 1
233
234 # Initialize ROS2
235 rclpy.init()
236
237 result = 1
238 node = None
239
240 try:
241 # Create migrator node
242 node = SyncSystemStateMigrator(target_state)
243
244 # Run migration
245 if node.run_migration():
246 print(f'\nMigration to {target_state} completed successfully!')
247 result = 0
248 else:
249 print(f'\nMigration to {target_state} failed')
250 result = 1
251
252 except KeyboardInterrupt:
253 print('\nProgram interrupted by user')
254 result = 1
255 except Exception as e:
256 print(f'Error: {e}', file=sys.stderr)
257 result = 1
258 finally:
259 # Cleanup
260 if node:
261 node.destroy_node()
262 if rclpy.ok():
263 rclpy.shutdown()
264
265 return result
266
267
268if __name__ == '__main__':
269 sys.exit(main())
Usage Instructions
ros2 run py_examples migrate_system_state Ready
Output Example
Readyis a transient state after boot — the system automatically transitions toBusiness. When targetingReady, the monitoring loop may complete in theBusinessstate (treated as successful migration).
[INFO] [1770125663.176574907] [sync_system_state_migrator]: Target state: Ready
[INFO] [1770125663.176758843] [sync_system_state_migrator]: Waiting for services...
[INFO] [1770125663.678726368] [sync_system_state_migrator]: Services are available
[INFO] [1770125663.679642487] [sync_system_state_migrator]: Sending migration request to Ready (attempt 1/5)...
[INFO] [1770125663.692394889] [sync_system_state_migrator]: Migration request accepted
[INFO] [1770125663.693117341] [sync_system_state_migrator]: Monitoring migration progress...
[INFO] [1770125664.728735142] [sync_system_state_migrator]: Check 1: State="Business" (match=True), Status=2 (match=True)
[INFO] [1770125664.728735142] [sync_system_state_migrator]: Migration to Ready completed successfully!
6.1.2 Get Robot Mode
Retrieve the robot’s current operating mode by calling the GetMcAction service, including the description, and status information.
1#!/usr/bin/env python3
2
3import rclpy
4import rclpy.logging
5from rclpy.node import Node
6
7from aimdk_msgs.srv import GetMcAction
8from aimdk_msgs.msg import CommonRequest
9
10
11class GetMcActionClient(Node):
12 def __init__(self):
13 super().__init__('get_mc_action_client')
14 self.client = self.create_client(
15 GetMcAction, '/aimdk_5Fmsgs/srv/GetMcAction')
16 self.get_logger().info('✅ GetMcAction client node created.')
17
18 # Wait for the service to become available
19 while not self.client.wait_for_service(timeout_sec=2.0):
20 self.get_logger().info('⏳ Service unavailable, waiting...')
21
22 self.get_logger().info('🟢 Service available, ready to send request.')
23
24 def send_request(self):
25 request = GetMcAction.Request()
26 request.request = CommonRequest()
27
28 self.get_logger().info('📨 Sending request to get robot mode')
29 for i in range(8):
30 request.request.header.stamp = self.get_clock().now().to_msg()
31 future = self.client.call_async(request)
32 rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)
33
34 if future.done():
35 break
36
37 # retry as remote peer is NOT handled well by ROS
38 self.get_logger().info(f'trying ... [{i}]')
39
40 response = future.result()
41 if response is None:
42 self.get_logger().error('❌ Service call failed or timed out.')
43 return
44
45 self.get_logger().info('✅ Robot mode get successfully.')
46 self.get_logger().info(f'Mode name: {response.info.action_desc}')
47 self.get_logger().info(f'Mode status: {response.info.status.value}')
48
49
50def main(args=None):
51 rclpy.init(args=args)
52 node = None
53 try:
54 node = GetMcActionClient()
55 node.send_request()
56 except KeyboardInterrupt:
57 pass
58 except Exception as e:
59 rclpy.logging.get_logger('main').error(
60 f'Program exited with exception: {e}')
61
62 if node:
63 node.destroy_node()
64 if rclpy.ok():
65 rclpy.shutdown()
66
67
68if __name__ == '__main__':
69 main()
Usage Instructions
ros2 run py_examples get_mc_action
Output Example
[INFO] [1779173975.378396584] [get_mc_action_client]: ✅ GetMcAction client node created.
[INFO] [1779173975.961720731] [get_mc_action_client]: 🟢 Service available, ready to send request.
[INFO] [1779173975.961868725] [get_mc_action_client]: 📨 Sending request to get robot mode
[INFO] [1779173975.965180630] [get_mc_action_client]: ✅ Robot mode get successfully.
[INFO] [1779173975.965282450] [get_mc_action_client]: Mode name: PASSIVE_DEFAULT
[INFO] [1779173975.965307345] [get_mc_action_client]: Mode status: 100
Interface Reference
Service:
/aimdk_5Fmsgs/srv/SetMcActionMessage:
aimdk_msgs/srv/SetMcAction
6.1.3 Set Robot Mode
This example uses the SetMcAction service. After running the node, enter the corresponding field value of the mode in the terminal, and the robot will immediately switch to the appropriate motion mode.
Before switching to the Stable Standing mode (STAND_DEFAULT), ensure the robot is standing and its feet are already on the ground.
The motion mode switching must follow its state transition diagram, other transitions will be rejected
Locomotion Mode (LOCOMOTION_DEFAULT) and Stable Standing Mode (STAND_DEFAULT) are unified and will auto switch internally, so switching manually to the nearer one is enough
1#!/usr/bin/env python3
2
3import sys
4import rclpy
5import rclpy.logging
6from rclpy.node import Node
7
8from aimdk_msgs.srv import SetMcAction
9from aimdk_msgs.msg import RequestHeader, CommonState, McActionCommand
10
11
12class SetMcActionClient(Node):
13 def __init__(self):
14 super().__init__('set_mc_action_client')
15 self.client = self.create_client(
16 SetMcAction, '/aimdk_5Fmsgs/srv/SetMcAction'
17 )
18 self.get_logger().info('✅ SetMcAction client node created.')
19
20 # Wait for the service to become available
21 while not self.client.wait_for_service(timeout_sec=2.0):
22 self.get_logger().info('⏳ Service unavailable, waiting...')
23
24 self.get_logger().info('🟢 Service available, ready to send request.')
25
26 def send_request(self, action_name: str):
27 req = SetMcAction.Request()
28 req.header = RequestHeader()
29 req.source = 'node.set_mc_action'
30 #
31 cmd = McActionCommand()
32 cmd.action_desc = action_name
33 req.command = cmd
34
35 self.get_logger().info(
36 f'📨 Sending request to set robot mode: {action_name}')
37 for i in range(8):
38 req.header.stamp = self.get_clock().now().to_msg()
39 future = self.client.call_async(req)
40 rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)
41
42 if future.done():
43 break
44
45 # retry as remote peer is NOT handled well by ROS
46 self.get_logger().info(f'trying ... [{i}]')
47
48 response = future.result()
49 if response is None:
50 self.get_logger().error('❌ Service call failed or timed out.')
51 return
52
53 # SUCCESS (1)
54 if response.response.status.value == CommonState.SUCCESS:
55 self.get_logger().info('✅ Robot mode set successfully.')
56 else:
57 self.get_logger().error(
58 f'❌ Failed (code={response.response.header.code}): '
59 f'{response.response.message}'
60 )
61
62
63def main(args=None):
64 action_info = {
65 'PASSIVE_DEFAULT': ('PD', 'joints with zero torque'),
66 'DAMPING_DEFAULT': ('DD', 'joints in damping mode'),
67 'JOINT_DEFAULT': ('JD', 'Position Control Stand (joints locked)'),
68 'STAND_DEFAULT': ('SD', 'Stable Stand (auto-balance)'),
69 'LOCOMOTION_DEFAULT': ('LD', 'locomotion mode (walk or run)'),
70 'HEAD_ONLY': ('HO', 'head-only control'),
71 'UPPERBODY_REMOTE_SPLIT': ('URS', 'upper body remote split (head + arms + hands)'),
72 }
73
74 choices = {}
75 for k, v in action_info.items():
76 choices[v[0]] = k
77
78 rclpy.init(args=args)
79 node = None
80 try:
81 # Prefer command-line argument, otherwise prompt for input
82 if len(sys.argv) > 1:
83 motion = sys.argv[1]
84 else:
85 print('{:<4} - {:<20} : {}'.format('abbr',
86 'robot mode', 'description'))
87 for k, v in action_info.items():
88 print(f'{v[0]:<4} - {k:<20} : {v[1]}')
89 motion = input('Enter abbr of robot mode:')
90
91 action_name = choices.get(motion)
92 if not action_name:
93 raise ValueError(f'Invalid abbr of robot mode: {motion}')
94
95 node = SetMcActionClient()
96 node.send_request(action_name)
97 except KeyboardInterrupt:
98 pass
99 except Exception as e:
100 rclpy.logging.get_logger('main').error(
101 f'Program exited with exception: {e}')
102
103 if node:
104 node.destroy_node()
105 if rclpy.ok():
106 rclpy.shutdown()
107
108
109if __name__ == '__main__':
110 main()
Usage Instructions
# Use command-line arguments to set the mode (recommended)
ros2 run py_examples set_mc_action JD # Zero-Torque >> Position-Control Standing
ros2 run py_examples set_mc_action SD # Ensure your robot's feet on the ground, Position-Control Standing >> Stable Standing
# Stable Standing >> Locomotion Mode. auto done internally, don't switch manually
# Or run without arguments and the program will prompt for input
ros2 run py_examples set_mc_action
Output Example
...
[INFO] [1764066567.502968540] [set_mc_action_client]: ✅ Robot mode set successfully.
Notes
Ensure the robot is standing and its feet are on the ground before switching to
STAND_DEFAULTmodeMode switching may take several seconds to complete
Interface Reference
Service:
/aimdk_5Fmsgs/srv/SetMcActionMessage:
aimdk_msgs/srv/SetMcAction
6.1.4 Set Robot Motion
This example uses preset_motion_client; after switching to Stable Stand Mode and starting the node, enter the corresponding field values to perform preset actions with the left (or right) hand such as handshake, raise hand, wave, or air kiss.
Available parameters are listed in the preset motions table
1#!/usr/bin/env python3
2
3import rclpy
4import rclpy.logging
5from rclpy.node import Node
6
7from aimdk_msgs.srv import SetMcPresetMotion, SetMcInputSource
8from aimdk_msgs.msg import (McPresetMotion, McControlArea, McInputAction,
9 RequestHeader, CommonState)
10
11INPUT_SOURCE_NAME = "node.preset_motion"
12INPUT_SOURCE_PRIORITY = 40
13INPUT_SOURCE_TIMEOUT = 1000
14
15
16class SetMcPresetMotionClient(Node):
17 def __init__(self):
18 super().__init__('preset_motion_client')
19 self.client = self.create_client(
20 SetMcPresetMotion, '/aimdk_5Fmsgs/srv/SetMcPresetMotion')
21 self.input_source_client = self.create_client(
22 SetMcInputSource, '/aimdk_5Fmsgs/srv/SetMcInputSource')
23 self.get_logger().info('✅ SetMcPresetMotion client node created.')
24
25 # Wait for the service to become available
26 while not self.client.wait_for_service(timeout_sec=2.0):
27 self.get_logger().info('⏳ Service unavailable, waiting...')
28
29 self.get_logger().info('🟢 Service available, ready to send request.')
30
31 def _call_input_source(self, action_value: int) -> bool:
32 action_name = {
33 McInputAction.INPUTACTION_ADD: "ADD",
34 McInputAction.INPUTACTION_ENABLE: "ENABLE",
35 }.get(action_value, str(action_value))
36
37 req = SetMcInputSource.Request()
38 req.action.value = action_value
39 req.input_source.name = INPUT_SOURCE_NAME
40 req.input_source.priority = INPUT_SOURCE_PRIORITY
41 req.input_source.timeout = INPUT_SOURCE_TIMEOUT
42
43 future = None
44 for i in range(8):
45 req.request.header.stamp = self.get_clock().now().to_msg()
46 future = self.input_source_client.call_async(req)
47 rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)
48 if future.done():
49 break
50 # retry as remote peer is NOT handled well by ROS
51 self.get_logger().info(
52 f'trying input source action {action_name} ({action_value})... [{i}]')
53
54 if future is None or not future.done():
55 return False
56 response = future.result()
57 return response is not None and response.response.header.code == 0
58
59 def register_input_source(self) -> bool:
60 if not self.input_source_client.wait_for_service(timeout_sec=8.0):
61 self.get_logger().error('❌ SetMcInputSource service unavailable.')
62 return False
63
64 # Try ADD first; if already registered, fall back to ENABLE
65 if self._call_input_source(McInputAction.INPUTACTION_ADD): # ADD (1001)
66 self.get_logger().info(
67 f'✅ Input source "{INPUT_SOURCE_NAME}" registered (ADD).')
68 return True
69 self.get_logger().info(
70 'ADD failed (likely already registered), trying ENABLE...')
71 if self._call_input_source(McInputAction.INPUTACTION_ENABLE): # ENABLE (2001)
72 self.get_logger().info(
73 f'✅ Input source "{INPUT_SOURCE_NAME}" enabled.')
74 return True
75 self.get_logger().error('❌ Failed to register input source via ADD or ENABLE.')
76 return False
77
78 def send_request(self, area_id: int, motion_id: int) -> bool:
79 request = SetMcPresetMotion.Request()
80 request.header = RequestHeader()
81
82 request.input_source.name = INPUT_SOURCE_NAME
83 request.input_source.priority = INPUT_SOURCE_PRIORITY
84 request.input_source.timeout = INPUT_SOURCE_TIMEOUT
85
86 motion = McPresetMotion()
87 area = McControlArea()
88
89 motion.value = motion_id
90 area.value = area_id
91
92 request.motion = motion
93 request.area = area
94 request.interrupt = False
95
96 self.get_logger().info(
97 f'📨 Sending request to set preset motion: motion={motion_id}, area={area_id}')
98
99 for i in range(8):
100 request.header.stamp = self.get_clock().now().to_msg()
101 future = self.client.call_async(request)
102 rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)
103
104 if future.done():
105 break
106
107 # retry as remote peer is NOT handled well by ROS
108 self.get_logger().info(f'trying ... [{i}]')
109
110 response = future.result()
111 if response is None:
112 self.get_logger().error('❌ Service call failed or timed out.')
113 return False
114
115 if response.response.header.code == 0:
116 self.get_logger().info(
117 f'✅ Preset motion set successfully: {response.response.task_id}')
118 return True
119 # RUNNING (400)
120 elif response.response.state.value == CommonState.RUNNING:
121 self.get_logger().info(
122 f'⏳ Preset motion executing: {response.response.task_id}')
123 return True
124 else:
125 self.get_logger().error(
126 f'❌ Failed to set preset motion: {response.response.task_id}'
127 )
128 return False
129
130
131def main(args=None):
132 rclpy.init(args=args)
133 node = None
134 try:
135 area = int(input("Enter arm area ID (1-left, 2-right): "))
136 motion = int(input(
137 "Enter preset motion ID (1001-raise, 1002-wave, 1003-handshake, 1004-airkiss): "))
138
139 node = SetMcPresetMotionClient()
140 if not node.register_input_source():
141 node.get_logger().error("❌ Input source registration failed, exiting")
142 else:
143 node.send_request(area, motion)
144 except KeyboardInterrupt:
145 pass
146 except Exception as e:
147 rclpy.logging.get_logger('main').error(
148 f'Program exited with exception: {e}')
149
150 if node:
151 node.destroy_node()
152 if rclpy.ok():
153 rclpy.shutdown()
154
155
156if __name__ == '__main__':
157 main()
6.1.5 Gripper Control
This example uses hand_control. Publish messages to the /aima/hal/joint/hand/command topic to control the gripper.
Attention
Note ⚠️: Before running this example, stop the native MC module on the Motion Control Computing Unit (PC1) by running aima em stop-app mc to obtain control of the robot. Ensure the robot is safe before operating.
1import rclpy
2from rclpy.node import Node
3from aimdk_msgs.msg import HandCommandArray, HandCommand, HandType, MessageHeader
4
5
6class HandControl(Node):
7 def __init__(self):
8 super().__init__('hand_control')
9
10 # Preset parameter list: [(left hand, right hand), ...]
11 self.position_pairs = [
12 (1.0, 1.0), # fully open
13 (0.0, 0.0), # fully closed
14 (0.5, 0.5), # half open
15 (0.2, 0.8), # left slightly closed, right more open
16 (0.7, 0.3) # left more open, right slightly closed
17 ]
18 self.current_index = 0
19 self.last_switch_time = self.get_clock().now().nanoseconds / 1e9 # seconds
20
21 # Create publisher
22 self.publisher_ = self.create_publisher(
23 HandCommandArray,
24 '/aima/hal/joint/hand/command',
25 10
26 )
27
28 # 50 Hz timer
29 self.timer_ = self.create_timer(
30 0.02, # 20 ms = 50 Hz
31 self.publish_hand_commands
32 )
33
34 self.get_logger().info("Hand control node started!")
35
36 def publish_hand_commands(self):
37 # Check time to decide whether to switch to the next preset
38 now_sec = self.get_clock().now().nanoseconds / 1e9
39 if now_sec - self.last_switch_time >= 2.0:
40 self.current_index = (self.current_index +
41 1) % len(self.position_pairs)
42 self.last_switch_time = now_sec
43 self.get_logger().info(
44 f"Switched to preset: {self.current_index}, left={self.position_pairs[self.current_index][0]:.2f}, right={self.position_pairs[self.current_index][1]:.2f}"
45 )
46
47 # Use current preset
48 left_position, right_position = self.position_pairs[self.current_index]
49
50 msg = HandCommandArray()
51 msg.header = MessageHeader()
52
53 # Configure left hand
54 left_hand = HandCommand()
55 left_hand.name = "left_hand"
56 left_hand.position = float(left_position)
57 left_hand.velocity = 1.0
58 left_hand.acceleration = 1.0
59 left_hand.deceleration = 1.0
60 left_hand.effort = 1.0
61
62 # Configure right hand
63 right_hand = HandCommand()
64 right_hand.name = "right_hand"
65 right_hand.position = float(right_position)
66 right_hand.velocity = 1.0
67 right_hand.acceleration = 1.0
68 right_hand.deceleration = 1.0
69 right_hand.effort = 1.0
70
71 msg.left_hand_type = HandType(value=HandType.CLAW) # grippers (2)
72 msg.right_hand_type = HandType(value=HandType.CLAW) # grippers (2)
73 msg.left_hands = [left_hand]
74 msg.right_hands = [right_hand]
75
76 # Publish message
77 self.publisher_.publish(msg)
78 # We only log when switching presets to avoid too much log output
79
80
81def main(args=None):
82 rclpy.init(args=args)
83 hand_control_node = HandControl()
84
85 try:
86 rclpy.spin(hand_control_node)
87 except KeyboardInterrupt:
88 pass
89 finally:
90 hand_control_node.destroy_node()
91 if rclpy.ok():
92 rclpy.shutdown()
93
94
95if __name__ == '__main__':
96 main()
6.1.6 Dexterous Hand Control
This example uses omnihand_control. Publish messages to the /aima/hal/joint/hand/command topic to control the omnihand.
Attention
Note ⚠️: Before running this example, stop the native MC module on the Motion Control Computing Unit (PC1) by running aima em stop-app mc to obtain control of the robot. Ensure the robot is safe before operating.
1import rclpy
2from rclpy.node import Node
3from aimdk_msgs.msg import HandCommandArray, HandCommand, HandType
4
5
6class HandControl(Node):
7 def __init__(self):
8 super().__init__('hand_control')
9
10 # Create publisher
11 self.publisher_ = self.create_publisher(
12 HandCommandArray,
13 '/aima/hal/joint/hand/command',
14 10
15 )
16
17 self.timer_ = self.create_timer(
18 1.0,
19 self.publish_hand_commands
20 )
21
22 # Initialize variables
23 self.target_finger = 0
24 self.step = 1
25 self.increasing = True
26 self.get_logger().info("Hand control node started!")
27
28 def build_hand_cmd(self, name: str) -> HandCommand:
29 cmd = HandCommand()
30 cmd.name = name
31 cmd.position = 0.0
32 cmd.velocity = 0.1
33 cmd.acceleration = 0.0
34 cmd.deceleration = 0.0
35 cmd.effort = 0.0
36 return cmd
37
38 def publish_hand_commands(self):
39 msg = HandCommandArray()
40 msg.header.stamp = self.get_clock().now().to_msg()
41 msg.header.frame_id = 'hand_command'
42 # dexterous hands (1)
43 msg.left_hand_type.value = HandType.NIMBLE_HANDS
44 # dexterous hands (1)
45 msg.right_hand_type.value = HandType.NIMBLE_HANDS
46
47 left_names = ['L_thumb_roll_joint', 'L_thumb_abad_joint', 'L_thumb_mcp_joint',
48 'L_index_abad_joint', 'L_index_pip_joint', 'L_middle_pip_joint',
49 'L_ring_abad_joint', 'L_ring_pip_joint', 'L_pinky_abad_joint', 'L_pinky_pip_joint']
50 right_names = [n.replace('L_', 'R_', 1) for n in left_names]
51
52 # left hand
53 msg.left_hands = [self.build_hand_cmd(
54 left_names[i]) for i in range(10)]
55
56 # right hand
57 msg.right_hands = [self.build_hand_cmd(
58 right_names[i]) for i in range(10)]
59
60 # Absolute joint targets in rad = half-grip
61 # Joint order: [thumb_roll, thumb_abad, thumb_mcp, index_abad, index_pip,
62 # middle_pip, ring_abad, ring_pip, pinky_abad, pinky_pip]
63 left_hand_target = [-0.6, 0.8, -0.4,
64 0.1, 0.7, 0.7, -0.1, 0.7, -0.1, 0.7]
65 right_hand_target = [0.6, -0.8, 0.4,
66 -0.1, 0.7, 0.7, 0.1, 0.7, 0.1, 0.7]
67
68 if self.target_finger < 10:
69 msg.right_hands[self.target_finger].position = right_hand_target[self.target_finger]
70 else:
71 target_finger_ = self.target_finger - 10
72 msg.left_hands[target_finger_].position = left_hand_target[target_finger_]
73
74 self.publisher_.publish(msg)
75 self.get_logger().info(
76 f'Published hand command with target_finger: {self.target_finger}')
77 self.update_target_finger()
78
79 def update_target_finger(self):
80 if self.increasing:
81 self.target_finger += self.step
82 if self.target_finger >= 19:
83 self.target_finger = 19
84 self.increasing = False
85 else:
86 self.target_finger -= self.step
87 if self.target_finger <= 0:
88 self.target_finger = 0
89 self.increasing = True
90
91
92def main(args=None):
93 rclpy.init(args=args)
94 hand_control_node = HandControl()
95
96 try:
97 rclpy.spin(hand_control_node)
98 except KeyboardInterrupt:
99 pass
100 finally:
101 hand_control_node.destroy_node()
102 if rclpy.ok():
103 rclpy.shutdown()
104
105
106if __name__ == '__main__':
107 main()
6.1.7 Dexterous Hand Tactile Data (T2.1 Support)
This example uses omnihand_touch. By subscribing to the /aima/hal/joint/hand/state, you can monitor the pressure of the omnihand finger touch sensor.
Attention
Note ⚠️: Before running this example, stop the native MC module on the Motion Control Computing Unit (PC1) by running aima em stop-app mc to obtain control of the robot. Ensure the robot is safe before operating.
1#!/usr/bin/env python3
2
3import rclpy
4from rclpy.node import Node
5from rclpy.qos import QoSProfile, ReliabilityPolicy
6from aimdk_msgs.msg import HandStateArray, HandCommandArray, HandType
7
8
9class HandStateSubscriber(Node):
10 def __init__(self):
11 super().__init__('hand_state_subscriber')
12
13 # Create publisher
14 self.publisher_ = self.create_publisher(
15 HandCommandArray,
16 '/aima/hal/joint/hand/command',
17 10
18 )
19
20 # Create QoS profile with BEST_EFFORT reliability
21 qos_profile = QoSProfile(depth=10)
22 qos_profile.reliability = ReliabilityPolicy.BEST_EFFORT
23
24 # Create subscriber
25 self.subscription = self.create_subscription(
26 HandStateArray,
27 '/aima/hal/joint/hand/state',
28 self.topic_callback,
29 qos_profile
30 )
31
32 # Create a timer to publish once per second
33 self.timer = self.create_timer(1.0, self.publish_command)
34
35 self.get_logger().info(
36 "Subscriber started, listening to /aima/hal/joint/hand/state topic..."
37 )
38
39 def topic_callback(self, msg):
40 """Callback function for handling incoming messages"""
41 # Print message header information
42 self.get_logger().info(
43 f"Message received - Sequence: {msg.header.sequence}, "
44 f"Timestamp: {msg.header.stamp.sec}.{msg.header.stamp.nanosec:09d}"
45 )
46
47 # Print left hand touch sensor data
48 self.print_touch_sensor_data("Left Hand", msg.left_touch_sensors)
49
50 # Print right hand touch sensor data
51 self.print_touch_sensor_data("Right Hand", msg.right_touch_sensors)
52
53 print()
54
55 def print_touch_sensor_data(self, hand_name, sensor_data):
56 """
57 Print touch sensor data for a specific hand
58
59 Args:
60 hand_name: Name of the hand (Left Hand/Right Hand)
61 sensor_data: Touch sensor data structure
62 """
63 print(f"=== {hand_name} Touch Sensor Data ===")
64
65 # Print palm touch data
66 print("Palm Touch Data (36 elements): ", end="")
67 self.print_array(sensor_data.palm_touch_data)
68
69 # Print back of hand touch data
70 print("Back of Hand Touch Data (36 elements): ", end="")
71 self.print_array(sensor_data.back_of_hand_touch_data)
72
73 # Print finger touch data
74 print("Thumb Touch Data (16 elements): ", end="")
75 self.print_array(sensor_data.thumb_touch_data)
76
77 print("Index Finger Touch Data (16 elements): ", end="")
78 self.print_array(sensor_data.index_finger_touch_data)
79
80 print("Middle Finger Touch Data (16 elements): ", end="")
81 self.print_array(sensor_data.middle_finger_touch_data)
82
83 print("Ring Finger Touch Data (16 elements): ", end="")
84 self.print_array(sensor_data.ring_finger_touch_data)
85
86 print("Little Finger Touch Data (16 elements): ", end="")
87 self.print_array(sensor_data.little_finger_touch_data)
88
89 def print_array(self, arr):
90 """
91 Print array of uint8_t elements
92
93 Args:
94 arr: Array to print (list or tuple)
95 """
96 print("[", end="")
97 for i, val in enumerate(arr):
98 print(f"{val:3d}", end="")
99 if i < len(arr) - 1:
100 print(" ", end="")
101 print("]")
102
103 def publish_command(self):
104 """Publish hand command message"""
105 message = HandCommandArray()
106
107 # Set header
108 message.header.stamp = self.get_clock().now().to_msg()
109 message.header.frame_id = "hand_command"
110
111 # Set the hand type
112 # dexterous hands (1)
113 message.left_hand_type.value = HandType.NIMBLE_HANDS
114 # dexterous hands (1)
115 message.right_hand_type.value = HandType.NIMBLE_HANDS
116
117 # Create hand command arrays
118 from aimdk_msgs.msg import HandCommand
119
120 # Joint names for readability, mirroring the /aima/hal/joint/hand/state feedback (URDF names).
121 # The driver routes by array index/order (see endeffector.md), not by these name strings.
122 left_names = ["L_thumb_roll_joint", "L_thumb_abad_joint", "L_thumb_mcp_joint",
123 "L_index_abad_joint", "L_index_pip_joint", "L_middle_pip_joint",
124 "L_ring_abad_joint", "L_ring_pip_joint", "L_pinky_abad_joint", "L_pinky_pip_joint"]
125 right_names = [n.replace("L_", "R_", 1) for n in left_names]
126
127 def build_cmd(name):
128 cmd = HandCommand()
129 cmd.name = name
130 cmd.position = 0.0
131 cmd.velocity = 0.1
132 cmd.acceleration = 0.0
133 cmd.deceleration = 0.0
134 cmd.effort = 0.0
135 return cmd
136
137 message.left_hands = [build_cmd(n) for n in left_names]
138 message.right_hands = [build_cmd(n) for n in right_names]
139
140 # Publish the message
141 self.publisher_.publish(message)
142
143 self.get_logger().info("Published hand command")
144
145
146def main(args=None):
147 rclpy.init(args=args)
148 node = HandStateSubscriber()
149 try:
150 rclpy.spin(node)
151 except KeyboardInterrupt:
152 pass
153 finally:
154 node.destroy_node()
155 if rclpy.ok():
156 rclpy.shutdown()
157
158
159if __name__ == '__main__':
160 main()
The pressure sensors are located at the fingertips of all five fingers, as well as on the palm and the back of the hand. At least one motion command needs to be sent to the dexterous hand in order to receive the corresponding pressure data report.
6.1.8 Register Secondary Development Input Source
For versions after v0.7, you must register an input source before controlling the MC. This example registers a custom input source via the /aimdk_5Fmsgs/srv/SetMcInputSource service so MC becomes aware of it; only registered input sources can perform robot velocity control.
1#!/usr/bin/env python3
2
3import rclpy
4import rclpy.logging
5from rclpy.node import Node
6
7from aimdk_msgs.srv import SetMcInputSource
8from aimdk_msgs.msg import RequestHeader, McInputAction
9
10ACTION_NAMES = {
11 McInputAction.INPUTACTION_ADD: "ADD",
12 McInputAction.INPUTACTION_MODIFY: "MODIFY",
13 McInputAction.INPUTACTION_DELETE: "DELETE",
14 McInputAction.INPUTACTION_ENABLE: "ENABLE",
15 McInputAction.INPUTACTION_DISABLE: "DISABLE",
16}
17
18
19class McInputClient(Node):
20 def __init__(self):
21 super().__init__('set_mc_input_source_client')
22 self.client = self.create_client(
23 SetMcInputSource, '/aimdk_5Fmsgs/srv/SetMcInputSource'
24 )
25
26 self.get_logger().info('✅ SetMcInputSource client node created.')
27
28 # Wait for the service to become available
29 while not self.client.wait_for_service(timeout_sec=2.0):
30 self.get_logger().info('⏳ Service unavailable, waiting...')
31
32 self.get_logger().info('🟢 Service available, ready to send request.')
33
34 def send_request(self):
35 req = SetMcInputSource.Request()
36
37 # header
38 req.request.header = RequestHeader()
39
40 # action (e.g. ADD = register new input source)
41 req.action = McInputAction()
42 req.action.value = McInputAction.INPUTACTION_ADD # ADD (1001)
43
44 # input source info
45 req.input_source.name = 'node.set_mc_input_source'
46 req.input_source.priority = 40
47 req.input_source.timeout = 1000 # ms
48
49 # Send request and wait for response
50 act_name = ACTION_NAMES.get(req.action.value, str(req.action.value))
51 self.get_logger().info(
52 f'📨 Sending input source request: {act_name} ({req.action.value}), '
53 f'name={req.input_source.name}, priority={req.input_source.priority}'
54 )
55 for i in range(8):
56 req.request.header.stamp = self.get_clock().now().to_msg()
57 future = self.client.call_async(req)
58 rclpy.spin_until_future_complete(
59 self, future, timeout_sec=0.25)
60
61 if future.done():
62 break
63
64 # retry as remote peer is NOT handled well by ROS
65 self.get_logger().info(f'trying ... [{i}]')
66
67 if not future.done():
68 self.get_logger().error('❌ Service call failed or timed out.')
69 return False
70
71 response = future.result()
72 ret_code = response.response.header.code
73 task_id = response.response.task_id
74
75 if ret_code == 0:
76 self.get_logger().info(
77 f'✅ Input source set successfully. task_id={task_id}'
78 )
79 return True
80 else:
81 self.get_logger().error(
82 f'❌ Input source set failed. ret_code={ret_code}, task_id={task_id} (duplicated ADD? or MODIFY/ENABLE/DISABLE for unknown source?)'
83 )
84 return False
85
86
87def main(args=None):
88 rclpy.init(args=args)
89
90 node = None
91 try:
92 node = McInputClient()
93 ok = node.send_request()
94 if not ok:
95 node.get_logger().error('Input source request failed.')
96 except KeyboardInterrupt:
97 pass
98 except Exception as e:
99 rclpy.logging.get_logger('main').error(
100 f'Program exited with exception: {e}')
101
102 if node:
103 node.destroy_node()
104 if rclpy.ok():
105 rclpy.shutdown()
106
107
108if __name__ == '__main__':
109 main()
6.1.9 Get Current Input Source
This example uses the GetCurrentInputSource service to retrieve information about the currently registered input source, including name, priority, and timeout.
1#!/usr/bin/env python3
2
3import rclpy
4from rclpy.node import Node
5from aimdk_msgs.srv import GetCurrentInputSource
6from aimdk_msgs.msg import CommonRequest
7
8
9class GetCurrentInputSourceClient(Node):
10 def __init__(self):
11 super().__init__('get_current_input_source_client')
12 self.client = self.create_client(
13 GetCurrentInputSource,
14 '/aimdk_5Fmsgs/srv/GetCurrentInputSource'
15 )
16
17 self.get_logger().info('✅ GetCurrentInputSource client node created.')
18
19 # Wait for the service to become available
20 while not self.client.wait_for_service(timeout_sec=2.0):
21 self.get_logger().info('⏳ Service unavailable, waiting...')
22
23 self.get_logger().info('🟢 Service available, ready to send request.')
24
25 def send_request(self):
26 # Create request
27 req = GetCurrentInputSource.Request()
28 req.request = CommonRequest()
29
30 # Send request and wait for response
31 self.get_logger().info('📨 Sending request to get current input source')
32 for i in range(8):
33 req.request.header.stamp = self.get_clock().now().to_msg()
34 future = self.client.call_async(req)
35 rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)
36
37 if future.done():
38 break
39
40 # retry as remote peer is NOT handled well by ROS
41 self.get_logger().info(f'trying ... [{i}]')
42
43 if not future.done():
44 self.get_logger().error('❌ Service call failed or timed out.')
45 return False
46
47 response = future.result()
48 ret_code = response.response.header.code
49 if ret_code == 0:
50 self.get_logger().info(
51 '✅ Current input source get successfully:')
52 self.get_logger().info(
53 f'Name: {response.input_source.name}')
54 self.get_logger().info(
55 f'Priority: {response.input_source.priority}')
56 self.get_logger().info(
57 f'Timeout: {response.input_source.timeout}')
58 return True
59 else:
60 self.get_logger().error(
61 f'❌ Current input source get failed, return code: {ret_code}')
62 return False
63
64
65def main(args=None):
66 rclpy.init(args=args)
67
68 node = None
69 try:
70 node = GetCurrentInputSourceClient()
71 success = node.send_request()
72 except KeyboardInterrupt:
73 pass
74 except Exception as e:
75 rclpy.logging.get_logger('main').error(
76 f'Program exited with exception: {e}')
77
78 if node:
79 node.destroy_node()
80 if rclpy.ok():
81 rclpy.shutdown()
82
83
84if __name__ == '__main__':
85 main()
Usage Instructions
# Get current input source info
ros2 run py_examples get_current_input_source
Output Example
[INFO] [1779174540.534817548] [get_current_input_source_client]: ✅ GetCurrentInputSource client node created.
[INFO] [1779174541.371095227] [get_current_input_source_client]: 🟢 Service available, ready to send request.
[INFO] [1779174541.371216694] [get_current_input_source_client]: 📨 Sending request to get current input source
[INFO] [1779174541.382602338] [get_current_input_source_client]: ✅ Current input source get successfully:
[INFO] [1779174541.382692894] [get_current_input_source_client]: Name: node
[INFO] [1779174541.382713533] [get_current_input_source_client]: Priority: 40
[INFO] [1779174541.382726205] [get_current_input_source_client]: Timeout: 1000
Notes
Ensure the GetCurrentInputSource service is running properly
Valid information can only be retrieved after registering an input source
A status code of 0 indicates a successful query
6.1.10 Robot Locomotion Control
This example uses mc_locomotion_velocity. The example below controls robot walking by publishing to the /aima/mc/locomotion/velocity topic. For versions after v0.7, you must register an input source before enabling velocity control (this example already registers an input source); see the code for registration steps.
Start the node after switching to Stable Standing Mode:
1#!/usr/bin/env python3
2
3import rclpy
4from rclpy.node import Node
5import time
6import signal
7import sys
8
9from aimdk_msgs.msg import McLocomotionVelocity, McInputAction, MessageHeader
10from aimdk_msgs.srv import SetMcInputSource
11
12
13class DirectVelocityControl(Node):
14 def __init__(self):
15 super().__init__('direct_velocity_control')
16
17 self.publisher = self.create_publisher(
18 McLocomotionVelocity, '/aima/mc/locomotion/velocity', 10)
19 self.client = self.create_client(
20 SetMcInputSource, '/aimdk_5Fmsgs/srv/SetMcInputSource')
21
22 self.forward_velocity = 0.0
23 self.lateral_velocity = 0.0
24 self.angular_velocity = 0.0
25
26 # Maximum speed limits
27 self.max_forward_speed = 1.0 # m/s
28 self.min_forward_speed = 0.2 # m/s
29
30 self.max_lateral_speed = 1.0 # m/s
31 self.min_lateral_speed = 0.2 # m/s
32
33 self.max_angular_speed = 1.0 # rad/s
34 self.min_angular_speed = 0.1 # rad/s
35
36 self.timer = None
37
38 self.get_logger().info("Direct velocity control node started!")
39
40 def start_publish(self):
41 if not self.timer:
42 self.timer = self.create_timer(0.02, self.publish_velocity)
43
44 def _call_input_source(self, action_value):
45 action_name = {
46 McInputAction.INPUTACTION_ADD: "ADD",
47 McInputAction.INPUTACTION_ENABLE: "ENABLE",
48 }.get(action_value, str(action_value))
49
50 req = SetMcInputSource.Request()
51 req.action.value = action_value
52 req.input_source.name = "node.locomotion_velocity"
53 req.input_source.priority = 40
54 req.input_source.timeout = 1000
55
56 future = None
57 for i in range(8):
58 req.request.header.stamp = self.get_clock().now().to_msg()
59 future = self.client.call_async(req)
60 rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)
61 if future.done():
62 break
63 # retry as remote peer is NOT handled well by ROS
64 self.get_logger().info(
65 f"trying input source action {action_name} ({action_value})... [{i}]")
66
67 if future is None or not future.done():
68 return False
69 try:
70 response = future.result()
71 except Exception as e:
72 self.get_logger().error(f"Service call exception: {str(e)}")
73 return False
74 return response is not None and response.response.header.code == 0
75
76 def register_input_source(self):
77 self.get_logger().info("Registering input source...")
78
79 timeout_sec = 8.0
80 start = self.get_clock().now().nanoseconds / 1e9
81
82 while not self.client.wait_for_service(timeout_sec=2.0):
83 now = self.get_clock().now().nanoseconds / 1e9
84 if now - start > timeout_sec:
85 self.get_logger().error("Waiting for service timed out")
86 return False
87 self.get_logger().info("Waiting for input source service...")
88
89 # Try ADD first; if already registered, fall back to ENABLE
90 if self._call_input_source(McInputAction.INPUTACTION_ADD): # ADD (1001)
91 self.get_logger().info("✅ Input source registered (ADD).")
92 return True
93 self.get_logger().info(
94 "ADD failed (likely already registered), trying ENABLE...")
95 if self._call_input_source(McInputAction.INPUTACTION_ENABLE): # ENABLE (2001)
96 self.get_logger().info("✅ Input source enabled.")
97 return True
98 self.get_logger().error(
99 "❌ Failed to register input source via ADD or ENABLE.")
100 return False
101
102 def publish_velocity(self):
103 msg = McLocomotionVelocity()
104 msg.header = MessageHeader()
105 msg.header.stamp = self.get_clock().now().to_msg()
106 msg.source = "node.locomotion_velocity"
107 msg.forward_velocity = self.forward_velocity
108 msg.lateral_velocity = self.lateral_velocity
109 msg.angular_velocity = self.angular_velocity
110
111 self.publisher.publish(msg)
112
113 self.get_logger().info(
114 f"Publishing velocity: forward {self.forward_velocity:.2f} m/s, "
115 f"lateral {self.lateral_velocity:.2f} m/s, "
116 f"angular {self.angular_velocity:.2f} rad/s"
117 )
118
119 def set_forward(self, forward):
120 # check value range, mc has thresholds to start movement
121 if abs(forward) < 0.005:
122 self.forward_velocity = 0.0
123 return True
124 elif abs(forward) > self.max_forward_speed or abs(forward) < self.min_forward_speed:
125 raise ValueError("out of range")
126 else:
127 self.forward_velocity = forward
128 return True
129
130 def set_lateral(self, lateral):
131 # check value range, mc has thresholds to start movement
132 if abs(lateral) < 0.005:
133 self.lateral_velocity = 0.0
134 return True
135 elif abs(lateral) > self.max_lateral_speed or abs(lateral) < self.min_lateral_speed:
136 raise ValueError("out of range")
137 else:
138 self.lateral_velocity = lateral
139 return True
140
141 def set_angular(self, angular):
142 # check value range, mc has thresholds to start movement
143 if abs(angular) < 0.005:
144 self.angular_velocity = 0.0
145 return True
146 elif abs(angular) > self.max_angular_speed or abs(angular) < self.min_angular_speed:
147 raise ValueError("out of range")
148 else:
149 self.angular_velocity = angular
150 return True
151
152 def clear_velocity(self):
153 self.forward_velocity = 0.0
154 self.lateral_velocity = 0.0
155 self.angular_velocity = 0.0
156
157
158# Global node instance for signal handling
159global_node = None
160
161
162def signal_handler(sig, frame):
163 global global_node
164 if global_node is not None:
165 global_node.clear_velocity()
166 global_node.get_logger().info(
167 f"Received signal {sig}, clearing velocity and shutting down")
168 if rclpy.ok():
169 rclpy.shutdown()
170 sys.exit(0)
171
172
173def main():
174 global global_node
175 rclpy.init()
176
177 node = DirectVelocityControl()
178 global_node = node
179
180 signal.signal(signal.SIGINT, signal_handler)
181 signal.signal(signal.SIGTERM, signal_handler)
182
183 if not node.register_input_source():
184 node.get_logger().error("❌ Input source registration failed, exiting")
185 if rclpy.ok():
186 rclpy.shutdown()
187 return
188
189 # get and check control values
190 # notice that mc has thresholds to start movement
191 try:
192 # get input forward
193 forward = float(
194 input("Please enter forward velocity 0 or ±(0.2 ~ 1.0) m/s: "))
195 node.set_forward(forward)
196 # get input lateral
197 lateral = float(
198 input("Please enter lateral velocity 0 or ±(0.2 ~ 1.0) m/s: "))
199 node.set_lateral(lateral)
200 # get input angular
201 angular = float(
202 input("Please enter angular velocity 0 or ±(0.1 ~ 1.0) rad/s: "))
203 node.set_angular(angular)
204 except Exception as e:
205 node.get_logger().error(f"Invalid input: {e}")
206 if rclpy.ok():
207 rclpy.shutdown()
208 return
209
210 node.get_logger().info("Setting velocity, moving for 5 seconds")
211 node.start_publish()
212
213 start = node.get_clock().now()
214 while (node.get_clock().now() - start).nanoseconds / 1e9 < 5.0:
215 rclpy.spin_once(node, timeout_sec=0.1)
216 time.sleep(0.001)
217
218 node.clear_velocity()
219 node.get_logger().info("5-second motion finished, robot stopped")
220
221 try:
222 rclpy.spin(node)
223 except KeyboardInterrupt:
224 pass
225 finally:
226 if rclpy.ok():
227 rclpy.shutdown()
228
229
230if __name__ == '__main__':
231 main()
6.1.11 Joint Motor Control
This example demonstrates how to use ROS2 and the Ruckig library to control robot joint motion.
Attention
Note ⚠️: Before running this example, stop the native MC module on the Motion Control Computing Unit (PC1) by running aima em stop-app mc to obtain control of the robot. Ensure the robot is safe before operating.
! This example directly controls the underlying motors (i.e. the HAL layer). Before running the program, please verify that the joint safety limits in the code match the actual robot model, and ensure safety!
Robot Joint Control Example
This example shows how to use ROS2 and the Ruckig library to control robot joints. The example implements the following features:
Robot joint model definition
Trajectory interpolation using Ruckig
Multi-joint coordinated control
Real-time position, velocity, and acceleration control
Example Function Overview
Four controller nodes are created to control:
Legs × 2 (12 joints)
Waist × 1 (3 joints)
Arms × 2 (14 joints)
Head × 1 (2 joints)
Demonstrated features:
Make a designated joint oscillate between ±0.5 radians every 10 seconds
Generate smooth motion trajectories using the Ruckig library
Publish joint control commands in real time
Customization
Add new control logic:
Modify the
set_target_positionfunctionAdd new control callback functions
Adjust control frequency:
Modify the
timerperiod (currently 3s) and the ruckigdt(currently 0.002s)
1#!/usr/bin/env python3
2"""
3Robot joint control example
4This script implements a ROS2-based robot joint control system, using the Ruckig trajectory
5planner to achieve smooth joint motion control.
6
7Main features:
81. Supports controlling multiple joint areas (head, arm, waist, leg)
92. Uses Ruckig for trajectory planning to ensure smooth motion
103. Supports real-time control of joint position, velocity, and acceleration
114. Provides joint limit and PID (stiffness/damping) parameter configuration
12"""
13
14import rclpy
15from rclpy.node import Node
16from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
17from aimdk_msgs.msg import JointCommandArray, JointStateArray, JointCommand
18import ruckig
19from enum import Enum
20from dataclasses import dataclass
21from typing import List, Dict
22from threading import Lock
23
24# QoS config: define ROS2 Quality of Service parameters
25# Subscriber/Publisher QoS: best-effort reliability, keep last 10 messages
26# (matches the robot-side joint command/state subscribers)
27qos = QoSProfile(
28 reliability=ReliabilityPolicy.BEST_EFFORT,
29 history=HistoryPolicy.KEEP_LAST,
30 depth=10,
31 durability=DurabilityPolicy.VOLATILE
32)
33
34
35class JointArea(Enum):
36 HEAD = 'HEAD' # Head joints
37 ARM = 'ARM' # Arm joints
38 WAIST = 'WAIST' # Waist joints
39 LEG = 'LEG' # Leg joints
40
41
42@dataclass
43class JointInfo:
44 # Joint information data class
45 name: str # Joint name
46 lower_limit: float # Joint lower angle limit
47 upper_limit: float # Joint upper angle limit
48 kp: float # Position control proportional gain
49 kd: float # Velocity control derivative gain
50
51
52# Robot model configuration: define all joint parameters
53robot_model: Dict[JointArea, List[JointInfo]] = {
54 # Leg joint configuration
55 JointArea.LEG: [
56 # Left leg joints
57 JointInfo("left_hip_pitch_joint", -2.704, 2.556, 40.0, 4.0),
58 JointInfo("left_hip_roll_joint", -0.235, 2.906, 40.0, 4.0),
59 JointInfo("left_hip_yaw_joint", -1.684, 3.430, 30.0, 3.0),
60 JointInfo("left_knee_joint", 0.0000, 2.4073, 80.0, 8.0),
61 JointInfo("left_ankle_pitch_joint", -0.803, 0.453, 40.0, 4.0),
62 JointInfo("left_ankle_roll_joint", -0.2625, 0.2625, 20.0, 2.0),
63 # Right leg joints
64 JointInfo("right_hip_pitch_joint", -2.704, 2.556, 40.0, 4.0),
65 JointInfo("right_hip_roll_joint", -2.906, 0.235, 40.0, 4.0),
66 JointInfo("right_hip_yaw_joint", -3.430, 1.684, 30.0, 3.0),
67 JointInfo("right_knee_joint", 0.0000, 2.4073, 80.0, 8.0),
68 JointInfo("right_ankle_pitch_joint", -0.803, 0.453, 40.0, 4.0),
69 JointInfo("right_ankle_roll_joint", -0.2625, 0.2625, 20.0, 2.0),
70 ],
71 # Waist joint configuration
72 JointArea.WAIST: [
73 JointInfo("waist_yaw_joint", -3.43, 2.382, 20.0, 4.0),
74 JointInfo("waist_pitch_joint", -0.314, 0.314, 20.0, 4.0),
75 JointInfo("waist_roll_joint", -0.488, 0.488, 20.0, 4.0),
76 ],
77 # Arm joint configuration
78 JointArea.ARM: [
79 # Left arm
80 JointInfo("left_shoulder_pitch_joint", -3.08, 2.04, 20.0, 2.0),
81 JointInfo("left_shoulder_roll_joint", -0.061, 2.993, 20.0, 2.0),
82 JointInfo("left_shoulder_yaw_joint", -2.556, 2.556, 20.0, 2.0),
83 JointInfo("left_elbow_joint", -2.3556, 0.0, 20.0, 2.0),
84 JointInfo("left_wrist_yaw_joint", -2.556, 2.556, 20.0, 2.0),
85 JointInfo("left_wrist_pitch_joint", -0.558, 0.558, 20.0, 2.0),
86 JointInfo("left_wrist_roll_joint", -1.571, 0.724, 20.0, 2.0),
87 # Right arm
88 JointInfo("right_shoulder_pitch_joint", -3.08, 2.04, 20.0, 2.0),
89 JointInfo("right_shoulder_roll_joint", -2.993, 0.061, 20.0, 2.0),
90 JointInfo("right_shoulder_yaw_joint", -2.556, 2.556, 20.0, 2.0),
91 JointInfo("right_elbow_joint", -2.3556, 0.0000, 20.0, 2.0),
92 JointInfo("right_wrist_yaw_joint", -2.556, 2.556, 20.0, 2.0),
93 JointInfo("right_wrist_pitch_joint", -0.558, 0.558, 20.0, 2.0),
94 JointInfo("right_wrist_roll_joint", -0.724, 1.571, 20.0, 2.0),
95 ],
96 # Head joint configuration
97 JointArea.HEAD: [
98 JointInfo("head_yaw_joint", -0.366, 0.366, 20.0, 2.0),
99 JointInfo("head_pitch_joint", -0.3838, 0.3838, 20.0, 2.0),
100 ],
101}
102
103
104class JointControllerNode(Node):
105 """
106 Joint controller node
107 Responsible for receiving joint states, using Ruckig for trajectory planning,
108 and publishing joint commands.
109 """
110
111 def __init__(self, node_name: str, sub_topic: str, pub_topic: str, area: JointArea, dofs: int):
112 """
113 Initialize joint controller
114 Args:
115 node_name: node name
116 sub_topic: topic name to subscribe (joint states)
117 pub_topic: topic name to publish (joint commands)
118 area: joint area (head/arm/waist/leg)
119 dofs: number of DOFs
120 """
121 super().__init__(node_name)
122 self.lock = Lock()
123 self.joint_info = robot_model[area]
124 self.dofs = dofs
125 self.ruckig = ruckig.Ruckig(dofs, 0.002) # 2 ms control period
126 self.input = ruckig.InputParameter(dofs)
127 self.output = ruckig.OutputParameter(dofs)
128 self.ruckig_initialized = False
129
130 # Initialize trajectory parameters
131 self.input.current_position = [0.0] * dofs
132 self.input.current_velocity = [0.0] * dofs
133 self.input.current_acceleration = [0.0] * dofs
134
135 # Motion limits
136 self.input.max_velocity = [1.0] * dofs
137 self.input.max_acceleration = [1.0] * dofs
138 self.input.max_jerk = [25.0] * dofs
139
140 # ROS2 subscriber and publisher
141 self.sub = self.create_subscription(
142 JointStateArray,
143 sub_topic,
144 self.joint_state_callback,
145 qos
146 )
147 self.pub = self.create_publisher(
148 JointCommandArray,
149 pub_topic,
150 qos
151 )
152
153 def joint_state_callback(self, msg: JointStateArray):
154 """
155 Joint state callback
156 Receives and processes joint state messages
157 """
158 self.ruckig_initialized = True
159
160 def control_callback(self, joint_idx):
161 """
162 Control callback
163 Uses Ruckig for trajectory planning and publishes control commands
164 Args:
165 joint_idx: target joint index
166 """
167 # Run Ruckig until the target is reached
168 while self.ruckig.update(self.input, self.output) in [ruckig.Result.Working, ruckig.Result.Finished]:
169 # Update current state
170 self.input.current_position = self.output.new_position
171 self.input.current_velocity = self.output.new_velocity
172 self.input.current_acceleration = self.output.new_acceleration
173
174 # Check if target is reached
175 tolerance = 1e-6
176 current_p = self.output.new_position[joint_idx]
177 if abs(current_p - self.input.target_position[joint_idx]) < tolerance:
178 break
179
180 # Create and publish command
181 cmd = JointCommandArray()
182 for i, joint in enumerate(self.joint_info):
183 j = JointCommand()
184 j.name = joint.name
185 j.position = self.output.new_position[i]
186 j.velocity = self.output.new_velocity[i]
187 j.effort = 0.0
188 j.stiffness = joint.kp
189 j.damping = joint.kd
190 cmd.joints.append(j)
191
192 self.pub.publish(cmd)
193
194 def set_target_position(self, joint_name, position):
195 """
196 Set target joint position
197 Args:
198 joint_name: joint name
199 position: target position
200 """
201 p_s = [0.0] * self.dofs
202 joint_idx = 0
203 for i, joint in enumerate(self.joint_info):
204 if joint.name == joint_name:
205 p_s[i] = position
206 joint_idx = i
207 self.input.target_position = p_s
208 self.input.target_velocity = [0.0] * self.dofs
209 self.input.target_acceleration = [0.0] * self.dofs
210 self.control_callback(joint_idx)
211
212
213def main(args=None):
214 """
215 Main function
216 Initialize ROS2 node and start joint controller
217 """
218 rclpy.init(args=args)
219
220 # Create leg controller node
221 leg_node = JointControllerNode(
222 "leg_node",
223 "/aima/hal/joint/leg/state",
224 "/aima/hal/joint/leg/command",
225 JointArea.LEG,
226 12
227 )
228
229 # waist_node = JointControllerNode(
230 # "waist_node",
231 # "/aima/hal/joint/waist/state",
232 # "/aima/hal/joint/waist/command",
233 # JointArea.WAIST,
234 # 3
235 # )
236
237 # arm_node = JointControllerNode(
238 # "arm_node",
239 # "/aima/hal/joint/arm/state",
240 # "/aima/hal/joint/arm/command",
241 # JointArea.ARM,
242 # 14
243 # )
244
245 # head_node = JointControllerNode(
246 # "head_node",
247 # "/aima/hal/joint/head/state",
248 # "/aima/hal/joint/head/command",
249 # JointArea.HEAD,
250 # 2
251 # )
252
253 position = 0.8
254
255 # Only control the left leg joint. If you want to control a specific joint, assign it directly.
256 def timer_callback():
257 """
258 Timer callback
259 Periodically change target position to achieve oscillating motion
260 """
261 nonlocal position
262 position = -position
263 position = 1.3 + position
264 leg_node.set_target_position("left_knee_joint", position)
265
266 # arm_node.set_target_position("left_shoulder_pitch_joint", position)
267 # waist_node.set_target_position("waist_yaw_joint", position)
268 # head_node.set_target_position("head_pitch_joint", position)
269
270 leg_node.create_timer(3.0, timer_callback)
271
272 # Multi-threaded executor
273 executor = rclpy.executors.MultiThreadedExecutor()
274 executor.add_node(leg_node)
275
276 # executor.add_node(waist_node)
277 # executor.add_node(arm_node)
278 # executor.add_node(head_node)
279
280 try:
281 executor.spin()
282 except KeyboardInterrupt:
283 pass
284 finally:
285 leg_node.destroy_node()
286 # waist_node.destroy_node()
287 # arm_node.destroy_node()
288 # head_node.destroy_node()
289 if rclpy.ok():
290 rclpy.shutdown()
291
292
293if __name__ == '__main__':
294 main()
6.1.12 Keyboard Robot Control
This example implements robot forward/backward and turning control using keyboard input from a PC.
Use W A S D to control robot direction; increase/decrease speed by ±0.2 m/s. Use Q/E to increase/decrease angular speed by ±0.1 rad/s. Press ESC to exit and release terminal resources. Press Space to immediately zero speed (emergency stop).
Caution
Note: Before running this example, use the controller to put the robot into the Stable Standing Mode mode. (For position-control stand / locomotion modes press R2 + X; see the mode transition diagram for other modes.) Then run aima em stop-app rc on the robot’s terminal to stop the remote controller and prevent channel occupation.
You must register an input source before using keyboard control (this example registers one).
1#!/usr/bin/env python3
2
3import rclpy
4import rclpy.logging
5from rclpy.node import Node
6from aimdk_msgs.msg import McLocomotionVelocity, McInputAction, MessageHeader
7from aimdk_msgs.srv import SetMcInputSource
8import curses
9
10
11class KeyboardVelocityController(Node):
12 def __init__(self, stdscr):
13 super().__init__('keyboard_velocity_controller')
14 self.stdscr = stdscr
15 self.forward_velocity = 0.0
16 self.lateral_velocity = 0.0
17 self.angular_velocity = 0.0
18 self.step = 0.2
19 self.angular_step = 0.1
20
21 self.publisher = self.create_publisher(
22 McLocomotionVelocity, '/aima/mc/locomotion/velocity', 10)
23 self.client = self.create_client(
24 SetMcInputSource, '/aimdk_5Fmsgs/srv/SetMcInputSource')
25
26 if not self.register_input_source():
27 self.get_logger().error("❌ Input source registration failed, exiting")
28 raise RuntimeError("Failed to register input source")
29
30 # Configure curses
31 curses.cbreak()
32 curses.noecho()
33 self.stdscr.keypad(True)
34 self.stdscr.nodelay(True)
35
36 self.get_logger().info(
37 "Control started: W/S forward/backward, A/D strafe, Q/E turn, Space stop, Esc exit")
38
39 # Timer: check keyboard every 50 ms
40 self.timer = self.create_timer(0.05, self.check_key_and_publish)
41
42 def _call_input_source(self, action_value):
43 action_name = {
44 McInputAction.INPUTACTION_ADD: "ADD",
45 McInputAction.INPUTACTION_ENABLE: "ENABLE",
46 }.get(action_value, str(action_value))
47
48 req = SetMcInputSource.Request()
49 req.action.value = action_value
50 req.input_source.name = "node.keyboard"
51 req.input_source.priority = 40
52 req.input_source.timeout = 1000
53
54 future = None
55 for i in range(8):
56 req.request.header.stamp = self.get_clock().now().to_msg()
57 future = self.client.call_async(req)
58 rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)
59 if future.done():
60 break
61 # retry as remote peer is NOT handled well by ROS
62 self.get_logger().info(
63 f"trying input source action {action_name} ({action_value})... [{i}]")
64
65 if future is None or not future.done():
66 return False
67 try:
68 resp = future.result()
69 except Exception as e:
70 self.get_logger().error(f"Service exception: {str(e)}")
71 return False
72 return resp is not None and resp.response.header.code == 0
73
74 def register_input_source(self):
75 self.get_logger().info("Registering input source...")
76
77 timeout_sec = 8.0
78 start = self.get_clock().now().nanoseconds / 1e9
79
80 while not self.client.wait_for_service(timeout_sec=2.0):
81 now = self.get_clock().now().nanoseconds / 1e9
82 if now - start > timeout_sec:
83 self.get_logger().error("Waiting for service timed out")
84 return False
85 self.get_logger().info("Waiting for input source service...")
86
87 # Try ADD first; if already registered, fall back to ENABLE
88 if self._call_input_source(McInputAction.INPUTACTION_ADD): # ADD (1001)
89 self.get_logger().info("✅ Input source registered (ADD).")
90 return True
91 self.get_logger().info(
92 "ADD failed (likely already registered), trying ENABLE...")
93 if self._call_input_source(McInputAction.INPUTACTION_ENABLE): # ENABLE (2001)
94 self.get_logger().info("✅ Input source enabled.")
95 return True
96 self.get_logger().error(
97 "❌ Failed to register input source via ADD or ENABLE.")
98 return False
99
100 def check_key_and_publish(self):
101 try:
102 ch = self.stdscr.getch()
103 except Exception:
104 ch = -1
105
106 if ch != -1:
107 if ch == ord(' '):
108 self.forward_velocity = 0.0
109 self.lateral_velocity = 0.0
110 self.angular_velocity = 0.0
111 elif ch == ord('w'):
112 self.forward_velocity = min(
113 self.forward_velocity + self.step, 1.0)
114 elif ch == ord('s'):
115 self.forward_velocity = max(
116 self.forward_velocity - self.step, -1.0)
117 elif ch == ord('a'):
118 self.lateral_velocity = min(
119 self.lateral_velocity + self.step, 1.0)
120 elif ch == ord('d'):
121 self.lateral_velocity = max(
122 self.lateral_velocity - self.step, -1.0)
123 elif ch == ord('q'):
124 self.angular_velocity = min(
125 self.angular_velocity + self.angular_step, 1.0)
126 elif ch == ord('e'):
127 self.angular_velocity = max(
128 self.angular_velocity - self.angular_step, -1.0)
129 elif ch == 27: # ESC
130 self.get_logger().info("Exiting control")
131 self.timer.cancel()
132 return
133
134 msg = McLocomotionVelocity()
135 msg.header = MessageHeader()
136 msg.header.stamp = self.get_clock().now().to_msg()
137 msg.source = "node.keyboard"
138 msg.forward_velocity = self.forward_velocity
139 msg.lateral_velocity = self.lateral_velocity
140 msg.angular_velocity = self.angular_velocity
141
142 self.publisher.publish(msg)
143
144 # Update UI
145 self.stdscr.clear()
146 self.stdscr.addstr(
147 0, 0, "W/S: Forward/Backward | A/D: Strafe | Q/E: Turn | Space: Stop | ESC: Exit")
148 self.stdscr.addstr(2, 0,
149 f"Speed Status: Forward: {self.forward_velocity:.2f} m/s | "
150 f"Lateral: {self.lateral_velocity:.2f} m/s | "
151 f"Angular: {self.angular_velocity:.2f} rad/s")
152 self.stdscr.refresh()
153
154
155def curses_main(stdscr):
156 rclpy.init()
157 try:
158 node = KeyboardVelocityController(stdscr)
159 while not node.timer.is_canceled():
160 rclpy.spin_once(node, timeout_sec=0.1)
161 except (Exception, KeyboardInterrupt) as e:
162 rclpy.logging.get_logger("main").fatal(
163 f"Program exited with exception: {e}")
164 finally:
165 curses.endwin()
166 if rclpy.ok():
167 rclpy.shutdown()
168
169
170def main():
171 curses.wrapper(curses_main)
172
173
174if __name__ == '__main__':
175 main()
6.1.13 Take Photo
This example uses take_photo. Before running the node, set the camera topic to capture. When started, the node creates an /images/ directory and saves the current frame into that directory.
1#!/usr/bin/env python3
2import threading
3import time
4from pathlib import Path
5
6import rclpy
7import rclpy.task
8from rclpy.node import Node
9from rclpy.qos import qos_profile_sensor_data
10from sensor_msgs.msg import Image
11from cv_bridge import CvBridge
12import cv2
13
14
15class SaveOneRawPy(Node):
16 def __init__(self):
17 super().__init__('save_one_image')
18
19 # parameter: image topic
20 self.declare_parameter(
21 'image_topic', '/aima/hal/sensor/stereo_head_front_left/rgb_image'
22 )
23 self.topic = self.get_parameter(
24 'image_topic').get_parameter_value().string_value
25
26 # save directory
27 self.save_dir = Path('images').resolve()
28 self.save_dir.mkdir(parents=True, exist_ok=True)
29
30 self._bridge = CvBridge()
31
32 self._future = rclpy.task.Future()
33 self._lock = threading.Lock()
34
35 # subscriber (sensor QoS)
36 self.sub = self.create_subscription(
37 Image,
38 self.topic,
39 self.image_cb,
40 qos_profile_sensor_data
41 )
42 self.get_logger().info(f'Subscribing to raw image: {self.topic}')
43 self.get_logger().info(f'Images will be saved to: {self.save_dir}')
44
45 def image_cb(self, msg: Image):
46 with self._lock:
47 if self._future.done():
48 return
49
50 try:
51 enc = msg.encoding.lower()
52 self.get_logger().info(f'Received image with encoding: {enc}')
53
54 img = self._bridge.imgmsg_to_cv2(
55 msg, desired_encoding='passthrough')
56
57 if enc == 'rgb8':
58 img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
59 elif enc == 'mono8':
60 img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
61
62 ts_ms = int(time.time() * 1000)
63 out_path = self.save_dir / f'frame_{ts_ms}.png'
64
65 ok = cv2.imwrite(str(out_path), img)
66 if ok:
67 self.get_logger().info(
68 f'Saved image: {out_path} ({img.shape[1]}x{img.shape[0]})'
69 )
70 with self._lock:
71 if not self._future.done():
72 self._future.set_result(str(out_path))
73 else:
74 self.get_logger().error(f'cv2.imwrite failed: {out_path}')
75 except Exception as e:
76 self.get_logger().error(f'Failed to decode / save image: {e}')
77 with self._lock:
78 if not self._future.done():
79 self._future.set_exception(e)
80
81
82def main():
83 rclpy.init()
84 node = SaveOneRawPy()
85
86 try:
87 rclpy.spin_until_future_complete(node, node._future)
88 except KeyboardInterrupt:
89 pass
90 finally:
91 node.destroy_node()
92 if rclpy.ok():
93 rclpy.shutdown()
94
95
96if __name__ == '__main__':
97 main()
6.1.14 Camera Streaming Example Collection
This example collection provides multiple camera subscription and processing demos, supporting depth, stereo, and mono camera streams.
These camera subscription examples are not application-level; they only print basic camera data. If you are familiar with ROS2, you can achieve similar results with ros2 topic echo + ros2 topic hz. You can consult the SDK topic list to jump directly into module development or use these camera examples as scaffolding for your own logic. The published sensor data are raw (no preprocessing like undistortion). For detailed sensor information (e.g., resolution, focal length), check the corresponding camera_info topic.
Depth Camera Data Subscription
This example uses echo_camera_rgbd to subscribe to /aima/hal/sensor/rgbd_head_front/ and receive the robot’s depth camera data. It supports depth images, RGB images, compressed RGB images, and camera intrinsics.
Features:
Supports multiple data type subscriptions (depth image, RGB image, compressed image, camera intrinsics)
Real-time FPS statistics and data display
Supports RGB video recording
Configurable topic type selection
Supported Data Types:
depth_image: Depth image (sensor_msgs/Image)rgb_image: RGB image (sensor_msgs/Image)rgb_image_compressed: Compressed RGB image (sensor_msgs/CompressedImage)camera_info: Camera intrinsic parameters (sensor_msgs/CameraInfo)
1#!/usr/bin/env python3
2"""
3Head depth camera multi-topic subscription example
4
5Supports selecting the topic type to subscribe via startup parameter --ros-args -p topic_type:=<type>:
6 - depth_image: Depth image (sensor_msgs/Image)
7 - depth_camera_info: Camera intrinsic parameters (sensor_msgs/CameraInfo)
8 - rgb_image: RGB image (sensor_msgs/Image)
9 - rgb_image_compressed: RGB compressed image (sensor_msgs/CompressedImage)
10 - rgb_camera_info: Camera intrinsic parameters (sensor_msgs/CameraInfo)
11
12Example:
13 ros2 run py_examples echo_camera_rgbd --ros-args -p topic_type:=rgb_image
14 ros2 run py_examples echo_camera_rgbd --ros-args -p topic_type:=rgb_camera_info
15
16Default topic_type is rgb_image
17"""
18
19import rclpy
20from rclpy.node import Node
21from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
22from sensor_msgs.msg import Image, CompressedImage, CameraInfo
23from collections import deque
24import cv2
25import numpy as np
26
27
28class CameraTopicEcho(Node):
29 def __init__(self):
30 super().__init__('camera_topic_echo')
31
32 # Select the topic type to subscribe
33 self.declare_parameter('topic_type', 'rgb_image')
34 self.declare_parameter('dump_video_path', '')
35 self.declare_parameter('dump_video_fps', 30.0)
36
37 self.topic_type = self.get_parameter('topic_type').value
38 self.dump_video_path = self.get_parameter('dump_video_path').value
39 self.dump_video_fps = self.get_parameter('dump_video_fps').value
40 self.video_writer = None
41 self.frame_count = 0
42
43 # SensorDataQoS: BEST_EFFORT + VOLATILE
44 qos = QoSProfile(
45 reliability=QoSReliabilityPolicy.BEST_EFFORT,
46 history=QoSHistoryPolicy.KEEP_LAST,
47 depth=5
48 )
49
50 # Create different subscribers based on topic_type
51 if self.topic_type == "depth_image":
52 self.topic_name = "/aima/hal/sensor/rgbd_head_front/depth_image"
53 self.sub_image = self.create_subscription(
54 Image, self.topic_name, self.cb_image, qos)
55 self.get_logger().info(
56 f"✅ Subscribing Depth Image: {self.topic_name}")
57
58 elif self.topic_type == "rgb_image":
59 self.topic_name = "/aima/hal/sensor/rgbd_head_front/rgb_image"
60 self.sub_image = self.create_subscription(
61 Image, self.topic_name, self.cb_image, qos)
62 self.get_logger().info(
63 f"✅ Subscribing RGB Image: {self.topic_name}")
64 if self.dump_video_path:
65 self.get_logger().info(
66 f"📝 Will dump received images to video: {self.dump_video_path}")
67
68 elif self.topic_type == "rgb_image_compressed":
69 self.topic_name = "/aima/hal/sensor/rgbd_head_front/rgb_image/compressed"
70 self.sub_compressed = self.create_subscription(
71 CompressedImage, self.topic_name, self.cb_compressed, qos)
72 self.get_logger().info(
73 f"✅ Subscribing CompressedImage: {self.topic_name}")
74
75 elif self.topic_type == "rgb_camera_info":
76 self.topic_name = "/aima/hal/sensor/rgbd_head_front/rgb_camera_info"
77 # RGB-D CameraInfo subscriptions are different from other cameras. The best_effort + volatile QoS is sufficient for 30Hz rgb_camera_info
78 self.sub_camerainfo = self.create_subscription(
79 CameraInfo, self.topic_name, self.cb_camerainfo, qos)
80 self.get_logger().info(
81 f"✅ Subscribing RGB CameraInfo: {self.topic_name}")
82
83 elif self.topic_type == "depth_camera_info":
84 self.topic_name = "/aima/hal/sensor/rgbd_head_front/depth_camera_info"
85 # RGB-D CameraInfo subscriptions are different from other cameras. The best_effort + volatile QoS is sufficient for 30Hz depth_camera_info
86 self.sub_camerainfo = self.create_subscription(
87 CameraInfo, self.topic_name, self.cb_camerainfo, qos)
88 self.get_logger().info(
89 f"✅ Subscribing Depth CameraInfo: {self.topic_name}")
90
91 else:
92 self.get_logger().error(f"Unknown topic_type: {self.topic_type}")
93 raise ValueError("Unknown topic_type")
94
95 # Internal state
96 self.last_print = self.get_clock().now()
97 self.print_allowed = False
98 self.arrivals = deque()
99
100 def update_arrivals(self):
101 """Calculate received FPS"""
102 now = self.get_clock().now()
103 self.arrivals.append(now)
104 while self.arrivals and (now - self.arrivals[0]).nanoseconds * 1e-9 > 1.0:
105 self.arrivals.popleft()
106
107 def get_fps(self):
108 """Get FPS"""
109 return len(self.arrivals)
110
111 def should_print(self, master=True):
112 """Control print frequency"""
113 if not master:
114 return self.print_allowed
115 now = self.get_clock().now()
116 if (now - self.last_print).nanoseconds * 1e-9 >= 1.0:
117 self.last_print = now
118 self.print_allowed = True
119 else:
120 self.print_allowed = False
121 return self.print_allowed
122
123 def cb_image(self, msg: Image):
124 """Image callback (Depth/RGB image)"""
125 self.update_arrivals()
126
127 if self.should_print():
128 stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
129 self.get_logger().info(
130 f"📸 {self.topic_type} received\n"
131 f" • frame_id: {msg.header.frame_id}\n"
132 f" • stamp (sec): {stamp_sec:.6f}\n"
133 f" • encoding: {msg.encoding}\n"
134 f" • size (WxH): {msg.width} x {msg.height}\n"
135 f" • step (bytes/row):{msg.step}\n"
136 f" • is_bigendian: {msg.is_bigendian}\n"
137 f" • recv FPS (1s): {self.get_fps():.1f}"
138 )
139
140 # Only RGB image supports video dump
141 if self.topic_type == "rgb_image" and self.dump_video_path:
142 self.dump_image_to_video(msg)
143
144 def cb_compressed(self, msg: CompressedImage):
145 """CompressedImage callback"""
146 self.update_arrivals()
147
148 if self.should_print():
149 stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
150 self.get_logger().info(
151 f"🗜️ CompressedImage received\n"
152 f" • frame_id: {msg.header.frame_id}\n"
153 f" • stamp (sec): {stamp_sec:.6f}\n"
154 f" • format: {msg.format}\n"
155 f" • data size: {len(msg.data)}\n"
156 f" • recv FPS (1s): {self.get_fps():.1f}"
157 )
158
159 def cb_camerainfo(self, msg: CameraInfo):
160 """CameraInfo callback (camera intrinsic parameters)"""
161 # Camera info will only receive one frame, print it directly
162 stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
163
164 # Format D array
165 d_str = ", ".join([f"{d:.6f}" for d in msg.d])
166
167 # Format K matrix
168 k_str = ", ".join([f"{k:.6f}" for k in msg.k])
169
170 # Format P matrix
171 p_str = ", ".join([f"{p:.6f}" for p in msg.p])
172
173 self.get_logger().info(
174 f"📷 {self.topic_type} received\n"
175 f" • frame_id: {msg.header.frame_id}\n"
176 f" • stamp (sec): {stamp_sec:.6f}\n"
177 f" • width x height: {msg.width} x {msg.height}\n"
178 f" • distortion_model:{msg.distortion_model}\n"
179 f" • D: [{d_str}]\n"
180 f" • K: [{k_str}]\n"
181 f" • P: [{p_str}]\n"
182 f" • binning_x: {msg.binning_x}\n"
183 f" • binning_y: {msg.binning_y}\n"
184 f" • roi: {{ x_offset: {msg.roi.x_offset}, y_offset: {msg.roi.y_offset}, height: {msg.roi.height}, width: {msg.roi.width}, do_rectify: {msg.roi.do_rectify} }}"
185 )
186
187 def dump_image_to_video(self, msg: Image):
188 """Dump RGB image to video file using mp4v encoding"""
189 if msg.encoding != "rgb8":
190 self.get_logger().warn(
191 f"image encoding not expected: {msg.encoding}")
192 return
193
194 np_array = np.frombuffer(msg.data, dtype=np.uint8).reshape(
195 msg.height, msg.width, 3)
196 cv_image = cv2.cvtColor(np_array, cv2.COLOR_RGB2BGR)
197
198 if self.video_writer is None:
199 fourcc = cv2.VideoWriter_fourcc(*'mp4v')
200 height, width = cv_image.shape[:2]
201 self.video_writer = cv2.VideoWriter(
202 self.dump_video_path, fourcc, self.dump_video_fps, (width, height))
203 if not self.video_writer.isOpened():
204 self.get_logger().error(
205 f"Failed to open video writer: {self.dump_video_path}")
206 self.dump_video_path = ''
207 return
208 self.get_logger().info(
209 f"📹 Video writer initialized: {width}x{height} @ {self.dump_video_fps}fps")
210
211 self.video_writer.write(cv_image)
212 self.frame_count += 1
213
214 if self.frame_count % 30 == 0:
215 self.get_logger().info(
216 f"📹 Written {self.frame_count} frames to video")
217
218 def destroy_node(self):
219 if self.video_writer is not None:
220 self.video_writer.release()
221 self.get_logger().info(
222 f"📹 Video saved: {self.dump_video_path} ({self.frame_count} frames)")
223 super().destroy_node()
224
225
226def main(args=None):
227 rclpy.init(args=args)
228 try:
229 node = CameraTopicEcho()
230 rclpy.spin(node)
231 except KeyboardInterrupt:
232 pass
233 except Exception as e:
234 print(f"Error: {e}")
235 finally:
236 if 'node' in locals():
237 node.destroy_node()
238 if rclpy.ok():
239 rclpy.shutdown()
240
241
242if __name__ == '__main__':
243 main()
Usage Instructions:
Subscribe to RGB image data:
ros2 run py_examples echo_camera_rgbd --ros-args -p topic_type:=rgb_image
Subscribe to camera intrinsic parameters:
ros2 run py_examples echo_camera_rgbd --ros-args -p topic_type:=rgb_camera_info ros2 run py_examples echo_camera_rgbd --ros-args -p topic_type:=depth_camera_info
Record RGB video:
# You can change dump_video_path to another path; ensure the directory exists before saving ros2 run py_examples echo_camera_rgbd --ros-args -p topic_type:=rgb_image -p dump_video_path:=$PWD/output.mp4
Stereo Camera Data Subscription
This example uses echo_camera_stereo to subscribe to /aima/hal/sensor/stereo_head_front_*/ and receive stereo camera data from the robot, supporting left/right RGB images, compressed images, and camera intrinsics.
Features:
Supports independent subscription for left and right cameras
Real-time FPS statistics and data display
Supports RGB video recording
Configurable camera selection (left/right)
Supported Data Types:
left_rgb_image: Left camera RGB image (sensor_msgs/Image)left_rgb_image_compressed: Left camera compressed RGB image (sensor_msgs/CompressedImage)left_camera_info: Left camera intrinsics (sensor_msgs/CameraInfo)right_rgb_image: Right camera RGB image (sensor_msgs/Image)right_rgb_image_compressed: Right camera compressed RGB image (sensor_msgs/CompressedImage)right_camera_info: Right camera intrinsics (sensor_msgs/CameraInfo)
1#!/usr/bin/env python3
2"""
3Head stereo camera multi-topic subscription example
4
5Supports selecting the topic type to subscribe via startup parameter --ros-args -p topic_type:=<type>:
6 - left_rgb_image: Left camera RGB image (sensor_msgs/Image)
7 - left_rgb_image_compressed: Left camera RGB compressed image (sensor_msgs/CompressedImage)
8 - left_camera_info: Left camera intrinsic parameters (sensor_msgs/CameraInfo)
9 - right_rgb_image: Right camera RGB image (sensor_msgs/Image)
10 - right_rgb_image_compressed: Right camera RGB compressed image (sensor_msgs/CompressedImage)
11 - right_camera_info: Right camera intrinsic parameters (sensor_msgs/CameraInfo)
12
13Example:
14 ros2 run py_examples echo_camera_stereo --ros-args -p topic_type:=left_rgb_image
15 ros2 run py_examples echo_camera_stereo --ros-args -p topic_type:=right_rgb_image
16 ros2 run py_examples echo_camera_stereo --ros-args -p topic_type:=left_camera_info
17
18Default topic_type is left_rgb_image
19"""
20
21import rclpy
22from rclpy.node import Node
23from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSDurabilityPolicy, QoSHistoryPolicy
24from sensor_msgs.msg import Image, CompressedImage, CameraInfo
25from collections import deque
26import cv2
27import numpy as np
28
29
30class StereoCameraTopicEcho(Node):
31 def __init__(self):
32 super().__init__('stereo_camera_topic_echo')
33
34 # Select the topic type to subscribe
35 self.declare_parameter('topic_type', 'left_rgb_image')
36 self.declare_parameter('dump_video_path', '')
37 self.declare_parameter('dump_video_fps', 10.0)
38
39 self.topic_type = self.get_parameter('topic_type').value
40 self.dump_video_path = self.get_parameter('dump_video_path').value
41 self.dump_video_fps = self.get_parameter('dump_video_fps').value
42 self.video_writer = None
43 self.frame_count = 0
44
45 # Set QoS parameters - use sensor data QoS
46 qos = QoSProfile(
47 reliability=QoSReliabilityPolicy.BEST_EFFORT,
48 history=QoSHistoryPolicy.KEEP_LAST,
49 depth=5,
50 durability=QoSDurabilityPolicy.VOLATILE
51 )
52
53 # Create different subscribers based on topic_type
54 if self.topic_type == "left_rgb_image":
55 self.topic_name = "/aima/hal/sensor/stereo_head_front_left/rgb_image"
56 self.sub_image = self.create_subscription(
57 Image, self.topic_name, self.cb_image, qos)
58 self.get_logger().info(
59 f"✅ Subscribing Left RGB Image: {self.topic_name}")
60 if self.dump_video_path:
61 self.get_logger().info(
62 f"📝 Will dump received images to video: {self.dump_video_path}")
63
64 elif self.topic_type == "left_rgb_image_compressed":
65 self.topic_name = "/aima/hal/sensor/stereo_head_front_left/rgb_image/compressed"
66 self.sub_compressed = self.create_subscription(
67 CompressedImage, self.topic_name, self.cb_compressed, qos)
68 self.get_logger().info(
69 f"✅ Subscribing Left CompressedImage: {self.topic_name}")
70
71 elif self.topic_type == "left_camera_info":
72 self.topic_name = "/aima/hal/sensor/stereo_head_front_left/camera_info"
73 # CameraInfo subscription must use reliable + transient_local QoS to receive historical messages (even if only one frame is published)
74 camera_qos = QoSProfile(
75 reliability=QoSReliabilityPolicy.RELIABLE,
76 history=QoSHistoryPolicy.KEEP_LAST,
77 depth=1,
78 durability=QoSDurabilityPolicy.TRANSIENT_LOCAL
79 )
80 self.sub_camerainfo = self.create_subscription(
81 CameraInfo, self.topic_name, self.cb_camerainfo, camera_qos)
82 self.get_logger().info(
83 f"✅ Subscribing Left CameraInfo (with transient_local): {self.topic_name}")
84
85 elif self.topic_type == "right_rgb_image":
86 self.topic_name = "/aima/hal/sensor/stereo_head_front_right/rgb_image"
87 self.sub_image = self.create_subscription(
88 Image, self.topic_name, self.cb_image, qos)
89 self.get_logger().info(
90 f"✅ Subscribing Right RGB Image: {self.topic_name}")
91 if self.dump_video_path:
92 self.get_logger().info(
93 f"📝 Will dump received images to video: {self.dump_video_path}")
94
95 elif self.topic_type == "right_rgb_image_compressed":
96 self.topic_name = "/aima/hal/sensor/stereo_head_front_right/rgb_image/compressed"
97 self.sub_compressed = self.create_subscription(
98 CompressedImage, self.topic_name, self.cb_compressed, qos)
99 self.get_logger().info(
100 f"✅ Subscribing Right CompressedImage: {self.topic_name}")
101
102 elif self.topic_type == "right_camera_info":
103 self.topic_name = "/aima/hal/sensor/stereo_head_front_right/camera_info"
104 # CameraInfo subscription must use reliable + transient_local QoS to receive historical messages (even if only one frame is published)
105 camera_qos = QoSProfile(
106 reliability=QoSReliabilityPolicy.RELIABLE,
107 history=QoSHistoryPolicy.KEEP_LAST,
108 depth=1,
109 durability=QoSDurabilityPolicy.TRANSIENT_LOCAL
110 )
111 self.sub_camerainfo = self.create_subscription(
112 CameraInfo, self.topic_name, self.cb_camerainfo, camera_qos)
113 self.get_logger().info(
114 f"✅ Subscribing Right CameraInfo (with transient_local): {self.topic_name}")
115
116 else:
117 self.get_logger().error(f"Unknown topic_type: {self.topic_type}")
118 raise ValueError("Unknown topic_type")
119
120 # Internal state
121 self.last_print = self.get_clock().now()
122 self.print_allowed = False
123 self.arrivals = deque()
124
125 def update_arrivals(self):
126 """Calculate received FPS"""
127 now = self.get_clock().now()
128 self.arrivals.append(now)
129 while self.arrivals and (now - self.arrivals[0]).nanoseconds * 1e-9 > 1.0:
130 self.arrivals.popleft()
131
132 def get_fps(self):
133 """Get FPS"""
134 return len(self.arrivals)
135
136 def should_print(self, master=True):
137 """Control print frequency"""
138 if not master:
139 return self.print_allowed
140 now = self.get_clock().now()
141 if (now - self.last_print).nanoseconds * 1e-9 >= 1.0:
142 self.last_print = now
143 self.print_allowed = True
144 else:
145 self.print_allowed = False
146 return self.print_allowed
147
148 def cb_image(self, msg: Image):
149 """Image callback (Left/Right camera RGB image)"""
150 self.update_arrivals()
151
152 if self.should_print():
153 stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
154 self.get_logger().info(
155 f"📸 {self.topic_type} received\n"
156 f" • frame_id: {msg.header.frame_id}\n"
157 f" • stamp (sec): {stamp_sec:.6f}\n"
158 f" • encoding: {msg.encoding}\n"
159 f" • size (WxH): {msg.width} x {msg.height}\n"
160 f" • step (bytes/row):{msg.step}\n"
161 f" • is_bigendian: {msg.is_bigendian}\n"
162 f" • recv FPS (1s): {self.get_fps():.1f}"
163 )
164
165 # Only RGB images support video dump
166 if (self.topic_type in ["left_rgb_image", "right_rgb_image"]) and self.dump_video_path:
167 self.dump_image_to_video(msg)
168
169 def cb_compressed(self, msg: CompressedImage):
170 """CompressedImage callback (Left/Right camera RGB compressed image)"""
171 self.update_arrivals()
172
173 if self.should_print():
174 stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
175 self.get_logger().info(
176 f"🗜️ {self.topic_type} received\n"
177 f" • frame_id: {msg.header.frame_id}\n"
178 f" • stamp (sec): {stamp_sec:.6f}\n"
179 f" • format: {msg.format}\n"
180 f" • data size: {len(msg.data)}\n"
181 f" • recv FPS (1s): {self.get_fps():.1f}"
182 )
183
184 def cb_camerainfo(self, msg: CameraInfo):
185 """CameraInfo callback (Left/Right camera intrinsic parameters)"""
186 # Camera info will only receive one frame, print it directly
187 stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
188
189 # Format D array
190 d_str = ", ".join([f"{d:.6f}" for d in msg.d])
191
192 # Format K matrix
193 k_str = ", ".join([f"{k:.6f}" for k in msg.k])
194
195 # Format P matrix
196 p_str = ", ".join([f"{p:.6f}" for p in msg.p])
197
198 self.get_logger().info(
199 f"📷 {self.topic_type} received\n"
200 f" • frame_id: {msg.header.frame_id}\n"
201 f" • stamp (sec): {stamp_sec:.6f}\n"
202 f" • width x height: {msg.width} x {msg.height}\n"
203 f" • distortion_model:{msg.distortion_model}\n"
204 f" • D: [{d_str}]\n"
205 f" • K: [{k_str}]\n"
206 f" • P: [{p_str}]\n"
207 f" • binning_x: {msg.binning_x}\n"
208 f" • binning_y: {msg.binning_y}\n"
209 f" • roi: {{ x_offset: {msg.roi.x_offset}, y_offset: {msg.roi.y_offset}, height: {msg.roi.height}, width: {msg.roi.width}, do_rectify: {msg.roi.do_rectify} }}"
210 )
211
212 def dump_image_to_video(self, msg: Image):
213 """Dump RGB image to video file using mp4v encoding"""
214 if msg.encoding != "rgb8":
215 self.get_logger().warn(
216 f"image encoding not expected: {msg.encoding}")
217 return
218
219 np_array = np.frombuffer(msg.data, dtype=np.uint8).reshape(
220 msg.height, msg.width, 3)
221 cv_image = cv2.cvtColor(np_array, cv2.COLOR_RGB2BGR)
222
223 if self.video_writer is None:
224 fourcc = cv2.VideoWriter_fourcc(*'mp4v')
225 height, width = cv_image.shape[:2]
226 self.video_writer = cv2.VideoWriter(
227 self.dump_video_path, fourcc, self.dump_video_fps, (width, height))
228 if not self.video_writer.isOpened():
229 self.get_logger().error(
230 f"Failed to open video writer: {self.dump_video_path}")
231 self.dump_video_path = ''
232 return
233 self.get_logger().info(
234 f"📹 Video writer initialized: {width}x{height} @ {self.dump_video_fps}fps")
235
236 self.video_writer.write(cv_image)
237 self.frame_count += 1
238
239 if self.frame_count % 30 == 0:
240 self.get_logger().info(
241 f"📹 Written {self.frame_count} frames to video")
242
243 def destroy_node(self):
244 if self.video_writer is not None:
245 self.video_writer.release()
246 self.get_logger().info(
247 f"📹 Video saved: {self.dump_video_path} ({self.frame_count} frames)")
248 super().destroy_node()
249
250
251def main(args=None):
252 rclpy.init(args=args)
253 try:
254 node = StereoCameraTopicEcho()
255 rclpy.spin(node)
256 except KeyboardInterrupt:
257 pass
258 except Exception as e:
259 print(f"Error: {e}")
260 finally:
261 if 'node' in locals():
262 node.destroy_node()
263 if rclpy.ok():
264 rclpy.shutdown()
265
266
267if __name__ == '__main__':
268 main()
Usage Instructions:
Subscribe to left camera RGB image:
ros2 run py_examples echo_camera_stereo --ros-args -p topic_type:=left_rgb_image
Subscribe to right camera RGB image:
ros2 run py_examples echo_camera_stereo --ros-args -p topic_type:=right_rgb_image
Subscribe to left camera intrinsic parameters:
ros2 run py_examples echo_camera_stereo --ros-args -p topic_type:=left_camera_info
Record video:
# You can change dump_video_path to another path; ensure the directory exists before saving ros2 run py_examples echo_camera_head_rear --ros-args -p topic_type:=rgb_image -p dump_video_path:=$PWD/rear_camera.mp4
Rear Head Monocular Camera Data Subscription
This example uses echo_camera_head_rear to subscribe to /aima/hal/sensor/rgb_head_rear/ and receive the robot’s head rear monocular camera data, supporting RGB images, compressed images, and camera intrinsics.
Features:
Supports rear head camera data subscription
Real-time FPS statistics and data display
Supports RGB video recording with/without obstructed area masked
Configurable topic type selection
Detects installation orientation and applies rotation compensation
Supported Data Types:
rgb_image: RGB image (sensor_msgs/Image)rgb_image_compressed: Compressed RGB image (sensor_msgs/CompressedImage)camera_info: Camera intrinsic parameters (sensor_msgs/CameraInfo)
1#!/usr/bin/env python3
2"""
3Head rear monocular camera multi-topic subscription example
4
5Supports selecting the topic type to subscribe via startup parameter --ros-args -p topic_type:=<type>:
6 - rgb_image: RGB image (sensor_msgs/Image)
7 - rgb_image_compressed: RGB compressed image (sensor_msgs/CompressedImage)
8 - camera_info: Camera intrinsic parameters (sensor_msgs/CameraInfo)
9
10Example:
11 ros2 run py_examples echo_camera_head_rear --ros-args -p topic_type:=rgb_image
12 ros2 run py_examples echo_camera_head_rear --ros-args -p topic_type:=rgb_image_compressed
13 ros2 run py_examples echo_camera_head_rear --ros-args -p topic_type:=camera_info
14
15Default topic_type is rgb_image
16
17Note: Some rear cameras are rotated by 90 degrees. This example automatically
18detects them and applies rotation compensation when needed.
19"""
20
21import rclpy
22from rclpy.node import Node
23from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSDurabilityPolicy, QoSHistoryPolicy
24from sensor_msgs.msg import Image, CompressedImage, CameraInfo
25from collections import deque
26import os
27import cv2
28import numpy as np
29import re
30
31REAR_ROTATION_PATTERN = re.compile(r't3[d.]5', re.IGNORECASE)
32
33
34class HeadRearCameraTopicEcho(Node):
35 def __init__(self):
36 super().__init__('head_rear_camera_topic_echo')
37
38 self.declare_parameter('topic_type', 'rgb_image')
39 self.declare_parameter('dump_video_path', '')
40 self.declare_parameter('dump_video_fps', 10.0)
41 self.declare_parameter('with_mask', False)
42 self.declare_parameter('auto_rotate', True)
43
44 self.topic_type = self.get_parameter('topic_type').value
45 self.dump_video_path = self.get_parameter('dump_video_path').value
46 self.dump_video_fps = self.get_parameter('dump_video_fps').value
47 self.with_mask = self.get_parameter('with_mask').value
48 self.auto_rotate = self.get_parameter('auto_rotate').value
49 self.mask_image = None
50 self.need_rotation = False
51 self.hw_version = None
52 self.video_writer = None
53 self.frame_count = 0
54
55 qos = QoSProfile(
56 reliability=QoSReliabilityPolicy.BEST_EFFORT,
57 history=QoSHistoryPolicy.KEEP_LAST,
58 depth=5,
59 durability=QoSDurabilityPolicy.VOLATILE
60 )
61
62 if self.auto_rotate:
63 self._detect_rotate_from_file()
64
65 if self.with_mask and self.dump_video_path:
66 mask_file = 'rgb_head_rear_mask_t35.png' if self.need_rotation else 'rgb_head_rear_mask.png'
67 mask_path = os.path.join(
68 os.path.dirname(__file__), 'data', mask_file)
69 self.mask_image = cv2.imread(mask_path, cv2.IMREAD_GRAYSCALE)
70 if self.mask_image is None:
71 self.get_logger().error(
72 f"Failed to load mask file: {mask_path}")
73 raise ValueError("Failed to load mask file")
74 self.get_logger().info(f"🎭 Loaded mask: {mask_file}")
75
76 if self.topic_type == "rgb_image":
77 self.topic_name = "/aima/hal/sensor/rgb_head_rear/rgb_image"
78 self.sub_image = self.create_subscription(
79 Image, self.topic_name, self.cb_image, qos)
80 self.get_logger().info(
81 f"✅ Subscribing RGB Image: {self.topic_name}")
82 if self.dump_video_path:
83 mask_state = "with mask" if self.with_mask else "without mask"
84 rotation_state = "with rotation compensation" if self.need_rotation else "without rotation"
85 self.get_logger().info(
86 f"📝 Will dump received images {mask_state} {rotation_state} to video: {self.dump_video_path}")
87
88 elif self.topic_type == "rgb_image_compressed":
89 self.topic_name = "/aima/hal/sensor/rgb_head_rear/rgb_image/compressed"
90 self.sub_compressed = self.create_subscription(
91 CompressedImage, self.topic_name, self.cb_compressed, qos)
92 self.get_logger().info(
93 f"✅ Subscribing CompressedImage: {self.topic_name}")
94
95 elif self.topic_type == "camera_info":
96 self.topic_name = "/aima/hal/sensor/rgb_head_rear/camera_info"
97 camera_qos = QoSProfile(
98 reliability=QoSReliabilityPolicy.RELIABLE,
99 history=QoSHistoryPolicy.KEEP_LAST,
100 depth=1,
101 durability=QoSDurabilityPolicy.TRANSIENT_LOCAL
102 )
103 self.sub_camerainfo = self.create_subscription(
104 CameraInfo, self.topic_name, self.cb_camerainfo, camera_qos)
105 self.get_logger().info(
106 f"✅ Subscribing CameraInfo (with transient_local): {self.topic_name}")
107
108 else:
109 self.get_logger().error(f"Unknown topic_type: {self.topic_type}")
110 raise ValueError("Unknown topic_type")
111
112 self.last_print = self.get_clock().now()
113 self.print_allowed = False
114 self.arrivals = deque()
115
116 def _detect_rotate_from_file(self):
117 try:
118 hw_file = '/agibot/data/info/hardware_version'
119 if os.path.exists(hw_file):
120 with open(hw_file, 'r') as f:
121 self.hw_version = f.read().strip()
122 self.get_logger().info(
123 f"📋 Hardware version from file: '{self.hw_version}'")
124 self._check_rotation(self.hw_version)
125 else:
126 self.get_logger().warn(
127 f"⚠️ Hardware version file not found: {hw_file}")
128 except Exception as e:
129 self.get_logger().warn(
130 f"⚠️ Failed to read hardware version from file: {e}")
131
132 def _check_rotation(self, version: str):
133 if REAR_ROTATION_PATTERN.search(version):
134 self.need_rotation = True
135 self.get_logger().info(
136 "🔄 Rotation detected: Rear camera rotation compensation enabled (90° counter-clockwise)"
137 )
138 else:
139 self.get_logger().info(
140 f"✅ No rotation detected ({version}): No rotation compensation needed"
141 )
142
143 def _rotate_image_90_ccw(self, image: np.ndarray) -> np.ndarray:
144 return cv2.rotate(image, cv2.ROTATE_90_COUNTERCLOCKWISE)
145
146 def update_arrivals(self):
147 now = self.get_clock().now()
148 self.arrivals.append(now)
149 while self.arrivals and (now - self.arrivals[0]).nanoseconds * 1e-9 > 1.0:
150 self.arrivals.popleft()
151
152 def get_fps(self):
153 return len(self.arrivals)
154
155 def should_print(self, master=True):
156 if not master:
157 return self.print_allowed
158 now = self.get_clock().now()
159 if (now - self.last_print).nanoseconds * 1e-9 >= 1.0:
160 self.last_print = now
161 self.print_allowed = True
162 else:
163 self.print_allowed = False
164 return self.print_allowed
165
166 def cb_image(self, msg: Image):
167 self.update_arrivals()
168
169 if self.should_print():
170 stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
171 rotation_note = " (rotated 90° CCW)" if self.need_rotation else ""
172 self.get_logger().info(
173 f"📸 {self.topic_type} received{rotation_note}\n"
174 f" • frame_id: {msg.header.frame_id}\n"
175 f" • stamp (sec): {stamp_sec:.6f}\n"
176 f" • encoding: {msg.encoding}\n"
177 f" • size (WxH): {msg.width} x {msg.height}\n"
178 f" • step (bytes/row):{msg.step}\n"
179 f" • is_bigendian: {msg.is_bigendian}\n"
180 f" • recv FPS (1s): {self.get_fps():.1f}"
181 )
182
183 if self.topic_type == "rgb_image" and self.dump_video_path:
184 self.dump_image_to_video(msg)
185
186 def cb_compressed(self, msg: CompressedImage):
187 self.update_arrivals()
188
189 if self.should_print():
190 stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
191 self.get_logger().info(
192 f"🗜️ {self.topic_type} received\n"
193 f" • frame_id: {msg.header.frame_id}\n"
194 f" • stamp (sec): {stamp_sec:.6f}\n"
195 f" • format: {msg.format}\n"
196 f" • data size: {len(msg.data)}\n"
197 f" • recv FPS (1s): {self.get_fps():.1f}"
198 )
199
200 def cb_camerainfo(self, msg: CameraInfo):
201 stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
202
203 d_str = ", ".join([f"{d:.6f}" for d in msg.d])
204 k_str = ", ".join([f"{k:.6f}" for k in msg.k])
205 p_str = ", ".join([f"{p:.6f}" for p in msg.p])
206
207 self.get_logger().info(
208 f"📷 {self.topic_type} received\n"
209 f" • frame_id: {msg.header.frame_id}\n"
210 f" • stamp (sec): {stamp_sec:.6f}\n"
211 f" • width x height: {msg.width} x {msg.height}\n"
212 f" • distortion_model:{msg.distortion_model}\n"
213 f" • D: [{d_str}]\n"
214 f" • K: [{k_str}]\n"
215 f" • P: [{p_str}]\n"
216 f" • binning_x: {msg.binning_x}\n"
217 f" • binning_y: {msg.binning_y}\n"
218 f" • roi: {{ x_offset: {msg.roi.x_offset}, y_offset: {msg.roi.y_offset}, height: {msg.roi.height}, width: {msg.roi.width}, do_rectify: {msg.roi.do_rectify} }}"
219 )
220
221 def dump_image_to_video(self, msg: Image):
222 """Dump RGB image to video file using mp4v encoding"""
223 try:
224 if msg.encoding != "rgb8":
225 self.get_logger().warn(
226 f"image encoding not expected: {msg.encoding}")
227 return
228
229 np_array = np.frombuffer(msg.data, dtype=np.uint8).reshape(
230 msg.height, msg.width, 3)
231 cv_image = cv2.cvtColor(np_array, cv2.COLOR_RGB2BGR)
232
233 if self.need_rotation:
234 cv_image = self._rotate_image_90_ccw(cv_image)
235
236 if self.mask_image is not None:
237 mask_resized = cv2.resize(
238 self.mask_image, (cv_image.shape[1], cv_image.shape[0]))
239 cv_image[mask_resized == 0] = 0
240
241 if self.video_writer is None:
242 fourcc = cv2.VideoWriter_fourcc(*'mp4v')
243 height, width = cv_image.shape[:2]
244 self.video_writer = cv2.VideoWriter(
245 self.dump_video_path, fourcc, self.dump_video_fps, (width, height))
246 if not self.video_writer.isOpened():
247 self.get_logger().error(
248 f"Failed to open video writer: {self.dump_video_path}")
249 self.dump_video_path = ''
250 return
251 self.get_logger().info(
252 f"📹 Video writer initialized: {width}x{height} @ {self.dump_video_fps}fps")
253
254 self.video_writer.write(cv_image)
255 self.frame_count += 1
256
257 if self.frame_count % 30 == 0:
258 self.get_logger().info(
259 f"📹 Written {self.frame_count} frames to video")
260
261 except Exception as e:
262 self.get_logger().error(f"Failed to process image for video: {e}")
263
264 def destroy_node(self):
265 if self.video_writer is not None:
266 self.video_writer.release()
267 self.get_logger().info(
268 f"� Video saved: {self.dump_video_path} ({self.frame_count} frames)")
269 super().destroy_node()
270
271
272def main(args=None):
273 rclpy.init(args=args)
274 node = None
275 try:
276 node = HeadRearCameraTopicEcho()
277 rclpy.spin(node)
278 except KeyboardInterrupt:
279 pass
280 except Exception as e:
281 print(f"Error: {e}")
282 finally:
283 if node is not None:
284 node.destroy_node()
285 if rclpy.ok():
286 rclpy.shutdown()
287
288
289if __name__ == '__main__':
290 main()
Usage Instructions:
Subscribe to RGB image data:
ros2 run py_examples echo_camera_head_rear --ros-args -p topic_type:=rgb_image
Subscribe to compressed image data:
ros2 run py_examples echo_camera_head_rear --ros-args -p topic_type:=rgb_image_compressed
Subscribe to camera intrinsic parameters:
ros2 run py_examples echo_camera_head_rear --ros-args -p topic_type:=camera_info
Record video:
# You can change dump_video_path to another path; ensure the directory exists before saving ros2 run py_examples echo_camera_head_rear --ros-args -p topic_type:=rgb_image -p dump_video_path:=$PWD/rear_camera.mp4
Record video and mask obstructed area:
# You can change dump_video_path to another path; ensure the directory exists before saving ros2 run py_examples echo_camera_head_rear --ros-args -p topic_type:=rgb_image -p with_mask:=true -p dump_video_path:=$PWD/rear_camera.mp4
Disable auto rotation compensation (get original image):
ros2 run py_examples echo_camera_head_rear --ros-args -p topic_type:=rgb_image -p auto_rotate:=false
Application Scenarios:
Face recognition and tracking
Object detection and recognition
Visual SLAM
Image processing and computer vision algorithm development
Robot visual navigation
6.1.15 Head Touch Sensor Data Subscription
This example uses echo_head_touch_sensor, which subscribes to the /aima/hal/sensor/touch_head topic to receive the robot’s touch sensor data on the head.
Features:
The event data would change from “IDLE” to “TOUCH” when robot’s head touched
1#!/usr/bin/env python3
2"""
3Head touch state subscription example
4"""
5
6import rclpy
7from rclpy.node import Node
8from aimdk_msgs.msg import TouchState
9
10
11class TouchStateSubscriber(Node):
12 def __init__(self):
13 super().__init__('touch_state_subscriber')
14
15 # touch event types
16 self.event_type_map = {
17 TouchState.UNKNOWN: "UNKNOWN",
18 TouchState.IDLE: "IDLE",
19 TouchState.TOUCH: "TOUCH",
20 }
21
22 # create subscriber
23 self.subscription = self.create_subscription(
24 TouchState,
25 '/aima/hal/sensor/touch_head',
26 self.touch_callback,
27 10
28 )
29
30 self.get_logger().info(
31 'TouchState subscriber started, listening to /aima/hal/sensor/touch_head')
32
33 def touch_callback(self, msg):
34 event_str = self.event_type_map.get(
35 msg.event_type, "INVALID")
36
37 self.get_logger().info(f'Timestamp: {msg.header.stamp.sec}.{msg.header.stamp.nanosec:09d}, '
38 f'Event: {event_str} ({msg.event_type})')
39
40
41def main(args=None):
42 rclpy.init(args=args)
43 node = TouchStateSubscriber()
44 try:
45 rclpy.spin(node)
46 except KeyboardInterrupt:
47 pass
48 finally:
49 node.destroy_node()
50 if rclpy.ok():
51 rclpy.shutdown()
52
53
54if __name__ == '__main__':
55 main()
Usage Instructions:
ros2 run py_examples echo_head_touch_sensor
Output Example:
...
[INFO] [1779174766.897555028] [touch_state_subscriber]: Timestamp: 1779174766.892997567, Event: IDLE (1)
[INFO] [1779174766.905498343] [touch_state_subscriber]: Timestamp: 1779174766.902994523, Event: TOUCH (2)
6.1.16 LiDAR Data Subscription
This example uses echo_lidar_data, which subscribes to the /aima/hal/sensor/lidar_chest_front/ topic to receive the robot’s LiDAR data, supporting both point cloud and IMU data types.
Features:
Supports LiDAR point cloud data subscription
Supports LiDAR IMU data subscription
Real-time FPS statistics and data display
Configurable topic type selection
Detailed output of data field information
Supported Data Types:
PointCloud2: LiDAR point cloud data (sensor_msgs/PointCloud2)Imu: LiDAR IMU data (sensor_msgs/Imu)
Technical Implementation:
Uses SensorDataQoS configuration (
BEST_EFFORT+VOLATILE)Supports parsing and displaying point cloud field information
Supports IMU quaternion, angular velocity, and linear acceleration data
Provides detailed debugging log output
Application Scenarios:
LiDAR data acquisition and analysis
Point cloud data processing and visualization
Robot navigation and localization
SLAM algorithm development
Environmental perception and mapping
1#!/usr/bin/env python3
2"""
3Chest LiDAR data subscription example
4
5Supports subscribing to the following topics:
6 1. /aima/hal/sensor/lidar_chest_front/lidar_pointcloud
7 - Data type: sensor_msgs/PointCloud2
8 - frame_id: lidar_chest_front
9 - child_frame_id: /
10 - Content: LiDAR point cloud data
11 2. /aima/hal/sensor/lidar_chest_front/imu
12 - Data type: sensor_msgs/Imu
13 - frame_id: lidar_imu_chest_front
14 - Content: LiDAR IMU data
15
16You can select the topic type to subscribe via startup parameter --ros-args -p topic_type:=<type>:
17 - pointcloud: subscribe to LiDAR point cloud
18 - imu: subscribe to LiDAR IMU
19Default topic_type is pointcloud
20
21Examples:
22 ros2 run py_examples echo_lidar_data --ros-args -p topic_type:=pointcloud
23 ros2 run py_examples echo_lidar_data --ros-args -p topic_type:=imu
24"""
25
26import rclpy
27from rclpy.node import Node
28from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
29from sensor_msgs.msg import PointCloud2, Imu
30from collections import deque
31
32
33class LidarChestEcho(Node):
34 def __init__(self):
35 super().__init__('lidar_chest_echo')
36
37 # Select the topic type to subscribe
38 self.declare_parameter('topic_type', 'pointcloud')
39 self.topic_type = self.get_parameter('topic_type').value
40
41 # SensorDataQoS: BEST_EFFORT + VOLATILE
42 qos = QoSProfile(
43 reliability=QoSReliabilityPolicy.BEST_EFFORT,
44 history=QoSHistoryPolicy.KEEP_LAST,
45 depth=5
46 )
47
48 # Create different subscribers based on topic_type
49 if self.topic_type == "pointcloud":
50 self.topic_name = "/aima/hal/sensor/lidar_chest_front/lidar_pointcloud"
51 self.sub_pointcloud = self.create_subscription(
52 PointCloud2, self.topic_name, self.cb_pointcloud, qos)
53 self.get_logger().info(
54 f"✅ Subscribing LIDAR PointCloud2: {self.topic_name}")
55
56 elif self.topic_type == "imu":
57 self.topic_name = "/aima/hal/sensor/lidar_chest_front/imu"
58 self.sub_imu = self.create_subscription(
59 Imu, self.topic_name, self.cb_imu, qos)
60 self.get_logger().info(
61 f"✅ Subscribing LIDAR IMU: {self.topic_name}")
62
63 else:
64 self.get_logger().error(f"Unknown topic_type: {self.topic_type}")
65 raise ValueError("Unknown topic_type")
66
67 # Internal state
68 self.last_print = self.get_clock().now()
69 self.arrivals = deque()
70
71 def update_arrivals(self):
72 """Calculate received FPS"""
73 now = self.get_clock().now()
74 self.arrivals.append(now)
75 while self.arrivals and (now - self.arrivals[0]).nanoseconds * 1e-9 > 1.0:
76 self.arrivals.popleft()
77
78 def get_fps(self):
79 """Get FPS"""
80 return len(self.arrivals)
81
82 def should_print(self):
83 """Control print frequency"""
84 now = self.get_clock().now()
85 if (now - self.last_print).nanoseconds * 1e-9 >= 1.0:
86 self.last_print = now
87 return True
88 return False
89
90 def cb_pointcloud(self, msg: PointCloud2):
91 """PointCloud2 callback (LiDAR point cloud)"""
92 self.update_arrivals()
93
94 if self.should_print():
95 stamp_sec = f"{msg.header.stamp.sec}.{msg.header.stamp.nanosec:09d}"
96
97 # Format fields info
98 fields_str = " ".join(
99 [f"{f.name}({int(f.datatype)})" for f in msg.fields])
100
101 self.get_logger().info(
102 f"🟢 LIDAR PointCloud2 received\n"
103 f" • frame_id: {msg.header.frame_id}\n"
104 f" • stamp (sec): {stamp_sec}\n"
105 f" • width x height: {msg.width} x {msg.height}\n"
106 f" • point_step: {msg.point_step}\n"
107 f" • row_step: {msg.row_step}\n"
108 f" • fields: {fields_str}\n"
109 f" • is_bigendian: {msg.is_bigendian}\n"
110 f" • is_dense: {msg.is_dense}\n"
111 f" • data size: {len(msg.data)}\n"
112 f" • recv FPS (1s): {self.get_fps():.1f}"
113 )
114
115 def cb_imu(self, msg: Imu):
116 """IMU callback (LiDAR IMU)"""
117 self.update_arrivals()
118
119 if self.should_print():
120 stamp_sec = f"{msg.header.stamp.sec}.{msg.header.stamp.nanosec:09d}"
121
122 self.get_logger().info(
123 f"🟢 LIDAR IMU received\n"
124 f" • frame_id: {msg.header.frame_id}\n"
125 f" • stamp (sec): {stamp_sec}\n"
126 f" • orientation: [{msg.orientation.x:.6f}, {msg.orientation.y:.6f}, {msg.orientation.z:.6f}, {msg.orientation.w:.6f}]\n"
127 f" • angular_velocity:[{msg.angular_velocity.x:.6f}, {msg.angular_velocity.y:.6f}, {msg.angular_velocity.z:.6f}]\n"
128 f" • linear_accel: [{msg.linear_acceleration.x:.6f}, {msg.linear_acceleration.y:.6f}, {msg.linear_acceleration.z:.6f}]\n"
129 f" • recv FPS (1s): {self.get_fps():.1f}"
130 )
131
132
133def main(args=None):
134 rclpy.init(args=args)
135 try:
136 node = LidarChestEcho()
137 rclpy.spin(node)
138 except KeyboardInterrupt:
139 pass
140 except Exception as e:
141 print(f"Error: {e}")
142 finally:
143 if 'node' in locals():
144 node.destroy_node()
145 if rclpy.ok():
146 rclpy.shutdown()
147
148
149if __name__ == '__main__':
150 main()
Usage Instructions:
# Subscribe to LiDAR point cloud data
ros2 run py_examples echo_lidar_data --ros-args -p topic_type:=pointcloud
# Subscribe to LiDAR IMU data
ros2 run py_examples echo_lidar_data --ros-args -p topic_type:=imu
Output Example:
...
[INFO] [1779175640.420801950] [lidar_chest_echo]: 🟢 LIDAR PointCloud2 received
• frame_id: lidar_chest_front
• stamp (sec): 1779175640.315327000
• width x height: 24779 x 1
• point_step: 32
• row_step: 792928
• fields: x(7) y(7) z(7) intensity(7) ring(4) timestamp(8)
• is_bigendian: False
• is_dense: True
• data size: 792928
• recv FPS (1s): 11.0
6.1.17 IMU Data Subscription
This example uses echo_imu_data to subscribe to IMU topics for the robot’s orientation, angular velocity, and linear acceleration data, and prints the covariance diagonals of each field. It subscribes to the chest IMU by default and can be switched to the torso IMU, LiDAR IMU, or depth-camera IMU via a parameter.
Features:
Subscribes to
sensor_msgs/Imudata, outputting orientation quaternion, angular velocity, and linear accelerationPrints the covariance diagonals of orientation/angular velocity/acceleration (
-1means unknown)Measures the receive interval (
recv_dt) to observe the actual arrival rateSwitch between IMU sources via the
imu_topicparameter
Supported Topics:
/aima/hal/imu/chest/state: Chest IMU (default)/aima/hal/imu/torso/state: Torso IMU/aima/hal/sensor/lidar_chest_front/imu: LiDAR IMU/aima/hal/sensor/rgbd_head_front/imu: Depth-camera IMU
Technical Implementation:
Uses
qos_profile_sensor_data(BEST_EFFORT+VOLATILE), consistent with the default QoS for sensor data
1#!/usr/bin/env python3
2import time
3import rclpy
4from rclpy.node import Node
5from rclpy.qos import qos_profile_sensor_data
6from rclpy.time import Time
7from sensor_msgs.msg import Imu
8
9
10class ImuPrinter(Node):
11 def __init__(self):
12 super().__init__('imu_printer')
13 # Allow overriding the topic name via a parameter
14 self.declare_parameter('imu_topic', '/aima/hal/imu/chest/state')
15 # Can be replaced with other IMU topic names.
16 # Waist IMU: /aima/hal/imu/torso/state
17 # LiDAR IMU: /aima/hal/sensor/lidar_chest_front/imu
18 self.topic = self.get_parameter(
19 'imu_topic').get_parameter_value().string_value
20
21 self._last_recv = None
22 # qos_profile_sensor_data = BestEffort / Volatile
23 self.sub = self.create_subscription(
24 Imu, self.topic, self.cb, qos_profile_sensor_data
25 )
26 self.get_logger().info(f'Subscribing IMU: {self.topic}')
27
28 def cb(self, msg: Imu):
29 # Compute receive interval (based on wall time, for observation only)
30 now = time.perf_counter()
31 dt_ms = 0.0 if self._last_recv is None else (
32 now - self._last_recv) * 1000.0
33 self._last_recv = now
34
35 t_sec = Time.from_msg(msg.header.stamp).nanoseconds / 1e9
36 q = msg.orientation
37 w = msg.angular_velocity
38 a = msg.linear_acceleration
39 oc = msg.orientation_covariance
40 wc = msg.angular_velocity_covariance
41 ac = msg.linear_acceleration_covariance
42
43 # Optional: print diagonal covariances (-1 means unknown)
44 self.get_logger().info(
45 (
46 f"stamp={t_sec:.6f}s frame={msg.header.frame_id} recv_dt={dt_ms:.3f}ms\n"
47 f" orientation (x,y,z,w): [{q.x:.6f}, {q.y:.6f}, {q.z:.6f}, {q.w:.6f}]\n"
48 f" angular_velocity (rad/s): [{w.x:.6f}, {w.y:.6f}, {w.z:.6f}]\n"
49 f" linear_accel (m/s^2): [{a.x:.6f}, {a.y:.6f}, {a.z:.6f}]\n"
50 f" cov_diag(ori, ang, acc): "
51 f"[{oc[0]:.6f},{oc[4]:.6f},{oc[8]:.6f} | "
52 f"{wc[0]:.6f},{wc[4]:.6f},{wc[8]:.6f} | "
53 f"{ac[0]:.6f},{ac[4]:.6f},{ac[8]:.6f}]"
54 )
55 )
56
57
58def main():
59 rclpy.init()
60 node = ImuPrinter()
61 try:
62 rclpy.spin(node)
63 except KeyboardInterrupt:
64 pass
65 finally:
66 node.destroy_node()
67 if rclpy.ok():
68 rclpy.shutdown()
69
70
71if __name__ == '__main__':
72 main()
Usage Instructions:
# Subscribe to the chest IMU (default)
ros2 run py_examples echo_imu_data
# Subscribe to the torso IMU
ros2 run py_examples echo_imu_data --ros-args -p imu_topic:=/aima/hal/imu/torso/state
# Subscribe to the LiDAR IMU
ros2 run py_examples echo_imu_data --ros-args -p imu_topic:=/aima/hal/sensor/lidar_chest_front/imu
Output Example:
[INFO] [1781764206.320174965] [imu_printer]: Subscribing IMU: /aima/hal/imu/chest/state
[INFO] [1781764206.318604000] [imu_printer]: stamp=1781764206.318604s frame=base_link recv_dt=2.039ms
orientation (x,y,z,w): [0.087036, -0.264735, -0.019103, -0.960195]
angular_velocity (rad/s): [-0.006531, 0.002874, 0.002391]
linear_accel (m/s^2): [-5.013936, -1.548364, 8.280822]
cov_diag(ori, ang, acc): [0.000000,0.000000,0.000000 | 0.000000,0.000000,0.000000 | 0.000000,0.000000,0.000000]
6.1.18 Play Video
This example uses play_video. Before running the node, you must upload the video to the robot’s Interaction Computing Unit (PC3) (you may create a directory on it to store videos, e.g. /var/tmp/videos/), and then change the video_path in the node program to the path of the video you want to play.
Attention
⚠️ Attention! The Interaction Computing Unit (PC3) is independent from the Development Computing Unit (PC2) where secondary development programs run. Audio and video files must be stored on the Interaction Computing Unit (IP: 10.0.1.42).
Audio and video files (and all parent directories up to root) must be readable by all users (new subdirectory under /var/tmp/ is recommended)
Attention
Before using this feature, run the following command on the Motion Control Computing Unit (PC1) to stop the task_manager module and prevent it from competing with your program for display control:
aima em stop-app task_manager
Function description: By calling the PlayVideo service, the robot can play a video file from a specified path on its screen. Ensure the video file has been uploaded to the Interaction Computing Unit, otherwise playback will fail.
💡 Tip: How to prepare a video file
You can use the C++ echo_camera_rgbd example to record the robot’s depth camera RGB stream as an
.mp4video file, then use it as input forplay_video:Step 1: Record a video on the Development Computing Unit (PC2) (using the C++ example)
ros2 run examples echo_camera_rgbd --ros-args -p topic_type:=rgb_image -p dump_video_path:=$PWD/output.mp4 # Press Ctrl+C to stop recording; the video is saved as output.mp4Step 2: Transfer the video to the Interaction Computing Unit (PC3, IP: 10.0.1.42)
scp output.mp4 user@10.0.1.42:/var/tmp/videos/output.mp4Step 3: Run play_video to play the video
ros2 run py_examples play_video # Set video_path in the code to /var/tmp/videos/output.mp4
1#!/usr/bin/env python3
2
3import rclpy
4import rclpy.logging
5from rclpy.node import Node
6
7from aimdk_msgs.srv import PlayVideo
8
9
10class PlayVideoClient(Node):
11 def __init__(self):
12 super().__init__('play_video_client')
13 self.client = self.create_client(
14 PlayVideo, '/aimdk_5Fmsgs/srv/PlayVideo')
15 self.get_logger().info('✅ PlayVideo client node created.')
16
17 # Wait for the service to become available
18 while not self.client.wait_for_service(timeout_sec=2.0):
19 self.get_logger().info('⏳ Service unavailable, waiting...')
20
21 self.get_logger().info('🟢 Service available, ready to send request.')
22
23 def send_request(self, video_path, mode, priority):
24 req = PlayVideo.Request()
25
26 req.video_path = video_path
27 req.mode = mode
28 req.priority = priority
29
30 # async call
31 self.get_logger().info(
32 f'📨 Sending request to play video: mode={mode} video={video_path}')
33 for i in range(8):
34 req.header.header.stamp = self.get_clock().now().to_msg()
35 future = self.client.call_async(req)
36 rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)
37
38 if future.done():
39 break
40
41 # retry as remote peer is NOT handled well by ROS
42 self.get_logger().info(f'trying ... [{i}]')
43
44 resp = future.result()
45 if resp is None:
46 self.get_logger().error('❌ Service call not completed or timed out.')
47 return False
48
49 if resp.success:
50 self.get_logger().info(
51 f'✅ Request to play video recorded successfully: {resp.message}')
52 return True
53 else:
54 self.get_logger().error(
55 f'❌ Failed to record play-video request: {resp.message}')
56 return False
57
58
59def main(args=None):
60 rclpy.init(args=args)
61 node = None
62
63 try:
64 # video path and priority can be customized
65 default_video_path = "/agibot/software/face_ui/bin/app/aarch64-generic/assets/mp4/emoji_x2_sleepy.mp4"
66 video_path = input(
67 f"Enter video path (default: {default_video_path}): ").strip()
68 if not video_path:
69 video_path = default_video_path
70 priority = 5
71 # input play mode
72 mode = int(input("Enter video play mode (1: play once, 2: loop): "))
73 if mode not in (1, 2):
74 raise ValueError(f'invalid mode {mode}')
75
76 node = PlayVideoClient()
77 node.send_request(video_path, mode, priority)
78 except KeyboardInterrupt:
79 pass
80 except Exception as e:
81 rclpy.logging.get_logger('main').error(
82 f'Program exited with exception: {e}')
83
84 if node:
85 node.destroy_node()
86 if rclpy.ok():
87 rclpy.shutdown()
88
89
90if __name__ == '__main__':
91 main()
6.1.19 Audio File Playback
This example uses play_audio, enabling playback of specified audio files, supporting WAV (PCM-encoded) and RAW PCM formats. Other formats such as MP3 are not supported.
Features:
Supports WAV (PCM-encoded) and RAW PCM playback; other formats such as MP3 are not supported
Supports priority control with configurable playback priority
Supports custom file paths and playback parameters
Provides comprehensive error handling and status feedback
Technical Implementation:
Uses the PlayAudioFile service for audio file playback
Supports priority settings (priority: 1–10, priority_weight: 1–100)
Provides detailed playback status feedback
Application Scenarios:
Audio file playback and media control
Voice prompts and sound effect playback
Multimedia application development
Robot interaction audio feedback
Attention
⚠️ Attention! The Interaction Computing Unit (PC3) is independent from the Development Computing Unit (PC2) where secondary development programs run. Audio and video files must be stored on the Interaction Computing Unit (IP: 10.0.1.42).
Audio and video files (and all parent directories up to root) must be readable by all users (new subdirectory under /var/tmp/ is recommended)
1#!/usr/bin/env python3
2
3import os
4import sys
5import rclpy
6import rclpy.logging
7from rclpy.node import Node
8
9from aimdk_msgs.srv import PlayAudioFile
10from aimdk_msgs.msg import CommonState
11
12
13class PlayAudioClient(Node):
14 def __init__(self):
15 super().__init__('play_audio_client')
16 self.client = self.create_client(
17 PlayAudioFile, '/aimdk_5Fmsgs/srv/PlayAudioFile')
18 self.get_logger().info('✅ PlayAudio client node created.')
19
20 # Wait for the service to become available
21 while not self.client.wait_for_service(timeout_sec=2.0):
22 self.get_logger().info('⏳ Service unavailable, waiting...')
23
24 self.get_logger().info('🟢 Service available, ready to send request.')
25
26 def send_request(self, audio_path):
27 req = PlayAudioFile.Request()
28
29 req.file.pkg_name = 'demo_client'
30 req.file.file_name = os.path.basename(audio_path)
31 req.file.file_path = os.path.dirname(audio_path) + '/'
32 req.file.priority = 8
33 req.file.priority_weight = 0
34
35 self.get_logger().info(
36 f'📨 Sending request to play audio: {audio_path}')
37 for i in range(5):
38 req.request.header.stamp = self.get_clock().now().to_msg()
39 future = self.client.call_async(req)
40 rclpy.spin_until_future_complete(self, future, timeout_sec=1.0)
41
42 if future.done():
43 break
44
45 if not rclpy.ok():
46 self.get_logger().warn('Interrupted while waiting')
47 return False
48
49 # retry as remote peer is NOT handled well by ROS
50 self.get_logger().info(f'trying ... [{i}]')
51
52 resp = future.result()
53 if resp is None:
54 self.get_logger().error('❌ Service call not completed or timed out.')
55 return False
56
57 if resp.reponse.status.value == CommonState.SUCCESS: # SUCCESS (1)
58 self.get_logger().info('✅ Request to play audio file recorded successfully.')
59 return True
60 else:
61 # FAILURE(2): focus-apply failed; INVALID(5): invalid params;
62 # other: see CommonState for details.
63 reasons = {CommonState.FAILURE: 'focus apply failed', # FAILURE (2)
64 CommonState.INVALID: 'invalid params'} # INVALID (5)
65 reason = reasons.get(resp.reponse.status.value, 'unknown')
66 self.get_logger().error(
67 f'❌ Failed to record play-audio request '
68 f'(status={resp.reponse.status.value}, {reason}): '
69 f'{resp.reponse.message}')
70 return False
71
72
73def main(args=None):
74 rclpy.init(args=args)
75 node = None
76
77 default_audio = '/agibot/data/var/hal_audio/file/bingo.wav'
78 try:
79 if len(sys.argv) > 1:
80 audio_path = sys.argv[1]
81 else:
82 audio_path = input(
83 f'Enter audio file path to play (default: {default_audio}): ').strip()
84 if not audio_path:
85 audio_path = default_audio
86
87 node = PlayAudioClient()
88 node.send_request(audio_path)
89 except KeyboardInterrupt:
90 pass
91 except Exception as e:
92 rclpy.logging.get_logger('main').error(
93 f'Program exited with exception: {e}')
94
95 if node:
96 node.destroy_node()
97 if rclpy.ok():
98 rclpy.shutdown()
99
100
101if __name__ == '__main__':
102 main()
Usage Instructions:
# Play default audio file
ros2 run py_examples play_audio
# Play a specified audio file
# Note: replace /path/to/your/audio_file.wav with the actual file path on the interaction unit
ros2 run py_examples play_audio /path/to/your/audio_file.wav
# Play TTS cached audio file
ros2 run py_examples play_audio /agibot/data/var/hal_audio/file/bingo.wav
Output Example:
[INFO] [1779183967.998445447] [play_audio_client]: ✅ PlayAudio client node created.
[INFO] [1779183968.005492518] [play_audio_client]: 🟢 Service available, ready to send request.
[INFO] [1779183968.007782727] [play_audio_client]: 📨 Sending request to play audio: /agibot/data/var/hal_audio/file/bingo.wav
[INFO] [1779183968.029756237] [play_audio_client]: ✅ Request to play audio file recorded successfully.
Notes:
Ensure the audio file path is correct and the file exists
Supported file formats: WAV (PCM-encoded), RAW PCM. Other formats such as MP3 are not supported
Priority settings affect playback queue order
6.1.20 TTS (Text-to-Speech)
This example uses play_tts, which enables the robot to speak the provided text through the node. Users can input different text according to various scenarios.
Features:
Supports command-line arguments and interactive input
Includes comprehensive service availability checks and error handling
Supports priority control and interruption mechanism
Provides detailed playback status feedback
Core Code
1#!/usr/bin/env python3
2
3import sys
4import rclpy
5import rclpy.logging
6from rclpy.node import Node
7
8from aimdk_msgs.srv import PlayTts
9from aimdk_msgs.msg import TtsPriorityLevel
10
11
12class PlayTTSClient(Node):
13 def __init__(self):
14 super().__init__('play_tts_client')
15
16 # fill in the actual service name
17 self.client = self.create_client(PlayTts, '/aimdk_5Fmsgs/srv/PlayTts')
18 self.get_logger().info('✅ PlayTts client node created.')
19
20 # Wait for the service to become available
21 while not self.client.wait_for_service(timeout_sec=2.0):
22 self.get_logger().info('⏳ Service unavailable, waiting...')
23
24 self.get_logger().info('🟢 Service available, ready to send request.')
25
26 def send_request(self, text):
27 req = PlayTts.Request()
28
29 req.tts_req.text = text
30 req.tts_req.domain = 'demo_client' # required: caller domain
31 req.tts_req.trace_id = 'demo' # optional: request id
32 req.tts_req.is_interrupted = True # required: interrupt same-priority
33 req.tts_req.priority_weight = 0
34 # L6 (6)
35 req.tts_req.priority_level.value = TtsPriorityLevel.INTERACTION_L6
36
37 self.get_logger().info(f'📨 Sending request to play tts: text={text}')
38 for i in range(8):
39 req.header.header.stamp = self.get_clock().now().to_msg()
40 future = self.client.call_async(req)
41 rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)
42
43 if future.done():
44 break
45
46 # retry as remote peer is NOT handled well by ROS
47 self.get_logger().info(f'trying ... [{i}]')
48
49 resp = future.result()
50 if resp is None:
51 self.get_logger().error('❌ Service call not completed or timed out.')
52 return False
53
54 if resp.tts_resp.is_success:
55 self.get_logger().info('✅ TTS sent successfully.')
56 return True
57 else:
58 self.get_logger().error(
59 f'❌ Failed to send TTS: error_code={resp.tts_resp.error_code}, '
60 f'error_message={resp.tts_resp.error_message}')
61 return False
62
63
64def main(args=None):
65 rclpy.init(args=args)
66 node = None
67
68 try:
69 # get text to speak
70 if len(sys.argv) > 1:
71 text = sys.argv[1]
72 else:
73 text = input('Enter text to speak: ')
74 if not text:
75 text = 'Hello, I am AgiBot X2.'
76
77 node = PlayTTSClient()
78 node.send_request(text)
79 except KeyboardInterrupt:
80 pass
81 except Exception as e:
82 rclpy.logging.get_logger('main').error(
83 f'Program exited with exception: {e}')
84
85 if node:
86 node.destroy_node()
87 if rclpy.ok():
88 rclpy.shutdown()
89
90
91if __name__ == '__main__':
92 main()
Usage Instructions
# Play text using command-line parameters (recommended)
ros2 run py_examples play_tts "Hello, I am the AgiBot X2 robot"
# Or run without parameters and the program will prompt for input
ros2 run py_examples play_tts
Output Example
[INFO] [1779181480.764266243] [play_tts_client]: ✅ PlayTts client node created.
[INFO] [1779181480.765467698] [play_tts_client]: 🟢 Service available, ready to send request.
[INFO] [1779181480.766450608] [play_tts_client]: 📨 Sending request to play tts: text=Hello
[INFO] [1779181480.774077262] [play_tts_client]: ✅ TTS sent successfully.
Notes
Ensure the TTS service is running properly
Supports both Chinese and English text-to-speech
Priority settings affect playback queue order
Interruption feature can stop the currently playing speech
Interface Reference
Service:
/aimdk_5Fmsgs/srv/PlayTtsMessage:
aimdk_msgs/srv/PlayTts
6.1.21 Microphone Audio Reception
This example uses mic_receiver, which subscribes to the /agent/process_audio_output topic to receive noise-reduced audio data from the robot, supporting both internal and external microphone audio streams, and automatically saving complete speech segments as PCM files based on VAD (Voice Activity Detection) status.
Features:
Automatically detects speech start, in-progress, and end based on VAD state
Automatically saves complete speech segments as PCM files (mono, 16 kHz sample rate, S16LE format)
Stores recordings categorized by timestamp and audio stream
Supports audio duration calculation and statistical output
Application Scenarios:
Speech recognition and speech processing
Audio data acquisition and analysis
Real-time speech monitoring
Audio quality evaluation
Multi-microphone array data processing
1#!/usr/bin/env python3
2"""
3Microphone data receiving example
4
5This example subscribes to the `/agent/process_audio_output` topic to receive the robot's
6noise-suppressed audio data. It supports both the built-in microphone and the external
7microphone audio streams, and automatically saves complete speech segments as PCM files
8based on the VAD (Voice Activity Detection) state.
9
10Features:
11- Supports receiving multiple audio streams at the same time (built-in mic stream_id=1, external mic stream_id=2)
12- Automatically detects speech start / in-progress / end based on VAD state
13- Automatically saves complete speech segments as PCM files
14- Stores files categorized by timestamp and audio stream
15- Supports audio duration calculation and logging
16
17VAD state description:
18- 1: Speech start
19- 2: Speech in progress
20- 3: Speech end
21"""
22
23import rclpy
24from rclpy.node import Node
25from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
26from aimdk_msgs.msg import ProcessedAudioOutput, AudioVadStateType
27import os
28from datetime import datetime
29from collections import defaultdict
30from typing import Dict, List
31
32
33class AudioSubscriber(Node):
34 def __init__(self):
35 super().__init__('audio_subscriber')
36
37 # Audio buffers, stored separately by stream_id
38 # stream_id -> buffer
39 self.audio_buffers: Dict[int, List[bytes]] = defaultdict(list)
40 self.recording_state: Dict[int, bool] = defaultdict(bool)
41
42 # Create audio output directory
43 self.audio_output_dir = "audio_recordings"
44 os.makedirs(self.audio_output_dir, exist_ok=True)
45
46 # VAD state name mapping
47 self.vad_state_names = {
48 AudioVadStateType.AUDIO_VAD_STATE_BEGIN: "Speech start",
49 AudioVadStateType.AUDIO_VAD_STATE_PROCESSING: "Speech in progress",
50 AudioVadStateType.AUDIO_VAD_STATE_END: "Speech end",
51 }
52
53 # Audio stream name mapping
54 self.stream_names = {
55 1: "Built-in microphone",
56 2: "External microphone"
57 }
58
59 # QoS settings
60 # Note: deep queue to avoid missing data in a burst at start of VAD.
61 qos = QoSProfile(
62 history=QoSHistoryPolicy.KEEP_LAST,
63 depth=500,
64 reliability=QoSReliabilityPolicy.BEST_EFFORT
65 )
66
67 # Create subscriber
68 self.subscription = self.create_subscription(
69 ProcessedAudioOutput,
70 '/agent/process_audio_output',
71 self.audio_callback,
72 qos
73 )
74
75 self.get_logger().info("Start subscribing to noise-suppressed audio data...")
76
77 def audio_callback(self, msg: ProcessedAudioOutput):
78 """Audio data callback"""
79 try:
80 stream_id = msg.stream_id
81 vad_state = msg.audio_vad_state.value
82 audio_data = bytes(msg.audio_data)
83
84 self.get_logger().info(
85 f"Received audio data: stream_id={stream_id}, "
86 f"vad_state={vad_state}({self.vad_state_names.get(vad_state, 'Unknown state')}), "
87 f"audio_size={len(audio_data)} bytes"
88 )
89
90 self.handle_vad_state(stream_id, vad_state, audio_data)
91
92 except Exception as e:
93 self.get_logger().error(
94 f"Error while processing audio message: {str(e)}")
95
96 def handle_vad_state(self, stream_id: int, vad_state: int, audio_data: bytes):
97 """Handle VAD state changes"""
98 stream_name = self.stream_names.get(
99 stream_id, f"Unknown stream {stream_id}")
100 vad_name = self.vad_state_names.get(
101 vad_state, f"Unknown state {vad_state}")
102
103 self.get_logger().info(
104 f"[{stream_name}] VAD state: {vad_name} audio: {len(audio_data)} bytes"
105 )
106
107 # Speech start
108 if vad_state == AudioVadStateType.AUDIO_VAD_STATE_BEGIN: # BEGIN (1)
109 self.get_logger().info("🎤 Speech start detected")
110 if not self.recording_state[stream_id]:
111 self.audio_buffers[stream_id].clear()
112 self.recording_state[stream_id] = True
113 if audio_data:
114 self.audio_buffers[stream_id].append(audio_data)
115
116 # Speech in progress
117 # PROCESSING (2)
118 elif vad_state == AudioVadStateType.AUDIO_VAD_STATE_PROCESSING:
119 self.get_logger().info("🔄 Speech in progress...")
120 if self.recording_state[stream_id] and audio_data:
121 self.audio_buffers[stream_id].append(audio_data)
122
123 # Speech end
124 elif vad_state == AudioVadStateType.AUDIO_VAD_STATE_END: # END (3)
125 self.get_logger().info("✅ Speech end")
126 if self.recording_state[stream_id] and audio_data:
127 self.audio_buffers[stream_id].append(audio_data)
128
129 if self.recording_state[stream_id] and self.audio_buffers[stream_id]:
130 self.save_audio_segment(stream_id)
131 self.recording_state[stream_id] = False
132
133 # Unknown speech state
134 else:
135 if self.recording_state[stream_id]:
136 self.get_logger().info("⏹️ Reset recording state")
137 self.recording_state[stream_id] = False
138
139 # Print current buffer status
140 buffer_size = sum(len(chunk)
141 for chunk in self.audio_buffers[stream_id])
142 recording = self.recording_state[stream_id]
143 self.get_logger().debug(
144 f"[Stream {stream_id}] Buffer size: {buffer_size} bytes, recording: {recording}"
145 )
146
147 def save_audio_segment(self, stream_id: int):
148 """Save audio segment"""
149 if not self.audio_buffers[stream_id]:
150 return
151
152 # Merge all audio data
153 audio_data = b''.join(self.audio_buffers[stream_id])
154
155 # Get current timestamp
156 now = datetime.now()
157 timestamp = now.strftime("%Y%m%d_%H%M%S_%f")[:-3] # to milliseconds
158
159 # Create subdirectory by stream_id
160 stream_dir = os.path.join(self.audio_output_dir, f"stream_{stream_id}")
161 os.makedirs(stream_dir, exist_ok=True)
162
163 # Generate filename
164 stream_name = "internal_mic" if stream_id == 1 else "external_mic" if stream_id == 2 else f"stream_{stream_id}"
165 filename = f"{stream_name}_{timestamp}.pcm"
166 filepath = os.path.join(stream_dir, filename)
167
168 try:
169 # Save PCM file
170 with open(filepath, 'wb') as f:
171 f.write(audio_data)
172
173 self.get_logger().info(
174 f"Audio segment saved: {filepath} (size: {len(audio_data)} bytes)")
175
176 # Calculate audio duration (assuming 16 kHz, 16-bit, mono)
177 sample_rate = 16000
178 bits_per_sample = 16
179 channels = 1
180 bytes_per_sample = bits_per_sample // 8
181 total_samples = len(audio_data) // (bytes_per_sample * channels)
182 duration_seconds = total_samples / sample_rate
183
184 self.get_logger().info(
185 f"Audio duration: {duration_seconds:.2f} s ({total_samples} samples)")
186
187 except Exception as e:
188 self.get_logger().error(f"Failed to save audio file: {str(e)}")
189
190
191def main(args=None):
192 rclpy.init(args=args)
193 node = AudioSubscriber()
194
195 try:
196 node.get_logger().info(
197 "Listening for noise-suppressed audio data, press Ctrl+C to exit...")
198 rclpy.spin(node)
199 except KeyboardInterrupt:
200 node.get_logger().info("Interrupt signal received, exiting...")
201 finally:
202 node.destroy_node()
203 if rclpy.ok():
204 rclpy.shutdown()
205
206
207if __name__ == '__main__':
208 main()
Usage Instructions:
Run the program:
# Build the Python package colcon build --packages-select py_examples # Run the microphone receiver and activate VAD with wake words ros2 run py_examples mic_receiver
Press Ctrl+C to safely exit; the program automatically handles the start and end of audio streams.
Play the saved PCM file:
After running, an
audio_recordingsdirectory is created automaticallyAudio files are stored by stream_id (currently not distinguished; stream_1 is always used):
stream_1/: Built-in microphone audiostream_2/: External microphone audio
File naming format:
{stream_name}_{timestamp}.pcminternal_mic_20250909_133649_738.pcm(built-in microphone)external_mic_20250909_133650_123.pcm(external microphone)
Play the saved PCM file:
# Play built-in microphone recording aplay -r 16000 -f S16_LE -c 1 audio_recordings/stream_1/internal_mic_20250909_133649_738.pcm # Play external microphone recording aplay -r 16000 -f S16_LE -c 1 audio_recordings/stream_2/external_mic_20250909_133650_123.pcm
Parameter explanation:
-r 16000 # Sampling rate 16 kHz
-f S16_LE # 16-bit little-endian format
-c 1 # Mono. You may also use other audio players (e.g., Audacity) to import and play the raw PCM format.
Convert to WAV format (optional):
# Convert to WAV format using ffmpeg ffmpeg -f s16le -ar 16000 -ac 1 -i external_mic_20250909_133649_738.pcm output.wav
Output Example:
Normal startup and operation:
[INFO] [1779196050.590953283] [audio_subscriber]: Start subscribing to noise-suppressed audio data...
[INFO] [1779196050.592515383] [audio_subscriber]: Listening for noise-suppressed audio data, press Ctrl+C to exit...
Speech start detected:
[INFO] [1779196055.100798381] [audio_subscriber]: Received audio data: stream_id=1, vad_state=1(Speech start), audio_size=2560 bytes
[INFO] [1779196055.101560663] [audio_subscriber]: [Built-in microphone] VAD state: Speech start audio: 2560 bytes
[INFO] [1779196055.102147334] [audio_subscriber]: 🎤 Speech start detected
Speech processing:
[INFO] [1779196055.103814262] [audio_subscriber]: Received audio data: stream_id=1, vad_state=2(Speech in progress), audio_size=19200 bytes
[INFO] [1779196055.104458211] [audio_subscriber]: [Built-in microphone] VAD state: Speech in progress audio: 19200 bytes
[INFO] [1779196055.105151599] [audio_subscriber]: 🔄 Speech in progress...
[INFO] [1779196055.164856185] [audio_subscriber]: Received audio data: stream_id=1, vad_state=2(Speech in progress), audio_size=3200 bytes
[INFO] [1779196055.165511654] [audio_subscriber]: [Built-in microphone] VAD state: Speech in progress audio: 3200 bytes
[INFO] [1779196055.166184626] [audio_subscriber]: 🔄 Speech in progress...
Speech end and save:
[INFO] [1779196055.867450360] [audio_subscriber]: Received audio data: stream_id=1, vad_state=3(Speech end), audio_size=2560 bytes
[INFO] [1779196055.868118086] [audio_subscriber]: [Built-in microphone] VAD state: Speech end audio: 2560 bytes
[INFO] [1779196055.868834482] [audio_subscriber]: ✅ Speech end
[INFO] [1779196055.870320425] [audio_subscriber]: Audio segment saved: audio_recordings/stream_1/internal_mic_20260519_210735_868.pcm (size: 49920 bytes)
[INFO] [1779196055.871051508] [audio_subscriber]: Audio duration: 1.56 s (24960 samples)
6.1.22 Raw Microphone Audio Stream Receiver
This example uses mic_raw_receiver, which subscribes to the /aima/hal/audio/capture topic to receive raw audio data from the robot and automatically saves it to PCM files split by channel.
Features:
Stores recordings categorized by timestamp and audio stream
Automatically saves audio channel data as PCM files (mono, 16 kHz sample rate, S16LE format)
Supports mic-source switch detection
1#!/usr/bin/env python3
2"""
3Microphone raw data receiving example
4
5This example subscribes to the `/aima/hal/audio/capture` topic to receive the robot's
6raw audio data. It supports both the built-in microphone and the external
7microphone audio streams, and automatically saves complete speech segments as PCM files.
8
9Features:
10- Automatically saves raw audio as PCM files
11- Stores files categorized by timestamp and mic source
12
13"""
14
15import rclpy
16from rclpy.node import Node
17from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
18from aimdk_msgs.msg import AudioCapture
19import os
20import time
21from datetime import datetime
22from collections import defaultdict
23from typing import Dict, List
24
25
26class RawAudioSubscriber(Node):
27 def __init__(self):
28 super().__init__('raw_audio_subscriber')
29
30 # Audio buffers, stored separately by channels
31 self.audio_buffers: Dict[int, List[bytes]] = defaultdict(list)
32 self.mic_channels = 0
33 self.ref_channels = 0
34 self.inited = 0
35 self.dump_timestamp = 0
36
37 # Create audio output directory
38 now = datetime.now()
39 timestamp = now.strftime("%Y%m%d_%H%M%S_%f")[:-3] # to milliseconds
40
41 self.audio_output_dir = os.path.join("audio_recordings", timestamp)
42 os.makedirs(self.audio_output_dir, exist_ok=True)
43
44 # QoS settings
45 # Note: deep queue to avoid missing data
46 qos = QoSProfile(
47 history=QoSHistoryPolicy.KEEP_LAST,
48 depth=500,
49 reliability=QoSReliabilityPolicy.BEST_EFFORT
50 )
51
52 # Create subscriber
53 self.subscription = self.create_subscription(
54 AudioCapture,
55 '/aima/hal/audio/capture',
56 self.audio_callback,
57 qos
58 )
59
60 self.get_logger().info("Start subscribing to raw audio data...")
61
62 def audio_callback(self, msg: AudioCapture):
63 """Audio data callback"""
64 if not self.inited:
65 self.mic_channels = msg.mic_channels
66 self.ref_channels = msg.ref_channels
67 self.dump_timestamp = time.time()
68 self.inited = 1
69 elif self.mic_channels != msg.mic_channels or self.ref_channels != msg.ref_channels:
70 # quit as MIC switched
71 self.get_logger().error(
72 f"MIC channels info changed (mic:{self.mic_channels} ref:{self.ref_channels}) -> (mic:{msg.mic_channels} ref:{msg.ref_channels})")
73 if rclpy.ok():
74 rclpy.shutdown()
75 return
76 audio_data = bytes(msg.data.data)
77
78 self.handle_audio_data(audio_data)
79
80 def handle_audio_data(self, audio_data: bytes):
81 """Handle raw audio data"""
82
83 # Split S16LE data into channels
84 channels = self.ref_channels + self.mic_channels
85 bytes_per_channel = 2
86 unit_size = channels * bytes_per_channel
87 base = 0
88 for k in range(len(audio_data) // unit_size):
89 for i in range(channels):
90 tail = base + bytes_per_channel
91 self.audio_buffers[i].append(audio_data[base:tail])
92 base = tail
93
94 def save_audio_segments(self):
95 self.get_logger().info("🔄 Flushing cached audio data...")
96 for i in range(self.mic_channels + self.ref_channels):
97 self.save_audio_segment(i, i >= self.mic_channels)
98
99 def save_audio_segment(self, channel: int, is_ref: bool):
100 """Save audio segment"""
101 if not self.audio_buffers[channel]:
102 return
103
104 # Merge all audio data
105 audio_data = b''.join(self.audio_buffers[channel])
106 self.audio_buffers[channel].clear()
107
108 channel_type = 'mic'
109 if is_ref:
110 channel_type = 'ref'
111
112 # Generate filename
113 filepath = os.path.join(self.audio_output_dir,
114 f"channel_{channel}_{channel_type}.pcm")
115
116 try:
117 # Save PCM file
118 with open(filepath, 'ab') as f:
119 f.write(audio_data)
120
121 self.get_logger().info(
122 f"Audio segment saved: {filepath} appended: {len(audio_data)} bytes")
123
124 except Exception as e:
125 self.get_logger().error(f"Failed to save audio file: {str(e)}")
126
127 def run(self):
128 self.timer = self.create_timer(1.0, self.save_audio_segments)
129 rclpy.spin(self)
130
131
132def main(args=None):
133 rclpy.init(args=args)
134 node = None
135
136 try:
137 node = RawAudioSubscriber()
138 node.get_logger().info("Listening to raw audio data, press Ctrl+C to exit...")
139 node.run()
140 except KeyboardInterrupt:
141 rclpy.logging.get_logger('main').info(
142 "Interrupt signal received, exiting...")
143
144 if node:
145 node.destroy_node()
146 if rclpy.ok():
147 rclpy.shutdown()
148
149
150if __name__ == '__main__':
151 main()
Usage Instructions:
Run the program:
# Build the Python package colcon build --packages-select py_examples # Run the mic receiver ros2 run py_examples mic_raw_receiver
Play the saved PCM file:
After running, an
audio_recordingsdirectory is created automaticallyAudio files are stored in a directory named by the program’s startup timestamp:
20250909_133650_123
File naming format:
channel_{index}_{type}.pcm, e.g.channel_0_mic.pcm(microphone)channel_4_ref.pcm(reference / echo-cancellation)
Play the saved PCM file:
Transfer to a computer and play
aplay -r 16000 -f S16_LE -c 1 audio_recordings/20250909_133650_123/channel_0_mic.pcm
Play on the robot:
# Use the raw audio stream playback example ros2 run py_examples play_audio_stream --ros-args -p raw_audio_path:=$PWD/audio_recordings/20250909_133650_123/channel_0_mic.pcm # Or use the media file playback example; note that the file must be transferred to the interaction compute unit first, replace the path below ros2 run py_examples play_audio please/use/path/on/PC3.pcm # Use the raw stream playback example ros2 run py_examples play_audio_stream --ros-args -p raw_audio_path:=$PWD/audio_recordings/20250909_133650_123/channel_0_mic.pcm # Or use the audio file playback example (copy the file to PC3 first and use its path) ros2 run py_examples play_audio please/use/path/on/PC3.pcm
6.1.23 Get Current Microphone
1#!/usr/bin/env python3
2
3import rclpy
4import rclpy.logging
5from rclpy.node import Node
6
7from aimdk_msgs.srv import GetMicSourceRequest
8from aimdk_msgs.msg import CommonRequest, CommonState
9
10
11class GetMicSourceRequestClient(Node):
12 def __init__(self):
13 super().__init__('get_mic_source_request_client')
14 self.client = self.create_client(
15 GetMicSourceRequest, '/aimdk_5Fmsgs/srv/GetMicSourceRequest'
16 )
17 self.get_logger().info('✅ GetMicSourceRequest client node created.')
18
19 # Wait for the service to become available
20 while not self.client.wait_for_service(timeout_sec=2.0):
21 self.get_logger().info('⏳ Service unavailable, waiting...')
22
23 self.get_logger().info('🟢 Service available, ready to send request.')
24
25 def send_request(self):
26 req = GetMicSourceRequest.Request()
27 req.header = CommonRequest()
28
29 self.get_logger().info(
30 '📨 Sending request to get MIC source')
31 for i in range(8):
32 req.header.header.stamp = self.get_clock().now().to_msg()
33 future = self.client.call_async(req)
34 rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)
35
36 if future.done():
37 break
38
39 # retry as remote peer is NOT handled well by ROS
40 self.get_logger().info(f'trying ... [{i}]')
41
42 response = future.result()
43 if response is None:
44 self.get_logger().error('❌ Service call failed or timed out.')
45 return
46
47 if response.header.status.value == CommonState.SUCCESS: # SUCCESS (1)
48 self.get_logger().info('✅ MIC source get successfully.')
49 self.get_logger().info(f'MIC id: {response.mic_source}')
50 else:
51 self.get_logger().error(
52 f'❌ Failed to get MIC source: {response.header.message}'
53 )
54
55
56def main(args=None):
57 rclpy.init(args=args)
58 node = None
59 try:
60 node = GetMicSourceRequestClient()
61 node.send_request()
62 except KeyboardInterrupt:
63 pass
64 except Exception as e:
65 rclpy.logging.get_logger('main').error(
66 f'Program exited with exception: {e}')
67
68 if node:
69 node.destroy_node()
70 if rclpy.ok():
71 rclpy.shutdown()
72
73
74if __name__ == '__main__':
75 main()
6.1.24 Switch Microphone
1#!/usr/bin/env python3
2
3import sys
4import rclpy
5import rclpy.logging
6from rclpy.node import Node
7
8from aimdk_msgs.srv import SetMicSourceRequest
9from aimdk_msgs.msg import CommonRequest, CommonState
10
11
12class SetMicSourceRequestClient(Node):
13 def __init__(self):
14 super().__init__('set_mic_source_request_client')
15 self.client = self.create_client(
16 SetMicSourceRequest, '/aimdk_5Fmsgs/srv/SetMicSourceRequest'
17 )
18 self.get_logger().info('✅ SetMicSourceRequest client node created.')
19
20 # Wait for the service to become available
21 while not self.client.wait_for_service(timeout_sec=2.0):
22 self.get_logger().info('⏳ Service unavailable, waiting...')
23
24 self.get_logger().info('🟢 Service available, ready to send request.')
25
26 def send_request(self, mic_source: int):
27 req = SetMicSourceRequest.Request()
28 req.header = CommonRequest()
29
30 req.mic_source = mic_source
31
32 self.get_logger().info(
33 f'📨 Sending request to set mic source: {mic_source}')
34 for i in range(8):
35 req.header.header.stamp = self.get_clock().now().to_msg()
36 future = self.client.call_async(req)
37 rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)
38
39 if future.done():
40 break
41
42 # retry as remote peer is NOT handled well by ROS
43 self.get_logger().info(f'trying ... [{i}]')
44
45 response = future.result()
46 if response is None:
47 self.get_logger().error('❌ Service call failed or timed out.')
48 return
49
50 if response.header.status.value == CommonState.SUCCESS: # SUCCESS (1)
51 self.get_logger().info('✅ MIC source set successfully.')
52 else:
53 self.get_logger().error(
54 f'❌ Failed to set MIC source: {response.header.message}'
55 )
56
57
58def main(args=None):
59 mic_info = {
60 0: ('int', 'internal MIC'),
61 1: ('ext', 'external MIC'),
62 }
63
64 choices = {}
65 for k, v in mic_info.items():
66 choices[v[0]] = k
67
68 rclpy.init(args=args)
69 node = None
70 try:
71 # Prefer command-line argument, otherwise prompt for input
72 if len(sys.argv) > 1:
73 mic = sys.argv[1]
74 else:
75 print('{:<4} - {:<5} : {}'.format('abbr',
76 'mic_id', 'description'))
77 for k, v in mic_info.items():
78 print(f'{v[0]:<4} - {k:<5} : {v[1]}')
79 mic = input('Enter abbr of MIC source:')
80
81 mic_id = choices.get(mic)
82 if mic_id is None:
83 raise ValueError(f'Invalid abbr of MIC source: {mic}')
84
85 node = SetMicSourceRequestClient()
86 node.send_request(mic_id)
87 except KeyboardInterrupt:
88 pass
89 except Exception as e:
90 rclpy.logging.get_logger('main').error(
91 f'Program exited with exception: {e}')
92
93 if node:
94 node.destroy_node()
95 if rclpy.ok():
96 rclpy.shutdown()
97
98
99if __name__ == '__main__':
100 main()
Usage Instructions
# Build
colcon build --packages-select py_examples
# Interactive run
ros2 run py_examples set_mic_source
# Run with argument
ros2 run py_examples set_mic_source ext # ext - external mic, int - built-in mic
6.1.25 Raw Audio Stream Playback
This example uses play_audio_stream, which first requests audio focus then publishes a PCM file at real-time rate to the /aima/hal/audio/playback topic; it exits automatically when audio focus is taken away.
1#!/usr/bin/env python3
2"""
3Raw audio stream playback example
4
5This example would get audio focus first and then publish raw audio data
6at fixed rate to `/aima/hal/audio/playback` topic. And it would stop playing
7when audio focus is lost.
8
9"""
10
11import rclpy
12from rclpy.node import Node
13from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSDurabilityPolicy, QoSHistoryPolicy
14from aimdk_msgs.msg import AudioPlayback, AudioInfo, AudioData, FocusResponse, FocusRequester, CommonState
15from aimdk_msgs.srv import RequestAudioFocus, AbandonAudioFocus
16import os
17import signal
18import time
19import sys
20
21global_node = None
22
23
24def signal_handler(sig, frame):
25 global global_node
26 if global_node is not None:
27 if global_node.is_holding_focus():
28 global_node.send_request(False)
29 global_node.get_logger().info(
30 f"Received signal {sig}, abandon audio focus and shutting down")
31 if rclpy.ok():
32 rclpy.shutdown()
33 sys.exit(0)
34
35
36class FakeMicDevice():
37 def __init__(self, raw_audio_file):
38 self.audio_data = []
39 self.channels = 1
40 self.sample_rate = 16000
41 self.bytes_per_sample = 2 # S16LE
42 self.audio_length = 0
43 with open(raw_audio_file, 'rb') as f:
44 self.audio_data = list(f.read())
45 self.audio_length = len(self.audio_data)
46
47 self.index = 0
48 self.start_time = 0
49
50 def get_channel_count(self):
51 return self.channels
52
53 def get_sample_rate(self):
54 return self.sample_rate
55
56 def get_cached_data(self):
57 now = time.monotonic()
58 if not self.start_time:
59 # fake init: assume audio data started 1s ago
60 self.start_time = now - 1
61
62 # cached data range since last call
63 tail = int((now - self.start_time) * self.sample_rate) * \
64 self.channels * self.bytes_per_sample
65 data = self.audio_data[self.index:tail]
66 if tail < self.audio_length:
67 self.index = tail
68 else:
69 # loopback
70 self.index = 0
71 self.start_time = now
72 return data
73
74
75class AudioStreamPlayer(Node):
76 def __init__(self):
77 super().__init__('audio_stream_player')
78
79 self.declare_parameter('raw_audio_path', '')
80 self.raw_audio_path = self.get_parameter('raw_audio_path').value
81
82 self.mic_device = FakeMicDevice(self.raw_audio_path)
83
84 self.pkg_name = 'audio_stream_player{}'.format(os.getpid())
85 self.focus = False
86 self.focus_force = True
87 self.kAudioPriority = 8
88
89 self.get_logger().info(f'local pkg name: {self.pkg_name}')
90 # Create focus request/release clients
91 self.request_client = self.create_client(
92 RequestAudioFocus,
93 '/aimdk_5Fmsgs/srv/RequestAudioFocus')
94 self.release_client = self.create_client(
95 AbandonAudioFocus,
96 '/aimdk_5Fmsgs/srv/AbandonAudioFocus')
97
98 # Wait for the service to become available
99 while not self.request_client.wait_for_service(timeout_sec=2.0):
100 self.get_logger().info('⏳ Service unavailable, waiting...')
101 self.get_logger().info('🟢 Service available, ready to send request.')
102
103 # Create focus event subscriber and raw audio publisher
104 focus_qos = QoSProfile(
105 reliability=QoSReliabilityPolicy.RELIABLE,
106 history=QoSHistoryPolicy.KEEP_LAST,
107 depth=10,
108 durability=QoSDurabilityPolicy.VOLATILE
109 )
110 self.sub = self.create_subscription(
111 FocusResponse,
112 '/aima/hal/audio/focus_response',
113 self.focus_event_callback, focus_qos
114 )
115 self.pub = self.create_publisher(
116 AudioPlayback,
117 '/aima/hal/audio/playback', 10
118 )
119
120 def send_request(self, focus: bool):
121 if focus:
122 self.send_focus_request()
123 else:
124 self.send_focus_release()
125
126 def send_focus_request(self):
127 req = RequestAudioFocus.Request()
128
129 requester = FocusRequester()
130 requester.pkg_name = self.pkg_name
131 requester.priority = self.kAudioPriority
132
133 req.focus_requester = requester
134
135 self.get_logger().info('📨 Sending RequestAudioFocus request')
136 for i in range(8):
137 future = self.request_client.call_async(req)
138 rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)
139
140 if future.done():
141 break
142
143 # retry as remote peer is NOT handled well by ROS
144 self.get_logger().info(f'trying ... [{i}]')
145
146 response = future.result()
147 if response is None:
148 self.get_logger().error('❌ Service call failed or timed out.')
149 return False
150
151 if response.reponse.status.value == CommonState.SUCCESS: # SUCCESS (1)
152 focus = response.focus_response.focus_gain
153 self.focus = focus
154 self.get_logger().info(f'✅ RequestAudioFocus done: focus {focus}')
155 return True
156 else:
157 self.get_logger().error(
158 f'❌ Failed in response of RequestAudioFocus: {response.reponse.message}'
159 )
160 return False
161
162 def send_focus_release(self):
163 req = AbandonAudioFocus.Request()
164
165 requester = FocusRequester()
166 requester.pkg_name = self.pkg_name
167 requester.priority = self.kAudioPriority # must match RequestAudioFocus
168
169 req.focus_requester = requester
170
171 self.get_logger().info('📨 Sending AbandonAudioFocus request')
172 for i in range(8):
173 future = self.release_client.call_async(req)
174 rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)
175
176 if future.done():
177 break
178
179 # retry as remote peer is NOT handled well by ROS
180 self.get_logger().info(f'trying ... [{i}]')
181
182 response = future.result()
183 if response is None:
184 self.get_logger().error('❌ Service call failed or timed out.')
185 return False
186
187 if response.reponse.status.value == CommonState.SUCCESS: # SUCCESS (1)
188 focus = response.focus_response.focus_gain
189 # always focus False
190 self.focus = focus
191 self.get_logger().info(f'✅ AbandonAudioFocus done: focus {focus}')
192 return True
193 else:
194 self.get_logger().error(
195 f'❌ Failed in response of AbandonAudioFocus: {response.reponse.message}'
196 )
197 return False
198
199 def focus_event_callback(self, msg: FocusResponse):
200 pkg_name = msg.pkg_name
201 focus = msg.focus_gain
202 if msg.pkg_name == self.pkg_name:
203 self.get_logger().info(
204 f'receive focus out event: focus state: {focus}')
205 self.focus_force = msg.focus_gain
206
207 def is_holding_focus(self):
208 return self.focus and self.focus_force
209
210 def spin_until_focus(self):
211 while rclpy.ok():
212 self.focus_force = True
213 self.send_request(True)
214 if self.is_holding_focus():
215 return
216 self.get_logger().info("need retry to get focus")
217 t1 = t0 = time.monotonic()
218 while rclpy.ok() and t1 - t0 < 1.0:
219 rclpy.spin_once(self, timeout_sec=0.1)
220 t1 = time.monotonic()
221 return
222
223 def run_once(self):
224 """ flush raw audio data cached since last call """
225
226 raw_audio_msg = AudioPlayback()
227
228 raw_audio_msg.pkg_name = self.pkg_name
229 raw_audio_msg.token_id = self.pkg_name
230 # fill AudioInfo
231 audio_info = AudioInfo()
232 audio_info.channels = self.mic_device.get_channel_count()
233 audio_info.sample_rate = self.mic_device.get_sample_rate()
234 # fill raw audio data
235 audio_data = AudioData()
236 cached_data = self.mic_device.get_cached_data()
237 audio_data.data = cached_data
238 raw_audio_msg.info = audio_info
239 raw_audio_msg.data = audio_data
240 # publish
241 self.pub.publish(raw_audio_msg)
242
243 def run(self):
244 self.get_logger().info('🟢 publishing audio data ...')
245 while rclpy.ok() and self.is_holding_focus():
246 self.run_once()
247 rclpy.spin_once(self, timeout_sec=0.05)
248 time.sleep(0.05)
249
250 if not self.is_holding_focus():
251 self.get_logger().info("focus out, exiting...")
252
253
254def main(args=None):
255 global global_node
256 rclpy.init(args=args)
257 node = None
258
259 try:
260 node = AudioStreamPlayer()
261
262 global_node = node
263 signal.signal(signal.SIGINT, signal_handler)
264 signal.signal(signal.SIGTERM, signal_handler)
265
266 node.spin_until_focus()
267 node.run()
268
269 except KeyboardInterrupt:
270 rclpy.logging.get_logger('main').info(
271 "Interrupt signal received, exiting...")
272 except Exception as e:
273 rclpy.logging.get_logger('main').error(
274 f'Program exited with exception: {e}')
275
276 if node:
277 node.destroy_node()
278 if rclpy.ok():
279 rclpy.shutdown()
280
281
282if __name__ == '__main__':
283 main()
Usage Instructions
# Replace /path/to/your/raw_audio_file.pcm with the actual PCM file path (mono, 16 kHz, S16LE)
ros2 run py_examples play_audio_stream --ros-args -p raw_audio_path:=/path/to/your/raw_audio_file.pcm
6.1.26 Emoji Control
This example uses play_emoji, which enables the robot to display a specified expression. Users can select expressions from the available list. For the full expression list, refer to the expression list
1#!/usr/bin/env python3
2
3import rclpy
4import rclpy.logging
5from rclpy.node import Node
6
7from aimdk_msgs.srv import PlayEmoji
8
9
10class PlayEmojiClient(Node):
11 def __init__(self):
12 super().__init__('play_emoji_client')
13 self.client = self.create_client(
14 PlayEmoji, '/aimdk_5Fmsgs/srv/PlayEmoji')
15 self.get_logger().info('✅ PlayEmoji client node created.')
16
17 # Wait for the service to become available
18 while not self.client.wait_for_service(timeout_sec=2.0):
19 self.get_logger().info('⏳ Service unavailable, waiting...')
20
21 self.get_logger().info('🟢 Service available, ready to send request.')
22
23 def send_request(self, emoji: int, mode: int, priority: int):
24 req = PlayEmoji.Request()
25
26 req.emotion_id = int(emoji)
27 req.mode = int(mode)
28 req.priority = int(priority)
29
30 self.get_logger().info(
31 f'📨 Sending request to play emoji: id={emoji}, mode={mode}, priority={priority}')
32 for i in range(8):
33 req.header.header.stamp = self.get_clock().now().to_msg()
34 future = self.client.call_async(req)
35 rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)
36
37 if future.done():
38 break
39
40 # retry as remote peer is NOT handled well by ROS
41 self.get_logger().info(f'trying ... [{i}]')
42
43 resp = future.result()
44 if resp is None:
45 self.get_logger().error('❌ Service call not completed or timed out.')
46 return False
47
48 if resp.success:
49 self.get_logger().info(
50 f'✅ Emoji played successfully: {resp.message}')
51 return True
52 else:
53 self.get_logger().error(f'❌ Failed to play emoji: {resp.message}')
54 return False
55
56
57def main(args=None):
58 rclpy.init(args=args)
59 node = None
60
61 # Interactive input, same as the original C++ version
62 try:
63 emotion = int(
64 input("Enter emoji ID: 1-blink, 60-bored, 70-abnormal, 80-sleeping, 90-happy ... 190-double angry, 200-adore: "))
65 mode = int(input("Enter play mode (1: play once, 2: loop): "))
66 if mode not in (1, 2):
67 raise ValueError("invalid mode")
68 priority = 10 # default priority
69
70 node = PlayEmojiClient()
71 node.send_request(emotion, mode, priority)
72 except KeyboardInterrupt:
73 pass
74 except Exception as e:
75 rclpy.logging.get_logger('main').error(
76 f'Program exited with exception: {e}')
77
78 if node:
79 node.destroy_node()
80 if rclpy.ok():
81 rclpy.shutdown()
82
83
84if __name__ == '__main__':
85 main()
6.1.27 Map Building
In this example, slam is used to send messages to the mapping service via this node to achieve mapping functionality.
1import rclpy
2from rclpy.node import Node
3from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSReliabilityPolicy
4from std_msgs.msg import String
5
6
7class SlamCommandPublisher(Node):
8 def __init__(self):
9 super().__init__('slam_command_publisher')
10 # Match SLAM subscriber QoS: TRANSIENT_LOCAL + RELIABLE
11 qos = QoSProfile(
12 depth=10,
13 durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
14 reliability=QoSReliabilityPolicy.RELIABLE,
15 )
16 self.publisher_ = self.create_publisher(
17 String, '/integrated_command', qos)
18
19 def publish_start_mapping(self):
20 msg = String()
21 msg.data = 'start_mapping'
22 self.get_logger().info(f'Publishing: [{msg.data}]')
23 self.publisher_.publish(msg)
24
25 def publish_stop_mapping(self, map_name):
26 msg = String()
27 msg.data = f'stop_mapping:{map_name}'
28 self.get_logger().info(f'Publishing: [{msg.data}]')
29 self.publisher_.publish(msg)
30
31
32def main(args=None):
33 rclpy.init(args=args)
34 slam_command_publisher = SlamCommandPublisher()
35
36 try:
37 while rclpy.ok():
38 input_value = input(
39 "Enter 1 to start mapping, 2 to stop mapping: ")
40
41 if input_value == '1':
42 slam_command_publisher.publish_start_mapping()
43 elif input_value == '2':
44 map_name = input("Enter map name: ")
45 slam_command_publisher.publish_stop_mapping(map_name)
46 else:
47 print("Invalid input. Please enter 1 or 2.")
48 except KeyboardInterrupt:
49 pass
50 finally:
51 slam_command_publisher.destroy_node()
52 if rclpy.ok():
53 rclpy.shutdown()
54
55
56if __name__ == '__main__':
57 main()
Usage Instructions:
ros2 run py_examples slam
Enter 1 to start mapping, 2 to stop mapping: 1
[INFO] [1772521830.713768092] [slam_command_publisher]: Publishing: [start_mapping]
Enter 1 to start mapping, 2 to stop mapping: 2
Enter map name: testmap
[INFO] [1772521837.760016698] [slam_command_publisher]: Publishing: [stop_mapping:testmap]
After starting the mapping process, push the machine or control the robot to move in order to complete the mapping.
6.1.28 Relocalization Using a Map
This example uses the map built in the Map Building step for localization.
1#!/usr/bin/env python3
2
3import rclpy
4from rclpy.node import Node
5from std_msgs.msg import String
6from geometry_msgs.msg import Pose
7from nav_msgs.msg import Odometry
8from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSDurabilityPolicy
9
10
11class RelocalizationNode(Node):
12 def __init__(self, map_id, init_x, init_y):
13 super().__init__('relocalization_node')
14
15 self.map_id = map_id
16 self.init_x = init_x
17 self.init_y = init_y
18
19 # /integrated_command subscriber requires TRANSIENT_LOCAL durability
20 command_qos = QoSProfile(
21 depth=10,
22 durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
23 )
24
25 # Create publishers
26 self.integrated_command_pub = self.create_publisher(
27 String, '/integrated_command', command_qos)
28 self.relocalization_pose_pub = self.create_publisher(
29 Pose, '/relocalization_pose', 10)
30
31 # Create subscriber with BEST_EFFORT QoS
32 lidar_loc_qos = QoSProfile(
33 depth=10,
34 reliability=QoSReliabilityPolicy.BEST_EFFORT
35 )
36
37 self.odometry_sub = self.create_subscription(
38 Odometry,
39 '/slam/lidar_odom',
40 self.odometry_callback,
41 qos_profile=lidar_loc_qos
42 )
43
44 # Setup timer to publish messages in sequence
45 self.success_received = False
46 self.timeout_timer = None
47
48 # Wait for subscribers before publishing
49 self.publish_timer = self.create_timer(0.2, self.wait_and_publish)
50
51 def wait_and_publish(self):
52 if (self.integrated_command_pub.get_subscription_count() == 0 or
53 self.relocalization_pose_pub.get_subscription_count() == 0):
54 self.get_logger().info(
55 f'Waiting for subscribers... '
56 f'(command: {self.integrated_command_pub.get_subscription_count()}, '
57 f'pose: {self.relocalization_pose_pub.get_subscription_count()})',
58 throttle_duration_sec=2.0)
59 return
60 self.publish_timer.cancel()
61 self.publish_sequence()
62
63 def publish_sequence(self):
64 # Publish integrated_command
65 integrated_command_msg = String()
66 integrated_command_msg.data = f'start_relocalization:{self.map_id}'
67 self.integrated_command_pub.publish(integrated_command_msg)
68 self.get_logger().info('Published integrated_command')
69
70 # Schedule relocalization_pose publication after delay
71 self.relocalization_pose_timer = self.create_timer(
72 1.0,
73 self.publish_relocalization_pose
74 )
75
76 def publish_relocalization_pose(self):
77 # Cancel this timer immediately so it only fires once
78 self.relocalization_pose_timer.cancel()
79
80 relocalization_pose_msg = Pose()
81 relocalization_pose_msg.position.x = self.init_x
82 relocalization_pose_msg.position.y = self.init_y
83 relocalization_pose_msg.position.z = 0.0
84 relocalization_pose_msg.orientation.x = 0.0
85 relocalization_pose_msg.orientation.y = 0.0
86 relocalization_pose_msg.orientation.z = 0.0
87 relocalization_pose_msg.orientation.w = 1.0
88
89 self.relocalization_pose_pub.publish(relocalization_pose_msg)
90 self.get_logger().info('Published relocalization_pose')
91
92 # Start timeout timer (30 seconds)
93 self.timeout_timer = self.create_timer(
94 30.0,
95 self.timeout_callback
96 )
97
98 self.get_logger().info('Waiting for robot pose data (timeout: 30s)...')
99
100 def odometry_callback(self, msg):
101 if not self.success_received:
102 self.success_received = True
103 if self.timeout_timer is not None:
104 self.timeout_timer.cancel()
105 self.get_logger().info('Received odometry data - Relocalization successful!')
106 if rclpy.ok():
107 rclpy.shutdown()
108
109 def timeout_callback(self):
110 if not self.success_received:
111 self.get_logger().error('Timeout reached - Relocalization failed!')
112 if rclpy.ok():
113 rclpy.shutdown()
114
115
116def main(args=None):
117 rclpy.init(args=args)
118
119 # Inputs needed for relocalization. To find them, see:
120 # map_id -> sqlite3 /agibot/data/var/MapManagerModule/map.db
121 # "SELECT * FROM map;"
122 # init_x/init_y -> origin (u, v) in
123 # /agibot/data/var/MapManagerModule/<map_id>/grid_map_info.txt
124 # (line 2; pixel coordinates)
125 try:
126 map_id = int(input("Enter map_id: ").strip())
127 init_x = float(input("Enter initial position.x (pixel u): ").strip())
128 init_y = float(input("Enter initial position.y (pixel v): ").strip())
129 except (ValueError, EOFError):
130 print("Invalid input, aborted.")
131 if rclpy.ok():
132 rclpy.shutdown()
133 return
134
135 node = RelocalizationNode(map_id, init_x, init_y)
136 try:
137 rclpy.spin(node)
138 except KeyboardInterrupt:
139 pass
140 finally:
141 if rclpy.ok():
142 rclpy.shutdown()
143
144
145if __name__ == '__main__':
146 main()
Usage Instructions:
ros2 run py_examples relocate
Enter map_id: 1778133306741
Enter initial position.x (pixel u): 1041
Enter initial position.y (pixel v): 630
[INFO] [1772526167.342953490] [relocalization_node]: Published integrated_command
[INFO] [1772526168.346034723] [relocalization_node]: Published relocalization_pose
[INFO] [1772526168.346915278] [relocalization_node]: Waiting for robot pose data (timeout: 30s)...
[INFO] [1772526173.347455156] [relocalization_node]: Received odometry data - Relocalization successful!
Move the robot to the starting point of the map construction, then run the relocation program.
How to obtain these parameters: for map_id and the starting-point pixel coordinates (position.x / position.y), see Obtaining Map Identifiers and Coordinates.
6.1.29 Retrieve a Stored Map
In this example, get_map is used to retrieve stored map data by name, including the map path, resolution, dimensions, and map ID.
1#!/usr/bin/env python3
2
3import rclpy
4from rclpy.node import Node
5from rclpy.executors import SingleThreadedExecutor
6from aimdk_msgs.srv import GetStoredMapByName
7from std_msgs.msg import Header
8
9
10class MapClient(Node):
11 def __init__(self):
12 super().__init__('get_map_client')
13 self.client = self.create_client(
14 GetStoredMapByName,
15 '/aimdk_5Fmsgs/srv/GetStoredMapByName'
16 )
17
18 def send_request(self, map_name):
19 while not self.client.wait_for_service(timeout_sec=1.0):
20 if not rclpy.ok():
21 self.get_logger().error("Interrupted while waiting for service.")
22 return
23 self.get_logger().info("Service not available, waiting again...")
24
25 request = GetStoredMapByName.Request()
26
27 request.header = Header()
28 request.header.stamp = self.get_clock().now().to_msg()
29 request.header.frame_id = ""
30
31 request.map_name = map_name
32
33 future = self.client.call_async(request)
34
35 executor = SingleThreadedExecutor()
36 executor.add_node(self)
37 executor.spin_until_future_complete(future, timeout_sec=60.0)
38 executor.remove_node(self)
39
40 if future.result() is None:
41 self.get_logger().error("Service call failed or timed out.")
42 return
43
44 result = future.result()
45
46 if result.code == 0:
47 self.get_logger().info("Service call succeeded")
48 self.get_logger().info(f"Map path: {result.map_path}")
49 self.get_logger().info(
50 f"Map resolution: {result.map_info.resolution}")
51 self.get_logger().info(f"Map width: {result.map_info.width}")
52 self.get_logger().info(f"Map height: {result.map_info.height}")
53 self.get_logger().info(f"Map id: {result.map_id}")
54 else:
55 self.get_logger().error(
56 f"Service call failed with code: {result.code}")
57
58
59def main(args=None):
60 rclpy.init(args=args)
61
62 client = MapClient()
63 try:
64 # Map name (NOT map_id) is required. To list stored maps, run:
65 # sqlite3 /agibot/data/var/MapManagerModule/map.db "SELECT * FROM map;"
66 map_name = input("Enter map name: ").strip()
67 if not map_name:
68 client.get_logger().error("Map name is empty, aborted.")
69 return
70 client.send_request(map_name)
71 except KeyboardInterrupt:
72 pass
73 finally:
74 if rclpy.ok():
75 rclpy.shutdown()
76
77
78if __name__ == '__main__':
79 main()
Usage Instructions:
ros2 run py_examples get_map
Enter map name: test_map
[INFO] [get_map_client]: Service call succeeded
[INFO] [get_map_client]: Map path: /path/to/test_map.png
[INFO] [get_map_client]: Map resolution: 0.05
[INFO] [get_map_client]: Map width: 200
[INFO] [get_map_client]: Map height: 200
[INFO] [get_map_client]: Map id: 1778133306741
How to obtain these parameters: the input is the map name (map_name), not map_id; to obtain map_name, see Obtaining Map Identifiers and Coordinates.
6.1.31 Play LinkCraft Action
In this example, play_linkcraft is used to list the LinkCraft actions available on the robot and select one for playback.
1#!/usr/bin/env python3
2
3import rclpy
4from rclpy.node import Node
5from aimdk_msgs.srv import GetRobotResources, ExecuteActionResource
6
7
8class RobotResourceClient(Node):
9 def __init__(self):
10 super().__init__('robot_resource_client')
11
12 self.get_resource_client = self.create_client(
13 GetRobotResources,
14 '/aimdk_5Fmsgs/srv/GetRobotResources'
15 )
16 self.play_resource_client = self.create_client(
17 ExecuteActionResource,
18 '/aimdk_5Fmsgs/srv/ExecuteActionResource'
19 )
20
21 self.resources = []
22
23 def get_robot_resources(self):
24 while not self.get_resource_client.wait_for_service(timeout_sec=1.0):
25 if not rclpy.ok():
26 self.get_logger().error("Interrupted while waiting for the service. Exiting.")
27 return False
28 self.get_logger().info("Service not available, waiting again...")
29
30 request = GetRobotResources.Request()
31 future = self.get_resource_client.call_async(request)
32 rclpy.spin_until_future_complete(self, future)
33
34 if future.result() is None:
35 self.get_logger().error("Failed to call GetRobotResources")
36 return False
37
38 response = future.result()
39 self.resources = list(response.robot_resources)
40 self.get_logger().info(f"Found {len(self.resources)} resources")
41 return True
42
43 def print_resource_menu(self):
44 if not self.resources:
45 print("(no resources)")
46 return
47
48 print("\n=== Available Robot Resources ===")
49 for idx, resource in enumerate(self.resources):
50 ver = resource.current_version
51 print(f"[{idx}] {ver.name or '<no name>'}")
52 print(f" key: {resource.resource_key}")
53 print(f" version: {ver.version}")
54 print("=================================\n")
55
56 def pick_resource(self):
57 if not self.resources:
58 return None
59
60 while True:
61 try:
62 raw = input(
63 f"Select resource index [0-{len(self.resources) - 1}] (q to quit): ").strip()
64 except EOFError:
65 return None
66 if raw.lower() in ("q", "quit", "exit"):
67 return None
68 if not raw.isdigit():
69 print("Please enter a number.")
70 continue
71 idx = int(raw)
72 if 0 <= idx < len(self.resources):
73 return self.resources[idx]
74 print(f"Index out of range (0-{len(self.resources) - 1}).")
75
76 def play_resource(self, resource):
77 while not self.play_resource_client.wait_for_service(timeout_sec=1.0):
78 if not rclpy.ok():
79 self.get_logger().error("Interrupted while waiting for service.")
80 return
81 self.get_logger().info("Service not available, waiting...")
82
83 request = ExecuteActionResource.Request()
84 request.resource_key = resource.resource_key
85 request.resource_version = resource.current_version.version
86
87 if "onnx" in request.resource_key:
88 request.meta = '{"resource_type": "BODY_MONTION"}'
89 else:
90 request.meta = '{"resource_type": "ARM_MONTION"}'
91
92 self.get_logger().info("Sending request:")
93 self.get_logger().info(f" resource_key: {request.resource_key}")
94 self.get_logger().info(
95 f" resource_version: {request.resource_version}")
96 self.get_logger().info(f" meta: {request.meta}")
97
98 future = self.play_resource_client.call_async(request)
99 rclpy.spin_until_future_complete(self, future)
100
101 if future.result() is not None:
102 self.handle_response(future.result())
103 else:
104 self.get_logger().error("Failed to call ExecuteActionResource")
105
106 def handle_response(self, response):
107 self.get_logger().info("Service response received:")
108 self.get_logger().info("Response Header:")
109 self.get_logger().info(
110 f" stamp: {response.header.header.stamp.sec}.{response.header.header.stamp.nanosec:09}")
111 self.get_logger().info(f" code: {response.header.header.code}")
112 self.get_logger().info("Status:")
113 self.get_logger().info(f" value: {response.header.status.value}")
114 self.get_logger().info(f" message: {response.header.message}")
115
116
117def main(args=None):
118 rclpy.init(args=args)
119 client_node = None
120 try:
121 client_node = RobotResourceClient()
122 if not client_node.get_robot_resources():
123 return
124 client_node.print_resource_menu()
125 chosen = client_node.pick_resource()
126 if chosen is None:
127 client_node.get_logger().info("No resource selected, exiting.")
128 return
129 client_node.play_resource(chosen)
130 except KeyboardInterrupt:
131 pass
132 finally:
133 if client_node:
134 client_node.destroy_node()
135 if rclpy.ok():
136 rclpy.shutdown()
137
138
139if __name__ == '__main__':
140 main()
Usage Instructions:
ros2 run py_examples play_linkcraft
[INFO] [1772526701.308025739] [robot_resource_client]: Found 1 resources
=== Available Robot Resources ===
[0] <no name>
key: linkcraft_resource_onnx_01KBM2BHNFM93Z0DK6ES9F6DJA
version: 0.0.1
=================================
[INFO] [1772526701.308537895] [robot_resource_client]: Sending request:
[INFO] [1772526701.308692872] [robot_resource_client]: resource_key: linkcraft_resource_onnx_01KBM2BHNFM93Z0DK6ES9F6DJA
[INFO] [1772526701.308853424] [robot_resource_client]: resource_version: 0.0.1
[INFO] [1772526701.309014801] [robot_resource_client]: meta: {"resource_type": "BODY_MONTION"}
[INFO] [1772526701.338307933] [robot_resource_client]: Service response received:
[INFO] [1772526701.338480169] [robot_resource_client]: Response Header:
[INFO] [1772526701.338647069] [robot_resource_client]: stamp: 0.000000000
[INFO] [1772526701.338800125] [robot_resource_client]: code: 0
[INFO] [1772526701.338951109] [robot_resource_client]: Status:
[INFO] [1772526701.339106459] [robot_resource_client]: value: 1
[INFO] [1772526701.339239979] [robot_resource_client]: message: 1/1 task(s) failed. Details: localhost: failed (status code: 400) - Failed with status code: 400
6.1.32 Play LinkCraft Action (Multi-Robot)
In this example, multi_play_linkcraft is used to synchronize time across multiple robots and perform group-controlled LinkCraft action playback.
1#!/usr/bin/env python3
2
3import rclpy
4from rclpy.node import Node
5from aimdk_msgs.msg import SlaveDevice
6from aimdk_msgs.srv import GetRobotResources, ExecuteActionResource, GroupControlTimeSync
7
8
9class RobotResourceClient(Node):
10 def __init__(self):
11 super().__init__('robot_resource_client')
12
13 self.get_resource_client = self.create_client(
14 GetRobotResources,
15 '/aimdk_5Fmsgs/srv/GetRobotResources'
16 )
17 self.play_resource_client = self.create_client(
18 ExecuteActionResource,
19 '/aimdk_5Fmsgs/srv/ExecuteActionResource'
20 )
21 self.time_sync_client = self.create_client(
22 GroupControlTimeSync,
23 '/aimdk_5Fmsgs/srv/GroupControlTimeSync'
24 )
25
26 self.resources = []
27 self.slave_ips = []
28
29 def get_robot_resources(self):
30 while not self.get_resource_client.wait_for_service(timeout_sec=1.0):
31 if not rclpy.ok():
32 self.get_logger().error("Interrupted while waiting for the service. Exiting.")
33 return False
34 self.get_logger().info("Service not available, waiting again...")
35
36 request = GetRobotResources.Request()
37 future = self.get_resource_client.call_async(request)
38 rclpy.spin_until_future_complete(self, future)
39
40 if future.result() is None:
41 self.get_logger().error("Failed to call GetRobotResources")
42 return False
43
44 response = future.result()
45 self.resources = list(response.robot_resources)
46 self.get_logger().info(f"Found {len(self.resources)} resources")
47 return True
48
49 def print_resource_menu(self):
50 if not self.resources:
51 print("(no resources)")
52 return
53
54 print("\n=== Available Robot Resources ===")
55 for idx, resource in enumerate(self.resources):
56 ver = resource.current_version
57 print(f"[{idx}] {ver.name or '<no name>'}")
58 print(f" key: {resource.resource_key}")
59 print(f" version: {ver.version}")
60 print("=================================\n")
61
62 def pick_resource(self):
63 if not self.resources:
64 return None
65
66 while True:
67 try:
68 raw = input(
69 f"Select resource index [0-{len(self.resources) - 1}] (q to quit): ").strip()
70 except EOFError:
71 return None
72 if raw.lower() in ("q", "quit", "exit"):
73 return None
74 if not raw.isdigit():
75 print("Please enter a number.")
76 continue
77 idx = int(raw)
78 if 0 <= idx < len(self.resources):
79 return self.resources[idx]
80 print(f"Index out of range (0-{len(self.resources) - 1}).")
81
82 def play_resource(self, resource):
83 while not self.play_resource_client.wait_for_service(timeout_sec=1.0):
84 if not rclpy.ok():
85 self.get_logger().error("Interrupted while waiting for service.")
86 return
87 self.get_logger().info("Service not available, waiting...")
88
89 request = ExecuteActionResource.Request()
90 request.resource_key = resource.resource_key
91 request.resource_version = resource.current_version.version
92
93 for ip in self.slave_ips:
94 slave = SlaveDevice()
95 slave.ip = ip
96 request.slaves.append(slave)
97
98 if "onnx" in request.resource_key:
99 request.meta = '{"control_type": "GROUP_CONTROL","resource_type": "BODY_MONTION"}'
100 else:
101 request.meta = '{"control_type": "GROUP_CONTROL","resource_type": "ARM_MONTION"}'
102
103 self.get_logger().info("Sending request:")
104 self.get_logger().info(f" resource_key: {request.resource_key}")
105 self.get_logger().info(
106 f" resource_version: {request.resource_version}")
107 self.get_logger().info(f" meta: {request.meta}")
108 for i, slave in enumerate(request.slaves):
109 self.get_logger().info(f" slaves[{i}] ip: {slave.ip}")
110
111 future = self.play_resource_client.call_async(request)
112 rclpy.spin_until_future_complete(self, future)
113
114 if future.result() is not None:
115 self.handle_response(future.result())
116 else:
117 self.get_logger().error("Failed to call ExecuteActionResource")
118
119 def sync_time(self, ips):
120 self.slave_ips = ips
121
122 while not self.time_sync_client.wait_for_service(timeout_sec=1.0):
123 if not rclpy.ok():
124 self.get_logger().error("Interrupted while waiting for time sync service.")
125 return False
126 self.get_logger().info("TimeSync service not available, waiting again...")
127
128 request = GroupControlTimeSync.Request()
129 for ip in ips:
130 slave = SlaveDevice()
131 slave.ip = ip
132 request.slaves.append(slave)
133
134 self.get_logger().info("Sending GroupControlTimeSync request:")
135 for slave in request.slaves:
136 self.get_logger().info(f" slave ip: {slave.ip}")
137
138 future = self.time_sync_client.call_async(request)
139 rclpy.spin_until_future_complete(self, future)
140
141 if future.result() is None:
142 self.get_logger().error("Failed to call GroupControlTimeSync")
143 return False
144
145 response = future.result()
146 self.get_logger().info("GroupControlTimeSync response:")
147 self.get_logger().info(f" error_code: {response.error_code}")
148 self.get_logger().info(f" error_msg: {response.error_msg}")
149 return response.error_code == 0
150
151 def handle_response(self, response):
152 self.get_logger().info("Service response received:")
153 self.get_logger().info("Response Header:")
154 self.get_logger().info(
155 f" stamp: {response.header.header.stamp.sec}.{response.header.header.stamp.nanosec:09}")
156 self.get_logger().info(f" code: {response.header.header.code}")
157 self.get_logger().info("Status:")
158 self.get_logger().info(f" value: {response.header.status.value}")
159 self.get_logger().info(f" message: {response.header.message}")
160
161
162def main(args=None):
163 rclpy.init(args=args)
164 client_node = None
165 try:
166 client_node = RobotResourceClient()
167 if not client_node.get_robot_resources():
168 return
169
170 # [USER_CONFIG] Replace the IPs below with your slave robot IPs.
171 slave_ips = ["172.22.133.178", "172.22.135.45"]
172 if not client_node.sync_time(slave_ips):
173 client_node.get_logger().warn("Time sync failed or returned error.")
174
175 client_node.print_resource_menu()
176 chosen = client_node.pick_resource()
177 if chosen is None:
178 client_node.get_logger().info("No resource selected, exiting.")
179 return
180 client_node.play_resource(chosen)
181 except KeyboardInterrupt:
182 pass
183 finally:
184 if client_node:
185 client_node.destroy_node()
186 if rclpy.ok():
187 rclpy.shutdown()
188
189
190if __name__ == '__main__':
191 main()
Usage Instructions:
ros2 run py_examples multi_play_linkcraft
[INFO] [1772526701.308025739] [robot_resource_client]: Found 1 resources
[INFO] [1772526701.308537895] [robot_resource_client]: Sending GroupControlTimeSync request:
[INFO] [1772526701.308692872] [robot_resource_client]: slave ip: 172.22.133.178
[INFO] [1772526701.308853424] [robot_resource_client]: slave ip: 172.22.135.45
[INFO] [1772526701.309014801] [robot_resource_client]: GroupControlTimeSync response:
[INFO] [1772526701.309114801] [robot_resource_client]: error_code: 0
[INFO] [1772526701.309214801] [robot_resource_client]: error_msg: ok
[INFO] [1772526701.308537895] [robot_resource_client]: Sending request:
[INFO] [1772526701.308692872] [robot_resource_client]: resource_key: linkcraft_resource_onnx_01KBM2BHNFM93Z0DK6ES9F6DJA
[INFO] [1772526701.308853424] [robot_resource_client]: resource_version: 0.0.1
[INFO] [1772526701.309014801] [robot_resource_client]: meta: {"control_type": "GROUP_CONTROL","resource_type": "BODY_MONTION"}
[INFO] [1772526701.309114801] [robot_resource_client]: slaves[0] ip: 172.22.133.178
[INFO] [1772526701.309214801] [robot_resource_client]: slaves[1] ip: 172.22.135.45
[INFO] [1772526701.338307933] [robot_resource_client]: Service response received:
[INFO] [1772526701.338480169] [robot_resource_client]: Response Header:
[INFO] [1772526701.338647069] [robot_resource_client]: stamp: 0.000000000
[INFO] [1772526701.338800125] [robot_resource_client]: code: 0
[INFO] [1772526701.338951109] [robot_resource_client]: Status:
[INFO] [1772526701.339106459] [robot_resource_client]: value: 1
[INFO] [1772526701.339239979] [robot_resource_client]: message: 1/1 task(s) failed. Details: localhost: failed (status code: 400) - Failed with status code: 400
6.1.33 LED Strip Control
Function description: Demonstrates how to control the robot’s LED light strip, supporting multiple display modes and custom colors.
Attention
Before using this feature, run the following command on the Motion Control Computing Unit (PC1) to stop the task_manager module and prevent it from competing with your program for LED strip control:
aima em stop-app task_manager
Core Code:
1#!/usr/bin/env python3
2
3import sys
4import rclpy
5import rclpy.logging
6from rclpy.node import Node
7
8from aimdk_msgs.srv import SetPmuLed
9
10
11class PlayLightsClient(Node):
12 def __init__(self):
13 super().__init__('play_lights_client')
14
15 # create service client
16 self.client = self.create_client(
17 SetPmuLed, '/aimdk_5Fmsgs/srv/SetPmuLed')
18
19 self.get_logger().info('✅ PlayLights client node created.')
20
21 # Wait for the service to become available
22 while not self.client.wait_for_service(timeout_sec=2.0):
23 self.get_logger().info('⏳ Service unavailable, waiting...')
24
25 self.get_logger().info('🟢 Service available, ready to send request.')
26
27 def send_request(self, led_mode, r, g, b, priority=6, reset_priority=False):
28 """Send LED control request"""
29 # create request
30 request = SetPmuLed.Request()
31 request.led_strip_mode = led_mode
32 request.r = r
33 request.g = g
34 request.b = b
35 request.priority = priority
36 request.reset_priority = reset_priority
37 request.trace_id = 'play_lights'
38
39 # send request
40 # Note: LED strip is slow to response (up to ~5s)
41 self.get_logger().info(
42 f'📨 Sending request to control led strip: mode={led_mode}, RGB=({r}, {g}, {b}), priority={priority}, reset_priority={reset_priority}')
43 for i in range(4):
44 request.request.header.stamp = self.get_clock().now().to_msg()
45 future = self.client.call_async(request)
46 rclpy.spin_until_future_complete(self, future, timeout_sec=5)
47
48 if future.done():
49 break
50
51 # retry as remote peer is NOT handled well by ROS
52 self.get_logger().info(f'trying ... [{i}]')
53
54 response = future.result()
55 if response is None:
56 self.get_logger().error('❌ Service call not completed or timed out.')
57 return False
58
59 if response.status_code == 0:
60 self.get_logger().info('✅ LED strip command sent successfully.')
61 return True
62 else:
63 # 0x1024: priority insufficient (priority < current threshold);
64 # use a higher priority or set reset_priority=True to reset the
65 # threshold to 0.
66 reason = ('priority insufficient'
67 if response.status_code == 0x1024
68 else 'failure')
69 self.get_logger().error(
70 f'❌ LED strip command failed with status: '
71 f'0x{response.status_code:X} ({reason})')
72 return False
73
74
75def main(args=None):
76 rclpy.init(args=args)
77 node = None
78
79 try:
80 # get command line args
81 if len(sys.argv) > 6:
82 # use CLI args
83 led_mode = int(sys.argv[1])
84 if led_mode not in (0, 1, 2, 3):
85 raise ValueError("invalid mode")
86 r = int(sys.argv[2])
87 if r < 0 or r > 255:
88 raise ValueError("invalid R value")
89 g = int(sys.argv[3])
90 if g < 0 or g > 255:
91 raise ValueError("invalid G value")
92 b = int(sys.argv[4])
93 if b < 0 or b > 255:
94 raise ValueError("invalid B value")
95 priority = int(sys.argv[5])
96 if priority < 0:
97 raise ValueError("invalid priority value")
98 reset_priority = sys.argv[6].lower() in ('true', '1', 'yes')
99 else:
100 # interactive input
101 print("=== LED strip control example ===")
102 print("Select LED strip mode:")
103 print("0 - Steady on")
104 print("1 - Breathing (4s cycle, sine brightness)")
105 print("2 - Blinking (1s cycle, 0.5s on, 0.5s off)")
106 print("3 - Flowing (2s cycle, light up from left to right)")
107
108 led_mode = int(input("Enter mode (0-3): "))
109 if led_mode not in (0, 1, 2, 3):
110 raise ValueError("invalid mode")
111
112 print("\nSet RGB color values (0-255):")
113 r = int(input("Red (R): "))
114 if r < 0 or r > 255:
115 raise ValueError("invalid R value")
116 g = int(input("Green (G): "))
117 if g < 0 or g > 255:
118 raise ValueError("invalid G value")
119 b = int(input("Blue (B): "))
120 if b < 0 or b > 255:
121 raise ValueError("invalid B value")
122
123 print("\nSet priority (higher value = higher priority, default=6):")
124 priority = int(input("Priority: "))
125 if priority < 0:
126 raise ValueError("invalid priority value")
127
128 reset_priority = input(
129 "Reset priority after command? (y/n, default=n): ").strip().lower() in ('y', 'yes', 'true', '1')
130
131 node = PlayLightsClient()
132 node.send_request(led_mode, r, g, b, priority, reset_priority)
133 except KeyboardInterrupt:
134 pass
135 except Exception as e:
136 rclpy.logging.get_logger('main').error(
137 f'Program exited with exception: {e}')
138
139 if node:
140 node.destroy_node()
141 if rclpy.ok():
142 rclpy.shutdown()
143
144
145if __name__ == '__main__':
146 main()
Usage Instructions:
# Build
colcon build --packages-select py_examples
# Run interactively
ros2 run py_examples play_lights
# Run with command-line parameters: <mode> <R> <G> <B> <priority> <reset_priority(true/false)>
ros2 run py_examples play_lights 1 255 0 0 6 false # Mode 1, red, priority 6
Output Example:
=== LED strip control example ===
Select LED strip mode:
0 - Steady on
1 - Breathing (4s cycle, sine brightness)
2 - Blinking (1s cycle, 0.5s on, 0.5s off)
3 - Flowing (2s cycle, light up from left to right)
Enter mode (0-3): 1
Set RGB color values (0-255):
Red (R): 255
Green (G): 0
Blue (B): 0
Set priority (higher value = higher priority, default=6):
Priority: 6
Reset priority after command? (y/n, default=n): n
[INFO] [play_lights_client]: 📨 Sending request to control led strip: mode=1, RGB=(255, 0, 0), priority=6, reset_priority=False
[INFO] [play_lights_client]: ✅ LED strip command sent successfully.
Technical Features:
Supports 4 LED display modes
Customizable RGB colors
Priority control (higher value = higher priority)
Priority reset support
Asynchronous service invocation
Command-line parameter support
Input parameter validation
User-friendly interactive interface
6.1.34 GNSS Positioning Data Subscription
This example uses echo_gnss_data to subscribe to GNSS topics for positioning data, outputting latitude, longitude, altitude, and covariance information.
Features:
Subscribes to NavSatFix positioning data (latitude, longitude, altitude)
Real-time display of positioning status and covariance
Topics:
/aima/hal/sensor/gnss: Positioning data (sensor_msgs/NavSatFix), published automatically by the system
1#!/usr/bin/env python3
2import time
3import rclpy
4from rclpy.node import Node
5from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, QoSDurabilityPolicy
6from rclpy.time import Time
7from sensor_msgs.msg import NavSatFix, NavSatStatus
8
9
10STATUS_STR = {
11 NavSatStatus.STATUS_NO_FIX: 'NO_FIX',
12 NavSatStatus.STATUS_FIX: 'FIX',
13}
14
15
16class GnssPrinter(Node):
17 NAVSAT_TOPIC = '/aima/hal/sensor/gnss'
18
19 def __init__(self):
20 super().__init__('gnss_printer')
21
22 qos = QoSProfile(
23 reliability=QoSReliabilityPolicy.RELIABLE,
24 history=QoSHistoryPolicy.KEEP_LAST,
25 durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
26 depth=10,
27 )
28
29 self._last_recv = None
30 self.sub = self.create_subscription(
31 NavSatFix, self.NAVSAT_TOPIC, self.cb, qos
32 )
33 self.get_logger().info(f'Subscribing NavSatFix: {self.NAVSAT_TOPIC}')
34
35 def cb(self, msg: NavSatFix):
36 now = time.perf_counter()
37 dt_ms = 0.0 if self._last_recv is None else (
38 now - self._last_recv) * 1000.0
39 self._last_recv = now
40
41 t_sec = Time.from_msg(msg.header.stamp).nanoseconds / 1e9
42 status_str = STATUS_STR.get(msg.status.status, 'UNKNOWN')
43 cov = msg.position_covariance
44
45 self.get_logger().info(
46 "\n========== NavSatFix ==========\n"
47 f"stamp={t_sec:.6f}s\n"
48 f"frame={msg.header.frame_id}\n"
49 f"recv_dt={dt_ms:.3f}ms\n"
50 f"latitude: {msg.latitude:.8f} deg\n"
51 f"longitude: {msg.longitude:.8f} deg\n"
52 f"altitude: {msg.altitude:.3f} m\n"
53 f"status: {status_str}\n"
54 f"service: {msg.status.service}\n"
55 f"position_covariance: [{cov[0]:.4f}, {cov[1]:.4f}, {cov[2]:.4f}, "
56 f"{cov[3]:.4f}, {cov[4]:.4f}, {cov[5]:.4f}, "
57 f"{cov[6]:.4f}, {cov[7]:.4f}, {cov[8]:.4f}]\n"
58 f"position_covariance_type: {msg.position_covariance_type}"
59 )
60
61
62def main():
63 rclpy.init()
64 node = GnssPrinter()
65 try:
66 rclpy.spin(node)
67 except KeyboardInterrupt:
68 pass
69 finally:
70 node.destroy_node()
71 if rclpy.ok():
72 rclpy.shutdown()
73
74
75if __name__ == '__main__':
76 main()
Usage Instructions:
# Build
colcon build --packages-select py_examples
# Subscribe to GNSS data
ros2 run py_examples echo_gnss_data
Output Example:
[INFO] [gnss_printer]: Subscribing NavSatFix: /aima/hal/sensor/gnss
========== NavSatFix ==========
stamp=1234567890.123456s
frame=gnss
recv_dt=1000.000ms
latitude: 31.84537000 deg
longitude: 117.19882000 deg
altitude: 30.000 m
status: FIX
service: 1
position_covariance: [1.0000, 0.0000, 0.0000, 0.0000, 1.0000, 0.0000, 0.0000, 0.0000, 1.0000]
position_covariance_type: 2
Notes:
Positioning status: NO_FIX (no fix), FIX (single point fix)
Indoors, status is usually NO_FIX; move to an open outdoor area and wait for a fix
6.1.35 Upper Body Control
Note
Switch to the corresponding mode before running. See Upper Body Control.
1#!/usr/bin/env python3
2"""
3Upper body control example.
4
5Usage:
6 ros2 run py_examples upper_body_control <mode>
7
8 mode:
9 head — head center pose, requires HEAD_ONLY mode
10 claw — claw half-open, requires UPPERBODY_REMOTE_SPLIT mode
11 joint — dexterous hand half-open, requires UPPERBODY_REMOTE_SPLIT mode
12 gesture — dexterous hand gesture 1, requires UPPERBODY_REMOTE_SPLIT mode
13
14Switch to the desired MC mode before running, e.g. for head:
15 ros2 run py_examples set_mc_action SD # → STAND_DEFAULT
16 ros2 run py_examples set_mc_action HO # → HEAD_ONLY
17or for claw / joint / gesture:
18 ros2 run py_examples set_mc_action URS # → UPPERBODY_REMOTE_SPLIT
19"""
20
21import sys
22import rclpy
23from rclpy.node import Node
24from aimdk_msgs.msg import UpperBodyCommandArray, MessageHeader
25
26# hand_sub_mode, head_pos, arm_pos, hand_pos
27MODES = {
28 # head_yaw=0, head_pitch=0 (center)
29 'head': (0, [0.0, 0.0],
30 [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
31 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
32 []),
33 # left_open=0.5, right_open=0.5
34 'claw': (UpperBodyCommandArray.HAND_CLAW_OPEN_CLOSE, # claw (1)
35 [0.0, 0.0],
36 [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
37 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
38 [0.5, 0.5]),
39 # left 10 joints + right 10 joints, in rad, all half-open
40 # (each joint at half its URDF range limit). Right is the mirror of left.
41 # Joint order: [thumb_roll, thumb_abad, thumb_mcp, index_abad, index_pip,
42 # middle_pip, ring_abad, ring_pip, pinky_abad, pinky_pip]
43 'joint': (UpperBodyCommandArray.HAND_DEXTEROUS_JOINT, # dexterous joint (2)
44 [0.0, 0.0],
45 [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
46 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
47 [-0.6, 0.8, -0.4, 0.1, 0.7, 0.7, -0.1, 0.7, -0.1, 0.7,
48 0.6, -0.8, 0.4, -0.1, 0.7, 0.7, 0.1, 0.7, 0.1, 0.7]),
49 # left_gesture=1, left_open=1.0, right_gesture=1, right_open=1.0
50 'gesture': (UpperBodyCommandArray.HAND_DEXTEROUS_GESTURE, # dexterous gesture (3)
51 [0.0, 0.0],
52 [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
53 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
54 [1.0, 1.0, 1.0, 1.0]),
55}
56
57
58class UpperBodyControlNode(Node):
59 def __init__(self, mode: str):
60 super().__init__('upper_body_control')
61 self.hand_sub_mode, self.head_pos, self.arm_pos, self.hand_pos = MODES[mode]
62 self.pub = self.create_publisher(
63 UpperBodyCommandArray, '/mc/upper_body_command', 10)
64 self.timer = self.create_timer(0.02, self.publish) # 50 Hz
65 self._seq = 0
66 self.get_logger().info(
67 f'mode={mode} hand_sub_mode={self.hand_sub_mode}')
68
69 def publish(self):
70 msg = UpperBodyCommandArray()
71 now = self.get_clock().now()
72 msg.header = MessageHeader()
73 msg.header.stamp.sec = now.nanoseconds // 1_000_000_000
74 msg.header.stamp.nanosec = now.nanoseconds % 1_000_000_000
75 msg.header.frame_id = 'mc_upper_body'
76 msg.header.sequence = self._seq
77 self._seq += 1
78 msg.source = 'upper_body_example'
79 msg.hand_sub_mode = self.hand_sub_mode
80 msg.head_pos = self.head_pos
81 msg.arm_pos = self.arm_pos
82 msg.hand_pos = self.hand_pos
83 self.pub.publish(msg)
84
85
86def main(args=None):
87 rclpy.init(args=args)
88
89 argv = sys.argv[1:]
90 if not argv or argv[0] not in MODES:
91 print(f'Usage: upper_body_control <{"│".join(MODES)}>')
92 rclpy.shutdown()
93 return
94
95 node = UpperBodyControlNode(argv[0])
96 try:
97 rclpy.spin(node)
98 except KeyboardInterrupt:
99 pass
100 finally:
101 node.destroy_node()
102 if rclpy.ok():
103 rclpy.shutdown()
104
105
106if __name__ == '__main__':
107 main()
Usage Instructions:
colcon build --packages-select py_examples
# Head control (HEAD_ONLY mode)
ros2 run py_examples upper_body_control head
# Claw (gripper) control (UPPERBODY_REMOTE_SPLIT mode)
ros2 run py_examples upper_body_control claw
# Dexterous hand joints (UPPERBODY_REMOTE_SPLIT mode)
ros2 run py_examples upper_body_control joint
# Dexterous hand gesture (UPPERBODY_REMOTE_SPLIT mode)
ros2 run py_examples upper_body_control gesture
6.1.36 Subscribe to HDS Diagnostics / Query Historical Events
Subscribe to /aima/hds/diag_code_list, /aima/hds/alert_code_list, and /aima/hds/post_proc/signal topics to receive diagnostic codes, alert codes, and post-processing signals in real time; also supports calling the GetHdsEventHistory service to query historical events. For the full list of diagnostic codes, see Diagnostic Code Reference.
1#!/usr/bin/env python3
2"""
3HDS diagnostics tool - two modes:
4
5 echo Subscribe to diag / alert / post_proc topics in real time.
6 history Query historical alert / diagnostic events via service.
7
8See docs/cn/example/Python.md section "Subscribe HDS diagnostics /
9query history events" for full usage and field descriptions.
10"""
11
12import argparse
13import sys
14import time
15
16import rclpy
17from rclpy.node import Node
18from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy
19
20import aimdk_msgs.msg as aimdk_msg
21import aimdk_msgs.srv as aimdk_srv
22
23
24DIAG_TYPE_NAMES = {
25 aimdk_msg.DiagnosticType.NORMAL: "NORMAL",
26 aimdk_msg.DiagnosticType.TRIGGER_APPEAR: "APPEAR",
27 aimdk_msg.DiagnosticType.TRIGGER_DISAPPEAR: "DISAPPEAR",
28}
29
30STATE_NAMES = {
31 aimdk_msg.HdsEventQuery.STATE_ALL: "ALL",
32 aimdk_msg.HdsEventQuery.STATE_ACTIVE: "ACTIVE",
33 aimdk_msg.HdsEventQuery.STATE_RECOVERED: "RECOVERED",
34}
35
36
37# ---------- echo mode ----------
38
39class HdsDiagnosticsEcho(Node):
40 """Subscribe to three HDS topics and print incoming messages."""
41
42 def __init__(self):
43 super().__init__('hds_diagnostics_echo')
44
45 # HDS publishers use BEST_EFFORT; match it to receive messages.
46 qos = QoSProfile(
47 depth=10,
48 reliability=ReliabilityPolicy.BEST_EFFORT,
49 durability=DurabilityPolicy.VOLATILE,
50 )
51
52 self.create_subscription(
53 aimdk_msg.DiagnosticInfoArray,
54 '/aima/hds/diag_code_list',
55 self.on_diagnostics, qos)
56
57 self.create_subscription(
58 aimdk_msg.AlertCodeArray,
59 '/aima/hds/alert_code_list',
60 self.on_alerts, qos)
61
62 self.create_subscription(
63 aimdk_msg.PostProcSigArray,
64 '/aima/hds/post_proc/signal',
65 self.on_post_proc, qos)
66
67 self.get_logger().info(
68 'Subscribed: diag_code_list / alert_code_list / post_proc/signal')
69
70 def on_diagnostics(self, msg: aimdk_msg.DiagnosticInfoArray):
71 if not msg.diagnostics:
72 return
73 lines = [f'[Diagnostics] count={len(msg.diagnostics)}']
74 for d in msg.diagnostics:
75 type_name = DIAG_TYPE_NAMES.get(
76 d.type.value, f'UNKNOWN({d.type.value})')
77 lines.append(
78 f' diag_code=0x{d.diag_code:010X} type={type_name} '
79 f'module_id={d.module_id} ts={d.timestamp} info="{d.info}"')
80 self.get_logger().info('\n'.join(lines))
81
82 def on_alerts(self, msg: aimdk_msg.AlertCodeArray):
83 if not msg.list:
84 return
85 lines = [f'[Alerts] count={len(msg.list)}']
86 for a in msg.list:
87 lines.append(
88 f' code="{a.code}" ts={a.timestamp} '
89 f'first_ts={a.first_timestamp}')
90 self.get_logger().info('\n'.join(lines))
91
92 def on_post_proc(self, msg: aimdk_msg.PostProcSigArray):
93 if not msg.post_proc_sig:
94 return
95 lines = [f'[PostProc] count={len(msg.post_proc_sig)}']
96 for p in msg.post_proc_sig:
97 s = p.sig_set
98 lines.append(
99 f' diag_code=0x{p.diag_code:010X} ts={p.timestamp} '
100 f'robot={s.robot_sig} motion={s.motion_sig} '
101 f'sound={s.sound_sig} led={s.led_sig} facial={s.facial_sig} '
102 f'funcdisable=0x{s.funcdisable_sig:016X}')
103 self.get_logger().info('\n'.join(lines))
104
105
106def run_echo(_args) -> int:
107 node = HdsDiagnosticsEcho()
108 try:
109 rclpy.spin(node)
110 except KeyboardInterrupt:
111 pass
112 finally:
113 node.destroy_node()
114 return 0
115
116
117# ---------- history mode ----------
118
119class HdsHistoryClient(Node):
120 """Call the GetHdsEventHistory service to query historical events."""
121
122 SERVICE_NAME = '/aimdk_5Fmsgs/srv/GetHdsEventHistory'
123
124 def __init__(self):
125 super().__init__('hds_history_client')
126 self.client = self.create_client(
127 aimdk_srv.GetHdsEventHistory, self.SERVICE_NAME)
128
129 def call(self, query: aimdk_msg.HdsEventQuery, timeout_sec: float = 10.0):
130 if not self.client.wait_for_service(timeout_sec=5.0):
131 self.get_logger().error(
132 f'Service {self.SERVICE_NAME} not available')
133 return None
134
135 request = aimdk_srv.GetHdsEventHistory.Request()
136 now = self.get_clock().now()
137 request.header.stamp.sec = now.nanoseconds // 1_000_000_000
138 request.header.stamp.nanosec = now.nanoseconds % 1_000_000_000
139 request.query = query
140
141 future = self.client.call_async(request)
142 start = time.time()
143 while rclpy.ok() and time.time() - start < timeout_sec:
144 rclpy.spin_once(self, timeout_sec=0.1)
145 if future.done():
146 return future.result()
147
148 self.get_logger().error('Service call timeout')
149 future.cancel()
150 return None
151
152
153def build_query(args) -> aimdk_msg.HdsEventQuery:
154 q = aimdk_msg.HdsEventQuery()
155 q.start_time = args.start_ns
156 q.end_time = args.end_ns
157 q.diag_codes = args.diag_codes
158 q.alert_codes = args.alert_codes
159 q.alert_locations = []
160 q.state = args.state
161 q.order_desc = args.desc
162 q.offset = args.offset
163 q.limit = args.limit
164 return q
165
166
167def print_records(response) -> None:
168 print(f'total_count = {response.total_count}, '
169 f'returned = {len(response.records)}, '
170 f'has_more = {response.has_more}')
171 if not response.records:
172 return
173 print('-' * 80)
174 for r in response.records:
175 recovered = 'YES' if r.recovered else 'NO'
176 duration_s = r.duration_ns / 1e9
177 print(f'id={r.id} diag=0x{r.diag_code:010X} '
178 f'alert="{r.alert_code}"')
179 print(f' first_ts={r.first_timestamp} '
180 f'last_ts={r.last_timestamp}')
181 print(f' recovered={recovered} '
182 f'recover_ts={r.recover_timestamp} '
183 f'duration={duration_s:.3f}s')
184 if r.alert_location:
185 print(f' location="{r.alert_location}"')
186 print('-' * 80)
187
188
189def run_history(args) -> int:
190 node = HdsHistoryClient()
191 try:
192 query = build_query(args)
193 node.get_logger().info(
194 f'Querying history: state={STATE_NAMES.get(query.state)}, '
195 f'offset={query.offset}, limit={query.limit}, '
196 f'desc={query.order_desc}')
197
198 response = node.call(query)
199 if response is None:
200 return 1
201 print_records(response)
202 return 0
203 finally:
204 node.destroy_node()
205
206
207# ---------- entry point ----------
208
209def parse_args():
210 parser = argparse.ArgumentParser(
211 description='HDS diagnostics tool - echo topics or query history')
212 sub = parser.add_subparsers(dest='mode', required=True)
213
214 sub.add_parser(
215 'echo', help='subscribe diag/alert/post_proc topics and print live')
216
217 h = sub.add_parser('history', help='query GetHdsEventHistory service')
218 h.add_argument('--start-ns', type=int, default=0,
219 help='start timestamp (ns), 0 means unlimited')
220 h.add_argument('--end-ns', type=int, default=0,
221 help='end timestamp (ns), 0 means unlimited')
222 h.add_argument('--diag-codes', type=lambda s: int(s, 0), nargs='*',
223 default=[], help='filter diag codes (e.g. 0x6200000001)')
224 h.add_argument('--alert-codes', nargs='*', default=[],
225 help='filter alert code strings')
226 h.add_argument('--state', type=int, default=aimdk_msg.HdsEventQuery.STATE_ALL,
227 choices=[0, 1, 2],
228 help='0=ALL, 1=ACTIVE, 2=RECOVERED')
229 h.add_argument('--offset', type=int, default=0)
230 h.add_argument('--limit', type=int, default=20)
231 h.add_argument('--desc', action='store_true',
232 help='order by time descending')
233
234 return parser.parse_args()
235
236
237def main() -> int:
238 args = parse_args()
239 rclpy.init()
240 try:
241 if args.mode == 'echo':
242 return run_echo(args)
243 if args.mode == 'history':
244 return run_history(args)
245 return 1
246 finally:
247 if rclpy.ok():
248 rclpy.shutdown()
249
250
251if __name__ == '__main__':
252 sys.exit(main())
Usage Instructions
Real-time subscription (echo):
# Subscribe to three topics, print diag codes, alerts, post-proc signals
ros2 run py_examples hds_diagnostics echo
Topic |
Message Type |
|---|---|
|
|
|
|
|
|
Query historical events (history) — all filter options correspond to HdsEventQuery fields. Usage examples:
# Query latest 20 records (default limit=20, ascending)
ros2 run py_examples hds_diagnostics history
# Query by time range (start_time / end_time)
ros2 run py_examples hds_diagnostics history \
--start-ns 1716000000000000000 \
--end-ns 1716100000000000000
# Show only active alerts (state = STATE_ACTIVE)
ros2 run py_examples hds_diagnostics history --state 1
# Filter by diagnostic codes (diag_codes, hex supported)
ros2 run py_examples hds_diagnostics history --diag-codes 0x6200000001 0x6200000002
# Filter by alert codes (alert_codes)
ros2 run py_examples hds_diagnostics history --alert-codes MOTOR_OVERHEAT COMM_LOST
# Pagination + descending (limit / offset / order_desc, newest first)
ros2 run py_examples hds_diagnostics history --limit 50 --offset 0 --desc
# Combined: only recovered, 10 per page, newest first
ros2 run py_examples hds_diagnostics history --state 2 --limit 10 --desc
Response fields correspond to HdsEventRecord.
echo Mode Output Example
[INFO] [hds_diagnostics_echo]: Subscribed: diag_code_list / alert_code_list / post_proc/signal
[INFO] [hds_diagnostics_echo]: [Diagnostics] count=1
diag_code=0x6200000007 type=APPEAR module_id=98 ts=1770125762202825297 info="Unable to get robot localization"
[INFO] [hds_diagnostics_echo]: [Alerts] count=1
code="LOCALIZATION_LOST" ts=1770125762202825297 first_ts=1770125762202825297
[INFO] [hds_diagnostics_echo]: [PostProc] count=1
diag_code=0x6200000007 ts=1770125762202825297 robot=1 motion=2 sound=1 led=3 facial=0 funcdisable=0x0000000000000004
history Mode Output Example
[INFO] [hds_history_client]: Querying history: state=ALL, offset=0, limit=20, desc=True
total_count = 5, returned = 5, has_more = False
--------------------------------------------------------------------------------
id=42 diag=0x6200000007 alert="LOCALIZATION_LOST"
first_ts=1770125762202825297 last_ts=1770125765310145000
recovered=YES recover_ts=1770125770000000000 duration=7.797s
--------------------------------------------------------------------------------
Interface Reference
Topics:
/aima/hds/diag_code_list,/aima/hds/alert_code_list,/aima/hds/post_proc/signalService:
/aimdk_5Fmsgs/srv/GetHdsEventHistoryMessages:
aimdk_msgs/msg/DiagnosticInfoArray,aimdk_msgs/msg/AlertCodeArray,aimdk_msgs/msg/PostProcSigArray