6.1 Python interface usage example

This chapter will guide you through implementing several features shown in the index

Build & Run Instructions

• Change into the aimdk directory and run the following commands

  source /opt/ros/humble/setup.bash
  colcon build
  source install/setup.bash
  ros2 run py_examples '<corresponding feature name e.g.: get_mc_action>'
  

📝 Code explanation

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)

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.

Definition of Developer Mode

#!/usr/bin/env python3
"""
System State Migrator - Synchronous Python version
"""

import sys
import time
import rclpy
from rclpy.node import Node
import aimdk_msgs.srv as aimdk_srv
import aimdk_msgs.msg as aimdk_msg


class SyncSystemStateMigrator(Node):
    """Synchronous version of system state migrator"""

    def __init__(self, target_state: str):
        super().__init__('sync_system_state_migrator')
        self.target_state = target_state

        # Create service clients
        self.migrate_client = self.create_client(
            aimdk_srv.MigrateSystemState,
            '/aimdk_5Fmsgs/srv/MigrateSystemState'
        )

        self.get_state_client = self.create_client(
            aimdk_srv.GetSystemState,
            '/aimdk_5Fmsgs/srv/GetSystemState'
        )

        self.get_logger().info(f'Target state: {self.target_state}')

    def wait_for_services(self, timeout_sec: float = 30.0) -> bool:
        """Wait for services to become available"""
        self.get_logger().info('Waiting for services...')

        start_time = time.time()
        while time.time() - start_time < timeout_sec and rclpy.ok():
            if (self.migrate_client.wait_for_service(timeout_sec=1.0) and
                    self.get_state_client.wait_for_service(timeout_sec=1.0)):
                self.get_logger().info('Services are available')
                return True

            self.get_logger().info('Services not available, waiting...')
            time.sleep(0.5)

        self.get_logger().error(
            f'Timeout waiting for services after {timeout_sec} seconds')
        return False

    def migrate_state_sync(self, max_retries: int = 5, timeout_sec: float = 30.0) -> bool:
        """Send migration request synchronously with retry"""
        request = aimdk_srv.MigrateSystemState.Request()
        now = self.get_clock().now()
        request.header.header.stamp.sec = now.nanoseconds // 1_000_000_000
        request.header.header.stamp.nanosec = now.nanoseconds % 1_000_000_000
        request.state = self.target_state

        for attempt in range(1, max_retries + 1):
            self.get_logger().info(
                f'Sending migration request to {self.target_state} '
                f'(attempt {attempt}/{max_retries})...'
            )

            future = None
            try:
                future = self.migrate_client.call_async(request)

                start_time = time.time()
                while rclpy.ok() and time.time() - start_time < timeout_sec:
                    rclpy.spin_once(self, timeout_sec=0.1)
                    if future.done():
                        response = future.result()
                        if response.header.status.value == aimdk_msg.CommonState.SUCCESS:
                            self.get_logger().info('Migration request accepted')
                            return True
                        else:
                            self.get_logger().error(
                                f'Migration failed: {response.header.message}'
                            )
                            return False

                self.get_logger().warning(
                    f'Timeout waiting for migration response '
                    f'(attempt {attempt}/{max_retries})'
                )

            except Exception as e:
                self.get_logger().error(
                    f'Migration service call failed (attempt {attempt}/{max_retries}): {e}'
                )
            finally:
                if future is not None and not future.done():
                    future.cancel()

            if attempt < max_retries:
                self.get_logger().info('Retrying migration request in 2.0 seconds...')
                time.sleep(2.0)

        self.get_logger().error(
            f'Migration request failed after {max_retries} attempts'
        )
        return False

    def get_current_state_sync(self, max_retries: int = 3, timeout_sec: float = 10.0):
        """Get current system state synchronously with retry"""
        request = aimdk_srv.GetSystemState.Request()
        now = self.get_clock().now()
        request.header.header.stamp.sec = now.nanoseconds // 1_000_000_000
        request.header.header.stamp.nanosec = now.nanoseconds % 1_000_000_000

        for attempt in range(1, max_retries + 1):
            future = None
            try:
                future = self.get_state_client.call_async(request)

                start_time = time.time()
                while rclpy.ok() and time.time() - start_time < timeout_sec:
                    rclpy.spin_once(self, timeout_sec=0.1)
                    if future.done():
                        return future.result()

                self.get_logger().warning(
                    f'Timeout getting system state (attempt {attempt}/{max_retries})'
                )

            except Exception as e:
                self.get_logger().error(
                    f'GetSystemState service call failed (attempt {attempt}/{max_retries}): {e}'
                )
            finally:
                if future is not None and not future.done():
                    future.cancel()

            if attempt < max_retries:
                time.sleep(1.0)

        self.get_logger().error(
            f'Failed to get system state after {max_retries} attempts'
        )
        return None

    def monitor_migration_sync(self, max_checks: int = 300) -> bool:
        """Monitor migration progress synchronously"""
        self.get_logger().info('Monitoring migration progress...')

        check_count = 0
        start_time = time.time()

        while check_count < max_checks and rclpy.ok():
            # Wait between checks
            time.sleep(1.0)

            # Check current state
            response = self.get_current_state_sync()
            if response is None:
                self.get_logger().warning('Failed to get system state, will retry...')
                continue

            check_count += 1

            # Check if migration is complete
            current_state_lower = response.cur_state.lower()
            target_state_lower = self.target_state.lower()

            state_match = (current_state_lower == target_state_lower)
            status_match = (response.curr_status.value ==
                            aimdk_msg.SystemStatus.IN_READY)

            self.get_logger().info(
                f'Check {check_count}: State="{response.cur_state}" '
                f'(match={state_match}), Status={response.curr_status.value} (match={status_match})'
            )

            if state_match and status_match:
                self.get_logger().info(
                    f'Migration to {self.target_state} completed successfully!')
                return True

            # Check timeout
            if time.time() - start_time > 300.0:  # 5 minutes total timeout
                self.get_logger().error('Migration timeout after 5 minutes')
                return False

        self.get_logger().error(f'Migration failed after {max_checks} checks')
        return False

    def run_migration(self) -> bool:
        """Run complete migration process"""
        # Step 1: Wait for services
        if not self.wait_for_services():
            return False

        # Step 2: Send migration request
        if not self.migrate_state_sync():
            return False

        # Step 3: Monitor migration progress
        return self.monitor_migration_sync()


def main():
    """
    Main function
    """
    # Check command line arguments
    if len(sys.argv) != 2:
        print(f'Usage: {sys.argv[0]} <target_state>')
        print('\nAvailable target states: Ready, Develop_Nav, Develop_Audio_Linux, Develop_Audio_ROS, Develop_MC')
        return 1

    # Get target state from command line argument
    target_state = sys.argv[1]

    # Validate target state
    if not target_state:
        print('Error: Target state cannot be empty', file=sys.stderr)
        return 1

    # Initialize ROS2
    rclpy.init()

    result = 1
    node = None

    try:
        # Create migrator node
        node = SyncSystemStateMigrator(target_state)

        # Run migration
        if node.run_migration():
            print(f'\nMigration to {target_state} completed successfully!')
            result = 0
        else:
            print(f'\nMigration to {target_state} failed')
            result = 1

    except KeyboardInterrupt:
        print('\nProgram interrupted by user')
        result = 1
    except Exception as e:
        print(f'Error: {e}', file=sys.stderr)
        result = 1
    finally:
        # Cleanup
        if node:
            node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()

    return result


if __name__ == '__main__':
    sys.exit(main())

Usage

ros2 run py_examples migrate_system_state Ready

Example output

[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...
[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=False), Status=2 (match=False)
[INFO] [1770125665.733183333] [sync_system_state_migrator]: Check 2: State="Business" (match=False), Status=2 (match=False)
[INFO] [1770125666.736488851] [sync_system_state_migrator]: Check 3: State="Ready" (match=True), Status=1 (match=True)
[INFO] [1770125666.736982424] [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.

Motion Mode Definitions

#!/usr/bin/env python3

import rclpy
import rclpy.logging
from rclpy.node import Node

from aimdk_msgs.srv import GetMcAction
from aimdk_msgs.msg import CommonRequest


class GetMcActionClient(Node):
    def __init__(self):
        super().__init__('get_mc_action_client')
        self.client = self.create_client(
            GetMcAction, '/aimdk_5Fmsgs/srv/GetMcAction')
        self.get_logger().info('✅ GetMcAction client node created.')

        # Wait for the service to become available
        while not self.client.wait_for_service(timeout_sec=2.0):
            self.get_logger().info('⏳ Service unavailable, waiting...')

        self.get_logger().info('🟢 Service available, ready to send request.')

    def send_request(self):
        request = GetMcAction.Request()
        request.request = CommonRequest()

        self.get_logger().info('📨 Sending request to get robot mode')
        for i in range(8):
            request.request.header.stamp = self.get_clock().now().to_msg()
            future = self.client.call_async(request)
            rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)

            if future.done():
                break

            # retry as remote peer is NOT handled well by ROS
            self.get_logger().info(f'trying ... [{i}]')

        response = future.result()
        if response is None:
            self.get_logger().error('❌ Service call failed or timed out.')
            return

        self.get_logger().info('✅ Robot mode get successfully.')
        self.get_logger().info(f'Mode name: {response.info.action_desc}')
        self.get_logger().info(f'Mode status: {response.info.status.value}')


def main(args=None):
    rclpy.init(args=args)
    node = None
    try:
        node = GetMcActionClient()
        node.send_request()
    except KeyboardInterrupt:
        pass
    except Exception as e:
        rclpy.logging.get_logger('main').error(
            f'Program exited with exception: {e}')

    if node:
        node.destroy_node()
    if rclpy.ok():
        rclpy.shutdown()


if __name__ == '__main__':
    main()

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 digram, other transitions would 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

#!/usr/bin/env python3

import sys
import rclpy
import rclpy.logging
from rclpy.node import Node

from aimdk_msgs.srv import SetMcAction
from aimdk_msgs.msg import RequestHeader, CommonState, McAction, McActionCommand


class SetMcActionClient(Node):
    def __init__(self):
        super().__init__('set_mc_action_client')
        self.client = self.create_client(
            SetMcAction, '/aimdk_5Fmsgs/srv/SetMcAction'
        )
        self.get_logger().info('✅ SetMcAction client node created.')

        # Wait for the service to become available
        while not self.client.wait_for_service(timeout_sec=2.0):
            self.get_logger().info('⏳ Service unavailable, waiting...')

        self.get_logger().info('🟢 Service available, ready to send request.')

    def send_request(self, action_name: str):
        req = SetMcAction.Request()
        req.header = RequestHeader()
        req.source = 'node'
        #
        cmd = McActionCommand()
        cmd.action_desc = action_name
        req.command = cmd

        self.get_logger().info(
            f'📨 Sending request to set robot mode: {action_name}')
        for i in range(8):
            req.header.stamp = self.get_clock().now().to_msg()
            future = self.client.call_async(req)
            rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)

            if future.done():
                break

            # retry as remote peer is NOT handled well by ROS
            self.get_logger().info(f'trying ... [{i}]')

        response = future.result()
        if response is None:
            self.get_logger().error('❌ Service call failed or timed out.')
            return

        if response.response.status.value == CommonState.SUCCESS:
            self.get_logger().info('✅ Robot mode set successfully.')
        else:
            self.get_logger().error(
                f'❌ Failed to set robot mode: {response.response.message}'
            )


def main(args=None):
    action_info = {
        'PASSIVE_DEFAULT': ('PD', 'joints with zero torque'),
        'DAMPING_DEFAULT': ('DD', 'joints in damping mode'),
        'JOINT_DEFAULT': ('JD', 'Position Control Stand (joints locked)'),
        'STAND_DEFAULT': ('SD', 'Stable Stand (auto-balance)'),
        'LOCOMOTION_DEFAULT': ('LD', 'locomotion mode (walk or run)'),
    }

    choices = {}
    for k, v in action_info.items():
        choices[v[0]] = k

    rclpy.init(args=args)
    node = None
    try:
        # Prefer command-line argument, otherwise prompt for input
        if len(sys.argv) > 1:
            motion = sys.argv[1]
        else:
            print('{:<4} - {:<20} : {}'.format('abbr',
                  'robot mode', 'description'))
            for k, v in action_info.items():
                print(f'{v[0]:<4} - {k:<20} : {v[1]}')
            motion = input('Enter abbr of robot mode:')

        action_name = choices.get(motion)
        if not action_name:
            raise ValueError(f'Invalid abbr of robot mode: {motion}')

        node = SetMcActionClient()
        node.send_request(action_name)
    except KeyboardInterrupt:
        pass
    except Exception as e:
        rclpy.logging.get_logger('main').error(
            f'Program exited with exception: {e}')

    if node:
        node.destroy_node()
    if rclpy.ok():
        rclpy.shutdown()


if __name__ == '__main__':
    main()

Usage

# 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

Example output

...
[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_DEFAULT mode

• Mode switching may take several seconds to complete

Interface reference

• Service: /aimdk_5Fmsgs/srv/SetMcAction

• Message: aimdk_msgs/srv/SetMcAction

6.1.4 Set robot action

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

#!/usr/bin/env python3

import rclpy
import rclpy.logging
from rclpy.node import Node

from aimdk_msgs.srv import SetMcPresetMotion
from aimdk_msgs.msg import McPresetMotion, McControlArea, RequestHeader, CommonState


class SetMcPresetMotionClient(Node):
    def __init__(self):
        super().__init__('preset_motion_client')
        self.client = self.create_client(
            SetMcPresetMotion, '/aimdk_5Fmsgs/srv/SetMcPresetMotion')
        self.get_logger().info('✅ SetMcPresetMotion client node created.')

        # Wait for the service to become available
        while not self.client.wait_for_service(timeout_sec=2.0):
            self.get_logger().info('⏳ Service unavailable, waiting...')

        self.get_logger().info('🟢 Service available, ready to send request.')

    def send_request(self, area_id: int, motion_id: int) -> bool:
        request = SetMcPresetMotion.Request()
        request.header = RequestHeader()

        motion = McPresetMotion()
        area = McControlArea()

        motion.value = motion_id
        area.value = area_id

        request.motion = motion
        request.area = area
        request.interrupt = False

        self.get_logger().info(
            f'📨 Sending request to set preset motion: motion={motion_id}, area={area_id}')

        for i in range(8):
            request.header.stamp = self.get_clock().now().to_msg()
            future = self.client.call_async(request)
            rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)

            if future.done():
                break

            # retry as remote peer is NOT handled well by ROS
            self.get_logger().info(f'trying ... [{i}]')

        response = future.result()
        if response is None:
            self.get_logger().error('❌ Service call failed or timed out.')
            return False

        if response.response.header.code == 0:
            self.get_logger().info(
                f'✅ Preset motion set successfully: {response.response.task_id}')
            return True
        elif response.response.state.value == CommonState.RUNNING:
            self.get_logger().info(
                f'⏳ Preset motion executing: {response.response.task_id}')
            return True
        else:
            self.get_logger().error(
                f'❌ Failed to set preset motion: {response.response.task_id}'
            )
            return False


def main(args=None):
    rclpy.init(args=args)
    node = None
    try:
        area = int(input("Enter arm area ID (1-left, 2-right): "))
        motion = int(input(
            "Enter preset motion ID (1001-raise, 1002-wave, 1003-handshake, 1004-airkiss): "))

        node = SetMcPresetMotionClient()
        node.send_request(area, motion)
    except KeyboardInterrupt:
        pass
    except Exception as e:
        rclpy.logging.get_logger('main').error(
            f'Program exited with exception: {e}')

    if node:
        node.destroy_node()
    if rclpy.ok():
        rclpy.shutdown()


if __name__ == '__main__':
    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 robot control unit (PC1) by running aima em stop-app mc to obtain control of the robot. Ensure the robot is safe before operating.

import rclpy
from rclpy.node import Node
from aimdk_msgs.msg import HandCommandArray, HandCommand, HandType, MessageHeader
import time


class HandControl(Node):
    def __init__(self):
        super().__init__('hand_control')

        # Preset parameter list: [(left hand, right hand), ...]
        self.position_pairs = [
            (1.0, 1.0),   # fully open
            (0.0, 0.0),   # fully closed
            (0.5, 0.5),   # half open
            (0.2, 0.8),   # left slightly closed, right more open
            (0.7, 0.3)    # left more open, right slightly closed
        ]
        self.current_index = 0
        self.last_switch_time = self.get_clock().now().nanoseconds / 1e9  # seconds

        # Create publisher
        self.publisher_ = self.create_publisher(
            HandCommandArray,
            '/aima/hal/joint/hand/command',
            10
        )

        # 50 Hz timer
        self.timer_ = self.create_timer(
            0.02,  # 20 ms = 50 Hz
            self.publish_hand_commands
        )

        self.get_logger().info("Hand control node started!")

    def publish_hand_commands(self):
        # Check time to decide whether to switch to the next preset
        now_sec = self.get_clock().now().nanoseconds / 1e9
        if now_sec - self.last_switch_time >= 2.0:
            self.current_index = (self.current_index +
                                  1) % len(self.position_pairs)
            self.last_switch_time = now_sec
            self.get_logger().info(
                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}"
            )

        # Use current preset
        left_position, right_position = self.position_pairs[self.current_index]

        msg = HandCommandArray()
        msg.header = MessageHeader()

        # Configure left hand
        left_hand = HandCommand()
        left_hand.name = "left_hand"
        left_hand.position = float(left_position)
        left_hand.velocity = 1.0
        left_hand.acceleration = 1.0
        left_hand.deceleration = 1.0
        left_hand.effort = 1.0

        # Configure right hand
        right_hand = HandCommand()
        right_hand.name = "right_hand"
        right_hand.position = float(right_position)
        right_hand.velocity = 1.0
        right_hand.acceleration = 1.0
        right_hand.deceleration = 1.0
        right_hand.effort = 1.0

        msg.left_hand_type = HandType(value=2)  # gripper mode
        msg.right_hand_type = HandType(value=2)
        msg.left_hands = [left_hand]
        msg.right_hands = [right_hand]

        # Publish message
        self.publisher_.publish(msg)
        # We only log when switching presets to avoid too much log output


def main(args=None):
    rclpy.init(args=args)
    hand_control_node = HandControl()

    try:
        rclpy.spin(hand_control_node)
    except KeyboardInterrupt:
        pass
    finally:
        hand_control_node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    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 robot control unit (PC1) by running aima em stop-app mc to obtain control of the robot. Ensure the robot is safe before operating.

import rclpy
from rclpy.node import Node
from aimdk_msgs.msg import HandCommandArray, HandCommand, HandType, MessageHeader
import time


class HandControl(Node):
    def __init__(self):
        super().__init__('hand_control')

        # Create publisher
        self.publisher_ = self.create_publisher(
            HandCommandArray,
            '/aima/hal/joint/hand/command',
            10
        )

        self.timer_ = self.create_timer(
            0.8,
            self.publish_hand_commands
        )

        # Initialize variables
        self.target_finger = 0
        self.step = 1
        self.increasing = True
        self.get_logger().info("Hand control node started!")

    def build_hand_cmd(self, name: str) -> HandCommand:
        cmd = HandCommand()
        cmd.name = name
        cmd.position = 0.0
        cmd.velocity = 0.1
        cmd.acceleration = 0.0
        cmd.deceleration = 0.0
        cmd.effort = 0.0
        return cmd

    def publish_hand_commands(self):
        msg = HandCommandArray()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = 'hand_command'
        msg.left_hand_type.value = 1      # NIMBLE_HANDS
        msg.right_hand_type.value = 1     # NIMBLE_HANDS

        # left hand
        msg.left_hands = [self.build_hand_cmd('') for _ in range(10)]
        msg.left_hands[0].name = 'left_thumb'
        for i in range(1, 10):
            msg.left_hands[i].name = 'left_index'

        # right hand
        msg.right_hands = [self.build_hand_cmd('') for _ in range(10)]
        msg.right_hands[0].name = 'right_thumb'
        for i in range(1, 10):
            msg.right_hands[i].name = 'right_pinky'

        if self.target_finger < 10:
            msg.right_hands[self.target_finger].position = 0.8
        else:
            target_finger_ = self.target_finger - 10
            target_position = 0.8
            if target_finger_ < 3:
                # The three thumb motors on the left hand need their signs inverted to mirror the right hand's motion
                target_position = -target_position
            msg.left_hands[target_finger_].position = target_position

        self.publisher_.publish(msg)
        self.get_logger().info(
            f'Published hand command with target_finger: {self.target_finger}')
        self.update_target_finger()

    def update_target_finger(self):
        if self.increasing:
            self.target_finger += self.step
            if self.target_finger >= 19:
                self.target_finger = 19
                self.increasing = False
        else:
            self.target_finger -= self.step
            if self.target_finger <= 0:
                self.target_finger = 0
                self.increasing = True


def main(args=None):
    rclpy.init(args=args)
    hand_control_node = HandControl()

    try:
        rclpy.spin(hand_control_node)
    except KeyboardInterrupt:
        pass
    finally:
        hand_control_node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()

6.1.7 Omnihand touch sensors (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 robot control unit (PC1) by running aima em stop-app mc to obtain control of the robot. Ensure the robot is safe before operating.

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy
from aimdk_msgs.msg import HandStateArray, HandCommandArray
import time


class HandStateSubscriber(Node):
    def __init__(self):
        super().__init__('hand_state_subscriber')

        # Create publisher
        self.publisher_ = self.create_publisher(
            HandCommandArray,
            '/aima/hal/joint/hand/command',
            10
        )

        # Create QoS profile with BEST_EFFORT reliability
        qos_profile = QoSProfile(depth=10)
        qos_profile.reliability = ReliabilityPolicy.BEST_EFFORT

        # Create subscriber
        self.subscription = self.create_subscription(
            HandStateArray,
            '/aima/hal/joint/hand/state',
            self.topic_callback,
            qos_profile
        )

        # Create a timer to publish once per second
        self.timer = self.create_timer(1.0, self.publish_command)

        self.get_logger().info(
            "Subscriber started, listening to /aima/hal/joint/hand/state topic..."
        )

    def topic_callback(self, msg):
        """Callback function for handling incoming messages"""
        # Print message header information
        self.get_logger().info(
            f"Message received - Sequence: {msg.header.sequence}, "
            f"Timestamp: {msg.header.stamp.sec}.{msg.header.stamp.nanosec:09d}"
        )

        # Print left hand touch sensor data
        self.print_touch_sensor_data("Left Hand", msg.left_touch_sensors)

        # Print right hand touch sensor data
        self.print_touch_sensor_data("Right Hand", msg.right_touch_sensors)

        print()

    def print_touch_sensor_data(self, hand_name, sensor_data):
        """
        Print touch sensor data for a specific hand

        Args:
            hand_name: Name of the hand (Left Hand/Right Hand)
            sensor_data: Touch sensor data structure
        """
        print(f"=== {hand_name} Touch Sensor Data ===")

        # Print palm touch data
        print("Palm Touch Data (36 elements): ", end="")
        self.print_array(sensor_data.palm_touch_data)

        # Print back of hand touch data
        print("Back of Hand Touch Data (36 elements): ", end="")
        self.print_array(sensor_data.back_of_hand_touch_data)

        # Print finger touch data
        print("Thumb Touch Data (16 elements): ", end="")
        self.print_array(sensor_data.thumb_touch_data)

        print("Index Finger Touch Data (16 elements): ", end="")
        self.print_array(sensor_data.index_finger_touch_data)

        print("Middle Finger Touch Data (16 elements): ", end="")
        self.print_array(sensor_data.middle_finger_touch_data)

        print("Ring Finger Touch Data (16 elements): ", end="")
        self.print_array(sensor_data.ring_finger_touch_data)

        print("Little Finger Touch Data (16 elements): ", end="")
        self.print_array(sensor_data.little_finger_touch_data)

    def print_array(self, arr):
        """
        Print array of uint8_t elements

        Args:
            arr: Array to print (list or tuple)
        """
        print("[", end="")
        for i, val in enumerate(arr):
            print(f"{val:3d}", end="")
            if i < len(arr) - 1:
                print(" ", end="")
        print("]")

    def publish_command(self):
        """Publish hand command message"""
        message = HandCommandArray()

        # Set header
        message.header.stamp = self.get_clock().now().to_msg()
        message.header.frame_id = "hand_command"

        # Set the hand type
        message.left_hand_type.value = 1  # NIMBLE_HANDS
        message.right_hand_type.value = 1  # NIMBLE_HANDS

        # Create left hand command array
        from aimdk_msgs.msg import HandCommand
        message.left_hands = []

        # Set left thumb
        left_thumb = HandCommand()
        left_thumb.name = "left_thumb"
        left_thumb.position = 0.0
        left_thumb.velocity = 0.1
        left_thumb.acceleration = 0.0
        left_thumb.deceleration = 0.0
        left_thumb.effort = 0.0
        message.left_hands.append(left_thumb)

        # Set other left fingers
        for i in range(1, 10):
            finger = HandCommand()
            finger.name = "left_index"
            finger.position = 0.0
            finger.velocity = 0.1
            finger.acceleration = 0.0
            finger.deceleration = 0.0
            finger.effort = 0.0
            message.left_hands.append(finger)

        # Create right hand command array
        message.right_hands = []

        # Set right thumb
        right_thumb = HandCommand()
        right_thumb.name = "right_thumb"
        right_thumb.position = 0.0
        right_thumb.velocity = 0.1
        right_thumb.acceleration = 0.0
        right_thumb.deceleration = 0.0
        right_thumb.effort = 0.0
        message.right_hands.append(right_thumb)

        # Set other right fingers (pinky)
        for i in range(1, 10):
            finger = HandCommand()
            finger.name = "right_pinky"
            finger.position = 0.0
            finger.velocity = 0.1
            finger.acceleration = 0.0
            finger.deceleration = 0.0
            finger.effort = 0.0
            message.right_hands.append(finger)

        # Publish the message
        self.publisher_.publish(message)

        self.get_logger().info("Published hand command")


def main(args=None):
    rclpy.init(args=args)
    node = HandStateSubscriber()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    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 custom 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.

#!/usr/bin/env python3

import rclpy
import rclpy.logging
from rclpy.node import Node

from aimdk_msgs.srv import SetMcInputSource
from aimdk_msgs.msg import RequestHeader, McInputAction


class McInputClient(Node):
    def __init__(self):
        super().__init__('set_mc_input_source_client')
        self.client = self.create_client(
            SetMcInputSource, '/aimdk_5Fmsgs/srv/SetMcInputSource'
        )

        self.get_logger().info('✅ SetMcInputSource client node created.')

        # Wait for the service to become available
        while not self.client.wait_for_service(timeout_sec=2.0):
            self.get_logger().info('⏳ Service unavailable, waiting...')

        self.get_logger().info('🟢 Service available, ready to send request.')

    def send_request(self):
        req = SetMcInputSource.Request()

        # header
        req.request.header = RequestHeader()

        # action (e.g. 1001 = register)
        req.action = McInputAction()
        req.action.value = 1001

        # input source info
        req.input_source.name = 'node'
        req.input_source.priority = 40
        req.input_source.timeout = 1000  # ms

        # Send request and wait for response
        self.get_logger().info(
            f'📨 Sending input source request: action_id={req.action.value}, '
            f'name={req.input_source.name}, priority={req.input_source.priority}'
        )
        for i in range(8):
            req.request.header.stamp = self.get_clock().now().to_msg()
            future = self.client.call_async(req)
            rclpy.spin_until_future_complete(
                self, future, timeout_sec=0.25)

            if future.done():
                break

            # retry as remote peer is NOT handled well by ROS
            self.get_logger().info(f'trying ... [{i}]')

        if not future.done():
            self.get_logger().error('❌ Service call failed or timed out.')
            return False

        response = future.result()
        ret_code = response.response.header.code
        task_id = response.response.task_id

        if ret_code == 0:
            self.get_logger().info(
                f'✅ Input source set successfully. task_id={task_id}'
            )
            return True
        else:
            self.get_logger().error(
                f'❌ Input source set failed. ret_code={ret_code}, task_id={task_id} (duplicated ADD? or MODIFY/ENABLE/DISABLE for unknown source?)'
            )
            return False


def main(args=None):
    rclpy.init(args=args)

    node = None
    try:
        node = McInputClient()
        ok = node.send_request()
        if not ok:
            node.get_logger().error('Input source request failed.')
    except KeyboardInterrupt:
        pass
    except Exception as e:
        rclpy.logging.get_logger('main').error(
            f'Program exited with exception: {e}')

    if node:
        node.destroy_node()
    if rclpy.ok():
        rclpy.shutdown()


if __name__ == '__main__':
    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.

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from aimdk_msgs.srv import GetCurrentInputSource
from aimdk_msgs.msg import CommonRequest


class GetCurrentInputSourceClient(Node):
    def __init__(self):
        super().__init__('get_current_input_source_client')
        self.client = self.create_client(
            GetCurrentInputSource,
            '/aimdk_5Fmsgs/srv/GetCurrentInputSource'
        )

        self.get_logger().info('✅ GetCurrentInputSource client node created.')

        # Wait for the service to become available
        while not self.client.wait_for_service(timeout_sec=2.0):
            self.get_logger().info('⏳ Service unavailable, waiting...')

        self.get_logger().info('🟢 Service available, ready to send request.')

    def send_request(self):
        # Create request
        req = GetCurrentInputSource.Request()
        req.request = CommonRequest()

        # Send request and wait for response
        self.get_logger().info('📨 Sending request to get current input source')
        for i in range(8):
            req.request.header.stamp = self.get_clock().now().to_msg()
            future = self.client.call_async(req)
            rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)

            if future.done():
                break

            # retry as remote peer is NOT handled well by ROS
            self.get_logger().info(f'trying ... [{i}]')

        if not future.done():
            self.get_logger().error('❌ Service call failed or timed out.')
            return False

        response = future.result()
        ret_code = response.response.header.code
        if ret_code == 0:
            self.get_logger().info(
                f'✅ Current input source get successfully:')
            self.get_logger().info(
                f'Name: {response.input_source.name}')
            self.get_logger().info(
                f'Priority: {response.input_source.priority}')
            self.get_logger().info(
                f'Timeout: {response.input_source.timeout}')
            return True
        else:
            self.get_logger().error(
                f'❌ Current input source get failed, return code: {ret_code}')
            return False


def main(args=None):
    rclpy.init(args=args)

    node = None
    try:
        node = GetCurrentInputSourceClient()
        success = node.send_request()
    except KeyboardInterrupt:
        pass
    except Exception as e:
        rclpy.logging.get_logger('main').error(
            f'Program exited with exception: {e}')

    if node:
        node.destroy_node()
    if rclpy.ok():
        rclpy.shutdown()


if __name__ == '__main__':
    main()

Usage

# Get current input source info
ros2 run py_examples get_current_input_source

Example output

[INFO] [get_current_input_source_client]: Current input source: node
[INFO] [get_current_input_source_client]: Priority: 40
[INFO] [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 the query succeeded

6.1.10 Control robot locomotion

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:

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
import time
import signal
import sys

from aimdk_msgs.msg import McLocomotionVelocity, MessageHeader
from aimdk_msgs.srv import SetMcInputSource


class DirectVelocityControl(Node):
    def __init__(self):
        super().__init__('direct_velocity_control')

        self.publisher = self.create_publisher(
            McLocomotionVelocity, '/aima/mc/locomotion/velocity', 10)
        self.client = self.create_client(
            SetMcInputSource, '/aimdk_5Fmsgs/srv/SetMcInputSource')

        self.forward_velocity = 0.0
        self.lateral_velocity = 0.0
        self.angular_velocity = 0.0

        self.max_forward_speed = 1.0
        self.min_forward_speed = 0.2

        self.max_lateral_speed = 1.0
        self.min_lateral_speed = 0.2

        self.max_angular_speed = 1.0
        self.min_angular_speed = 0.1

        self.timer = None

        self.get_logger().info("Direct velocity control node started!")

    def start_publish(self):
        if not self.timer:
            self.timer = self.create_timer(0.02, self.publish_velocity)

    def register_input_source(self):
        self.get_logger().info("Registering input source...")

        timeout_sec = 8.0
        start = self.get_clock().now().nanoseconds / 1e9

        while not self.client.wait_for_service(timeout_sec=2.0):
            now = self.get_clock().now().nanoseconds / 1e9
            if now - start > timeout_sec:
                self.get_logger().error("Waiting for service timed out")
                return False
            self.get_logger().info("Waiting for input source service...")

        req = SetMcInputSource.Request()
        req.action.value = 1001
        req.input_source.name = "node"
        req.input_source.priority = 40
        req.input_source.timeout = 1000

        for i in range(8):
            req.request.header.stamp = self.get_clock().now().to_msg()
            future = self.client.call_async(req)
            rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)

            if future.done():
                break

            # retry as remote peer is NOT handled well by ROS
            self.get_logger().info(f"trying to register input source... [{i}]")

        if future.done():
            try:
                response = future.result()
                state = response.response.state.value
                self.get_logger().info(
                    f"Input source set successfully: state={state}, task_id={response.response.task_id}")
                return True
            except Exception as e:
                self.get_logger().error(f"Service call exception: {str(e)}")
                return False
        else:
            self.get_logger().error("Service call failed or timed out")
            return False

    def publish_velocity(self):
        msg = McLocomotionVelocity()
        msg.header = MessageHeader()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.source = "node"
        msg.forward_velocity = self.forward_velocity
        msg.lateral_velocity = self.lateral_velocity
        msg.angular_velocity = self.angular_velocity

        self.publisher.publish(msg)

        self.get_logger().info(
            f"Publishing velocity: forward {self.forward_velocity:.2f} m/s, "
            f"lateral {self.lateral_velocity:.2f} m/s, "
            f"angular {self.angular_velocity:.2f} rad/s"
        )

    def set_forward(self, forward):
        # check value range, mc has thresholds to start movement
        if abs(forward) < 0.005:
            self.forward_velocity = 0.0
            return True
        elif abs(forward) > self.max_forward_speed or abs(forward) < self.min_forward_speed:
            raise ValueError("out of range")
        else:
            self.forward_velocity = forward
            return True

    def set_lateral(self, lateral):
        # check value range, mc has thresholds to start movement
        if abs(lateral) < 0.005:
            self.lateral_velocity = 0.0
            return True
        elif abs(lateral) > self.max_lateral_speed or abs(lateral) < self.min_lateral_speed:
            raise ValueError("out of range")
        else:
            self.lateral_velocity = lateral
            return True

    def set_angular(self, angular):
        # check value range, mc has thresholds to start movement
        if abs(angular) < 0.005:
            self.angular_velocity = 0.0
            return True
        elif abs(angular) > self.max_angular_speed or abs(angular) < self.min_angular_speed:
            raise ValueError("out of range")
        else:
            self.angular_velocity = angular
            return True

    def clear_velocity(self):
        self.forward_velocity = 0.0
        self.lateral_velocity = 0.0
        self.angular_velocity = 0.0


# Global node instance for signal handling
global_node = None


def signal_handler(sig, frame):
    global global_node
    if global_node is not None:
        global_node.clear_velocity()
        global_node.get_logger().info(
            f"Received signal {sig}, clearing velocity and shutting down")
    if rclpy.ok():
        rclpy.shutdown()
    sys.exit(0)


def main():
    global global_node
    rclpy.init()

    node = DirectVelocityControl()
    global_node = node

    signal.signal(signal.SIGINT, signal_handler)
    signal.signal(signal.SIGTERM, signal_handler)

    if not node.register_input_source():
        node.get_logger().error("Input source registration failed, exiting")
        if rclpy.ok():
            rclpy.shutdown()
        return

    # get and check control values
    # notice that mc has thresholds to start movement
    try:
        # get input forward
        forward = float(
            input("Please enter forward velocity 0 or ±(0.2 ~ 1.0) m/s: "))
        node.set_forward(forward)
        # get input lateral
        lateral = float(
            input("Please enter lateral velocity 0 or ±(0.2 ~ 1.0) m/s: "))
        node.set_lateral(lateral)
        # get input angular
        angular = float(
            input("Please enter angular velocity 0 or ±(0.1 ~ 1.0) rad/s: "))
        node.set_angular(angular)
    except Exception as e:
        node.get_logger().error(f"Invalid input: {e}")
        if rclpy.ok():
            rclpy.shutdown()
        return

    node.get_logger().info("Setting velocity, moving for 5 seconds")
    node.start_publish()

    start = node.get_clock().now()
    while (node.get_clock().now() - start).nanoseconds / 1e9 < 5.0:
        rclpy.spin_once(node, timeout_sec=0.1)
        time.sleep(0.001)

    node.clear_velocity()
    node.get_logger().info("5-second motion finished, robot stopped")

    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    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 robot control 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 low-level motors (the HAL layer). Before running, verify that the joint safety limits in the code match your robot 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:

1. Robot joint model definition

2. Trajectory interpolation using Ruckig

3. Multi-joint coordinated control

4. Real-time control of position, velocity, and acceleration

Example feature description

1. Creates four controller nodes, which control:

   • Legs x2 (12 joints)

   • Waist x1 (3 joints)

   • Arms x2 (14 joints)

   • Head x1 (2 joints)

2. Demo features:

   • Oscillate a specified joint between ±0.5 radians every 10 seconds

   • Use the Ruckig library to generate smooth motion trajectories

   • Publish joint control commands in real time

Custom usage

2. Add new control logic:

   • Add new control callback functions

   • Compose joint motions freely

3. Adjust control frequency:

   • Modify the control_timer_ period (currently 2 ms)

#!/usr/bin/env python3
"""
Robot joint control example
This script implements a ROS2-based robot joint control system, using the Ruckig trajectory
planner to achieve smooth joint motion control.

Main features:
1. Supports controlling multiple joint areas (head, arm, waist, leg)
2. Uses Ruckig for trajectory planning to ensure smooth motion
3. Supports real-time control of joint position, velocity, and acceleration
4. Provides joint limit and PID (stiffness/damping) parameter configuration
"""

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
from aimdk_msgs.msg import JointCommandArray, JointStateArray, JointCommand
from std_msgs.msg import Header
import ruckig
from enum import Enum
from dataclasses import dataclass
from typing import List, Dict
from threading import Lock

# QoS config: define ROS2 Quality of Service parameters
# Subscriber QoS: best-effort reliability, keep last 10 messages
subscriber_qos = QoSProfile(
    reliability=ReliabilityPolicy.BEST_EFFORT,
    history=HistoryPolicy.KEEP_LAST,
    depth=10,
    durability=DurabilityPolicy.VOLATILE
)

# Publisher QoS: reliable transport, keep last 10 messages
publisher_qos = QoSProfile(
    reliability=ReliabilityPolicy.RELIABLE,
    history=HistoryPolicy.KEEP_LAST,
    depth=10,
    durability=DurabilityPolicy.VOLATILE
)


class JointArea(Enum):
    HEAD = 'HEAD'    # Head joints
    ARM = 'ARM'      # Arm joints
    WAIST = 'WAIST'  # Waist joints
    LEG = 'LEG'      # Leg joints


@dataclass
class JointInfo:
    # Joint information data class
    name: str           # Joint name
    lower_limit: float  # Joint lower angle limit
    upper_limit: float  # Joint upper angle limit
    kp: float           # Position control proportional gain
    kd: float           # Velocity control derivative gain


# Robot model configuration: define all joint parameters
robot_model: Dict[JointArea, List[JointInfo]] = {
    # Leg joint configuration
    JointArea.LEG: [
        # Left leg joints
        JointInfo("left_hip_pitch_joint", -2.704, 2.556, 40.0, 4.0),
        JointInfo("left_hip_roll_joint", -0.235, 2.906, 40.0, 4.0),
        JointInfo("left_hip_yaw_joint", -1.684, 3.430, 30.0, 3.0),
        JointInfo("left_knee_joint", 0.0000, 2.4073, 80.0, 8.0),
        JointInfo("left_ankle_pitch_joint", -0.803, 0.453, 40.0, 4.0),
        JointInfo("left_ankle_roll_joint", -0.2625, 0.2625, 20.0, 2.0),
        # Right leg joints
        JointInfo("right_hip_pitch_joint", -2.704, 2.556, 40.0, 4.0),
        JointInfo("right_hip_roll_joint", -2.906, 0.235, 40.0, 4.0),
        JointInfo("right_hip_yaw_joint", -3.430, 1.684, 30.0, 3.0),
        JointInfo("right_knee_joint", 0.0000, 2.4073, 80.0, 8.0),
        JointInfo("right_ankle_pitch_joint", -0.803, 0.453, 40.0, 4.0),
        JointInfo("right_ankle_roll_joint", -0.2625, 0.2625, 20.0, 2.0),
    ],
    # Waist joint configuration
    JointArea.WAIST: [
        JointInfo("waist_yaw_joint", -3.43, 2.382, 20.0, 4.0),
        JointInfo("waist_pitch_joint", -0.314, 0.314, 20.0, 4.0),
        JointInfo("waist_roll_joint", -0.488, 0.488, 20.0, 4.0),
    ],
    # Arm joint configuration
    JointArea.ARM: [
        # Left arm
        JointInfo("left_shoulder_pitch_joint", -3.08, 2.04, 20.0, 2.0),
        JointInfo("left_shoulder_roll_joint", -0.061, 2.993, 20.0, 2.0),
        JointInfo("left_shoulder_yaw_joint", -2.556, 2.556, 20.0, 2.0),
        JointInfo("left_elbow_joint", -2.3556, 0.0, 20.0, 2.0),
        JointInfo("left_wrist_yaw_joint", -2.556, 2.556, 20.0, 2.0),
        JointInfo("left_wrist_pitch_joint", -0.558, 0.558, 20.0, 2.0),
        JointInfo("left_wrist_roll_joint", -1.571, 0.724, 20.0, 2.0),
        # Right arm
        JointInfo("right_shoulder_pitch_joint", -3.08, 2.04, 20.0, 2.0),
        JointInfo("right_shoulder_roll_joint", -2.993, 0.061, 20.0, 2.0),
        JointInfo("right_shoulder_yaw_joint", -2.556, 2.556, 20.0, 2.0),
        JointInfo("right_elbow_joint", -2.3556, 0.0000, 20.0, 2.0),
        JointInfo("right_wrist_yaw_joint", -2.556, 2.556, 20.0, 2.0),
        JointInfo("right_wrist_pitch_joint", -0.558, 0.558, 20.0, 2.0),
        JointInfo("right_wrist_roll_joint", -0.724, 1.571, 20.0, 2.0),
    ],
    # Head joint configuration
    JointArea.HEAD: [
        JointInfo("head_yaw_joint", -0.366, 0.366, 20.0, 2.0),
        JointInfo("head_pitch_joint", -0.3838, 0.3838, 20.0, 2.0),
    ],
}


class JointControllerNode(Node):
    """
    Joint controller node
    Responsible for receiving joint states, using Ruckig for trajectory planning,
    and publishing joint commands.
    """

    def __init__(self, node_name: str, sub_topic: str, pub_topic: str, area: JointArea, dofs: int):
        """
        Initialize joint controller
        Args:
            node_name: node name
            sub_topic: topic name to subscribe (joint states)
            pub_topic: topic name to publish (joint commands)
            area: joint area (head/arm/waist/leg)
            dofs: number of DOFs
        """
        super().__init__(node_name)
        self.lock = Lock()
        self.joint_info = robot_model[area]
        self.dofs = dofs
        self.ruckig = ruckig.Ruckig(dofs, 0.002)  # 2 ms control period
        self.input = ruckig.InputParameter(dofs)
        self.output = ruckig.OutputParameter(dofs)
        self.ruckig_initialized = False

        # Initialize trajectory parameters
        self.input.current_position = [0.0] * dofs
        self.input.current_velocity = [0.0] * dofs
        self.input.current_acceleration = [0.0] * dofs

        # Motion limits
        self.input.max_velocity = [1.0] * dofs
        self.input.max_acceleration = [1.0] * dofs
        self.input.max_jerk = [25.0] * dofs

        # ROS2 subscriber and publisher
        self.sub = self.create_subscription(
            JointStateArray,
            sub_topic,
            self.joint_state_callback,
            subscriber_qos
        )
        self.pub = self.create_publisher(
            JointCommandArray,
            pub_topic,
            publisher_qos
        )

    def joint_state_callback(self, msg: JointStateArray):
        """
        Joint state callback
        Receives and processes joint state messages
        """
        self.ruckig_initialized = True

    def control_callback(self, joint_idx):
        """
        Control callback
        Uses Ruckig for trajectory planning and publishes control commands
        Args:
            joint_idx: target joint index
        """
        # Run Ruckig until the target is reached
        while self.ruckig.update(self.input, self.output) in [ruckig.Result.Working, ruckig.Result.Finished]:
            # Update current state
            self.input.current_position = self.output.new_position
            self.input.current_velocity = self.output.new_velocity
            self.input.current_acceleration = self.output.new_acceleration

            # Check if target is reached
            tolerance = 1e-6
            current_p = self.output.new_position[joint_idx]
            if abs(current_p - self.input.target_position[joint_idx]) < tolerance:
                break

            # Create and publish command
            cmd = JointCommandArray()
            for i, joint in enumerate(self.joint_info):
                j = JointCommand()
                j.name = joint.name
                j.position = self.output.new_position[i]
                j.velocity = self.output.new_velocity[i]
                j.effort = 0.0
                j.stiffness = joint.kp
                j.damping = joint.kd
                cmd.joints.append(j)

            self.pub.publish(cmd)

    def set_target_position(self, joint_name, position):
        """
        Set target joint position
        Args:
            joint_name: joint name
            position: target position
        """
        p_s = [0.0] * self.dofs
        joint_idx = 0
        for i, joint in enumerate(self.joint_info):
            if joint.name == joint_name:
                p_s[i] = position
                joint_idx = i
        self.input.target_position = p_s
        self.input.target_velocity = [0.0] * self.dofs
        self.input.target_acceleration = [0.0] * self.dofs
        self.control_callback(joint_idx)


def main(args=None):
    """
    Main function
    Initialize ROS2 node and start joint controller
    """
    rclpy.init(args=args)

    # Create leg controller node
    leg_node = JointControllerNode(
        "leg_node",
        "/aima/hal/joint/leg/state",
        "/aima/hal/joint/leg/command",
        JointArea.LEG,
        12
    )

    # waist_node = JointControllerNode(
    #     "waist_node",
    #     "/aima/hal/joint/waist/state",
    #     "/aima/hal/joint/waist/command",
    #     JointArea.WAIST,
    #     3
    # )

    # arm_node = JointControllerNode(
    #     "arm_node",
    #     "/aima/hal/joint/arm/state",
    #     "/aima/hal/joint/arm/command",
    #     JointArea.ARM,
    #     14
    # )

    # head_node = JointControllerNode(
    #     "head_node",
    #     "/aima/hal/joint/head/state",
    #     "/aima/hal/joint/head/command",
    #     JointArea.HEAD,
    #     2
    # )

    position = 0.8

    # Only control the left leg joint. If you want to control a specific joint, assign it directly.
    def timer_callback():
        """
        Timer callback
        Periodically change target position to achieve oscillating motion
        """
        nonlocal position
        position = -position
        position = 1.3 + position
        leg_node.set_target_position("left_knee_joint", position)

    #     arm_node.set_target_position("left_shoulder_pitch_joint", position)
    #     waist_node.set_target_position("waist_yaw_joint", position)
    #     head_node.set_target_position("head_pitch_joint", position)

    leg_node.create_timer(3.0, timer_callback)

    # Multi-threaded executor
    executor = rclpy.executors.MultiThreadedExecutor()
    executor.add_node(leg_node)

    # executor.add_node(waist_node)
    # executor.add_node(arm_node)
    # executor.add_node(head_node)

    try:
        executor.spin()
    except KeyboardInterrupt:
        pass
    finally:
        leg_node.destroy_node()
        # waist_node.destroy_node()
        # arm_node.destroy_node()
        # head_node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()

6.1.12 Keyboard 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).

#!/usr/bin/env python3

import rclpy
import rclpy.logging
from rclpy.node import Node
from aimdk_msgs.msg import McLocomotionVelocity, MessageHeader
from aimdk_msgs.srv import SetMcInputSource
import curses
import time


class KeyboardVelocityController(Node):
    def __init__(self, stdscr):
        super().__init__('keyboard_velocity_controller')
        self.stdscr = stdscr
        self.forward_velocity = 0.0
        self.lateral_velocity = 0.0
        self.angular_velocity = 0.0
        self.step = 0.2
        self.angular_step = 0.1

        self.publisher = self.create_publisher(
            McLocomotionVelocity, '/aima/mc/locomotion/velocity', 10)
        self.client = self.create_client(
            SetMcInputSource, '/aimdk_5Fmsgs/srv/SetMcInputSource')

        if not self.register_input_source():
            self.get_logger().error("Input source registration failed, exiting")
            raise RuntimeError("Failed to register input source")

        # Configure curses
        curses.cbreak()
        curses.noecho()
        self.stdscr.keypad(True)
        self.stdscr.nodelay(True)

        self.get_logger().info(
            "Control started: W/S forward/backward, A/D strafe, Q/E turn, Space stop, Esc exit")

        # Timer: check keyboard every 50 ms
        self.timer = self.create_timer(0.05, self.check_key_and_publish)

    def register_input_source(self):
        self.get_logger().info("Registering input source...")

        timeout_sec = 8.0
        start = self.get_clock().now().nanoseconds / 1e9

        while not self.client.wait_for_service(timeout_sec=2.0):
            now = self.get_clock().now().nanoseconds / 1e9
            if now - start > timeout_sec:
                self.get_logger().error("Waiting for service timed out")
                return False
            self.get_logger().info("Waiting for input source service...")

        req = SetMcInputSource.Request()
        req.action.value = 1001
        req.input_source.name = "node"
        req.input_source.priority = 40
        req.input_source.timeout = 1000

        for i in range(8):
            req.request.header.stamp = self.get_clock().now().to_msg()
            future = self.client.call_async(req)
            rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)

            if future.done():
                break

            # retry as remote peer is NOT handled well by ROS
            self.get_logger().info(f"trying to register input source... [{i}]")

        if future.done():
            try:
                resp = future.result()
                state = resp.response.state.value
                self.get_logger().info(
                    f"Input source set successfully: state={state}, task_id={resp.response.task_id}")
                return True
            except Exception as e:
                self.get_logger().error(f"Service exception: {str(e)}")
                return False
        else:
            self.get_logger().error("Service call failed or timed out")
            return False

    def check_key_and_publish(self):
        try:
            ch = self.stdscr.getch()
        except Exception:
            ch = -1

        if ch != -1:
            if ch == ord(' '):
                self.forward_velocity = 0.0
                self.lateral_velocity = 0.0
                self.angular_velocity = 0.0
            elif ch == ord('w'):
                self.forward_velocity = min(
                    self.forward_velocity + self.step, 1.0)
            elif ch == ord('s'):
                self.forward_velocity = max(
                    self.forward_velocity - self.step, -1.0)
            elif ch == ord('a'):
                self.lateral_velocity = min(
                    self.lateral_velocity + self.step, 1.0)
            elif ch == ord('d'):
                self.lateral_velocity = max(
                    self.lateral_velocity - self.step, -1.0)
            elif ch == ord('q'):
                self.angular_velocity = min(
                    self.angular_velocity + self.angular_step, 1.0)
            elif ch == ord('e'):
                self.angular_velocity = max(
                    self.angular_velocity - self.angular_step, -1.0)
            elif ch == 27:  # ESC
                self.get_logger().info("Exiting control")
                self.timer.cancel()
                return

        msg = McLocomotionVelocity()
        msg.header = MessageHeader()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.source = "node"
        msg.forward_velocity = self.forward_velocity
        msg.lateral_velocity = self.lateral_velocity
        msg.angular_velocity = self.angular_velocity

        self.publisher.publish(msg)

        # Update UI
        self.stdscr.clear()
        self.stdscr.addstr(
            0, 0, "W/S: Forward/Backward | A/D: Strafe | Q/E: Turn | Space: Stop | ESC: Exit")
        self.stdscr.addstr(2, 0,
                           f"Speed Status: Forward: {self.forward_velocity:.2f} m/s | "
                           f"Lateral: {self.lateral_velocity:.2f} m/s | "
                           f"Angular: {self.angular_velocity:.2f} rad/s")
        self.stdscr.refresh()


def curses_main(stdscr):
    rclpy.init()
    try:
        node = KeyboardVelocityController(stdscr)
        while not node.timer.is_canceled():
            rclpy.spin_once(node, timeout_sec=0.1)
    except (Exception, KeyboardInterrupt) as e:
        rclpy.logging.get_logger("main").fatal(
            f"Program exited with exception: {e}")
    finally:
        curses.endwin()
        if rclpy.ok():
            rclpy.shutdown()


def main():
    curses.wrapper(curses_main)


if __name__ == '__main__':
    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.

#!/usr/bin/env python3
import threading
import time
from pathlib import Path

import rclpy
import rclpy.task
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2


class SaveOneRawPy(Node):
    def __init__(self):
        super().__init__('save_one_image')

        # parameter: image topic
        self.declare_parameter(
            'image_topic', '/aima/hal/sensor/stereo_head_front_left/rgb_image'
        )
        self.topic = self.get_parameter(
            'image_topic').get_parameter_value().string_value

        # save directory
        self.save_dir = Path('images').resolve()
        self.save_dir.mkdir(parents=True, exist_ok=True)

        self._bridge = CvBridge()

        self._future = rclpy.task.Future()
        self._lock = threading.Lock()

        # subscriber (sensor QoS)
        self.sub = self.create_subscription(
            Image,
            self.topic,
            self.image_cb,
            qos_profile_sensor_data
        )
        self.get_logger().info(f'Subscribing to raw image: {self.topic}')
        self.get_logger().info(f'Images will be saved to: {self.save_dir}')

    def image_cb(self, msg: Image):
        with self._lock:
            if self._future.done():
                return

        try:
            enc = msg.encoding.lower()
            self.get_logger().info(f'Received image with encoding: {enc}')

            img = self._bridge.imgmsg_to_cv2(
                msg, desired_encoding='passthrough')

            if enc == 'rgb8':
                img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
            elif enc == 'mono8':
                img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)

            ts_ms = int(time.time() * 1000)
            out_path = self.save_dir / f'frame_{ts_ms}.png'

            ok = cv2.imwrite(str(out_path), img)
            if ok:
                self.get_logger().info(
                    f'Saved image: {out_path}  ({img.shape[1]}x{img.shape[0]})'
                )
                with self._lock:
                    if not self._future.done():
                        self._future.set_result(str(out_path))
            else:
                self.get_logger().error(f'cv2.imwrite failed: {out_path}')
        except Exception as e:
            self.get_logger().error(f'Failed to decode / save image: {e}')
            with self._lock:
                if not self._future.done():
                    self._future.set_exception(e)


def main():
    rclpy.init()
    node = SaveOneRawPy()

    try:
        rclpy.spin_until_future_complete(node, node._future)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()

6.1.14 Camera stream examples

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 image video recording(TBD, see C++ examples)

• 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)

#!/usr/bin/env python3
"""
Head depth camera multi-topic subscription example

Supports selecting the topic type to subscribe via startup parameter --ros-args -p topic_type:=<type>:
  - depth_image: Depth image (sensor_msgs/Image)
  - depth_camera_info: Camera intrinsic parameters (sensor_msgs/CameraInfo)
  - rgb_image: RGB image (sensor_msgs/Image)
  - rgb_image_compressed: RGB compressed image (sensor_msgs/CompressedImage)
  - rgb_camera_info: Camera intrinsic parameters (sensor_msgs/CameraInfo)

Example:
  ros2 run py_examples echo_camera_rgbd --ros-args -p topic_type:=rgb_image
  ros2 run py_examples echo_camera_rgbd --ros-args -p topic_type:=rgb_camera_info

Default topic_type is rgb_image
"""

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
from sensor_msgs.msg import Image, CompressedImage, CameraInfo
from collections import deque
import time


class CameraTopicEcho(Node):
    def __init__(self):
        super().__init__('camera_topic_echo')

        # Select the topic type to subscribe
        self.declare_parameter('topic_type', 'rgb_image')
        self.declare_parameter('dump_video_path', '')

        self.topic_type = self.get_parameter('topic_type').value
        self.dump_video_path = self.get_parameter('dump_video_path').value

        # SensorDataQoS: BEST_EFFORT + VOLATILE
        qos = QoSProfile(
            reliability=QoSReliabilityPolicy.BEST_EFFORT,
            history=QoSHistoryPolicy.KEEP_LAST,
            depth=5
        )

        # Create different subscribers based on topic_type
        if self.topic_type == "depth_image":
            self.topic_name = "/aima/hal/sensor/rgbd_head_front/depth_image"
            self.sub_image = self.create_subscription(
                Image, self.topic_name, self.cb_image, qos)
            self.get_logger().info(
                f"✅ Subscribing Depth Image: {self.topic_name}")

        elif self.topic_type == "rgb_image":
            self.topic_name = "/aima/hal/sensor/rgbd_head_front/rgb_image"
            self.sub_image = self.create_subscription(
                Image, self.topic_name, self.cb_image, qos)
            self.get_logger().info(
                f"✅ Subscribing RGB Image: {self.topic_name}")
            if self.dump_video_path:
                self.get_logger().info(
                    f"📝 Will dump received images to video: {self.dump_video_path}")

        elif self.topic_type == "rgb_image_compressed":
            self.topic_name = "/aima/hal/sensor/rgbd_head_front/rgb_image/compressed"
            self.sub_compressed = self.create_subscription(
                CompressedImage, self.topic_name, self.cb_compressed, qos)
            self.get_logger().info(
                f"✅ Subscribing CompressedImage: {self.topic_name}")

        elif self.topic_type == "rgb_camera_info":
            self.topic_name = "/aima/hal/sensor/rgbd_head_front/rgb_camera_info"
            # RGB-D CameraInfo is different with other cameras. The best_effort + volatile QoS is enough for 10Hz rgb_camera_info
            self.sub_camerainfo = self.create_subscription(
                CameraInfo, self.topic_name, self.cb_camerainfo, qos)
            self.get_logger().info(
                f"✅ Subscribing RGB CameraInfo: {self.topic_name}")

        elif self.topic_type == "depth_camera_info":
            self.topic_name = "/aima/hal/sensor/rgbd_head_front/depth_camera_info"
            # RGB-D CameraInfo is different with other cameras. The best_effort + volatile QoS is enough for 10Hz depth_camera_info
            self.sub_camerainfo = self.create_subscription(
                CameraInfo, self.topic_name, self.cb_camerainfo, qos)
            self.get_logger().info(
                f"✅ Subscribing Depth CameraInfo: {self.topic_name}")

        else:
            self.get_logger().error(f"Unknown topic_type: {self.topic_type}")
            raise ValueError("Unknown topic_type")

        # Internal state
        self.last_print = self.get_clock().now()
        self.print_allowed = False
        self.arrivals = deque()

    def update_arrivals(self):
        """Calculate received FPS"""
        now = self.get_clock().now()
        self.arrivals.append(now)
        while self.arrivals and (now - self.arrivals[0]).nanoseconds * 1e-9 > 1.0:
            self.arrivals.popleft()

    def get_fps(self):
        """Get FPS"""
        return len(self.arrivals)

    def should_print(self, master=True):
        """Control print frequency"""
        if not master:
            return self.print_allowed
        now = self.get_clock().now()
        if (now - self.last_print).nanoseconds * 1e-9 >= 1.0:
            self.last_print = now
            self.print_allowed = True
        else:
            self.print_allowed = False
        return self.print_allowed

    def cb_image(self, msg: Image):
        """Image callback (Depth/RGB image)"""
        self.update_arrivals()

        if self.should_print():
            stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
            self.get_logger().info(
                f"📸 {self.topic_type} received\n"
                f"  • frame_id:        {msg.header.frame_id}\n"
                f"  • stamp (sec):     {stamp_sec:.6f}\n"
                f"  • encoding:        {msg.encoding}\n"
                f"  • size (WxH):      {msg.width} x {msg.height}\n"
                f"  • step (bytes/row):{msg.step}\n"
                f"  • is_bigendian:    {msg.is_bigendian}\n"
                f"  • recv FPS (1s):   {self.get_fps():.1f}"
            )

        # Only RGB image supports video dump
        if self.topic_type == "rgb_image" and self.dump_video_path:
            self.dump_image_to_video(msg)

    def cb_compressed(self, msg: CompressedImage):
        """CompressedImage callback"""
        self.update_arrivals()

        if self.should_print():
            stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
            self.get_logger().info(
                f"🗜️  CompressedImage received\n"
                f"  • frame_id:        {msg.header.frame_id}\n"
                f"  • stamp (sec):     {stamp_sec:.6f}\n"
                f"  • format:          {msg.format}\n"
                f"  • data size:       {len(msg.data)}\n"
                f"  • recv FPS (1s):   {self.get_fps():.1f}"
            )

    def cb_camerainfo(self, msg: CameraInfo):
        """CameraInfo callback (camera intrinsic parameters)"""
        # Camera info will only receive one frame, print it directly
        stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9

        # Format D array
        d_str = ", ".join([f"{d:.6f}" for d in msg.d])

        # Format K matrix
        k_str = ", ".join([f"{k:.6f}" for k in msg.k])

        # Format P matrix
        p_str = ", ".join([f"{p:.6f}" for p in msg.p])

        self.get_logger().info(
            f"📷 {self.topic_type} received\n"
            f"  • frame_id:        {msg.header.frame_id}\n"
            f"  • stamp (sec):     {stamp_sec:.6f}\n"
            f"  • width x height:  {msg.width} x {msg.height}\n"
            f"  • distortion_model:{msg.distortion_model}\n"
            f"  • D: [{d_str}]\n"
            f"  • K: [{k_str}]\n"
            f"  • P: [{p_str}]\n"
            f"  • binning_x: {msg.binning_x}\n"
            f"  • binning_y: {msg.binning_y}\n"
            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} }}"
        )

    def dump_image_to_video(self, msg: Image):
        """Video dump is only supported for RGB images"""
        # You can add video recording functionality here
        # Simplified in the Python version, only logs instead
        if self.should_print(master=False):
            self.get_logger().info(f"📝 Video dump not implemented in Python version")


def main(args=None):
    rclpy.init(args=args)
    try:
        node = CameraTopicEcho()
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    except Exception as e:
        print(f"Error: {e}")
    finally:
        if 'node' in locals():
            node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()

Usage:

1. Subscribe to RGB image data:

   ros2 run py_examples echo_camera_rgbd --ros-args -p topic_type:=rgb_image
   

2. Subscribe to camera intrinsics:

   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
   

3. 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.avi
   

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 left/right camera subscriptions

• Real-time FPS statistics and data display

• Supports RGB image video recording(TBD, see C++ examples)

• 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)

#!/usr/bin/env python3
"""
Head stereo camera multi-topic subscription example

Supports selecting the topic type to subscribe via startup parameter --ros-args -p topic_type:=<type>:
  - left_rgb_image: Left camera RGB image (sensor_msgs/Image)
  - left_rgb_image_compressed: Left camera RGB compressed image (sensor_msgs/CompressedImage)
  - left_camera_info: Left camera intrinsic parameters (sensor_msgs/CameraInfo)
  - right_rgb_image: Right camera RGB image (sensor_msgs/Image)
  - right_rgb_image_compressed: Right camera RGB compressed image (sensor_msgs/CompressedImage)
  - right_camera_info: Right camera intrinsic parameters (sensor_msgs/CameraInfo)

Example:
  ros2 run py_examples echo_camera_stereo --ros-args -p topic_type:=left_rgb_image
  ros2 run py_examples echo_camera_stereo --ros-args -p topic_type:=right_rgb_image
  ros2 run py_examples echo_camera_stereo --ros-args -p topic_type:=left_camera_info

Default topic_type is left_rgb_image
"""

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSDurabilityPolicy, QoSHistoryPolicy
from sensor_msgs.msg import Image, CompressedImage, CameraInfo
from collections import deque
import time


class StereoCameraTopicEcho(Node):
    def __init__(self):
        super().__init__('stereo_camera_topic_echo')

        # Select the topic type to subscribe
        self.declare_parameter('topic_type', 'left_rgb_image')
        self.declare_parameter('dump_video_path', '')

        self.topic_type = self.get_parameter('topic_type').value
        self.dump_video_path = self.get_parameter('dump_video_path').value

        # Set QoS parameters - use sensor data QoS
        qos = QoSProfile(
            reliability=QoSReliabilityPolicy.BEST_EFFORT,
            history=QoSHistoryPolicy.KEEP_LAST,
            depth=5,
            durability=QoSDurabilityPolicy.VOLATILE
        )

        # Create different subscribers based on topic_type
        if self.topic_type == "left_rgb_image":
            self.topic_name = "/aima/hal/sensor/stereo_head_front_left/rgb_image"
            self.sub_image = self.create_subscription(
                Image, self.topic_name, self.cb_image, qos)
            self.get_logger().info(
                f"✅ Subscribing Left RGB Image: {self.topic_name}")
            if self.dump_video_path:
                self.get_logger().info(
                    f"📝 Will dump received images to video: {self.dump_video_path}")

        elif self.topic_type == "left_rgb_image_compressed":
            self.topic_name = "/aima/hal/sensor/stereo_head_front_left/rgb_image/compressed"
            self.sub_compressed = self.create_subscription(
                CompressedImage, self.topic_name, self.cb_compressed, qos)
            self.get_logger().info(
                f"✅ Subscribing Left CompressedImage: {self.topic_name}")

        elif self.topic_type == "left_camera_info":
            self.topic_name = "/aima/hal/sensor/stereo_head_front_left/camera_info"
            # CameraInfo subscription must use reliable + transient_local QoS to receive historical messages (even if only one frame is published)
            camera_qos = QoSProfile(
                reliability=QoSReliabilityPolicy.RELIABLE,
                history=QoSHistoryPolicy.KEEP_LAST,
                depth=1,
                durability=QoSDurabilityPolicy.TRANSIENT_LOCAL
            )
            self.sub_camerainfo = self.create_subscription(
                CameraInfo, self.topic_name, self.cb_camerainfo, camera_qos)
            self.get_logger().info(
                f"✅ Subscribing Left CameraInfo (with transient_local): {self.topic_name}")

        elif self.topic_type == "right_rgb_image":
            self.topic_name = "/aima/hal/sensor/stereo_head_front_right/rgb_image"
            self.sub_image = self.create_subscription(
                Image, self.topic_name, self.cb_image, qos)
            self.get_logger().info(
                f"✅ Subscribing Right RGB Image: {self.topic_name}")
            if self.dump_video_path:
                self.get_logger().info(
                    f"📝 Will dump received images to video: {self.dump_video_path}")

        elif self.topic_type == "right_rgb_image_compressed":
            self.topic_name = "/aima/hal/sensor/stereo_head_front_right/rgb_image/compressed"
            self.sub_compressed = self.create_subscription(
                CompressedImage, self.topic_name, self.cb_compressed, qos)
            self.get_logger().info(
                f"✅ Subscribing Right CompressedImage: {self.topic_name}")

        elif self.topic_type == "right_camera_info":
            self.topic_name = "/aima/hal/sensor/stereo_head_front_right/camera_info"
            # CameraInfo subscription must use reliable + transient_local QoS to receive historical messages (even if only one frame is published)
            camera_qos = QoSProfile(
                reliability=QoSReliabilityPolicy.RELIABLE,
                history=QoSHistoryPolicy.KEEP_LAST,
                depth=1,
                durability=QoSDurabilityPolicy.TRANSIENT_LOCAL
            )
            self.sub_camerainfo = self.create_subscription(
                CameraInfo, self.topic_name, self.cb_camerainfo, camera_qos)
            self.get_logger().info(
                f"✅ Subscribing Right CameraInfo (with transient_local): {self.topic_name}")

        else:
            self.get_logger().error(f"Unknown topic_type: {self.topic_type}")
            raise ValueError("Unknown topic_type")

        # Internal state
        self.last_print = self.get_clock().now()
        self.print_allowed = False
        self.arrivals = deque()

    def update_arrivals(self):
        """Calculate received FPS"""
        now = self.get_clock().now()
        self.arrivals.append(now)
        while self.arrivals and (now - self.arrivals[0]).nanoseconds * 1e-9 > 1.0:
            self.arrivals.popleft()

    def get_fps(self):
        """Get FPS"""
        return len(self.arrivals)

    def should_print(self, master=True):
        """Control print frequency"""
        if not master:
            return self.print_allowed
        now = self.get_clock().now()
        if (now - self.last_print).nanoseconds * 1e-9 >= 1.0:
            self.last_print = now
            self.print_allowed = True
        else:
            self.print_allowed = False
        return self.print_allowed

    def cb_image(self, msg: Image):
        """Image callback (Left/Right camera RGB image)"""
        self.update_arrivals()

        if self.should_print():
            stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
            self.get_logger().info(
                f"📸 {self.topic_type} received\n"
                f"  • frame_id:        {msg.header.frame_id}\n"
                f"  • stamp (sec):     {stamp_sec:.6f}\n"
                f"  • encoding:        {msg.encoding}\n"
                f"  • size (WxH):      {msg.width} x {msg.height}\n"
                f"  • step (bytes/row):{msg.step}\n"
                f"  • is_bigendian:    {msg.is_bigendian}\n"
                f"  • recv FPS (1s):   {self.get_fps():.1f}"
            )

        # Only RGB images support video dump
        if (self.topic_type in ["left_rgb_image", "right_rgb_image"]) and self.dump_video_path:
            self.dump_image_to_video(msg)

    def cb_compressed(self, msg: CompressedImage):
        """CompressedImage callback (Left/Right camera RGB compressed image)"""
        self.update_arrivals()

        if self.should_print():
            stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
            self.get_logger().info(
                f"🗜️  {self.topic_type} received\n"
                f"  • frame_id:        {msg.header.frame_id}\n"
                f"  • stamp (sec):     {stamp_sec:.6f}\n"
                f"  • format:          {msg.format}\n"
                f"  • data size:       {len(msg.data)}\n"
                f"  • recv FPS (1s):   {self.get_fps():.1f}"
            )

    def cb_camerainfo(self, msg: CameraInfo):
        """CameraInfo callback (Left/Right camera intrinsic parameters)"""
        # Camera info will only receive one frame, print it directly
        stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9

        # Format D array
        d_str = ", ".join([f"{d:.6f}" for d in msg.d])

        # Format K matrix
        k_str = ", ".join([f"{k:.6f}" for k in msg.k])

        # Format P matrix
        p_str = ", ".join([f"{p:.6f}" for p in msg.p])

        self.get_logger().info(
            f"📷 {self.topic_type} received\n"
            f"  • frame_id:        {msg.header.frame_id}\n"
            f"  • stamp (sec):     {stamp_sec:.6f}\n"
            f"  • width x height:  {msg.width} x {msg.height}\n"
            f"  • distortion_model:{msg.distortion_model}\n"
            f"  • D: [{d_str}]\n"
            f"  • K: [{k_str}]\n"
            f"  • P: [{p_str}]\n"
            f"  • binning_x: {msg.binning_x}\n"
            f"  • binning_y: {msg.binning_y}\n"
            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} }}"
        )

    def dump_image_to_video(self, msg: Image):
        """Video dump is only supported for RGB images"""
        # You can add video recording functionality here
        # Simplified in the Python version, only logs instead
        if self.should_print(master=False):
            self.get_logger().info(f"📝 Video dump not implemented in Python version")


def main(args=None):
    rclpy.init(args=args)
    try:
        node = StereoCameraTopicEcho()
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    except Exception as e:
        print(f"Error: {e}")
    finally:
        if 'node' in locals():
            node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()

Usage:

1. Subscribe to left camera RGB image:

   ros2 run py_examples echo_camera_stereo --ros-args -p topic_type:=left_rgb_image
   

2. Subscribe to right camera RGB image:

   ros2 run py_examples echo_camera_stereo --ros-args -p topic_type:=right_rgb_image
   

3. Subscribe to left camera intrinsics:

   ros2 run py_examples echo_camera_stereo --ros-args -p topic_type:=left_camera_info
   

4. Record left camera video:

   # You can change dump_video_path to another path; ensure the directory exists before saving
   ros2 run py_examples echo_camera_stereo --ros-args -p topic_type:=left_rgb_image -p dump_video_path:=$PWD/left_camera.avi
   

Head rear 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 head rear camera data subscription

• Real-time FPS statistics and data display

• Supports RGB image video recording with/without obstructed area masked (TBD, see C++ examples)

• Configurable topic type selection

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)

#!/usr/bin/env python3
"""
Head rear monocular camera multi-topic subscription example

Supports selecting the topic type to subscribe via startup parameter --ros-args -p topic_type:=<type>:
  - rgb_image: RGB image (sensor_msgs/Image)
  - rgb_image_compressed: RGB compressed image (sensor_msgs/CompressedImage)
  - camera_info: Camera intrinsic parameters (sensor_msgs/CameraInfo)

Example:
  ros2 run py_examples echo_camera_head_rear --ros-args -p topic_type:=rgb_image
  ros2 run py_examples echo_camera_head_rear --ros-args -p topic_type:=rgb_image_compressed
  ros2 run py_examples echo_camera_head_rear --ros-args -p topic_type:=camera_info

Default topic_type is rgb_image
"""

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSDurabilityPolicy, QoSHistoryPolicy
from sensor_msgs.msg import Image, CompressedImage, CameraInfo
from collections import deque
import time
import os
import cv2


class HeadRearCameraTopicEcho(Node):
    def __init__(self):
        super().__init__('head_rear_camera_topic_echo')

        # Select the topic type to subscribe
        self.declare_parameter('topic_type', 'rgb_image')
        self.declare_parameter('dump_video_path', '')
        self.declare_parameter('with_mask', False)

        self.topic_type = self.get_parameter('topic_type').value
        self.dump_video_path = self.get_parameter('dump_video_path').value
        self.with_mask = self.get_parameter('with_mask').value
        self.mask_image = None

        # Set QoS parameters - use sensor data QoS
        qos = QoSProfile(
            reliability=QoSReliabilityPolicy.BEST_EFFORT,
            history=QoSHistoryPolicy.KEEP_LAST,
            depth=5,
            durability=QoSDurabilityPolicy.VOLATILE
        )

        if self.with_mask and self.dump_video_path:
            mask_path = os.path.join(os.path.dirname(
                __file__), 'data', 'rgb_head_rear_mask.png')
            self.mask_image = cv2.imread(mask_path, cv2.IMREAD_GRAYSCALE)
            if self.mask_image is None:
                self.get_logger().error(
                    f"Failed to load mask file: {mask_path}")
                raise ValueError("Failed to load mask file")

        # Create different subscribers based on topic_type
        if self.topic_type == "rgb_image":
            self.topic_name = "/aima/hal/sensor/rgb_head_rear/rgb_image"
            self.sub_image = self.create_subscription(
                Image, self.topic_name, self.cb_image, qos)
            self.get_logger().info(
                f"✅ Subscribing RGB Image: {self.topic_name}")
            if self.dump_video_path:
                mask_state = "with mask" if self.with_mask else "without mask"
                self.get_logger().info(
                    f"📝 Will dump received images {mask_state} to video: {self.dump_video_path}")

        elif self.topic_type == "rgb_image_compressed":
            self.topic_name = "/aima/hal/sensor/rgb_head_rear/rgb_image/compressed"
            self.sub_compressed = self.create_subscription(
                CompressedImage, self.topic_name, self.cb_compressed, qos)
            self.get_logger().info(
                f"✅ Subscribing CompressedImage: {self.topic_name}")

        elif self.topic_type == "camera_info":
            self.topic_name = "/aima/hal/sensor/rgb_head_rear/camera_info"
            # CameraInfo subscription must use reliable + transient_local QoS to receive historical messages (even if only one frame is published)
            camera_qos = QoSProfile(
                reliability=QoSReliabilityPolicy.RELIABLE,
                history=QoSHistoryPolicy.KEEP_LAST,
                depth=1,
                durability=QoSDurabilityPolicy.TRANSIENT_LOCAL
            )
            self.sub_camerainfo = self.create_subscription(
                CameraInfo, self.topic_name, self.cb_camerainfo, camera_qos)
            self.get_logger().info(
                f"✅ Subscribing CameraInfo (with transient_local): {self.topic_name}")

        else:
            self.get_logger().error(f"Unknown topic_type: {self.topic_type}")
            raise ValueError("Unknown topic_type")

        # Internal state
        self.last_print = self.get_clock().now()
        self.print_allowed = False
        self.arrivals = deque()

    def update_arrivals(self):
        """Calculate received FPS"""
        now = self.get_clock().now()
        self.arrivals.append(now)
        while self.arrivals and (now - self.arrivals[0]).nanoseconds * 1e-9 > 1.0:
            self.arrivals.popleft()

    def get_fps(self):
        """Get FPS"""
        return len(self.arrivals)

    def should_print(self, master=True):
        """Control print frequency"""
        if not master:
            return self.print_allowed
        now = self.get_clock().now()
        if (now - self.last_print).nanoseconds * 1e-9 >= 1.0:
            self.last_print = now
            self.print_allowed = True
        else:
            self.print_allowed = False
        return self.print_allowed

    def cb_image(self, msg: Image):
        """Image callback (RGB image)"""
        self.update_arrivals()

        if self.should_print():
            stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
            self.get_logger().info(
                f"📸 {self.topic_type} received\n"
                f"  • frame_id:        {msg.header.frame_id}\n"
                f"  • stamp (sec):     {stamp_sec:.6f}\n"
                f"  • encoding:        {msg.encoding}\n"
                f"  • size (WxH):      {msg.width} x {msg.height}\n"
                f"  • step (bytes/row):{msg.step}\n"
                f"  • is_bigendian:    {msg.is_bigendian}\n"
                f"  • recv FPS (1s):   {self.get_fps():.1f}"
            )

        # Only RGB image supports video dump
        if self.topic_type == "rgb_image" and self.dump_video_path:
            self.dump_image_to_video(msg)

    def cb_compressed(self, msg: CompressedImage):
        """CompressedImage callback (RGB compressed image)"""
        self.update_arrivals()

        if self.should_print():
            stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
            self.get_logger().info(
                f"🗜️  {self.topic_type} received\n"
                f"  • frame_id:        {msg.header.frame_id}\n"
                f"  • stamp (sec):     {stamp_sec:.6f}\n"
                f"  • format:          {msg.format}\n"
                f"  • data size:       {len(msg.data)}\n"
                f"  • recv FPS (1s):   {self.get_fps():.1f}"
            )

    def cb_camerainfo(self, msg: CameraInfo):
        """CameraInfo callback (camera intrinsic parameters)"""
        # Camera info will only receive one frame, print it directly
        stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9

        # Format D array
        d_str = ", ".join([f"{d:.6f}" for d in msg.d])

        # Format K matrix
        k_str = ", ".join([f"{k:.6f}" for k in msg.k])

        # Format P matrix
        p_str = ", ".join([f"{p:.6f}" for p in msg.p])

        self.get_logger().info(
            f"📷 {self.topic_type} received\n"
            f"  • frame_id:        {msg.header.frame_id}\n"
            f"  • stamp (sec):     {stamp_sec:.6f}\n"
            f"  • width x height:  {msg.width} x {msg.height}\n"
            f"  • distortion_model:{msg.distortion_model}\n"
            f"  • D: [{d_str}]\n"
            f"  • K: [{k_str}]\n"
            f"  • P: [{p_str}]\n"
            f"  • binning_x: {msg.binning_x}\n"
            f"  • binning_y: {msg.binning_y}\n"
            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} }}"
        )

    def dump_image_to_video(self, msg: Image):
        """Video dump is only supported for RGB images"""
        # You can add video recording functionality here
        # Simplified in the Python version, only logs instead
        # Note: Refer to cpp implementation, get cv images by cv_bridge first,
        # then you can use 'image[self.mask_image == 0] = 0' to mask them and
        # finally use VideoWriter to save them as video
        if self.should_print(master=False):
            self.get_logger().info(f"📝 Video dump not implemented in Python version")


def main(args=None):
    rclpy.init(args=args)
    try:
        node = HeadRearCameraTopicEcho()
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    except Exception as e:
        print(f"Error: {e}")
    finally:
        if 'node' in locals():
            node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()

Usage:

1. Subscribe to RGB image data:

   ros2 run py_examples echo_camera_head_rear --ros-args -p topic_type:=rgb_image
   

2. Subscribe to compressed image data:

   ros2 run py_examples echo_camera_head_rear --ros-args -p topic_type:=rgb_image_compressed
   

3. Subscribe to camera intrinsics:

   ros2 run py_examples echo_camera_head_rear --ros-args -p topic_type:=camera_info
   

4. 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.avi
   

Use cases:

• 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

#!/usr/bin/env python3
"""
Head touch state subscription example
"""

import rclpy
from rclpy.node import Node
from aimdk_msgs.msg import TouchState


class TouchStateSubscriber(Node):
    def __init__(self):
        super().__init__('touch_state_subscriber')

        # touch event types
        self.event_type_map = {
            TouchState.UNKNOWN: "UNKNOWN",
            TouchState.IDLE: "IDLE",
            TouchState.TOUCH: "TOUCH",
            TouchState.SLIDE: "SLIDE",
            TouchState.PAT_ONCE: "PAT_ONCE",
            TouchState.PAT_TWICE: "PAT_TWICE",
            TouchState.PAT_TRIPLE: "PAT_TRIPLE"
        }

        # create subscriber
        self.subscription = self.create_subscription(
            TouchState,
            '/aima/hal/sensor/touch_head',
            self.touch_callback,
            10
        )

        self.get_logger().info(
            'TouchState subscriber started, listening to /aima/hal/sensor/touch_head')

    def touch_callback(self, msg):
        event_str = self.event_type_map.get(
            msg.event_type, f"INVALID({msg.event_type})")

        self.get_logger().info(f'Timestamp: {msg.header.stamp.sec}.{msg.header.stamp.nanosec:09d}, '
                               f'Event: {event_str} ({msg.event_type})')


def main(args=None):
    rclpy.init(args=args)
    node = TouchStateSubscriber()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()

Usage:

ros2 run py_examples echo_head_touch_sensor

Example output:

[INFO] [1769420383.315173538] [touch_state_subscriber]: Timestamp: 1769420394.129927670, Event: IDLE (1)
[INFO] [1769420383.324978563] [touch_state_subscriber]: Timestamp: 1769420394.139941215, Event: IDLE (1)
[INFO] [1769420383.335265681] [touch_state_subscriber]: Timestamp: 1769420394.149990634, Event: TOUCH (2)
[INFO] [1769420383.344826732] [touch_state_subscriber]: Timestamp: 1769420394.159926892, 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 data field information output

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 point cloud field parsing and visualization

• Supports IMU quaternion, angular velocity, and linear acceleration data

• Provides detailed debugging log output

Use cases:

• LiDAR data acquisition and analysis

• Point cloud data processing and visualization

• Robot navigation and localization

• SLAM algorithm development

• Environmental perception and mapping

#!/usr/bin/env python3
"""
Chest LiDAR data subscription example

Supports subscribing to the following topics:
  1. /aima/hal/sensor/lidar_chest_front/lidar_pointcloud
     - Data type: sensor_msgs/PointCloud2
     - frame_id: lidar_chest_front
     - child_frame_id: /
     - Content: LiDAR point cloud data
  2. /aima/hal/sensor/lidar_chest_front/imu
     - Data type: sensor_msgs/Imu
     - frame_id: lidar_imu_chest_front
     - Content: LiDAR IMU data

You can select the topic type to subscribe via startup parameter --ros-args -p topic_type:=<type>:
  - pointcloud: subscribe to LiDAR point cloud
  - imu: subscribe to LiDAR IMU
Default topic_type is pointcloud

Examples:
  ros2 run py_examples echo_lidar_data --ros-args -p topic_type:=pointcloud
  ros2 run py_examples echo_lidar_data --ros-args -p topic_type:=imu
"""

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
from sensor_msgs.msg import PointCloud2, Imu
from collections import deque
import time


class LidarChestEcho(Node):
    def __init__(self):
        super().__init__('lidar_chest_echo')

        # Select the topic type to subscribe
        self.declare_parameter('topic_type', 'pointcloud')
        self.topic_type = self.get_parameter('topic_type').value

        # SensorDataQoS: BEST_EFFORT + VOLATILE
        qos = QoSProfile(
            reliability=QoSReliabilityPolicy.BEST_EFFORT,
            history=QoSHistoryPolicy.KEEP_LAST,
            depth=5
        )

        # Create different subscribers based on topic_type
        if self.topic_type == "pointcloud":
            self.topic_name = "/aima/hal/sensor/lidar_chest_front/lidar_pointcloud"
            self.sub_pointcloud = self.create_subscription(
                PointCloud2, self.topic_name, self.cb_pointcloud, qos)
            self.get_logger().info(
                f"✅ Subscribing LIDAR PointCloud2: {self.topic_name}")

        elif self.topic_type == "imu":
            self.topic_name = "/aima/hal/sensor/lidar_chest_front/imu"
            self.sub_imu = self.create_subscription(
                Imu, self.topic_name, self.cb_imu, qos)
            self.get_logger().info(
                f"✅ Subscribing LIDAR IMU: {self.topic_name}")

        else:
            self.get_logger().error(f"Unknown topic_type: {self.topic_type}")
            raise ValueError("Unknown topic_type")

        # Internal state
        self.last_print = self.get_clock().now()
        self.arrivals = deque()

    def update_arrivals(self):
        """Calculate received FPS"""
        now = self.get_clock().now()
        self.arrivals.append(now)
        while self.arrivals and (now - self.arrivals[0]).nanoseconds * 1e-9 > 1.0:
            self.arrivals.popleft()

    def get_fps(self):
        """Get FPS"""
        return len(self.arrivals)

    def should_print(self):
        """Control print frequency"""
        now = self.get_clock().now()
        if (now - self.last_print).nanoseconds * 1e-9 >= 1.0:
            self.last_print = now
            return True
        return False

    def cb_pointcloud(self, msg: PointCloud2):
        """PointCloud2 callback (LiDAR point cloud)"""
        self.update_arrivals()

        if self.should_print():
            stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9

            # Format fields info
            fields_str = " ".join(
                [f"{f.name}({f.datatype})" for f in msg.fields])

            self.get_logger().info(
                f"🟢 LIDAR PointCloud2 received\n"
                f"  • frame_id:        {msg.header.frame_id}\n"
                f"  • stamp (sec):     {stamp_sec:.6f}\n"
                f"  • width x height:  {msg.width} x {msg.height}\n"
                f"  • point_step:      {msg.point_step}\n"
                f"  • row_step:        {msg.row_step}\n"
                f"  • fields:          {fields_str}\n"
                f"  • is_bigendian:    {msg.is_bigendian}\n"
                f"  • is_dense:        {msg.is_dense}\n"
                f"  • data size:       {len(msg.data)}\n"
                f"  • recv FPS (1s):   {self.get_fps():1.1f}"
            )

    def cb_imu(self, msg: Imu):
        """IMU callback (LiDAR IMU)"""
        self.update_arrivals()

        if self.should_print():
            stamp_sec = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9

            self.get_logger().info(
                f"🟢 LIDAR IMU received\n"
                f"  • frame_id:        {msg.header.frame_id}\n"
                f"  • stamp (sec):     {stamp_sec:.6f}\n"
                f"  • orientation:     [{msg.orientation.x:.6f}, {msg.orientation.y:.6f}, {msg.orientation.z:.6f}, {msg.orientation.w:.6f}]\n"
                f"  • angular_velocity:[{msg.angular_velocity.x:.6f}, {msg.angular_velocity.y:.6f}, {msg.angular_velocity.z:.6f}]\n"
                f"  • linear_accel:    [{msg.linear_acceleration.x:.6f}, {msg.linear_acceleration.y:.6f}, {msg.linear_acceleration.z:.6f}]\n"
                f"  • recv FPS (1s):   {self.get_fps():.1f}"
            )


def main(args=None):
    rclpy.init(args=args)
    try:
        node = LidarChestEcho()
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    except Exception as e:
        print(f"Error: {e}")
    finally:
        if 'node' in locals():
            node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()

Usage:

# 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

Example output:

[INFO] [lidar_chest_echo]: ✅ Subscribing LIDAR PointCloud2: /aima/hal/sensor/lidar_chest_front/lidar_pointcloud
[INFO] [lidar_chest_echo]: 🟢 LIDAR PointCloud2 received
  • frame_id:        lidar_chest_front
  • stamp (sec):     1234567890.123456
  • width x height:  1 x 36000
  • point_step:      16
  • row_step:        16
  • fields:          x(7) y(7) z(7) intensity(7)
  • is_bigendian:    False
  • is_dense:        True
  • data size:       576000
  • recv FPS (1s):   10.0

6.1.17 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 .mp4 video file, then use it as input for play_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.mp4

Step 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.mp4

Step 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

#!/usr/bin/env python3

import rclpy
import rclpy.logging
from rclpy.node import Node

from aimdk_msgs.srv import PlayVideo


class PlayVideoClient(Node):
    def __init__(self):
        super().__init__('play_video_client')
        self.client = self.create_client(
            PlayVideo, '/face_ui_proxy/play_video')
        self.get_logger().info('✅ PlayVideo client node created.')

        # Wait for the service to become available
        while not self.client.wait_for_service(timeout_sec=2.0):
            self.get_logger().info('⏳ Service unavailable, waiting...')

        self.get_logger().info('🟢 Service available, ready to send request.')

    def send_request(self, video_path, mode, priority):
        req = PlayVideo.Request()

        req.video_path = video_path
        req.mode = mode
        req.priority = priority

        # async call
        self.get_logger().info(
            f'📨 Sending request to play video: mode={mode} video={video_path}')
        for i in range(8):
            req.header.header.stamp = self.get_clock().now().to_msg()
            future = self.client.call_async(req)
            rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)

            if future.done():
                break

            # retry as remote peer is NOT handled well by ROS
            self.get_logger().info(f'trying ... [{i}]')

        resp = future.result()
        if resp is None:
            self.get_logger().error('❌ Service call not completed or timed out.')
            return False

        if resp.success:
            self.get_logger().info(
                f'✅ Request to play video recorded successfully: {resp.message}')
            return True
        else:
            self.get_logger().error(
                f'❌ Failed to record play-video request: {resp.message}')
            return False


def main(args=None):
    rclpy.init(args=args)
    node = None

    try:
        # video path and priority can be customized
        video_path = "/agibot/data/home/agi/zhiyuan.mp4"
        priority = 5
        # input play mode
        mode = int(input("Enter video play mode (1: play once, 2: loop): "))
        if mode not in (1, 2):
            raise ValueError(f'invalid mode {mode}')

        node = PlayVideoClient()
        node.send_request(video_path, mode, priority)
    except KeyboardInterrupt:
        pass
    except Exception as e:
        rclpy.logging.get_logger('main').error(
            f'Program exited with exception: {e}')

    if node:
        node.destroy_node()
    if rclpy.ok():
        rclpy.shutdown()


if __name__ == '__main__':
    main()

6.1.18 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 complete 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

Use cases:

• 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)

#!/usr/bin/env python3

import os
import sys
import rclpy
import rclpy.logging
from rclpy.node import Node

from aimdk_msgs.srv import PlayAudioFile


class PlayAudioClient(Node):
    def __init__(self):
        super().__init__('PlayAudioClient')
        self.client = self.create_client(
            PlayAudioFile, '/aimdk_5Fmsgs/srv/PlayAudioFile')
        self.get_logger().info('✅ PlayAudio client node created.')

        # Wait for the service to become available
        while not self.client.wait_for_service(timeout_sec=2.0):
            self.get_logger().info('⏳ Service unavailable, waiting...')

        self.get_logger().info('🟢 Service available, ready to send request.')

    def send_request(self, media_path):
        req = PlayAudioFile.Request()

        req.file.pkg_name = 'demo_client'
        req.file.file_name = os.path.basename(media_path)
        req.file.file_path = os.path.dirname(media_path) + '/'
        req.file.priority = 6
        req.file.priority_weight = 0

        self.get_logger().info(
            f'📨 Sending request to play media: {media_path}')
        for i in range(5):
            req.request.header.stamp = self.get_clock().now().to_msg()
            future = self.client.call_async(req)
            rclpy.spin_until_future_complete(self, future, timeout_sec=1.0)

            if future.done():
                break

            if not rclpy.ok():
                self.get_logger().warn('Interrupted while waiting')
                return False

            # retry as remote peer is NOT handled well by ROS
            self.get_logger().info(f'trying ... [{i}]')

        resp = future.result()
        if resp is None:
            self.get_logger().error('❌ Service call not completed or timed out.')
            return False

        if resp.reponse.status.value == 1:
            self.get_logger().info('✅ Request to play media file recorded successfully.')
            return True
        else:
            self.get_logger().error('❌ Failed to record play-media request.')
            return False


def main(args=None):
    rclpy.init(args=args)
    node = None

    default_media = '/agibot/data/var/interaction/tts_cache/normal/demo.wav'
    try:
        if len(sys.argv) > 1:
            media_path = sys.argv[1]
        else:
            media_path = input(
                f'Enter media file path to play (default: {default_media}): ').strip()
            if not media_path:
                media_path = default_media

        node = PlayAudioClient()
        node.send_request(media_path)
    except KeyboardInterrupt:
        pass
    except Exception as e:
        rclpy.logging.get_logger('main').error(
            f'Program exited with exception: {e}')

    if node:
        node.destroy_node()
    if rclpy.ok():
        rclpy.shutdown()


if __name__ == '__main__':
    main()

Usage:

# 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 a TTS cached file
ros2 run py_examples play_audio /agibot/data/var/interaction/tts_cache/normal/demo.wav

Example output:

[INFO] [PlayAudioClient]: ✅ Request to play media 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

• The program includes complete exception handling mechanisms

6.1.19 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 parameters and interactive input

• Includes complete service availability checks and error handling

• Supports priority control and interruption mechanism

• Provides detailed playback status feedback

Core code

#!/usr/bin/env python3

import sys
import rclpy
import rclpy.logging
from rclpy.node import Node

from aimdk_msgs.srv import PlayTts
from aimdk_msgs.msg import TtsPriorityLevel


class PlayTTSClient(Node):
    def __init__(self):
        super().__init__('play_tts_client')

        # fill in the actual service name
        self.client = self.create_client(PlayTts, '/aimdk_5Fmsgs/srv/PlayTts')
        self.get_logger().info('✅ PlayTts client node created.')

        # Wait for the service to become available
        while not self.client.wait_for_service(timeout_sec=2.0):
            self.get_logger().info('⏳ Service unavailable, waiting...')

        self.get_logger().info('🟢 Service available, ready to send request.')

    def send_request(self, text):
        req = PlayTts.Request()

        req.tts_req.text = text
        req.tts_req.domain = 'demo_client'   # required: caller domain
        req.tts_req.trace_id = 'demo'        # optional: request id
        req.tts_req.is_interrupted = True    # required: interrupt same-priority
        req.tts_req.priority_weight = 0
        req.tts_req.priority_level.value = 6

        self.get_logger().info(f'📨 Sending request to play tts: text={text}')
        for i in range(8):
            req.header.header.stamp = self.get_clock().now().to_msg()
            future = self.client.call_async(req)
            rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)

            if future.done():
                break

            # retry as remote peer is NOT handled well by ROS
            self.get_logger().info(f'trying ... [{i}]')

        resp = future.result()
        if resp is None:
            self.get_logger().error('❌ Service call not completed or timed out.')
            return False

        if resp.tts_resp.is_success:
            self.get_logger().info('✅ TTS sent successfully.')
            return True
        else:
            self.get_logger().error('❌ Failed to send TTS.')
            return False


def main(args=None):
    rclpy.init(args=args)
    node = None

    try:
        # get text to speak
        if len(sys.argv) > 1:
            text = sys.argv[1]
        else:
            text = input('Enter text to speak: ')
            if not text:
                text = 'Hello, I am AgiBot X2.'

        node = PlayTTSClient()
        node.send_request(text)
    except KeyboardInterrupt:
        pass
    except Exception as e:
        rclpy.logging.get_logger('main').error(
            f'Program exited with exception: {e}')

    if node:
        node.destroy_node()
    if rclpy.ok():
        rclpy.shutdown()


if __name__ == '__main__':
    main()

Usage

# 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

Example output

[INFO] [play_tts_client_min]: ✅ Speech playback request succeeded

Notes

• Ensure the TTS service is running properly

• Supports Chinese and English text playback

• Priority settings affect playback queue order

• Interruption feature can stop the currently playing speech

Interface reference

• Service: /aimdk_5Fmsgs/srv/PlayTts

• Message: aimdk_msgs/srv/PlayTts

6.1.20 Microphone data 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

• Stores recordings categorized by timestamp and audio stream

• Supports duration calculation and statistical information output

• Intelligent buffer management to prevent memory leaks

• Complete error handling and exception management

• Detailed debugging log output

VAD state description:

• 0: No speech

• 1: Speech start

• 2: Speech in progress

• 3: Speech end

Technical implementation:

• Supports saving PCM audio files (16 kHz, 16-bit, mono)

• Provides detailed log output and status monitoring

• Supports real-time audio stream processing and file saving

Use cases:

• Speech recognition and speech processing

• Audio data acquisition and analysis

• Real-time speech monitoring

• Audio quality evaluation

• Multi-microphone array data processing

#!/usr/bin/env python3
"""
Microphone data receiving example

This example subscribes to the `/agent/process_audio_output` topic to receive the robot's
noise-suppressed audio data. It supports both the built-in microphone and the external
microphone audio streams, and automatically saves complete speech segments as PCM files
based on the VAD (Voice Activity Detection) state.

Features:
- Supports receiving multiple audio streams at the same time (built-in mic stream_id=1, external mic stream_id=2)
- Automatically detects speech start / in-progress / end based on VAD state
- Automatically saves complete speech segments as PCM files
- Stores files categorized by timestamp and audio stream
- Supports audio duration calculation and logging

VAD state description:
- 0: No speech
- 1: Speech start
- 2: Speech in progress
- 3: Speech end
"""

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
from aimdk_msgs.msg import ProcessedAudioOutput, AudioVadStateType
import os
import time
from datetime import datetime
from collections import defaultdict
from typing import Dict, List


class AudioSubscriber(Node):
    def __init__(self):
        super().__init__('audio_subscriber')

        # Audio buffers, stored separately by stream_id
        # stream_id -> buffer
        self.audio_buffers: Dict[int, List[bytes]] = defaultdict(list)
        self.recording_state: Dict[int, bool] = defaultdict(bool)

        # Create audio output directory
        self.audio_output_dir = "audio_recordings"
        os.makedirs(self.audio_output_dir, exist_ok=True)

        # VAD state name mapping
        self.vad_state_names = {
            0: "No speech",
            1: "Speech start",
            2: "Speech in progress",
            3: "Speech end"
        }

        # Audio stream name mapping
        self.stream_names = {
            1: "Built-in microphone",
            2: "External microphone"
        }

        # QoS settings
        # Note: deep queue to avoid missing data in a burst at start of VAD.
        qos = QoSProfile(
            history=QoSHistoryPolicy.KEEP_LAST,
            depth=500,
            reliability=QoSReliabilityPolicy.BEST_EFFORT
        )

        # Create subscriber
        self.subscription = self.create_subscription(
            ProcessedAudioOutput,
            '/agent/process_audio_output',
            self.audio_callback,
            qos
        )

        self.get_logger().info("Start subscribing to noise-suppressed audio data...")

    def audio_callback(self, msg: ProcessedAudioOutput):
        """Audio data callback"""
        try:
            stream_id = msg.stream_id
            vad_state = msg.audio_vad_state.value
            audio_data = bytes(msg.audio_data)

            self.get_logger().info(
                f"Received audio data: stream_id={stream_id}, "
                f"vad_state={vad_state}({self.vad_state_names.get(vad_state, 'Unknown state')}), "
                f"audio_size={len(audio_data)} bytes"
            )

            self.handle_vad_state(stream_id, vad_state, audio_data)

        except Exception as e:
            self.get_logger().error(
                f"Error while processing audio message: {str(e)}")

    def handle_vad_state(self, stream_id: int, vad_state: int, audio_data: bytes):
        """Handle VAD state changes"""
        stream_name = self.stream_names.get(
            stream_id, f"Unknown stream {stream_id}")
        vad_name = self.vad_state_names.get(
            vad_state, f"Unknown state {vad_state}")

        self.get_logger().info(
            f"[{stream_name}] VAD state: {vad_name} audio: {len(audio_data)} bytes"
        )

        # Speech start
        if vad_state == 1:
            self.get_logger().info("🎤 Speech start detected")
            if not self.recording_state[stream_id]:
                self.audio_buffers[stream_id].clear()
                self.recording_state[stream_id] = True
            if audio_data:
                self.audio_buffers[stream_id].append(audio_data)

        # Speech in progress
        elif vad_state == 2:
            self.get_logger().info("🔄 Speech in progress...")
            if self.recording_state[stream_id] and audio_data:
                self.audio_buffers[stream_id].append(audio_data)

        # Speech end
        elif vad_state == 3:
            self.get_logger().info("✅ Speech end")
            if self.recording_state[stream_id] and audio_data:
                self.audio_buffers[stream_id].append(audio_data)

            if self.recording_state[stream_id] and self.audio_buffers[stream_id]:
                self.save_audio_segment(stream_id)
            self.recording_state[stream_id] = False

        # No speech
        elif vad_state == 0:
            if self.recording_state[stream_id]:
                self.get_logger().info("⏹️ Reset recording state")
                self.recording_state[stream_id] = False

        # Print current buffer status
        buffer_size = sum(len(chunk)
                          for chunk in self.audio_buffers[stream_id])
        recording = self.recording_state[stream_id]
        self.get_logger().debug(
            f"[Stream {stream_id}] Buffer size: {buffer_size} bytes, recording: {recording}"
        )

    def save_audio_segment(self, stream_id: int):
        """Save audio segment"""
        if not self.audio_buffers[stream_id]:
            return

        # Merge all audio data
        audio_data = b''.join(self.audio_buffers[stream_id])

        # Get current timestamp
        now = datetime.now()
        timestamp = now.strftime("%Y%m%d_%H%M%S_%f")[:-3]  # to milliseconds

        # Create subdirectory by stream_id
        stream_dir = os.path.join(self.audio_output_dir, f"stream_{stream_id}")
        os.makedirs(stream_dir, exist_ok=True)

        # Generate filename
        stream_name = "internal_mic" if stream_id == 1 else "external_mic" if stream_id == 2 else f"stream_{stream_id}"
        filename = f"{stream_name}_{timestamp}.pcm"
        filepath = os.path.join(stream_dir, filename)

        try:
            # Save PCM file
            with open(filepath, 'wb') as f:
                f.write(audio_data)

            self.get_logger().info(
                f"Audio segment saved: {filepath} (size: {len(audio_data)} bytes)")

            # Calculate audio duration (assuming 16 kHz, 16-bit, mono)
            sample_rate = 16000
            bits_per_sample = 16
            channels = 1
            bytes_per_sample = bits_per_sample // 8
            total_samples = len(audio_data) // (bytes_per_sample * channels)
            duration_seconds = total_samples / sample_rate

            self.get_logger().info(
                f"Audio duration: {duration_seconds:.2f} s ({total_samples} samples)")

        except Exception as e:
            self.get_logger().error(f"Failed to save audio file: {str(e)}")


def main(args=None):
    rclpy.init(args=args)
    node = AudioSubscriber()

    try:
        node.get_logger().info("Listening to noise-suppressed audio data, press Ctrl+C to exit...")
        rclpy.spin(node)
    except KeyboardInterrupt:
        node.get_logger().info("Interrupt signal received, exiting...")
    finally:
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()

Usage:

1. 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
   

2. Directory structure:

   • After running, an audio_recordings directory is created automatically

   • Audio files are stored by stream_id (currently not distinguished; stream_1 is always used):

     • stream_1/: Internal microphone audio

     • stream_2/: External microphone audio

3. File naming format: {stream_name}_{timestamp}.pcm

   • internal_mic_20250909_133649_738.pcm (internal microphone)

   • external_mic_20250909_133650_123.pcm (external microphone)

4. Audio format: 16 kHz, 16-bit, mono PCM

5. Play the saved PCM file:

   # Play internal 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
   

6. 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
   

7. Program control:

   • Press Ctrl+C to safely exit the program

   • The program automatically handles audio stream start and end

   • Supports processing multiple audio streams simultaneously

Example output:

Normal startup and operation:

[INFO] Started subscribing to noise-reduced audio data...
[INFO] Received audio data: stream_id=1, vad_state=0(no speech), audio_size=0 bytes
[INFO] [Internal mic] VAD state: no speech Audio data: 0 bytes
[INFO] Received audio data: stream_id=2, vad_state=0(no speech), audio_size=0 bytes
[INFO] [External mic] VAD state: no speech Audio data: 0 bytes

Speech start detected:

[INFO] Received audio data: stream_id=2, vad_state=1(speech start), audio_size=320 bytes
[INFO] [External mic] VAD state: speech start Audio data: 320 bytes
[INFO] 🎤 Speech start detected

Speech processing:

[INFO] Received audio data: stream_id=2, vad_state=2(speech in progress), audio_size=320 bytes
[INFO] [External mic] VAD state: speech in progress Audio data: 320 bytes
[INFO] 🔄 Speech in progress...
[INFO] Received audio data: stream_id=2, vad_state=2(speech in progress), audio_size=320 bytes
[INFO] [External mic] VAD state: speech in progress Audio data: 320 bytes
[INFO] 🔄 Speech in progress...

Speech end and save:

[INFO] Received audio data: stream_id=2, vad_state=3(speech end), audio_size=320 bytes
[INFO] [External mic] VAD state: speech end Audio data: 320 bytes
[INFO] ✅ Speech ended
[INFO] Audio segment saved: audio_recordings/stream_2/external_mic_20250909_133649_738.pcm (size: 960 bytes)
[INFO] Audio duration: 0.06 seconds (480 samples)

Simultaneous multi-stream processing:

[INFO] Received audio data: stream_id=1, vad_state=1(speech start), audio_size=320 bytes
[INFO] [Internal mic] VAD state: speech start Audio data: 320 bytes
[INFO] 🎤 Speech start detected
[INFO] Received audio data: stream_id=2, vad_state=1(speech start), audio_size=320 bytes
[INFO] [External mic] VAD state: speech start Audio data: 320 bytes
[INFO] 🎤 Speech start detected

Program exit:

^C[INFO] Interrupt signal received, exiting...
[INFO] Program exited safely

Notes:

• The program supports processing multiple audio streams (internal and external microphones)

• Each audio stream has an independent buffer and recording state

• Audio files are automatically saved by timestamp and audio stream

• The program includes complete error handling mechanisms to ensure stable operation

6.1.21 Raw Microphone Audio Stream Receiver

#!/usr/bin/env python3
"""
Microphone raw data receiving example

This example subscribes to the `/aima/hal/audio/capture` topic to receive the robot's
raw audio data. It supports both the built-in microphone and the external
microphone audio streams, and automatically saves complete speech segments as PCM files.

Features:
- Automatically saves raw audio as PCM files
- Stores files categorized by timestamp and mic source

"""

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
from aimdk_msgs.msg import AudioCapture
import os
import time
from datetime import datetime
from collections import defaultdict
from typing import Dict, List


class RawAudioSubscriber(Node):
    def __init__(self):
        super().__init__('raw_audio_subscriber')

        # Audio buffers, stored separately by channels
        self.audio_buffers: Dict[int, List[bytes]] = defaultdict(list)
        self.mic_channels = 0
        self.ref_channels = 0
        self.inited = 0
        self.dump_timestamp = 0

        # Create audio output directory
        now = datetime.now()
        timestamp = now.strftime("%Y%m%d_%H%M%S_%f")[:-3]  # to milliseconds

        self.audio_output_dir = os.path.join("audio_recordings", timestamp)
        os.makedirs(self.audio_output_dir, exist_ok=True)

        # QoS settings
        # Note: deep queue to avoid missing data
        qos = QoSProfile(
            history=QoSHistoryPolicy.KEEP_LAST,
            depth=500,
            reliability=QoSReliabilityPolicy.BEST_EFFORT
        )

        # Create subscriber
        self.subscription = self.create_subscription(
            AudioCapture,
            '/aima/hal/audio/capture',
            self.audio_callback,
            qos
        )

        self.get_logger().info("Start subscribing to raw audio data...")

    def audio_callback(self, msg: AudioCapture):
        """Audio data callback"""
        if not self.inited:
            self.mic_channels = msg.mic_channels
            self.ref_channels = msg.ref_channels
            self.dump_timestamp = time.time()
            self.inited = 1
        elif self.mic_channels != msg.mic_channels or self.ref_channels != msg.ref_channels:
            # quit as MIC switched
            self.get_logger().error(
                "MIC channels info changed (mic:{self.mic_channels} ref:{self.ref_channels}) -> (mic:{msg.mic_channels} ref:{msg.ref_channels})")
            if rclpy.ok():
                rclpy.shutdown()
            return
        audio_data = bytes(msg.data.data)

        self.handle_audio_data(audio_data)

    def handle_audio_data(self, audio_data: bytes):
        """Handle raw audio data"""

        # Split S16LE data into channels
        channels = self.ref_channels + self.mic_channels
        bytes_per_channel = 2
        unit_size = channels * bytes_per_channel
        base = 0
        for k in range(len(audio_data) // unit_size):
            for i in range(channels):
                tail = base + bytes_per_channel
                self.audio_buffers[i].append(audio_data[base:tail])
                base = tail

    def save_audio_segments(self):
        self.get_logger().info("🔄 Flashing cached audio data...")
        for i in range(self.mic_channels + self.ref_channels):
            self.save_audio_segment(i, i >= self.mic_channels)

    def save_audio_segment(self, channel: int, is_ref: bool):
        """Save audio segment"""
        if not self.audio_buffers[channel]:
            return

        # Merge all audio data
        audio_data = b''.join(self.audio_buffers[channel])
        self.audio_buffers[channel].clear()

        channel_type = 'mic'
        if is_ref:
            channel_type = 'ref'

        # Generate filename
        filepath = os.path.join(self.audio_output_dir,
                                f"channel_{channel}_{channel_type}.pcm")

        try:
            # Save PCM file
            with open(filepath, 'ab') as f:
                f.write(audio_data)

            self.get_logger().info(
                f"Audio segment saved: {filepath} append: {len(audio_data)} bytes)")

        except Exception as e:
            self.get_logger().error(f"Failed to save audio file: {str(e)}")

    def run(self):
        self.timer = self.create_timer(1.0, self.save_audio_segments)
        rclpy.spin(self)


def main(args=None):
    rclpy.init(args=args)
    node = None

    try:
        node = RawAudioSubscriber()
        node.get_logger().info("Listening to raw audio data, press Ctrl+C to exit...")
        node.run()
    except KeyboardInterrupt:
        rclpy.logging.get_logger('main').info(
            "Interrupt signal received, exiting...")

    if node:
        node.destroy_node()
    if rclpy.ok():
        rclpy.shutdown()


if __name__ == '__main__':
    main()

Usage:

1. Run the program:

   # Build the Python package
   colcon build --packages-select py_examples
   
   # Run the mic receiver
   ros2 run py_examples mic_raw_receiver
   

2. Directory structure:

   • After running, an audio_recordings directory is created automatically

   • Audio files are grouped by the program’s start time into a subdirectory:

     • 20250909_133650_123

3. File naming format: channel_{index}_{type}.pcm, e.g.

   • channel_0_mic.pcm (microphone)

   • channel_4_ref.pcm (reference / echo-cancellation)

4. Audio format: 16 kHz, 16-bit, mono PCM

5. Play the saved PCM file:

   • Transfer to a computer and play

   • Play on the robot:

     # 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.22 Get Current Microphone Source

#!/usr/bin/env python3

import sys
import rclpy
import rclpy.logging
from rclpy.node import Node

from aimdk_msgs.srv import GetMicSourceRequest
from aimdk_msgs.msg import CommonRequest, CommonState


class GetMicSourceRequestClient(Node):
    def __init__(self):
        super().__init__('get_mic_source_request_client')
        self.client = self.create_client(
            GetMicSourceRequest, '/aimdk_5Fmsgs/srv/GetMicSourceRequest'
        )
        self.get_logger().info('✅ GetMicSourceRequest client node created.')

        # Wait for the service to become available
        while not self.client.wait_for_service(timeout_sec=2.0):
            self.get_logger().info('⏳ Service unavailable, waiting...')

        self.get_logger().info('🟢 Service available, ready to send request.')

    def send_request(self):
        req = GetMicSourceRequest.Request()
        req.header = CommonRequest()

        self.get_logger().info(
            f'📨 Sending request to get MIC source')
        for i in range(8):
            req.header.header.stamp = self.get_clock().now().to_msg()
            future = self.client.call_async(req)
            rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)

            if future.done():
                break

            # retry as remote peer is NOT handled well by ROS
            self.get_logger().info(f'trying ... [{i}]')

        response = future.result()
        if response is None:
            self.get_logger().error('❌ Service call failed or timed out.')
            return

        if response.header.status.value == CommonState.SUCCESS:
            self.get_logger().info('✅ MIC source get successfully.')
            self.get_logger().info(f'MIC id: {response.mic_source}')
        else:
            self.get_logger().error(
                f'❌ Failed to get MIC source: {response.header.message}'
            )


def main(args=None):
    rclpy.init(args=args)
    node = None
    try:
        node = GetMicSourceRequestClient()
        node.send_request()
    except KeyboardInterrupt:
        pass
    except Exception as e:
        rclpy.logging.get_logger('main').error(
            f'Program exited with exception: {e}')

    if node:
        node.destroy_node()
    if rclpy.ok():
        rclpy.shutdown()


if __name__ == '__main__':
    main()

6.1.23 Switch Microphone Source

#!/usr/bin/env python3

import sys
import rclpy
import rclpy.logging
from rclpy.node import Node

from aimdk_msgs.srv import SetMicSourceRequest
from aimdk_msgs.msg import CommonRequest, CommonState


class SetMicSourceRequestClient(Node):
    def __init__(self):
        super().__init__('set_mic_source_request_client')
        self.client = self.create_client(
            SetMicSourceRequest, '/aimdk_5Fmsgs/srv/SetMicSourceRequest'
        )
        self.get_logger().info('✅ SetMicSourceRequest client node created.')

        # Wait for the service to become available
        while not self.client.wait_for_service(timeout_sec=2.0):
            self.get_logger().info('⏳ Service unavailable, waiting...')

        self.get_logger().info('🟢 Service available, ready to send request.')

    def send_request(self, mic_source: int):
        req = SetMicSourceRequest.Request()
        req.header = CommonRequest()

        req.mic_source = mic_source

        self.get_logger().info(
            f'📨 Sending request to set mic source: {mic_source}')
        for i in range(8):
            req.header.header.stamp = self.get_clock().now().to_msg()
            future = self.client.call_async(req)
            rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)

            if future.done():
                break

            # retry as remote peer is NOT handled well by ROS
            self.get_logger().info(f'trying ... [{i}]')

        response = future.result()
        if response is None:
            self.get_logger().error('❌ Service call failed or timed out.')
            return

        if response.header.status.value == CommonState.SUCCESS:
            self.get_logger().info('✅ MIC source set successfully.')
        else:
            self.get_logger().error(
                f'❌ Failed to set MIC source: {response.header.message}'
            )


def main(args=None):
    mic_info = {
        0: ('int', 'internal MIC'),
        1: ('ext', 'external MIC'),
    }

    choices = {}
    for k, v in mic_info.items():
        choices[v[0]] = k

    rclpy.init(args=args)
    node = None
    try:
        # Prefer command-line argument, otherwise prompt for input
        if len(sys.argv) > 1:
            mic = sys.argv[1]
        else:
            print('{:<4} - {:<5} : {}'.format('abbr',
                  'mic_id', 'description'))
            for k, v in mic_info.items():
                print(f'{v[0]:<4} - {k:<5} : {v[1]}')
            mic = input('Enter abbr of MIC source:')

        mic_id = choices.get(mic)
        if mic_id is None:
            raise ValueError(f'Invalid abbr of MIC source: {mic}')

        node = SetMicSourceRequestClient()
        node.send_request(mic_id)
    except KeyboardInterrupt:
        pass
    except Exception as e:
        rclpy.logging.get_logger('main').error(
            f'Program exited with exception: {e}')

    if node:
        node.destroy_node()
    if rclpy.ok():
        rclpy.shutdown()


if __name__ == '__main__':
    main()

Usage

# 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.24 Raw Audio Stream Playback

#!/usr/bin/env python3
"""
Raw audio stream playback example

This example would get audio focus first and then publish raw audio data
at fixed rate to `/aima/hal/audio/playback` topic. And it would stop playing
when audio focus is lost.

"""

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSDurabilityPolicy, QoSHistoryPolicy
from aimdk_msgs.msg import AudioPlayback, AudioInfo, AudioData, FocusResponse, FocusRequester, CommonState
from aimdk_msgs.srv import RequestAudioFocus, AbandonAudioFocus
import os
import signal
import time
import sys

global_node = None


def signal_handler(sig, frame):
    global global_node
    if global_node is not None:
        if global_node.is_holding_focus():
            global_node.send_request(False)
        global_node.get_logger().info(
            f"Received signal {sig}, abandon audio focus and shutting down")
    if rclpy.ok():
        rclpy.shutdown()
    sys.exit(0)


class FakeMicDevice():
    def __init__(self, raw_audio_file):
        self.audio_data = []
        self.channels = 1
        self.sample_rate = 16000
        self.bytes_per_sample = 2  # S16LE
        self.audio_length = 0
        with open(raw_audio_file, 'rb') as f:
            self.audio_data = list(f.read())
            self.audio_length = len(self.audio_data)

        self.index = 0
        self.start_time = 0

    def get_channel_count(self):
        return self.channels

    def get_sample_rate(self):
        return self.sample_rate

    def get_cached_data(self):
        now = time.monotonic()
        if not self.start_time:
            # fake init: assume audio data started 1s ago
            self.start_time = now - 1

        # cached data range since last call
        tail = int((now - self.start_time) * self.sample_rate) * \
            self.channels * self.bytes_per_sample
        data = self.audio_data[self.index:tail]
        if tail < self.audio_length:
            self.index = tail
        else:
            # loopback
            self.index = 0
            self.start_time = now
        return data


class AudioStreamPlayer(Node):
    def __init__(self):
        super().__init__('audio_stream_player')

        self.declare_parameter('raw_audio_path', '')
        self.raw_audio_path = self.get_parameter('raw_audio_path').value

        self.mic_device = FakeMicDevice(self.raw_audio_path)

        self.pkg_name = 'audio_stream_player{}'.format(os.getpid())
        self.focus = False
        self.focus_force = True

        self.get_logger().info(f'local pkg name: {self.pkg_name}')
        # Create focus request/release clients
        self.request_client = self.create_client(
            RequestAudioFocus,
            '/aimdk_5Fmsgs/srv/RequestAudioFocus')
        self.release_client = self.create_client(
            AbandonAudioFocus,
            '/aimdk_5Fmsgs/srv/AbandonAudioFocus')

        # Wait for the service to become available
        while not self.request_client.wait_for_service(timeout_sec=2.0):
            self.get_logger().info('⏳ Service unavailable, waiting...')
        self.get_logger().info('🟢 Service available, ready to send request.')

        # Create focus event subscriber and raw audio publisher
        focus_qos = QoSProfile(
            reliability=QoSReliabilityPolicy.RELIABLE,
            history=QoSHistoryPolicy.KEEP_LAST,
            depth=10,
            durability=QoSDurabilityPolicy.VOLATILE
        )
        self.sub = self.create_subscription(
            FocusResponse,
            '/aima/hal/audio/focus_response',
            self.focus_event_callback, focus_qos
        )
        self.pub = self.create_publisher(
            AudioPlayback,
            '/aima/hal/audio/playback', 10
        )

    def send_request(self, focus: bool):
        if focus:
            self.send_focus_request()
        else:
            self.send_focus_release()

    def send_focus_request(self):
        req = RequestAudioFocus.Request()

        requester = FocusRequester()
        requester.pkg_name = self.pkg_name
        requester.priority = 6

        req.focus_requester = requester

        self.get_logger().info('📨 Sending RequestAudioFocus request')
        for i in range(8):
            future = self.request_client.call_async(req)
            rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)

            if future.done():
                break

            # retry as remote peer is NOT handled well by ROS
            self.get_logger().info(f'trying ... [{i}]')

        response = future.result()
        if response is None:
            self.get_logger().error('❌ Service call failed or timed out.')
            return False

        if response.reponse.status.value == CommonState.SUCCESS:
            focus = response.focus_response.focus_gain
            self.focus = focus
            self.get_logger().info(f'✅ RequestAudioFocus done: focus {focus}')
            return True
        else:
            self.get_logger().error(
                f'❌ Failed in response of RequestAudioFocus: {response.reponse.message}'
            )
            return False

    def send_focus_release(self):
        req = AbandonAudioFocus.Request()

        requester = FocusRequester()
        requester.pkg_name = self.pkg_name

        req.focus_requester = requester

        self.get_logger().info('📨 Sending AbandonAudioFocus request')
        for i in range(8):
            future = self.release_client.call_async(req)
            rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)

            if future.done():
                break

            # retry as remote peer is NOT handled well by ROS
            self.get_logger().info(f'trying ... [{i}]')

        response = future.result()
        if response is None:
            self.get_logger().error('❌ Service call failed or timed out.')
            return False

        if response.reponse.status.value == CommonState.SUCCESS:
            focus = response.focus_response.focus_gain
            # always focus False
            self.focus = focus
            self.get_logger().info(f'✅ AbandonAudioFocus done: focus {focus}')
            return True
        else:
            self.get_logger().error(
                f'❌ Failed in response of AbandonAudioFocus: {response.reponse.message}'
            )
            return False

    def focus_event_callback(self, msg: FocusResponse):
        pkg_name = msg.pkg_name
        focus = msg.focus_gain
        if msg.pkg_name == self.pkg_name:
            self.get_logger().info(
                f'receive foucs out event: focus state: {focus}')
            self.focus_force = msg.focus_gain

    def is_holding_focus(self):
        return self.focus and self.focus_force

    def spin_untill_focus(self):
        while rclpy.ok():
            self.focus_force = True
            self.send_request(True)
            if self.is_holding_focus():
                return
            self.get_logger().info("need retry to get focus")
            t1 = t0 = time.monotonic()
            while rclpy.ok() and t1 - t0 < 1.0:
                rclpy.spin_once(self, timeout_sec=0.1)
                t1 = time.monotonic()
        return

    def run_once(self):
        """ flush raw audio data cached since last call """

        raw_audio_msg = AudioPlayback()

        raw_audio_msg.pkg_name = self.pkg_name
        raw_audio_msg.token_id = self.pkg_name
        # fill AudioInfo
        audio_info = AudioInfo()
        audio_info.channels = self.mic_device.get_channel_count()
        audio_info.sample_rate = self.mic_device.get_sample_rate()
        # fill raw audio data
        audio_data = AudioData()
        cached_data = self.mic_device.get_cached_data()
        audio_data.data = cached_data
        raw_audio_msg.info = audio_info
        raw_audio_msg.data = audio_data
        # publish
        self.pub.publish(raw_audio_msg)

    def run(self):
        self.get_logger().info('🟢 publishing audio data ...')
        while rclpy.ok() and self.is_holding_focus():
            self.run_once()
            rclpy.spin_once(self, timeout_sec=0.05)
            time.sleep(0.05)

        if not self.is_holding_focus():
            self.get_logger().info("focus out, exiting...")


def main(args=None):
    global global_node
    rclpy.init(args=args)
    node = None

    try:
        node = AudioStreamPlayer()

        global_node = node
        signal.signal(signal.SIGINT, signal_handler)
        signal.signal(signal.SIGTERM, signal_handler)

        node.spin_untill_focus()
        node.run()

    except KeyboardInterrupt:
        rclpy.logging.get_logger('main').info(
            "Interrupt signal received, exiting...")
    except Exception as e:
        rclpy.logging.get_logger('main').error(
            f'Program exited with exception: {e}')

    if node:
        node.destroy_node()
    if rclpy.ok():
        rclpy.shutdown()


if __name__ == '__main__':
    main()

Usage

# 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.25 Emoji (expression) 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

#!/usr/bin/env python3

import rclpy
import rclpy.logging
from rclpy.node import Node

from aimdk_msgs.srv import PlayEmoji


class PlayEmojiClient(Node):
    def __init__(self):
        super().__init__('play_emoji_client')
        self.client = self.create_client(
            PlayEmoji, '/face_ui_proxy/play_emoji')
        self.get_logger().info('✅ PlayEmoji client node created.')

        # Wait for the service to become available
        while not self.client.wait_for_service(timeout_sec=2.0):
            self.get_logger().info('⏳ Service unavailable, waiting...')

        self.get_logger().info('🟢 Service available, ready to send request.')

    def send_request(self, emoji: int, mode: int, priority: int):
        req = PlayEmoji.Request()

        req.emotion_id = int(emoji)
        req.mode = int(mode)
        req.priority = int(priority)

        self.get_logger().info(
            f'📨 Sending request to play emoji: id={emoji}, mode={mode}, priority={priority}')
        for i in range(8):
            req.header.header.stamp = self.get_clock().now().to_msg()
            future = self.client.call_async(req)
            rclpy.spin_until_future_complete(self, future, timeout_sec=0.25)

            if future.done():
                break

            # retry as remote peer is NOT handled well by ROS
            self.get_logger().info(f'trying ... [{i}]')

        resp = future.result()
        if resp is None:
            self.get_logger().error('❌ Service call not completed or timed out.')
            return False

        if resp.success:
            self.get_logger().info(
                f'✅ Emoji played successfully: {resp.message}')
            return True
        else:
            self.get_logger().error(f'❌ Failed to play emoji: {resp.message}')
            return False


def main(args=None):
    rclpy.init(args=args)
    node = None

    # Interactive input, same as the original C++ version
    try:
        emotion = int(
            input("Enter emoji ID: 1-blink, 60-bored, 70-abnormal, 80-sleeping, 90-happy ... 190-double angry, 200-adore: "))
        mode = int(input("Enter play mode (1: play once, 2: loop): "))
        if mode not in (1, 2):
            raise ValueError("invalid mode")
        priority = 10  # default priority

        node = PlayEmojiClient()
        node.send_request(emotion, mode, priority)
    except KeyboardInterrupt:
        pass
    except Exception as e:
        rclpy.logging.get_logger('main').error(
            f'Program exited with exception: {e}')

    if node:
        node.destroy_node()
    if rclpy.ok():
        rclpy.shutdown()


if __name__ == '__main__':
    main()

6.1.26 Slam

In this example, slam is used to send messages to the mapping service via this node to achieve mapping functionality.

import sys
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSReliabilityPolicy
from std_msgs.msg import String


class SlamCommandPublisher(Node):
    def __init__(self):
        super().__init__('slam_command_publisher')
        qos = QoSProfile(
            depth=10,
            durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
            reliability=QoSReliabilityPolicy.RELIABLE,
        )
        self.publisher_ = self.create_publisher(
            String, '/integrated_command', qos)

    def publish_start_mapping(self):
        msg = String()
        msg.data = 'start_mapping'
        self.get_logger().info(f'Publishing: "{msg.data}"')
        self.publisher_.publish(msg)

    def publish_stop_mapping(self, map_name):
        msg = String()
        msg.data = f'stop_mapping:{map_name}'
        self.get_logger().info(f'Publishing: "{msg.data}"')
        self.publisher_.publish(msg)


def main(args=None):
    rclpy.init(args=args)
    slam_command_publisher = SlamCommandPublisher()

    try:
        while rclpy.ok():
            input_value = input(
                "Enter 1 to start mapping, 2 to stop mapping: ")

            if input_value == '1':
                slam_command_publisher.publish_start_mapping()
            elif input_value == '2':
                map_name = input("Enter map name: ")
                slam_command_publisher.publish_stop_mapping(map_name)
            else:
                print("Invalid input. Please enter 1 or 2.")
    except KeyboardInterrupt:
        pass
    finally:
        slam_command_publisher.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()

Usage

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.27 Relocate

This example uses the map built in the Map Building step for localization.

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from std_msgs.msg import String
from geometry_msgs.msg import Pose
from nav_msgs.msg import Odometry
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSDurabilityPolicy


class RelocalizationNode(Node):
    def __init__(self):
        super().__init__('relocalization_node')

        # /integrated_command subscriber requires TRANSIENT_LOCAL durability
        command_qos = QoSProfile(
            depth=10,
            durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
        )

        # Create publishers
        self.integrated_command_pub = self.create_publisher(
            String, '/integrated_command', command_qos)
        self.relocalization_pose_pub = self.create_publisher(
            Pose, '/relocalization_pose', 10)

        # Create subscriber with BEST_EFFORT QoS
        lidar_loc_qos = QoSProfile(
            depth=10,
            reliability=QoSReliabilityPolicy.BEST_EFFORT
        )

        self.odometry_sub = self.create_subscription(
            Odometry,
            '/slam/lidar_odom',
            self.odometry_callback,
            qos_profile=lidar_loc_qos
        )

        # Setup timer to publish messages in sequence
        self.success_received = False
        self.timeout_timer = None

        # Wait for subscribers before publishing
        self.publish_timer = self.create_timer(0.2, self.wait_and_publish)

    def wait_and_publish(self):
        if (self.integrated_command_pub.get_subscription_count() == 0 or
                self.relocalization_pose_pub.get_subscription_count() == 0):
            self.get_logger().info(
                f'Waiting for subscribers... '
                f'(command: {self.integrated_command_pub.get_subscription_count()}, '
                f'pose: {self.relocalization_pose_pub.get_subscription_count()})',
                throttle_duration_sec=2.0)
            return
        self.publish_timer.cancel()
        self.publish_sequence()

    def publish_sequence(self):
        # Publish integrated_command
        integrated_command_msg = String()
        integrated_command_msg.data = 'start_relocalization:1778133306741'
        self.integrated_command_pub.publish(integrated_command_msg)
        self.get_logger().info('Published integrated_command')

        # Schedule relocalization_pose publication after delay
        self.relocalization_pose_timer = self.create_timer(
            1.0,
            self.publish_relocalization_pose
        )

    def publish_relocalization_pose(self):
        # Cancel this timer immediately so it only fires once
        self.relocalization_pose_timer.cancel()

        relocalization_pose_msg = Pose()
        relocalization_pose_msg.position.x = 1041.0
        relocalization_pose_msg.position.y = 630.0
        relocalization_pose_msg.position.z = 0.0
        relocalization_pose_msg.orientation.x = 0.0
        relocalization_pose_msg.orientation.y = 0.0
        relocalization_pose_msg.orientation.z = 0.0
        relocalization_pose_msg.orientation.w = 1.0

        self.relocalization_pose_pub.publish(relocalization_pose_msg)
        self.get_logger().info('Published relocalization_pose')

        # Start timeout timer (30 seconds)
        self.timeout_timer = self.create_timer(
            30.0,
            self.timeout_callback
        )

        self.get_logger().info('Waiting for robot pose data (timeout: 30s)...')

    def odometry_callback(self, msg):
        if not self.success_received:
            self.success_received = True
            if self.timeout_timer is not None:
                self.timeout_timer.cancel()
            self.get_logger().info('Received odometry data - Relocalization successful!')
            if rclpy.ok():
                rclpy.shutdown()

    def timeout_callback(self):
        if not self.success_received:
            self.get_logger().error('Timeout reached - Relocalization failed!')
            if rclpy.ok():
                rclpy.shutdown()


def main(args=None):
    rclpy.init(args=args)
    node = RelocalizationNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()

Usage

ros2 run py_examples relocate 
[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: 5s)...
[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.28 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.

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from rclpy.executors import SingleThreadedExecutor
from aimdk_msgs.srv import GetStoredMapByName
from std_msgs.msg import Header


class MapClient(Node):
    def __init__(self):
        super().__init__('get_map_client')
        self.client = self.create_client(
            GetStoredMapByName,
            '/aimdk_5Fmsgs/srv/GetStoredMapByName'
        )

    def send_request(self, map_name):
        while not self.client.wait_for_service(timeout_sec=1.0):
            if not rclpy.ok():
                self.get_logger().error("Interrupted while waiting for service.")
                return
            self.get_logger().info("Service not available, waiting again...")

        request = GetStoredMapByName.Request()

        request.header = Header()
        request.header.stamp = self.get_clock().now().to_msg()
        request.header.frame_id = ""

        request.map_name = map_name

        future = self.client.call_async(request)

        executor = SingleThreadedExecutor()
        executor.add_node(self)
        executor.spin_until_future_complete(future, timeout_sec=60.0)
        executor.remove_node(self)

        if future.result() is None:
            self.get_logger().error("Service call failed or timed out.")
            return

        result = future.result()

        if result.code == 0:
            self.get_logger().info("Service call succeeded")
            self.get_logger().info(f"Map path: {result.map_path}")
            self.get_logger().info(
                f"Map resolution: {result.map_info.resolution}")
            self.get_logger().info(f"Map width: {result.map_info.width}")
            self.get_logger().info(f"Map height: {result.map_info.height}")
            self.get_logger().info(f"Map id: {result.map_id}")
        else:
            self.get_logger().error(
                f"Service call failed with code: {result.code}")


def main(args=None):
    rclpy.init(args=args)

    client = MapClient()
    try:
        client.send_request("map_name")
    except KeyboardInterrupt:
        pass
    finally:
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()

Usage

ros2 run py_examples get_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: 1

The example requests the map named test_map by default. Modify the argument to send_request in the code to request a different map.

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.29 Play Linkcraft Action

In this example, play_linkcraft is used to list the LinkCraft actions available on the robot and select one for playback.

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from aimdk_msgs.srv import GetRobotResources, ExecuteActionResource


class RobotResourceClient(Node):
    def __init__(self):
        super().__init__('robot_resource_client')

        self.get_resource_client = self.create_client(
            GetRobotResources,
            '/aimdk_5Fmsgs/srv/GetRobotResources'
        )
        self.play_resource_client = self.create_client(
            ExecuteActionResource,
            '/aimdk_5Fmsgs/srv/ExecuteActionResource'
        )

        self.resources = []

    def get_robot_resources(self):
        while not self.get_resource_client.wait_for_service(timeout_sec=1.0):
            if not rclpy.ok():
                self.get_logger().error("Interrupted while waiting for the service. Exiting.")
                return False
            self.get_logger().info("Service not available, waiting again...")

        request = GetRobotResources.Request()
        future = self.get_resource_client.call_async(request)
        rclpy.spin_until_future_complete(self, future)

        if future.result() is None:
            self.get_logger().error("Failed to call GetRobotResources")
            return False

        response = future.result()
        self.resources = list(response.robot_resources)
        self.get_logger().info(f"Found {len(self.resources)} resources")
        return True

    def print_resource_menu(self):
        if not self.resources:
            print("(no resources)")
            return

        print("\n=== Available Robot Resources ===")
        for idx, resource in enumerate(self.resources):
            ver = resource.current_version
            print(f"[{idx}] {ver.name or '<no name>'}")
            print(f"     key:     {resource.resource_key}")
            print(f"     version: {ver.version}")
        print("=================================\n")

    def pick_resource(self):
        if not self.resources:
            return None

        while True:
            try:
                raw = input(
                    f"Select resource index [0-{len(self.resources) - 1}] (q to quit): ").strip()
            except EOFError:
                return None
            if raw.lower() in ("q", "quit", "exit"):
                return None
            if not raw.isdigit():
                print("Please enter a number.")
                continue
            idx = int(raw)
            if 0 <= idx < len(self.resources):
                return self.resources[idx]
            print(f"Index out of range (0-{len(self.resources) - 1}).")

    def play_resource(self, resource):
        while not self.play_resource_client.wait_for_service(timeout_sec=1.0):
            if not rclpy.ok():
                self.get_logger().error("Interrupted while waiting for service.")
                return
            self.get_logger().info("Service not available, waiting...")

        request = ExecuteActionResource.Request()
        request.resource_key = resource.resource_key
        request.resource_version = resource.current_version.version

        if "onnx" in request.resource_key:
            request.meta = '{"resource_type": "BODY_MONTION"}'
        else:
            request.meta = '{"resource_type": "ARM_MONTION"}'

        self.get_logger().info("Sending request:")
        self.get_logger().info(f"  resource_key: {request.resource_key}")
        self.get_logger().info(
            f"  resource_version: {request.resource_version}")
        self.get_logger().info(f"  meta: {request.meta}")

        future = self.play_resource_client.call_async(request)
        rclpy.spin_until_future_complete(self, future)

        if future.result() is not None:
            self.handle_response(future.result())
        else:
            self.get_logger().error("Failed to call ExecuteActionResource")

    def handle_response(self, response):
        self.get_logger().info("Service response received:")
        self.get_logger().info("Response Header:")
        self.get_logger().info(
            f"  stamp: {response.header.header.stamp.sec}.{response.header.header.stamp.nanosec:09}")
        self.get_logger().info(f"  code: {response.header.header.code}")
        self.get_logger().info("Status:")
        self.get_logger().info(f"  value: {response.header.status.value}")
        self.get_logger().info(f"  message: {response.header.message}")


def main(args=None):
    rclpy.init(args=args)
    client_node = None
    try:
        client_node = RobotResourceClient()
        if not client_node.get_robot_resources():
            return
        client_node.print_resource_menu()
        chosen = client_node.pick_resource()
        if chosen is None:
            client_node.get_logger().info("No resource selected, exiting.")
            return
        client_node.play_resource(chosen)
    except KeyboardInterrupt:
        pass
    finally:
        if client_node:
            client_node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()

Usage

ros2 run py_examples play_linkcraft
[INFO] [1772526701.308025739] [robot_resource_client]: Service call succeeded
[INFO] [1772526701.308253238] [robot_resource_client]: Robot Resources (0):
[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.30 LED light 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:

#!/usr/bin/env python3

import sys
import rclpy
import rclpy.logging
from rclpy.node import Node

from aimdk_msgs.srv import SetPmuLed


class PlayLightsClient(Node):
    def __init__(self):
        super().__init__('play_lights_client')

        # create service client
        self.client = self.create_client(
            SetPmuLed, '/aimdk_5Fmsgs/srv/SetPmuLed')

        self.get_logger().info('✅ PlayLights client node created.')

        # Wait for the service to become available
        while not self.client.wait_for_service(timeout_sec=2.0):
            self.get_logger().info('⏳ Service unavailable, waiting...')

        self.get_logger().info('🟢 Service available, ready to send request.')

    def send_request(self, led_mode, r, g, b, priority=6, reset_priority=False):
        """Send LED control request"""
        # create request
        request = SetPmuLed.Request()
        request.led_strip_mode = led_mode
        request.r = r
        request.g = g
        request.b = b
        request.priority = priority
        request.reset_priority = reset_priority
        request.trace_id = 'play_lights'

        # send request
        # Note: LED strip is slow to response (up to ~5s)
        self.get_logger().info(
            f'📨 Sending request to control led strip: mode={led_mode}, RGB=({r}, {g}, {b}), priority={priority}, reset_priority={reset_priority}')
        for i in range(4):
            request.request.header.stamp = self.get_clock().now().to_msg()
            future = self.client.call_async(request)
            rclpy.spin_until_future_complete(self, future, timeout_sec=5)

            if future.done():
                break

            # retry as remote peer is NOT handled well by ROS
            self.get_logger().info(f'trying ... [{i}]')

        response = future.result()
        if response is None:
            self.get_logger().error('❌ Service call not completed or timed out.')
            return False

        if response.status_code == 0:
            self.get_logger().info('✅ LED strip command sent successfully.')
            return True
        else:
            self.get_logger().error(
                f'❌ LED strip command failed with status: {response.status_code}')
            return False


def main(args=None):
    rclpy.init(args=args)
    node = None

    try:
        # get command line args
        if len(sys.argv) > 6:
            # use CLI args
            led_mode = int(sys.argv[1])
            if led_mode not in (0, 1, 2, 3):
                raise ValueError("invalid mode")
            r = int(sys.argv[2])
            if r < 0 or r > 255:
                raise ValueError("invalid R value")
            g = int(sys.argv[3])
            if g < 0 or g > 255:
                raise ValueError("invalid G value")
            b = int(sys.argv[4])
            if b < 0 or b > 255:
                raise ValueError("invalid B value")
            priority = int(sys.argv[5])
            if priority < 0:
                raise ValueError("invalid priority value")
            reset_priority = sys.argv[6].lower() in ('true', '1', 'yes')
        else:
            # interactive input
            print("=== LED strip control example ===")
            print("Select LED strip mode:")
            print("0 - Steady on")
            print("1 - Breathing (4s cycle, sine brightness)")
            print("2 - Blinking (1s cycle, 0.5s on, 0.5s off)")
            print("3 - Flowing (2s cycle, light up from left to right)")

            led_mode = int(input("Enter mode (0-3): "))
            if led_mode not in (0, 1, 2, 3):
                raise ValueError("invalid mode")

            print("\nSet RGB color values (0-255):")
            r = int(input("Red (R): "))
            if r < 0 or r > 255:
                raise ValueError("invalid R value")
            g = int(input("Green (G): "))
            if g < 0 or g > 255:
                raise ValueError("invalid G value")
            b = int(input("Blue (B): "))
            if b < 0 or b > 255:
                raise ValueError("invalid B value")

            print("\nSet priority (higher value = higher priority, default=6):")
            priority = int(input("Priority: "))
            if priority < 0:
                raise ValueError("invalid priority value")

            reset_priority = input(
                "Reset priority after command? (y/n, default=n): ").strip().lower() == 'y'

        node = PlayLightsClient()
        node.send_request(led_mode, r, g, b, priority, reset_priority)
    except KeyboardInterrupt:
        pass
    except Exception as e:
        rclpy.logging.get_logger('main').error(
            f'Program exited with exception: {e}')

    if node:
        node.destroy_node()
    if rclpy.ok():
        rclpy.shutdown()


if __name__ == '__main__':
    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

Example output:

=== 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 four LED display modes

• Custom RGB color configuration

• Priority control (higher value = higher priority)

• Priority reset support

• Synchronous service invocation

• Command-line parameter support

• Input parameter validation

• User-friendly interactive interface