6.2 C++ Interface Usage Example

This section will guide you through implementing the functions listed in the index

Build & Run Instructions

• Enter the top-level AimDK directory (~/aimdk on the robot) and run the following commands

  source /opt/ros/humble/setup.bash
  colcon build
  source install/local_setup.bash
  ros2 run examples 'function name, e.g., get_mc_action'
  

📝 Code Notes

The full implementation includes complete mechanisms for error handling, signal handling, timeout handling, and more, ensuring the robustness of the program. Please check/modify in the examples directory

Caution

As standard ROS DO NOT handle cross-host service (request-response) well, please refer to SDK examples to use open interfaces in a robust way (with protection mechanisms e.g. exception safety and retransmission)

While the robot is in Stable Standing Mode or Locomotion Mode, DO NOT launch ROS nodes in rapid bulk (no more than 2 nodes per second is recommended), as a large number of nodes joining DDS discovery within a short period causes communication congestion and degrades motion control real-time performance, which may cause the robot to lose balance and fall

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

#include <algorithm>
#include <chrono>
#include <iostream>
#include <memory>
#include <string>

#include "aimdk_msgs/srv/get_system_state.hpp"
#include "aimdk_msgs/srv/migrate_system_state.hpp"
#include "rclcpp/rclcpp.hpp"

using namespace std::chrono_literals;

/**
 * @class SystemStateMigrator
 * @brief ROS2 client for migrating system state to any target state
 */
class SystemStateMigrator : public rclcpp::Node {
public:
  /**
   * @brief Constructor for SystemStateMigrator
   * @param target_state The target system state to migrate to
   */
  SystemStateMigrator(const std::string &target_state)
      : Node("system_state_migrator"), target_state_(target_state) {
    // Create service clients
    migrate_client_ = this->create_client<aimdk_msgs::srv::MigrateSystemState>(
        "/aimdk_5Fmsgs/srv/MigrateSystemState");

    get_state_client_ = this->create_client<aimdk_msgs::srv::GetSystemState>(
        "/aimdk_5Fmsgs/srv/GetSystemState");

    // Log target state
    RCLCPP_INFO(this->get_logger(), "Target state: %s", target_state_.c_str());

    // Start the migration process
    start_migration_process();
  }

private:
  /**
   * @brief Start the migration process
   */
  void start_migration_process() {
    // Create a timer to check service availability
    check_timer_ = this->create_wall_timer(
        1s, std::bind(&SystemStateMigrator::check_services_and_start, this));
  }

  /**
   * @brief Check if services are available and start migration
   */
  void check_services_and_start() {
    // Check if both services are available
    if (migrate_client_->service_is_ready() &&
        get_state_client_->service_is_ready()) {
      // Services are available, cancel timer and start migration
      check_timer_->cancel();
      RCLCPP_INFO(this->get_logger(),
                  "Both services are available, starting migration to %s...",
                  target_state_.c_str());
      migrate_to_target_state();
    } else {
      RCLCPP_INFO(this->get_logger(), "Waiting for services...");
    }
  }

  /**
   * @brief Migrate system state to target state with retry
   */
  void migrate_to_target_state() {
    if (migrate_pending_) {
      return;
    }
    if (migrate_retry_count_ >= max_migrate_retries_) {
      RCLCPP_ERROR(this->get_logger(),
                   "Migration request failed after %d attempts",
                   max_migrate_retries_);
      rclcpp::shutdown();
      return;
    }

    auto request =
        std::make_shared<aimdk_msgs::srv::MigrateSystemState::Request>();

    auto now = this->now();
    request->header.header.stamp.sec = now.seconds();
    request->header.header.stamp.nanosec = now.nanoseconds() % 1000000000;

    request->state = target_state_;

    migrate_retry_count_++;
    migrate_pending_ = true;
    uint64_t req_id = ++migrate_request_id_;

    RCLCPP_INFO(this->get_logger(),
                "Sending migration request to %s state (attempt %d/%d)...",
                target_state_.c_str(), migrate_retry_count_,
                max_migrate_retries_);

    migrate_timeout_timer_ = this->create_wall_timer(30s, [this, req_id]() {
      if (req_id != migrate_request_id_) {
        return;
      }
      migrate_timeout_timer_.reset();
      migrate_pending_ = false;
      RCLCPP_WARN(this->get_logger(), "Migration request timeout, retrying...");
      migrate_to_target_state();
    });

    auto future = migrate_client_->async_send_request(
        request,
        [this, req_id](
            rclcpp::Client<aimdk_msgs::srv::MigrateSystemState>::SharedFuture
                future) {
          if (req_id != migrate_request_id_) {
            return;
          }
          if (migrate_timeout_timer_) {
            migrate_timeout_timer_->cancel();
            migrate_timeout_timer_.reset();
          }
          migrate_pending_ = false;

          try {
            auto response = future.get();
            if (response->header.status.value ==
                aimdk_msgs::msg::CommonState::SUCCESS) { // SUCCESS (1)
              migrate_retry_count_ = 0;
              ++migrate_request_id_;
              RCLCPP_INFO(this->get_logger(),
                          "Migration request accepted, starting to monitor "
                          "state...");
              start_state_monitoring();
            } else {
              ++migrate_request_id_;
              RCLCPP_ERROR(this->get_logger(),
                           "Migration request failed with status: %d, message: "
                           "%s",
                           response->header.status.value,
                           response->header.message.c_str());
              rclcpp::shutdown();
            }
          } catch (const std::exception &e) {
            ++migrate_request_id_;
            RCLCPP_ERROR(this->get_logger(),
                         "Migration service call failed: %s", e.what());
            rclcpp::shutdown();
          }
        });
  }

  /**
   * @brief Start monitoring system state
   */
  void start_state_monitoring() {
    RCLCPP_INFO(this->get_logger(), "Starting to monitor system state...");

    // Create a timer to periodically check system state
    monitor_timer_ = this->create_wall_timer(
        1s, std::bind(&SystemStateMigrator::check_system_state, this));
  }

  /**
   * @brief Check current system state with retry
   */
  void check_system_state() {
    if (get_state_pending_) {
      return;
    }
    if (get_state_retry_count_ >= max_get_state_retries_) {
      RCLCPP_ERROR(this->get_logger(),
                   "Get system state failed after %d attempts, will retry on "
                   "next monitor cycle",
                   max_get_state_retries_);
      get_state_retry_count_ = 0;
      return;
    }

    auto request = std::make_shared<aimdk_msgs::srv::GetSystemState::Request>();

    auto now = this->now();
    request->header.header.stamp.sec = now.seconds();
    request->header.header.stamp.nanosec = now.nanoseconds() % 1000000000;

    get_state_retry_count_++;
    get_state_pending_ = true;
    uint64_t req_id = ++get_state_request_id_;

    get_state_timeout_timer_ = this->create_wall_timer(10s, [this, req_id]() {
      if (req_id != get_state_request_id_) {
        return;
      }
      get_state_timeout_timer_.reset();
      get_state_pending_ = false;
      RCLCPP_WARN(this->get_logger(),
                  "GetSystemState request timeout (attempt %d/%d), "
                  "retrying...",
                  get_state_retry_count_, max_get_state_retries_);
      check_system_state();
    });

    auto future = get_state_client_->async_send_request(
        request,
        [this,
         req_id](rclcpp::Client<aimdk_msgs::srv::GetSystemState>::SharedFuture
                     future) {
          if (req_id != get_state_request_id_) {
            return;
          }
          if (get_state_timeout_timer_) {
            get_state_timeout_timer_->cancel();
            get_state_timeout_timer_.reset();
          }
          get_state_pending_ = false;

          try {
            auto response = future.get();
            get_state_retry_count_ = 0;
            ++get_state_request_id_;

            RCLCPP_INFO(
                this->get_logger(), "Current State: %s, System Status: %u",
                response->cur_state.c_str(), response->curr_status.value);

            if (is_migration_complete(response)) {
              RCLCPP_INFO(this->get_logger(),
                          "Migration to %s completed successfully!",
                          target_state_.c_str());
              monitor_timer_->cancel();
              rclcpp::shutdown();
            } else {
              RCLCPP_INFO(this->get_logger(),
                          "Migration in progress, will check again in 1 "
                          "second...");
            }
          } catch (const std::exception &e) {
            RCLCPP_ERROR(this->get_logger(),
                         "GetSystemState service call failed: %s", e.what());
          }
        });
  }

  /**
   * @brief Check if migration to target state is complete
   * @param response Response from GetSystemState service
   * @return true if migration is complete, false otherwise
   */
  bool is_migration_complete(
      const aimdk_msgs::srv::GetSystemState::Response::SharedPtr response) {
    // Convert current state and target state to lowercase for case-insensitive
    // comparison
    std::string current_state_lower = response->cur_state;
    std::transform(current_state_lower.begin(), current_state_lower.end(),
                   current_state_lower.begin(), ::tolower);

    std::string target_state_lower = target_state_;
    std::transform(target_state_lower.begin(), target_state_lower.end(),
                   target_state_lower.begin(), ::tolower);

    // Check conditions:
    // 1. Current state equals target state (case-insensitive)
    //    Special case: Ready auto-transitions to Business, so also accept
    //    Business when the target is Ready
    // 2. System status equals 1 (IN_READY), or the target is Ready and
    //    the system has already auto-transitioned to Business (IN_MOVE)
    bool state_match =
        (current_state_lower == target_state_lower) ||
        (target_state_lower == "ready" && current_state_lower == "business");
    bool ready_auto_transitioned =
        (target_state_lower == "ready" && current_state_lower == "business");
    bool status_match = ready_auto_transitioned ||
                        (response->curr_status.value ==
                         aimdk_msgs::msg::SystemStatus::IN_READY); // READY (1)

    if (state_match && status_match) {
      return true;
    }

    // Log detailed information if not complete
    if (!state_match) {
      RCLCPP_INFO(this->get_logger(),
                  "State mismatch: current='%s', expected='%s'",
                  response->cur_state.c_str(), target_state_.c_str());
    }
    if (!status_match) {
      RCLCPP_INFO(this->get_logger(),
                  "Status mismatch: current= %u, expected=1 (IN_READY)",
                  response->curr_status.value);
    }
    RCLCPP_ERROR(this->get_logger(), "State Migrate Fail");

    return false;
  }

  // Target state to migrate to
  std::string target_state_;

  // Service clients
  rclcpp::Client<aimdk_msgs::srv::MigrateSystemState>::SharedPtr
      migrate_client_;
  rclcpp::Client<aimdk_msgs::srv::GetSystemState>::SharedPtr get_state_client_;

  // Timers
  rclcpp::TimerBase::SharedPtr check_timer_;
  rclcpp::TimerBase::SharedPtr monitor_timer_;

  // Retry counters and timeout timers for migration
  int migrate_retry_count_ = 0;
  const int max_migrate_retries_ = 5;
  uint64_t migrate_request_id_ = 0;
  bool migrate_pending_ = false;
  rclcpp::TimerBase::SharedPtr migrate_timeout_timer_;

  // Retry counters and timeout timers for get state
  int get_state_retry_count_ = 0;
  const int max_get_state_retries_ = 3;
  uint64_t get_state_request_id_ = 0;
  bool get_state_pending_ = false;
  rclcpp::TimerBase::SharedPtr get_state_timeout_timer_;
};

/**
 * @brief Print usage information with detailed mode descriptions
 */
void print_usage(const char *program_name) {
  std::cout << "Usage: " << program_name << " <target_state>" << std::endl;
  std::cout << std::endl;
  std::cout << "Available target states:" << std::endl;
  std::cout << "  " << program_name << " Ready" << std::endl;
  std::cout << "  " << program_name << " Develop_Nav" << std::endl;
  std::cout << "  " << program_name << " Develop_Audio_Linux" << std::endl;
  std::cout << "  " << program_name << " Develop_Audio_ROS" << std::endl;
  std::cout << "  " << program_name << " Develop_MC" << std::endl;
  std::cout << std::endl;

  std::cout << "Mode Descriptions:" << std::endl;
  std::cout << "==================" << std::endl;

  // Ready mode description
  std::cout << "1. Ready - System Default Mode" << std::endl;
  std::cout
      << "   • Description: System enters this mode by default after startup"
      << std::endl;
  std::cout << "   • Purpose: Normal operation mode, ready for general tasks"
            << std::endl;
  std::cout
      << "   • Use case: Return to normal operation after development work"
      << std::endl;
  std::cout << std::endl;

  // Develop_Nav mode description
  std::cout << "2. Develop_Nav - Navigation Development Mode" << std::endl;
  std::cout
      << "   • Description: Special mode for navigation system development"
      << std::endl;
  std::cout << "   • Purpose: Enables navigation-related debugging and testing"
            << std::endl;
  std::cout << "   • Use case: When developing or testing navigation "
               "algorithms, SLAM, path planning"
            << std::endl;
  std::cout << "   • Features: May provide access to raw sensor data, "
               "navigation debug topics"
            << std::endl;
  std::cout << std::endl;

  // Develop_Audio_Linux mode description
  std::cout << "3. Develop_Audio_Linux - System-Level Audio Development Mode"
            << std::endl;
  std::cout << "   • Description: Mode for low-level audio system development"
            << std::endl;
  std::cout << "   • Purpose: Access to system audio streams at Linux level"
            << std::endl;
  std::cout << "   • Use case: When developing audio drivers, audio processing "
               "at system level"
            << std::endl;
  std::cout << "   • Features: Direct access to audio hardware, "
               "ALSA/PulseAudio interfaces"
            << std::endl;
  std::cout << std::endl;

  // Develop_Audio_ROS mode description
  std::cout << "4. Develop_Audio_ROS - ROS Audio Development Mode" << std::endl;
  std::cout
      << "   • Description: Mode for ROS-based audio application development"
      << std::endl;
  std::cout << "   • Purpose: Access to audio data through ROS topics"
            << std::endl;
  std::cout << "   • Use case: When developing ROS nodes that process audio, "
               "speech recognition"
            << std::endl;
  std::cout << "   • Features: Audio data published as ROS topics, ROS message "
               "interfaces"
            << std::endl;
  std::cout << std::endl;

  // Develop_MC mode description
  std::cout << "5. Develop_MC - Motion Control Development Mode" << std::endl;
  std::cout << "   • Description: Mode for motion control system development"
            << std::endl;
  std::cout
      << "   • Purpose: Enables direct control and testing of motion systems"
      << std::endl;
  std::cout << "   • Use case: When developing motor control algorithms, "
               "testing motion hardware"
            << std::endl;
  std::cout << "   • Features: Low-level motor control access, motion system "
               "debugging tools"
            << std::endl;
  std::cout << std::endl;

  std::cout << "Program Workflow:" << std::endl;
  std::cout << "=================" << std::endl;
  std::cout
      << "1. Calls MigrateSystemState service with the specified target state"
      << std::endl;
  std::cout << "2. Continuously monitors GetSystemState service every second"
            << std::endl;
  std::cout << "3. Exits when both conditions are met:" << std::endl;
  std::cout << "   • Current State equals target state (case-insensitive)"
            << std::endl;
  std::cout << "   • System Status equals 1 (IN_READY)" << std::endl;
  std::cout << std::endl;
  std::cout << "Note: The program will automatically exit when migration is "
               "complete or on error."
            << std::endl;
}

/**
 * @brief Validate if the target state is a known development mode
 * @param state The state to validate
 * @return true if state is valid, false otherwise
 */
bool is_valid_state(const std::string &state) {
  // List of known valid states
  const std::vector<std::string> valid_states = {
      "Ready", "Develop_Nav", "Develop_Audio_Linux", "Develop_Audio_ROS",
      "Develop_MC"};

  // Convert input to lowercase for case-insensitive comparison
  std::string state_lower = state;
  std::transform(state_lower.begin(), state_lower.end(), state_lower.begin(),
                 ::tolower);

  // Check if state is in the list of valid states
  for (const auto &valid_state : valid_states) {
    std::string valid_state_lower = valid_state;
    std::transform(valid_state_lower.begin(), valid_state_lower.end(),
                   valid_state_lower.begin(), ::tolower);

    if (state_lower == valid_state_lower) {
      return true;
    }
  }

  return false;
}

/**
 * @brief Main function
 * @param argc Argument count
 * @param argv Argument vector
 * @return Exit code
 */
int main(int argc, char **argv) {
  // Check command line arguments
  if (argc != 2) {
    print_usage(argv[0]);
    return 1;
  }

  // Get target state from command line argument
  std::string target_state = argv[1];

  // Validate target state
  if (target_state.empty()) {
    std::cerr << "Error: Target state cannot be empty" << std::endl;
    print_usage(argv[0]);
    return 1;
  }

  // Optional: Validate against known states
  if (!is_valid_state(target_state)) {
    std::cout << "Warning: '" << target_state
              << "' is not in the list of known states." << std::endl;
    std::cout << "The program will still attempt to migrate to this state."
              << std::endl;
    std::cout << "Continue? (y/n): ";

    std::string response;
    std::getline(std::cin, response);

    if (response != "y" && response != "Y") {
      std::cout << "Operation cancelled." << std::endl;
      return 0;
    }
  }

  // Initialize ROS2
  rclcpp::init(argc, argv);

  // Create migrator node with target state
  auto node = std::make_shared<SystemStateMigrator>(target_state);

  // Create single-threaded executor
  auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();

  // Add node to executor
  executor->add_node(node);

  // Run executor (blocking call)
  executor->spin();

  // Cleanup
  executor->remove_node(node);
  rclcpp::shutdown();

  return 0;
}

Usage Instructions

ros2 run examples migrate_system_state Develop_MC

Output Example

[INFO] [1770125761.202173770] [system_state_migrator]: Target state: Develop_MC
[INFO] [1770125762.202825297] [system_state_migrator]: Both services are available, starting migration to Develop_MC...
[INFO] [1770125762.202949414] [system_state_migrator]: Sending migration request to Develop_MC state (attempt 1/5)...
[INFO] [1770125762.264580770] [system_state_migrator]: Migration request accepted, starting to monitor state...
[INFO] [1770125762.264608479] [system_state_migrator]: Starting to monitor system state...
[INFO] [1770125763.268422431] [system_state_migrator]: Current State: Business, System Status: 2
[INFO] [1770125763.268513271] [system_state_migrator]: State mismatch: current='Business', expected='Develop_MC'
[INFO] [1770125763.268531394] [system_state_migrator]: Status mismatch: current= 2, expected=1 (IN_READY)
[INFO] [1770125763.268545617] [system_state_migrator]: Migration in progress, will check again in 1 second...
[INFO] [1770125764.266388052] [system_state_migrator]: Current State: Business, System Status: 2
[INFO] [1770125764.266492040] [system_state_migrator]: State mismatch: current='Business', expected='Develop_MC'
[INFO] [1770125764.266509640] [system_state_migrator]: Status mismatch: current= 2, expected=1 (IN_READY)
[INFO] [1770125764.266532986] [system_state_migrator]: Migration in progress, will check again in 1 second...
[INFO] [1770125765.266452308] [system_state_migrator]: Current State: Develop_MC, System Status: 1
[INFO] [1770125765.266562888] [system_state_migrator]: Migration to Develop_MC completed successfully!

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

#include "aimdk_msgs/srv/get_mc_action.hpp"
#include "aimdk_msgs/msg/common_request.hpp"
#include "aimdk_msgs/msg/response_header.hpp"
#include "rclcpp/rclcpp.hpp"
#include <chrono>
#include <memory>
#include <signal.h>

// Global variable used for signal handling
std::shared_ptr<rclcpp::Node> g_node = nullptr;

// Signal handler function
void signal_handler(int signal) {
  if (g_node) {
    RCLCPP_INFO(g_node->get_logger(), "Received signal %d, shutting down...",
                signal);
    g_node.reset();
  }
  rclcpp::shutdown();
  exit(signal);
}

class GetMcActionClient : public rclcpp::Node {
public:
  GetMcActionClient() : Node("get_mc_action_client") {

    client_ = this->create_client<aimdk_msgs::srv::GetMcAction>(
        "/aimdk_5Fmsgs/srv/GetMcAction"); // correct the service path
    RCLCPP_INFO(this->get_logger(), "✅ GetMcAction client node created.");

    while (!client_->wait_for_service(std::chrono::seconds(2))) {
      RCLCPP_INFO(this->get_logger(), "⏳ Service unavailable, waiting...");
    }
    RCLCPP_INFO(this->get_logger(),
                "🟢 Service available, ready to send request.");
  }

  void send_request() {
    try {
      auto request = std::make_shared<aimdk_msgs::srv::GetMcAction::Request>();
      request->request = aimdk_msgs::msg::CommonRequest();

      RCLCPP_INFO(this->get_logger(), "📨 Sending request to get robot mode");

      // Set a service call timeout
      const std::chrono::milliseconds timeout(250);
      for (int i = 0; i < 8; i++) {
        request->request.header.stamp = this->now();
        auto future = client_->async_send_request(request);
        auto retcode = rclcpp::spin_until_future_complete(shared_from_this(),
                                                          future, timeout);
        if (retcode != rclcpp::FutureReturnCode::SUCCESS) {
          // retry as remote peer is NOT handled well by ROS
          RCLCPP_INFO(this->get_logger(), "trying ... [%d]", i);
          continue;
        }
        // future.done
        auto response = future.get();
        RCLCPP_INFO(this->get_logger(), "✅ Robot mode get successfully.");
        RCLCPP_INFO(this->get_logger(), "Mode name: %s",
                    response->info.action_desc.c_str());
        RCLCPP_INFO(this->get_logger(), "Mode status: %d",
                    response->info.status.value);
        return;
      }
      RCLCPP_ERROR(this->get_logger(), "❌ Service call failed or timed out.");
    } catch (const std::exception &e) {
      RCLCPP_ERROR(this->get_logger(), "Exception occurred: %s", e.what());
    }
  }

private:
  rclcpp::Client<aimdk_msgs::srv::GetMcAction>::SharedPtr client_;
};

int main(int argc, char *argv[]) {
  try {
    rclcpp::init(argc, argv);

    // Set up signal handlers
    signal(SIGINT, signal_handler);
    signal(SIGTERM, signal_handler);

    // Create node
    g_node = std::make_shared<GetMcActionClient>();
    auto client = std::dynamic_pointer_cast<GetMcActionClient>(g_node);
    if (client) {
      client->send_request();
    }

    // Clean up resources
    g_node.reset();
    rclcpp::shutdown();

    return 0;
  } catch (const std::exception &e) {
    RCLCPP_ERROR(rclcpp::get_logger("main"),
                 "Program exited with exception: %s", e.what());
    return 1;
  }
}

Usage Instructions

ros2 run examples get_mc_action

Output Example

[INFO] [1779173975.378396584] [get_mc_action_client]: ✅ GetMcAction client node created.
[INFO] [1779173975.961720731] [get_mc_action_client]: 🟢 Service available, ready to send request.
[INFO] [1779173975.961868725] [get_mc_action_client]: 📨 Sending request to get robot mode
[INFO] [1779173975.965180630] [get_mc_action_client]: ✅ Robot mode get successfully.
[INFO] [1779173975.965282450] [get_mc_action_client]: Mode name: PASSIVE_DEFAULT
[INFO] [1779173975.965307345] [get_mc_action_client]: Mode status: 100

Interface Reference

• Service: /aimdk_5Fmsgs/srv/GetMcAction

• Message: aimdk_msgs/srv/GetMcAction

6.2.3 Set Robot Mode

This example uses the SetMcAction service. After running the node, enter the corresponding field value of the mode in the terminal, and the robot will immediately switch to the appropriate motion mode.
Before switching to the Stable Standing mode (STAND_DEFAULT), ensure the robot is standing and its feet are already on the ground.
The motion mode switching must follow its state transition diagram, other transitions will be rejected
Locomotion Mode (LOCOMOTION_DEFAULT) and Stable Standing Mode (STAND_DEFAULT) are unified and will auto switch internally, so switching manually to the nearer one is enough

#include "aimdk_msgs/srv/set_mc_action.hpp"
#include "aimdk_msgs/msg/common_response.hpp"
#include "aimdk_msgs/msg/common_state.hpp"
#include "aimdk_msgs/msg/mc_action.hpp"
#include "aimdk_msgs/msg/mc_action_command.hpp"
#include "aimdk_msgs/msg/request_header.hpp"
#include "rclcpp/rclcpp.hpp"
#include <chrono>
#include <iomanip>
#include <iostream>
#include <memory>
#include <signal.h>
#include <unordered_map>
#include <vector>

// Global variable used for signal handling
std::shared_ptr<rclcpp::Node> g_node = nullptr;

// Signal handler function
void signal_handler(int signal) {
  if (g_node) {
    RCLCPP_INFO(g_node->get_logger(), "Received signal %d, shutting down...",
                signal);
    g_node.reset();
  }
  rclcpp::shutdown();
  exit(signal);
}

class SetMcActionClient : public rclcpp::Node {
public:
  SetMcActionClient() : Node("set_mc_action_client") {

    client_ = this->create_client<aimdk_msgs::srv::SetMcAction>(
        "/aimdk_5Fmsgs/srv/SetMcAction");
    RCLCPP_INFO(this->get_logger(), "✅ SetMcAction client node created.");

    // Wait for the service to become available
    while (!client_->wait_for_service(std::chrono::seconds(2))) {
      RCLCPP_INFO(this->get_logger(), "⏳ Service unavailable, waiting...");
    }
    RCLCPP_INFO(this->get_logger(),
                "🟢 Service available, ready to send request.");
  }

  bool send_request(std::string &action_name) {
    try {
      auto request = std::make_shared<aimdk_msgs::srv::SetMcAction::Request>();
      request->header = aimdk_msgs::msg::RequestHeader();

      request->source = "node.set_mc_action";

      // Set robot mode
      aimdk_msgs::msg::McActionCommand command;
      command.action_desc = action_name;
      request->command = command;

      RCLCPP_INFO(this->get_logger(), "📨 Sending request to set robot mode: %s",
                  action_name.c_str());

      // Set Service Call Timeout
      const std::chrono::milliseconds timeout(250);
      for (int i = 0; i < 8; i++) {
        request->header.stamp = this->now();
        auto future = client_->async_send_request(request);
        auto retcode = rclcpp::spin_until_future_complete(shared_from_this(),
                                                          future, timeout);
        if (retcode != rclcpp::FutureReturnCode::SUCCESS) {
          // retry as remote peer is NOT handled well by ROS
          RCLCPP_INFO(this->get_logger(), "trying ... [%d]", i);
          continue;
        }
        // future.done
        auto response = future.get();
        if (response->response.status.value ==
            aimdk_msgs::msg::CommonState::SUCCESS) { // SUCCESS (1)
          RCLCPP_INFO(this->get_logger(), "✅ Robot mode set successfully.");
          return true;
        } else {
          RCLCPP_ERROR(this->get_logger(), "❌ Failed (code=%ld): %s",
                       static_cast<long>(response->response.header.code),
                       response->response.message.c_str());
          return false;
        }
      }
      RCLCPP_ERROR(this->get_logger(), "❌ Service call failed or timed out.");
      return false;
    } catch (const std::exception &e) {
      RCLCPP_ERROR(this->get_logger(), "Exception occurred: %s", e.what());
      return false;
    }
  }

private:
  rclcpp::Client<aimdk_msgs::srv::SetMcAction>::SharedPtr client_;
};

static std::unordered_map<std::string, std::vector<std::string>> g_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)"}},
        {"HEAD_ONLY", {"HO", "head-only control"}},
        {"UPPERBODY_REMOTE_SPLIT",
         {"URS", "upper body remote split (head + arms + hands)"}},
};

int main(int argc, char *argv[]) {
  try {
    rclcpp::init(argc, argv);

    // Set up signal handlers
    signal(SIGINT, signal_handler);
    signal(SIGTERM, signal_handler);

    // Create node
    g_node = std::make_shared<SetMcActionClient>();
    auto client = std::dynamic_pointer_cast<SetMcActionClient>(g_node);

    if (client) {
      std::unordered_map<std::string, std::string> choices;
      std::string motion;

      // Prefer command-line argument; otherwise prompt user
      if (argc > 1) {
        motion = argv[1];
        RCLCPP_INFO(g_node->get_logger(),
                    "Using abbr of motion mode from cmdline: %s", argv[1]);
      } else {
        std::cout << std::left << std::setw(4) << "abbr"
                  << " - " << std::setw(20) << "robot mode"
                  << " : "
                  << "description" << std::endl;
        for (auto &it : g_action_info) {
          std::cout << std::left << std::setw(4) << it.second[0] << " - "
                    << std::setw(20) << it.first << " : " << it.second[1]
                    << std::endl;
        }
        std::cout << "Enter abbr of motion mode:";
        std::cin >> motion;
      }
      for (auto &it : g_action_info) {
        choices[it.second[0]] = it.first;
      }

      auto m = choices.find(motion);
      if (m != choices.end()) {
        auto &action_name = m->second;
        client->send_request(action_name);
      } else {
        RCLCPP_ERROR(g_node->get_logger(), "Invalid abbr of robot mode: %s",
                     motion.c_str());
      }
    }

    // Clean up resources
    g_node.reset();
    rclcpp::shutdown();

    return 0;
  } catch (const std::exception &e) {
    RCLCPP_ERROR(rclcpp::get_logger("main"),
                 "Program exited with exception: %s", e.what());
    return 1;
  }
}

Usage Instructions

# Use command-line arguments to set the mode (recommended)
ros2 run py_examples set_mc_action JD  # Zero-Torque >> Position-Control Standing
ros2 run py_examples set_mc_action SD  # Ensure your robot's feet on the ground, Position-Control Standing >> Stable Standing
# Stable Standing >> Locomotion Mode. auto done internally, don't switch manually

# Or run without arguments and the program will prompt for input
ros2 run py_examples set_mc_action

Output Example

...
[INFO] [1764066567.502968540] [set_mc_action_client]: ✅ Robot mode set successfully.

Notes

• Ensure the robot is standing and its feet are on the ground before switching to STAND_DEFAULT mode

• Mode switching may take several seconds to complete

Interface Reference

• Service: /aimdk_5Fmsgs/srv/SetMcAction

• Message: aimdk_msgs/srv/SetMcAction

6.2.4 Set Robot Motion

This example uses preset_motion_client; after switching to Stable Stand Mode and starting the node, enter the corresponding field values to perform preset actions with the left (or right) hand such as handshake, raise hand, wave, or air kiss.

Available parameters can be found in the Preset Motion Table

#include "aimdk_msgs/msg/common_response.hpp"
#include "aimdk_msgs/msg/common_state.hpp"
#include "aimdk_msgs/msg/common_task_response.hpp"
#include "aimdk_msgs/msg/mc_control_area.hpp"
#include "aimdk_msgs/msg/mc_input_action.hpp"
#include "aimdk_msgs/msg/mc_preset_motion.hpp"
#include "aimdk_msgs/msg/request_header.hpp"
#include "aimdk_msgs/srv/set_mc_input_source.hpp"
#include "aimdk_msgs/srv/set_mc_preset_motion.hpp"
#include "rclcpp/rclcpp.hpp"
#include <chrono>
#include <cinttypes>
#include <iostream>
#include <memory>
#include <signal.h>

namespace {
constexpr const char *kInputSourceName = "node.preset_motion";
constexpr int32_t kInputSourcePriority = 40;
constexpr int32_t kInputSourceTimeout = 1000;
} // namespace

std::shared_ptr<rclcpp::Node> g_node = nullptr;

void signal_handler(int signal) {
  if (g_node) {
    RCLCPP_INFO(g_node->get_logger(), "Received signal %d, shutting down...",
                signal);
    g_node.reset();
  }
  rclcpp::shutdown();
  exit(signal);
}

class PresetMotionClient : public rclcpp::Node {
public:
  PresetMotionClient() : Node("preset_motion_client") {
    client_ = this->create_client<aimdk_msgs::srv::SetMcPresetMotion>(
        "/aimdk_5Fmsgs/srv/SetMcPresetMotion");
    input_source_client_ =
        this->create_client<aimdk_msgs::srv::SetMcInputSource>(
            "/aimdk_5Fmsgs/srv/SetMcInputSource");

    RCLCPP_INFO(this->get_logger(), "✅ SetMcPresetMotion client node created.");

    // Wait for the service to become available
    while (!client_->wait_for_service(std::chrono::seconds(2))) {
      RCLCPP_INFO(this->get_logger(), "⏳ Service unavailable, waiting...");
    }
    RCLCPP_INFO(this->get_logger(),
                "🟢 Service available, ready to send request.");
  }

  bool register_input_source() {
    if (!input_source_client_->wait_for_service(std::chrono::seconds(8))) {
      RCLCPP_ERROR(this->get_logger(),
                   "❌ SetMcInputSource service unavailable.");
      return false;
    }

    // Try ADD first; if it fails (likely already registered), fall back to
    // ENABLE
    if (call_input_source( // ADD (1001)
            aimdk_msgs::msg::McInputAction::INPUTACTION_ADD)) {
      RCLCPP_INFO(this->get_logger(), "✅ Input source \"%s\" registered (ADD).",
                  kInputSourceName);
      return true;
    }
    RCLCPP_INFO(this->get_logger(),
                "ADD failed (likely already registered), trying ENABLE...");
    if (call_input_source( // ENABLE (2001)
            aimdk_msgs::msg::McInputAction::INPUTACTION_ENABLE)) {
      RCLCPP_INFO(this->get_logger(), "✅ Input source \"%s\" enabled.",
                  kInputSourceName);
      return true;
    }
    RCLCPP_ERROR(this->get_logger(),
                 "❌ Failed to register input source via ADD or ENABLE.");
    return false;
  }

  bool send_request(int area_id, int motion_id) {
    try {
      auto request =
          std::make_shared<aimdk_msgs::srv::SetMcPresetMotion::Request>();
      request->header = aimdk_msgs::msg::RequestHeader();

      request->input_source.name = kInputSourceName;
      request->input_source.priority = kInputSourcePriority;
      request->input_source.timeout = kInputSourceTimeout;

      aimdk_msgs::msg::McPresetMotion motion;
      aimdk_msgs::msg::McControlArea area;

      motion.value = motion_id; // Preset motion ID
      area.value = area_id;     // Control area ID
      request->motion = motion;
      request->area = area;
      request->interrupt = false; // Not interrupt current motion

      RCLCPP_INFO(this->get_logger(),
                  "📨 Sending request to set preset motion: motion=%d, area=%d",
                  motion_id, area_id);

      const std::chrono::milliseconds timeout(250);
      for (int i = 0; i < 8; i++) {
        request->header.stamp = this->now();
        auto future = client_->async_send_request(request);
        auto retcode = rclcpp::spin_until_future_complete(shared_from_this(),
                                                          future, timeout);
        if (retcode != rclcpp::FutureReturnCode::SUCCESS) {
          // retry as remote peer is NOT handled well by ROS
          RCLCPP_INFO(this->get_logger(), "trying ... [%d]", i);
          continue;
        }
        // future.done
        auto response = future.get();
        if (response->response.header.code == 0) {
          RCLCPP_INFO(this->get_logger(),
                      "✅ Preset motion set successfully: %" PRIu64,
                      response->response.task_id);
          return true;
        } else if (response->response.state.value ==
                   aimdk_msgs::msg::CommonState::RUNNING) { // RUNNING (400)
          RCLCPP_INFO(this->get_logger(), "⏳ Preset motion executing: %" PRIu64,
                      response->response.task_id);
          return true;
        } else {
          RCLCPP_WARN(this->get_logger(),
                      "❌ Failed to set preset motion: %" PRIu64,
                      response->response.task_id);
          return false;
        }
      }
      RCLCPP_ERROR(this->get_logger(), "❌ Service call failed or timed out.");
      return false;
    } catch (const std::exception &e) {
      RCLCPP_ERROR(this->get_logger(), "Exception occurred: %s", e.what());
      return false;
    }
  }

private:
  bool call_input_source(int32_t action_value) {
    auto req = std::make_shared<aimdk_msgs::srv::SetMcInputSource::Request>();
    req->action.value = action_value;
    req->input_source.name = kInputSourceName;
    req->input_source.priority = kInputSourcePriority;
    req->input_source.timeout = kInputSourceTimeout;

    const std::chrono::milliseconds timeout(250);
    for (int i = 0; i < 8; i++) {
      req->request.header.stamp = this->now();
      auto future = input_source_client_->async_send_request(req);
      auto retcode = rclcpp::spin_until_future_complete(shared_from_this(),
                                                        future, timeout);
      if (retcode != rclcpp::FutureReturnCode::SUCCESS) {
        // retry as remote peer is NOT handled well by ROS
        RCLCPP_INFO(
            this->get_logger(), "trying input source action %s (%d)... [%d]",
            action_value == aimdk_msgs::msg::McInputAction::INPUTACTION_ADD
                ? "ADD"
                : "ENABLE",
            action_value, i);
        continue;
      }
      auto response = future.get();
      return response->response.header.code == 0;
    }
    return false;
  }

  rclcpp::Client<aimdk_msgs::srv::SetMcPresetMotion>::SharedPtr client_;
  rclcpp::Client<aimdk_msgs::srv::SetMcInputSource>::SharedPtr
      input_source_client_;
};

int main(int argc, char *argv[]) {
  try {
    rclcpp::init(argc, argv);
    signal(SIGINT, signal_handler);
    signal(SIGTERM, signal_handler);

    g_node = std::make_shared<PresetMotionClient>();
    // Cast g_node (std::shared_ptr<rclcpp::Node>) to a derived
    // PresetMotionClient pointer (std::shared_ptr<PresetMotionClient>)
    auto client = std::dynamic_pointer_cast<PresetMotionClient>(g_node);

    int area = 1;
    int motion = 1003;
    std::cout << "Enter arm area ID (1-left, 2-right): ";
    std::cin >> area;
    std::cout
        << "Enter preset motion ID (1001-raise, 1002-wave, 1003-handshake, "
           "1004-airkiss): ";
    std::cin >> motion;
    if (client) {
      if (!client->register_input_source()) {
        RCLCPP_ERROR(g_node->get_logger(),
                     "❌ Input source registration failed, exiting");
      } else {
        client->send_request(area, motion);
      }
    }

    // Clean up resources
    g_node.reset();
    rclcpp::shutdown();

    return 0;
  } catch (const std::exception &e) {
    RCLCPP_ERROR(rclcpp::get_logger("main"),
                 "Program exited with exception: %s", e.what());
    return 1;
  }
}

6.2.5 Gripper Control

This example uses hand_control. By publishing messages to the topic /aima/hal/joint/hand/command, you can control the movement of the gripper.

Attention

Warning ⚠️ : Before running this example, you must stop the native motion control module using aima em stop-app mc on the robot’s Motion Control Computing Unit (PC1) to obtain control authority. Ensure robot safety at all times.

#include "aimdk_msgs/msg/hand_command.hpp"
#include "aimdk_msgs/msg/hand_command_array.hpp"
#include "aimdk_msgs/msg/hand_type.hpp"
#include "aimdk_msgs/msg/message_header.hpp"
#include "rclcpp/rclcpp.hpp"
#include <chrono>
#include <memory>
#include <vector>

class HandControl : public rclcpp::Node {
public:
  HandControl()
      : Node("hand_control"), position_pairs_({
                                  {1.0, 1.0},
                                  {0.0, 0.0},
                                  {0.5, 0.5},
                                  {0.2, 0.8},
                                  {0.7, 0.3},
                              }),
        current_index_(0) {
    publisher_ = this->create_publisher<aimdk_msgs::msg::HandCommandArray>(
        "/aima/hal/joint/hand/command", 10);

    timer_ = this->create_wall_timer(
        std::chrono::milliseconds(20), // 50Hz
        std::bind(&HandControl::publish_hand_commands, this));

    last_switch_time_ = now();
    RCLCPP_INFO(this->get_logger(), "The hand control node has been started!");
  }

  void publish_hand_commands() {
    // 1. Determine if it's time to switch parameters.
    auto now_time = this->now();
    if ((now_time - last_switch_time_).seconds() >= 2.0) {
      current_index_ = (current_index_ + 1) % position_pairs_.size();
      last_switch_time_ = now_time;
      RCLCPP_INFO(this->get_logger(),
                  "Switched to the next parameter group, index=%zu (left=%.2f, "
                  "right=%.2f)",
                  current_index_, position_pairs_[current_index_].first,
                  position_pairs_[current_index_].second);
    }

    auto msg = std::make_unique<aimdk_msgs::msg::HandCommandArray>();
    msg->header = aimdk_msgs::msg::MessageHeader();

    float left_position = position_pairs_[current_index_].first;
    float right_position = position_pairs_[current_index_].second;

    aimdk_msgs::msg::HandCommand left_hands;
    left_hands.name = "left_hand";
    left_hands.position = left_position;
    left_hands.velocity = 1.0;
    left_hands.acceleration = 1.0;
    left_hands.deceleration = 1.0;
    left_hands.effort = 1.0;

    aimdk_msgs::msg::HandCommand right_hands;
    right_hands.name = "right_hand";
    right_hands.position = right_position;
    right_hands.velocity = 1.0;
    right_hands.acceleration = 1.0;
    right_hands.deceleration = 1.0;
    right_hands.effort = 1.0;

    msg->left_hands.push_back(left_hands);
    msg->right_hands.push_back(right_hands);
    msg->left_hand_type.value = aimdk_msgs::msg::HandType::CLAW; // grippers (2)
    msg->right_hand_type.value =
        aimdk_msgs::msg::HandType::CLAW; // grippers (2)

    publisher_->publish(std::move(msg));
  }

private:
  rclcpp::Publisher<aimdk_msgs::msg::HandCommandArray>::SharedPtr publisher_;
  rclcpp::TimerBase::SharedPtr timer_;

  std::vector<std::pair<float, float>> position_pairs_;
  size_t current_index_;

  rclcpp::Time last_switch_time_;
};

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  auto hand_control_node = std::make_shared<HandControl>();
  rclcpp::spin(hand_control_node);
  rclcpp::shutdown();
  return 0;
}

6.2.6 Dexterous Hand Control

This example uses omnihand_control. By publishing messages to the topic /aima/hal/joint/hand/command, you can control the movement of the omnihand.

Attention

Warning ⚠️ : Before running this example, you must stop the native motion control module using aima em stop-app mc on the robot’s Motion Control Computing Unit (PC1) to obtain control authority. Ensure robot safety at all times.

#include <aimdk_msgs/msg/hand_command_array.hpp>
#include <aimdk_msgs/msg/hand_type.hpp>
#include <array>
#include <chrono>
#include <memory>
#include <rclcpp/rclcpp.hpp>

using namespace std::chrono_literals;

class HandCommandPublisher : public rclcpp::Node {
public:
  HandCommandPublisher() : Node("hand_command_publisher") {
    publisher_ = this->create_publisher<aimdk_msgs::msg::HandCommandArray>(
        "/aima/hal/joint/hand/command", 10);

    // Create a timer to publish once per second
    timer_ = this->create_wall_timer(
        1s, std::bind(&HandCommandPublisher::publish_command, this));

    RCLCPP_INFO(this->get_logger(), "Hand control node started!");
  }

private:
  void publish_command() {
    auto message = aimdk_msgs::msg::HandCommandArray();

    // Set header
    message.header.stamp = this->now();
    message.header.frame_id = "hand_command";

    // Set the hand type
    message.left_hand_type.value =
        aimdk_msgs::msg::HandType::NIMBLE_HANDS; // dexterous hands (1)
    message.right_hand_type.value =
        aimdk_msgs::msg::HandType::NIMBLE_HANDS; // dexterous hands (1)

    const char *left_names[10] = {"L_thumb_roll_joint", "L_thumb_abad_joint",
                                  "L_thumb_mcp_joint",  "L_index_abad_joint",
                                  "L_index_pip_joint",  "L_middle_pip_joint",
                                  "L_ring_abad_joint",  "L_ring_pip_joint",
                                  "L_pinky_abad_joint", "L_pinky_pip_joint"};
    const char *right_names[10] = {"R_thumb_roll_joint", "R_thumb_abad_joint",
                                   "R_thumb_mcp_joint",  "R_index_abad_joint",
                                   "R_index_pip_joint",  "R_middle_pip_joint",
                                   "R_ring_abad_joint",  "R_ring_pip_joint",
                                   "R_pinky_abad_joint", "R_pinky_pip_joint"};

    // Create left hand command array
    message.left_hands.resize(10);
    for (int i = 0; i < 10; i++) {
      message.left_hands[i].name = left_names[i];
      message.left_hands[i].position = 0.0;
      message.left_hands[i].velocity = 0.1;
      message.left_hands[i].acceleration = 0.0;
      message.left_hands[i].deceleration = 0.0;
      message.left_hands[i].effort = 0.0;
    }

    // Create right hand command array
    message.right_hands.resize(10);
    for (int i = 0; i < 10; i++) {
      message.right_hands[i].name = right_names[i];
      message.right_hands[i].position = 0.0;
      message.right_hands[i].velocity = 0.1;
      message.right_hands[i].acceleration = 0.0;
      message.right_hands[i].deceleration = 0.0;
      message.right_hands[i].effort = 0.0;
    }

    // Absolute joint targets in rad = half-grip
    // Joint order: [thumb_roll, thumb_abad, thumb_mcp, index_abad, index_pip,
    //               middle_pip, ring_abad, ring_pip, pinky_abad, pinky_pip]
    const std::array<double, 10> left_hand_target = {
        -0.6, 0.8, -0.4, 0.1, 0.7, 0.7, -0.1, 0.7, -0.1, 0.7};
    const std::array<double, 10> right_hand_target = {
        0.6, -0.8, 0.4, -0.1, 0.7, 0.7, 0.1, 0.7, 0.1, 0.7};

    if (target_finger < 10) {
      message.right_hands[target_finger].position =
          right_hand_target[target_finger];
    } else {
      int target_finger_ = target_finger - 10;
      message.left_hands[target_finger_].position =
          left_hand_target[target_finger_];
    }

    // Publish the message
    publisher_->publish(message);

    RCLCPP_INFO(this->get_logger(),
                "Published hand command with target_finger: %d", target_finger);

    update_target_finger();
  }

  void update_target_finger() {
    if (increasing_) {
      target_finger += step_;
      if (target_finger >= 19) {
        target_finger = 19;
        increasing_ = false;
      }
    } else {
      target_finger -= step_;
      if (target_finger <= 0) {
        target_finger = 0;
        increasing_ = true;
      }
    }
  }

  rclcpp::Publisher<aimdk_msgs::msg::HandCommandArray>::SharedPtr publisher_;
  rclcpp::TimerBase::SharedPtr timer_;

  int target_finger = 0;
  int step_ = 1;
  bool increasing_ = true;
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<HandCommandPublisher>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

6.2.7 Dexterous Hand Tactile Data (T2.1 Support)

This example uses omnihand_touch. By subscribing to the /aima/hal/joint/hand/state, you can monitor the pressure of the omnihand finger touch sensor.

Attention

Warning ⚠️ : Before running this example, you must stop the native motion control module using aima em stop-app mc on the robot’s Motion Control Computing Unit (PC1) to obtain control authority. Ensure robot safety at all times.

//
// Created by agiuser on 2026/1/27.
//
#include <aimdk_msgs/msg/hand_command_array.hpp>
#include <aimdk_msgs/msg/hand_state_array.hpp>
#include <aimdk_msgs/msg/hand_type.hpp>
#include <chrono>
#include <iomanip>
#include <iostream>
#include <rclcpp/rclcpp.hpp>

using namespace std::chrono_literals;

class HandStateSubscriber : public rclcpp::Node {
public:
  HandStateSubscriber() : Node("hand_state_subscriber") {
    publisher_ = this->create_publisher<aimdk_msgs::msg::HandCommandArray>(
        "/aima/hal/joint/hand/command", 10);
    rclcpp::QoS qos_profile(rclcpp::KeepLast(10));
    qos_profile.best_effort(); // Set reliability to BEST_EFFORT
    // Create subscriber
    subscription_ = this->create_subscription<aimdk_msgs::msg::HandStateArray>(
        "/aima/hal/joint/hand/state", qos_profile,
        std::bind(&HandStateSubscriber::topic_callback, this,
                  std::placeholders::_1));
    // Create a timer to publish once per second
    timer_ = this->create_wall_timer(
        1s, std::bind(&HandStateSubscriber::publish_command, this));
    RCLCPP_INFO(
        this->get_logger(),
        "Subscriber started, listening to /aima/hal/joint/hand/state topic...");
  }

private:
  void topic_callback(const aimdk_msgs::msg::HandStateArray::SharedPtr msg) {
    // Print message header information
    RCLCPP_INFO(this->get_logger(),
                "Message received - Sequence: %u, Timestamp: %d.%09d",
                msg->header.sequence, msg->header.stamp.sec,
                msg->header.stamp.nanosec);

    // Print left hand touch sensor data
    print_touch_sensor_data("Left Hand", msg->left_touch_sensors);

    // Print right hand touch sensor data
    print_touch_sensor_data("Right Hand", msg->right_touch_sensors);

    std::cout << std::endl;
  }

  /**
   * @brief Print touch sensor data for a specific hand
   * @param hand_name Name of the hand (Left Hand/Right Hand)
   * @param sensor_data Touch sensor data structure
   */
  void print_touch_sensor_data(
      const std::string &hand_name,
      const aimdk_msgs::msg::HandTouchSensorData &sensor_data) {
    std::cout << "=== " << hand_name << " Touch Sensor Data ===" << std::endl;

    // Print palm touch data
    std::cout << "Palm Touch Data (36 elements): ";
    print_array(sensor_data.palm_touch_data);

    // Print back of hand touch data
    std::cout << "Back of Hand Touch Data (36 elements): ";
    print_array(sensor_data.back_of_hand_touch_data);

    // Print finger touch data
    std::cout << "Thumb Touch Data (16 elements): ";
    print_array(sensor_data.thumb_touch_data);

    std::cout << "Index Finger Touch Data (16 elements): ";
    print_array(sensor_data.index_finger_touch_data);

    std::cout << "Middle Finger Touch Data (16 elements): ";
    print_array(sensor_data.middle_finger_touch_data);

    std::cout << "Ring Finger Touch Data (16 elements): ";
    print_array(sensor_data.ring_finger_touch_data);

    std::cout << "Little Finger Touch Data (16 elements): ";
    print_array(sensor_data.little_finger_touch_data);
  }

  /**
   * @brief Print array of 36 uint8_t elements
   * @param arr Array to print
   */
  void print_array(const std::array<uint8_t, 36> &arr) {
    std::cout << "[";
    for (size_t i = 0; i < arr.size(); ++i) {
      std::cout << std::setw(3) << static_cast<int>(arr[i]);
      if (i < arr.size() - 1)
        std::cout << " ";
    }
    std::cout << "]" << std::endl;
  }

  /**
   * @brief Print array of 16 uint8_t elements
   * @param arr Array to print
   */
  void print_array(const std::array<uint8_t, 16> &arr) {
    std::cout << "[";
    for (size_t i = 0; i < arr.size(); ++i) {
      std::cout << std::setw(3) << static_cast<int>(arr[i]);
      if (i < arr.size() - 1)
        std::cout << " ";
    }
    std::cout << "]" << std::endl;
  }

  void publish_command() {
    auto message = aimdk_msgs::msg::HandCommandArray();

    // Set header
    message.header.stamp = this->now();
    message.header.frame_id = "hand_command";

    // Set the hand type
    message.left_hand_type.value =
        aimdk_msgs::msg::HandType::NIMBLE_HANDS; // dexterous hands (1)
    message.right_hand_type.value =
        aimdk_msgs::msg::HandType::NIMBLE_HANDS; // dexterous hands (1)

    // Joint names for readability, mirroring the /aima/hal/joint/hand/state
    // feedback (URDF names). The driver routes by array index/order (see
    // endeffector.md), not by these name strings.
    const char *left_names[10] = {"L_thumb_roll_joint", "L_thumb_abad_joint",
                                  "L_thumb_mcp_joint",  "L_index_abad_joint",
                                  "L_index_pip_joint",  "L_middle_pip_joint",
                                  "L_ring_abad_joint",  "L_ring_pip_joint",
                                  "L_pinky_abad_joint", "L_pinky_pip_joint"};
    const char *right_names[10] = {"R_thumb_roll_joint", "R_thumb_abad_joint",
                                   "R_thumb_mcp_joint",  "R_index_abad_joint",
                                   "R_index_pip_joint",  "R_middle_pip_joint",
                                   "R_ring_abad_joint",  "R_ring_pip_joint",
                                   "R_pinky_abad_joint", "R_pinky_pip_joint"};

    // Create left hand command array
    message.left_hands.resize(10);
    for (int i = 0; i < 10; i++) {
      message.left_hands[i].name = left_names[i];
      message.left_hands[i].position = 0.0;
      message.left_hands[i].velocity = 0.1;
      message.left_hands[i].acceleration = 0.0;
      message.left_hands[i].deceleration = 0.0;
      message.left_hands[i].effort = 0.0;
    }

    // Create right hand command array
    message.right_hands.resize(10);
    for (int i = 0; i < 10; i++) {
      message.right_hands[i].name = right_names[i];
      message.right_hands[i].position = 0.0;
      message.right_hands[i].velocity = 0.1;
      message.right_hands[i].acceleration = 0.0;
      message.right_hands[i].deceleration = 0.0;
      message.right_hands[i].effort = 0.0;
    }

    // Publish the message
    publisher_->publish(message);

    RCLCPP_INFO(this->get_logger(), "Published hand command");
  }

  rclcpp::Publisher<aimdk_msgs::msg::HandCommandArray>::SharedPtr publisher_;
  rclcpp::Subscription<aimdk_msgs::msg::HandStateArray>::SharedPtr
      subscription_;
  rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<HandStateSubscriber>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

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.2.8 Register Secondary Development Input Source

For versions after v0.7, an input source must be registered before controlling the MC. In this example, the /aimdk_5Fmsgs/srv/SetMcInputSource service is used to register the secondary development input source, so that the MC can recognize it. Only after registration can robot velocity control be performed.

#include "aimdk_msgs/srv/set_mc_input_source.hpp"
#include "aimdk_msgs/msg/common_request.hpp"
#include "aimdk_msgs/msg/common_response.hpp"
#include "aimdk_msgs/msg/common_state.hpp"
#include "aimdk_msgs/msg/common_task_response.hpp"
#include "aimdk_msgs/msg/mc_input_action.hpp"
#include "rclcpp/rclcpp.hpp"
#include <chrono>
#include <cinttypes>
#include <memory>
#include <signal.h>

namespace {
const char *action_name(int32_t value) {
  using A = aimdk_msgs::msg::McInputAction;
  switch (value) {
  case A::INPUTACTION_ADD:
    return "ADD";
  case A::INPUTACTION_MODIFY:
    return "MODIFY";
  case A::INPUTACTION_DELETE:
    return "DELETE";
  case A::INPUTACTION_ENABLE:
    return "ENABLE";
  case A::INPUTACTION_DISABLE:
    return "DISABLE";
  default:
    return "UNKNOWN";
  }
}
} // namespace

std::shared_ptr<rclcpp::Node> g_node = nullptr;

void signal_handler(int signal) {
  if (g_node) {
    RCLCPP_INFO(g_node->get_logger(), "Received signal %d, shutting down...",
                signal);
    g_node.reset();
  }
  rclcpp::shutdown();
  exit(signal);
}

class McInputClient : public rclcpp::Node {
public:
  McInputClient() : Node("set_mc_input_source_client") {
    client_ = this->create_client<aimdk_msgs::srv::SetMcInputSource>(
        "/aimdk_5Fmsgs/srv/SetMcInputSource");

    RCLCPP_INFO(this->get_logger(), "✅ SetMcInputSource client node created.");

    // Wait for the service to become available
    while (!client_->wait_for_service(std::chrono::seconds(2))) {
      RCLCPP_INFO(this->get_logger(), "⏳ Service unavailable, waiting...");
    }
    RCLCPP_INFO(this->get_logger(),
                "🟢 Service available, ready to send request.");
  }

  bool send_request() {
    try {
      auto request =
          std::make_shared<aimdk_msgs::srv::SetMcInputSource::Request>();

      // Set request data
      request->action.value =
          aimdk_msgs::msg::McInputAction::INPUTACTION_ADD; // ADD (1001)
      request->input_source.name = "node.set_mc_input_source";
      request->input_source.priority = 40;
      request->input_source.timeout = 1000;

      RCLCPP_INFO(this->get_logger(), "📨 Sending input source request: %s (%d)",
                  action_name(request->action.value), request->action.value);

      auto timeout = std::chrono::milliseconds(250);
      for (int i = 0; i < 8; i++) {
        // Set header timestamp
        request->request.header.stamp = this->now(); // use Node::now()
        auto future = client_->async_send_request(request);
        auto retcode = rclcpp::spin_until_future_complete(
            this->shared_from_this(), future, timeout);
        if (retcode != rclcpp::FutureReturnCode::SUCCESS) {
          // retry as remote peer is NOT handled well by ROS
          RCLCPP_INFO(this->get_logger(), "trying ... [%d]", i);
          continue;
        }
        // future.done
        auto response = future.get();
        auto code = response->response.header.code;
        if (code == 0) {
          RCLCPP_INFO(this->get_logger(),
                      "✅ Input source set successfully. task_id=%" PRIu64,
                      response->response.task_id);
          return true;
        } else {
          RCLCPP_ERROR(
              this->get_logger(),
              "❌ Input source set failed. ret_code=%" PRId64
              ", task_id=%" PRIu64 " "
              "(duplicated ADD? or MODIFY/ENABLE/DISABLE for unknown source?)",
              code, response->response.task_id);
          return false;
        }
      }
      RCLCPP_ERROR(this->get_logger(), "❌ Service call failed or timed out.");
      return false;
    } catch (const std::exception &e) {
      RCLCPP_ERROR(this->get_logger(), "Exception occurred: %s", e.what());
      return false;
    }
  }

private:
  rclcpp::Client<aimdk_msgs::srv::SetMcInputSource>::SharedPtr client_;
};

int main(int argc, char *argv[]) {
  try {
    rclcpp::init(argc, argv);
    signal(SIGINT, signal_handler);
    signal(SIGTERM, signal_handler);

    g_node = std::make_shared<McInputClient>();
    auto client = std::dynamic_pointer_cast<McInputClient>(g_node);

    if (client) {
      client->send_request();
    }

    g_node.reset();
    rclcpp::shutdown();

    return 0;
  } catch (const std::exception &e) {
    RCLCPP_ERROR(rclcpp::get_logger("main"),
                 "Program terminated with exception: %s", e.what());
    return 1;
  }
}

6.2.9 Get Current Input Source

This example uses the GetCurrentInputSource service, which is used to obtain information about the currently registered input source, including the input source name, priority, and timeout settings.

#include "aimdk_msgs/srv/get_current_input_source.hpp"
#include "aimdk_msgs/msg/common_request.hpp"
#include "aimdk_msgs/msg/response_header.hpp"
#include "rclcpp/rclcpp.hpp"
#include <chrono>
#include <cinttypes>
#include <memory>
#include <signal.h>

// Global node object
std::shared_ptr<rclcpp::Node> g_node = nullptr;

// Signal handler
void signal_handler(int signal) {
  if (g_node) {
    RCLCPP_INFO(g_node->get_logger(), "Received signal %d, shutting down...",
                signal);
    g_node.reset();
  }
  rclcpp::shutdown();
  exit(signal);
}

// Client Class
class GetCurrentInputSourceClient : public rclcpp::Node {
public:
  GetCurrentInputSourceClient() : Node("get_current_input_source_client") {

    client_ = this->create_client<aimdk_msgs::srv::GetCurrentInputSource>(
        "/aimdk_5Fmsgs/srv/GetCurrentInputSource");

    RCLCPP_INFO(this->get_logger(),
                "✅ GetCurrentInputSource client node created.");

    // Wait for the service to become available
    while (!client_->wait_for_service(std::chrono::seconds(2))) {
      RCLCPP_INFO(this->get_logger(), "⏳ Service unavailable, waiting...");
    }
    RCLCPP_INFO(this->get_logger(),
                "🟢 Service available, ready to send request.");
  }

  void send_request() {
    try {
      auto request =
          std::make_shared<aimdk_msgs::srv::GetCurrentInputSource::Request>();
      request->request = aimdk_msgs::msg::CommonRequest();

      RCLCPP_INFO(this->get_logger(),
                  "📨 Sending request to get current input source");

      auto timeout = std::chrono::milliseconds(250);

      for (int i = 0; i < 8; i++) {
        request->request.header.stamp = this->now();
        auto future = client_->async_send_request(request);
        auto retcode = rclcpp::spin_until_future_complete(
            this->shared_from_this(), future, timeout);
        if (retcode != rclcpp::FutureReturnCode::SUCCESS) {
          // retry as remote peer is NOT handled well by ROS
          RCLCPP_INFO(this->get_logger(), "trying ... [%d]", i);
          continue;
        }
        // future.done
        auto response = future.get();
        if (response->response.header.code == 0) {
          RCLCPP_INFO(this->get_logger(),
                      "✅ Current input source get successfully:");
          RCLCPP_INFO(this->get_logger(), "Name: %s",
                      response->input_source.name.c_str());
          RCLCPP_INFO(this->get_logger(), "Priority: %d",
                      response->input_source.priority);
          RCLCPP_INFO(this->get_logger(), "Timeout: %d",
                      response->input_source.timeout);
        } else {
          RCLCPP_WARN(
              this->get_logger(),
              "❌ Current input source get failed, return code: %" PRId64,
              response->response.header.code);
        }
        return;
      }
      RCLCPP_ERROR(this->get_logger(), "❌ Service call failed or timed out.");
    } catch (const std::exception &e) {
      RCLCPP_ERROR(this->get_logger(), "Exception occurred: %s", e.what());
    }
  }

private:
  rclcpp::Client<aimdk_msgs::srv::GetCurrentInputSource>::SharedPtr client_;
};

int main(int argc, char *argv[]) {
  try {
    rclcpp::init(argc, argv);

    signal(SIGINT, signal_handler);
    signal(SIGTERM, signal_handler);

    g_node = std::make_shared<GetCurrentInputSourceClient>();
    auto client =
        std::dynamic_pointer_cast<GetCurrentInputSourceClient>(g_node);

    if (client) {
      client->send_request();
    }

    g_node.reset();
    rclcpp::shutdown();
    return 0;
  } catch (const std::exception &e) {
    RCLCPP_ERROR(rclcpp::get_logger("main"),
                 "Program exited with exception: %s", e.what());
    return 1;
  }
}

Usage Instructions

# Get current input source information
ros2 run examples get_current_input_source

Output Example

[INFO] [1779174540.534817548] [get_current_input_source_client]: ✅ GetCurrentInputSource client node created.
[INFO] [1779174541.371095227] [get_current_input_source_client]: 🟢 Service available, ready to send request.
[INFO] [1779174541.371216694] [get_current_input_source_client]: 📨 Sending request to get current input source
[INFO] [1779174541.382602338] [get_current_input_source_client]: ✅ Current input source get successfully:
[INFO] [1779174541.382692894] [get_current_input_source_client]: Name: node
[INFO] [1779174541.382713533] [get_current_input_source_client]: Priority: 40
[INFO] [1779174541.382726205] [get_current_input_source_client]: Timeout: 1000

Notes

• Ensure the GetCurrentInputSource service is running properly

• Valid information can only be obtained after an input source has been registered

• A status code of 0 indicates a successful query

6.2.10 Robot Locomotion Control

This example uses mc_locomotion_velocity. The following example controls the robot’s walking by publishing to the /aima/mc/locomotion/velocity topic. For versions after v0.7, an input source must be registered before enabling velocity control (this example already includes input source registration). Refer to the code for detailed registration steps.

Start the node after switching to Stable Standing Mode:

#include "aimdk_msgs/msg/mc_locomotion_velocity.hpp"
#include "aimdk_msgs/msg/common_request.hpp"
#include "aimdk_msgs/msg/common_response.hpp"
#include "aimdk_msgs/msg/common_state.hpp"
#include "aimdk_msgs/msg/common_task_response.hpp"
#include "aimdk_msgs/msg/mc_input_action.hpp"
#include "aimdk_msgs/msg/message_header.hpp"
#include "aimdk_msgs/srv/set_mc_input_source.hpp"

#include "rclcpp/rclcpp.hpp"
#include <chrono>
#include <cmath>
#include <iostream>
#include <memory>
#include <signal.h>
#include <thread>

class DirectVelocityControl : public rclcpp::Node {
public:
  DirectVelocityControl() : Node("direct_velocity_control") {
    // Create publisher
    publisher_ = this->create_publisher<aimdk_msgs::msg::McLocomotionVelocity>(
        "/aima/mc/locomotion/velocity", 10);
    // Create service client
    client_ = this->create_client<aimdk_msgs::srv::SetMcInputSource>(
        "/aimdk_5Fmsgs/srv/SetMcInputSource");

    // Maximum speed limits
    max_forward_speed_ = 1.0; // m/s
    max_lateral_speed_ = 1.0; // m/s
    max_angular_speed_ = 1.0; // rad/s
    // Minimum speed limits (0 is also OK)
    min_forward_speed_ = 0.2; // m/s
    min_lateral_speed_ = 0.2; // m/s
    min_angular_speed_ = 0.1; // rad/s

    RCLCPP_INFO(this->get_logger(), "Direct velocity control node started.");
  }

  void start_publish() {
    if (timer_ != nullptr) {
      return;
    }
    // Set timer to periodically publish velocity messages (50Hz)
    timer_ = this->create_wall_timer(
        std::chrono::milliseconds(20),
        std::bind(&DirectVelocityControl::publish_velocity, this));
  }

  bool register_input_source() {
    const std::chrono::seconds timeout(8);
    auto start_time = std::chrono::steady_clock::now();
    while (!client_->wait_for_service(std::chrono::seconds(2))) {
      if (std::chrono::steady_clock::now() - start_time > timeout) {
        RCLCPP_ERROR(this->get_logger(), "Waiting for service timed out");
        return false;
      }
      RCLCPP_INFO(this->get_logger(), "Waiting for input source service...");
    }

    // Try ADD first; if it fails (likely already registered), fall back to
    // ENABLE
    if (call_input_source( // ADD (1001)
            aimdk_msgs::msg::McInputAction::INPUTACTION_ADD)) {
      RCLCPP_INFO(this->get_logger(), "✅ Input source registered (ADD).");
      return true;
    }
    RCLCPP_INFO(this->get_logger(),
                "ADD failed (likely already registered), trying ENABLE...");
    if (call_input_source( // ENABLE (2001)
            aimdk_msgs::msg::McInputAction::INPUTACTION_ENABLE)) {
      RCLCPP_INFO(this->get_logger(), "✅ Input source enabled.");
      return true;
    }
    RCLCPP_ERROR(this->get_logger(),
                 "❌ Failed to register input source via ADD or ENABLE.");
    return false;
  }

  bool call_input_source(int32_t action_value) {
    auto request =
        std::make_shared<aimdk_msgs::srv::SetMcInputSource::Request>();
    request->action.value = action_value;
    request->input_source.name = "node.locomotion_velocity";
    request->input_source.priority = 40;
    request->input_source.timeout = 1000;

    auto timeout2 = std::chrono::milliseconds(250);

    for (int i = 0; i < 8; i++) {
      request->request.header.stamp = this->now();
      auto future = client_->async_send_request(request);
      auto retcode = rclcpp::spin_until_future_complete(
          this->shared_from_this(), future, timeout2);
      if (retcode != rclcpp::FutureReturnCode::SUCCESS) {
        // retry as remote peer is NOT handled well by ROS
        RCLCPP_INFO(
            this->get_logger(), "trying input source action %s (%d)... [%d]",
            action_value == aimdk_msgs::msg::McInputAction::INPUTACTION_ADD
                ? "ADD"
                : "ENABLE",
            action_value, i);
        continue;
      }
      auto response = future.get();
      return response->response.header.code == 0;
    }
    return false;
  }

  void publish_velocity() {
    auto msg = std::make_unique<aimdk_msgs::msg::McLocomotionVelocity>();
    msg->header = aimdk_msgs::msg::MessageHeader();
    msg->header.stamp = this->now();
    msg->source = "node.locomotion_velocity"; // Set message source
    msg->forward_velocity = forward_velocity_;
    msg->lateral_velocity = lateral_velocity_;
    msg->angular_velocity = angular_velocity_;

    publisher_->publish(std::move(msg));
    RCLCPP_INFO(this->get_logger(),
                "Published velocity: Forward %.2f m/s, Lateral %.2f m/s, "
                "Angular %.2f rad/s",
                forward_velocity_, lateral_velocity_, angular_velocity_);
  }

  void clear_velocity() {
    forward_velocity_ = 0.0;
    lateral_velocity_ = 0.0;
    angular_velocity_ = 0.0;
  }

  bool set_forward(double forward) {
    // check value range, mc has thresholds to start movement
    if (std::abs(forward) < 0.005) {
      forward_velocity_ = 0.0;
      return true;
    } else if ((std::abs(forward) > max_forward_speed_) ||
               (std::abs(forward) < min_forward_speed_)) {
      RCLCPP_ERROR(this->get_logger(), "input value out of range, exiting");
      return false;
    } else {
      forward_velocity_ = forward;
      return true;
    }
  }

  bool set_lateral(double lateral) {
    // check value range, mc has thresholds to start movement
    if (std::abs(lateral) < 0.005) {
      lateral_velocity_ = 0.0;
      return true;
    } else if ((std::abs(lateral) > max_lateral_speed_) ||
               (std::abs(lateral) < min_lateral_speed_)) {
      RCLCPP_ERROR(this->get_logger(), "input value out of range, exiting");
      return false;
    } else {
      lateral_velocity_ = lateral;
      return true;
    }
  }

  bool set_angular(double angular) {
    // check value range, mc has thresholds to start movement
    if (std::abs(angular) < 0.005) {
      angular_velocity_ = 0.0;
      return true;
    } else if ((std::abs(angular) > max_angular_speed_) ||
               (std::abs(angular) < min_angular_speed_)) {
      RCLCPP_ERROR(this->get_logger(), "input value out of range, exiting");
      return false;
    } else {
      angular_velocity_ = angular;
      return true;
    }
  }

private:
  rclcpp::Publisher<aimdk_msgs::msg::McLocomotionVelocity>::SharedPtr
      publisher_;
  rclcpp::Client<aimdk_msgs::srv::SetMcInputSource>::SharedPtr client_;
  rclcpp::TimerBase::SharedPtr timer_;

  double forward_velocity_;
  double lateral_velocity_;
  double angular_velocity_;

  double max_forward_speed_;
  double max_lateral_speed_;
  double max_angular_speed_;

  double min_forward_speed_;
  double min_lateral_speed_;
  double min_angular_speed_;
};

// Signal Processing
std::shared_ptr<DirectVelocityControl> global_node = nullptr;
void signal_handler(int sig) {
  if (global_node) {
    global_node->clear_velocity();
    RCLCPP_INFO(global_node->get_logger(),
                "Received signal %d: clearing velocity and shutting down", sig);
  }
  rclcpp::shutdown();
  exit(sig);
}

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  signal(SIGINT, signal_handler);
  signal(SIGTERM, signal_handler);

  global_node = std::make_shared<DirectVelocityControl>();
  auto node = global_node;

  if (!node->register_input_source()) {
    RCLCPP_ERROR(node->get_logger(),
                 "❌ Input source registration failed, exiting");
    global_node.reset();
    rclcpp::shutdown();
    return 1;
  }

  // get and check control values
  // notice that mc has thresholds to start movement
  double forward, lateral, angular;
  std::cout << "Enter forward speed 0 or ±(0.2 ~ 1.0) m/s: ";
  std::cin >> forward;
  if (!node->set_forward(forward)) {
    return 2;
  }
  std::cout << "Enter lateral speed 0 or ±(0.2 ~ 1.0) m/s: ";
  std::cin >> lateral;
  if (!node->set_lateral(lateral)) {
    return 2;
  }
  std::cout << "Enter angular speed 0 or ±(0.1 ~ 1.0) rad/s: ";
  std::cin >> angular;
  if (!node->set_angular(angular)) {
    return 2;
  }

  RCLCPP_INFO(node->get_logger(), "Setting velocity; moving for 5 seconds");

  node->start_publish();

  auto start_time = node->now();
  while ((node->now() - start_time).seconds() < 5.0) {
    rclcpp::spin_some(node);
    std::this_thread::sleep_for(std::chrono::milliseconds(1));
  }

  node->clear_velocity();
  RCLCPP_INFO(node->get_logger(), "5 seconds elapsed; robot stopped");

  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

6.2.11 Joint Motor Control

This example demonstrates how to use ROS2 and the Ruckig library to control the robot’s joint movements.

Attention

Warning ⚠️ : Before running this example, you must stop the native motion control module using aima em stop-app mc on the robot’s Motion Control Computing Unit (PC1) to obtain control authority. Ensure robot safety at all times.

! This example directly controls the underlying motors (i.e. the HAL layer). Before running the program, please verify that the joint safety limits in the code match the actual robot model, and ensure safety!

Robot Joint Control Example

This example demonstrates how to control the robot’s joints using ROS2 and the Ruckig library. It includes the following features:

1. Robot joint model definition

2. Trajectory interpolation using Ruckig

3. Multi-joint coordinated control

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

Dependencies

• ROS2

• Ruckig library

• aimdk_msgs package

Build Instructions

1. Place the code in the src directory of your ROS2 workspace

2. Add the following to your CMakeLists.txt:

find_package(rclcpp REQUIRED)
find_package(aimdk_msgs REQUIRED)
find_package(ruckig REQUIRED)

add_executable(joint_control_example joint_control_example.cpp)
ament_target_dependencies(joint_control_example
  rclcpp
  aimdk_msgs
  ruckig
)

3. Add dependencies in package.xml:

<depend>rclcpp</depend>
<depend>aimdk_msgs</depend>
<depend>ruckig</depend>

Example Function Overview

1. Four controller nodes are created to control:

   • Legs × 2 (12 joints)

   • Waist × 1 (3 joints)

   • Arms × 2 (14 joints)

   • Head × 1 (2 joints)

2. Demonstrated features:

   • Make a designated joint oscillate between ±0.5 radians every 10 seconds

   • Generate smooth motion trajectories using the Ruckig library

   • Publish joint control commands in real time

Customization

2. Add new control logic:

   • Modify the SetTargetPosition function

   • Add new control callback functions

3. Adjust control frequency:

   • Modify the timer period (currently 3s) and the ruckig dt (currently 0.002s)

#include <aimdk_msgs/msg/joint_command_array.hpp>
#include <aimdk_msgs/msg/joint_state_array.hpp>
#include <atomic>
#include <cmath>
#include <cstdlib>
#include <map>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <ruckig/ruckig.hpp>
#include <signal.h>
#include <string>
#include <thread>
#include <vector>

/**
 * @brief Global variables and signal handling
 */
// Global variables to control program state
std::atomic<bool> g_running(true);
std::atomic<bool> g_emergency_stop(false);

// Signal handler function
void signal_handler(int) {
  g_running = false;
  RCLCPP_INFO(rclcpp::get_logger("main"),
              "Received termination signal, shutting down...");
}

/**
 * @brief Robot model definition
 */
enum class JointArea {
  HEAD,  // Head joints
  ARM,   // Arm joints
  WAIST, // Waist joints
  LEG,   // Leg joints
};

/**
 * @brief Joint information structure
 */
struct JointInfo {
  std::string name;   // Joint name
  double lower_limit; // Joint angle lower limit
  double upper_limit; // Joint angle upper limit
  double kp;          // Position control gain
  double kd;          // Velocity control gain
};

/**
 * @brief Robot model configuration
 * Contains parameters for all joints, enabling or disabling specific joints as
 * needed
 */
std::map<JointArea, std::vector<JointInfo>> robot_model = {
    {JointArea::LEG,
     {
         // Left leg joint configuration
         {"left_hip_pitch_joint", -2.704, 2.556, 40.0, 4.0},
         {"left_hip_roll_joint", -0.235, 2.906, 40.0, 4.0},
         {"left_hip_yaw_joint", -1.684, 3.430, 30.0, 3.0},
         {"left_knee_joint", 0.0000, 2.4073, 80.0, 8.0},
         {"left_ankle_pitch_joint", -0.803, 0.453, 40.0, 4.0},
         {"left_ankle_roll_joint", -0.2625, 0.2625, 20.0, 2.0},
         // Right leg joint configuration
         {"right_hip_pitch_joint", -2.704, 2.556, 40.0, 4.0},
         {"right_hip_roll_joint", -2.906, 0.235, 40.0, 4.0},
         {"right_hip_yaw_joint", -3.430, 1.684, 30.0, 3.0},
         {"right_knee_joint", 0.0000, 2.4073, 80.0, 8.0},
         {"right_ankle_pitch_joint", -0.803, 0.453, 40.0, 4.0},
         {"right_ankle_roll_joint", -0.2625, 0.2625, 20.0, 2.0},
     }},

    {JointArea::WAIST,
     {
         // Waist joint configuration
         {"waist_yaw_joint", -3.43, 2.382, 20.0, 4.0},
         {"waist_pitch_joint", -0.314, 0.314, 20.0, 4.0},
         {"waist_roll_joint", -0.488, 0.488, 20.0, 4.0},
     }},
    {JointArea::ARM,
     {
         // Left arm joint configuration
         {"left_shoulder_pitch_joint", -3.08, 2.04, 20.0, 2.0},
         {"left_shoulder_roll_joint", -0.061, 2.993, 20.0, 2.0},
         {"left_shoulder_yaw_joint", -2.556, 2.556, 20.0, 2.0},
         {"left_elbow_joint", -2.3556, 0.0, 20.0, 2.0},
         {"left_wrist_yaw_joint", -2.556, 2.556, 20.0, 2.0},
         {"left_wrist_pitch_joint", -0.558, 0.558, 20.0, 2.0},
         {"left_wrist_roll_joint", -1.571, 0.724, 20.0, 2.0},
         // Right arm joint configuration
         {"right_shoulder_pitch_joint", -3.08, 2.04, 20.0, 2.0},
         {"right_shoulder_roll_joint", -2.993, 0.061, 20.0, 2.0},
         {"right_shoulder_yaw_joint", -2.556, 2.556, 20.0, 2.0},
         {"right_elbow_joint", -2.3556, 0.0000, 20.0, 2.0},
         {"right_wrist_yaw_joint", -2.556, 2.556, 20.0, 2.0},
         {"right_wrist_pitch_joint", -0.558, 0.558, 20.0, 2.0},
         {"right_wrist_roll_joint", -0.724, 1.571, 20.0, 2.0},
     }},
    {JointArea::HEAD,
     {
         // Head joint configuration
         {"head_yaw_joint", -0.366, 0.366, 20.0, 2.0},
         {"head_pitch_joint", -0.3838, 0.3838, 20.0, 2.0},
     }},
};

/**
 * @brief Joint controller node class
 * @tparam DOFs Degrees of freedom
 * @tparam Area Joint area
 */
template <int DOFs, JointArea Area>
class JointControllerNode : public rclcpp::Node {
public:
  /**
   * @brief Constructor
   * @param node_name Node name
   * @param sub_topic Subscription topic name
   * @param pub_topic Publication topic name
   * @param qos QoS configuration
   */
  JointControllerNode(std::string node_name, std::string sub_topic,
                      std::string pub_topic,
                      rclcpp::QoS qos = rclcpp::SensorDataQoS())
      : Node(node_name), ruckig(0.002) {
    joint_info_ = robot_model[Area];
    if (joint_info_.size() != DOFs) {
      RCLCPP_ERROR(this->get_logger(), "Joint count mismatch.");
      exit(1);
    }

    // Set motion constraints for Ruckig trajectory planner
    for (int i = 0; i < DOFs; ++i) {
      input.max_velocity[i] = 1.0;     // Max velocity limit
      input.max_acceleration[i] = 1.0; // Max acceleration limit
      input.max_jerk[i] = 25.0; // Max jerk (change of acceleration) limit
    }

    // Create joint state subscriber
    sub_ = this->create_subscription<aimdk_msgs::msg::JointStateArray>(
        sub_topic, qos,
        std::bind(&JointControllerNode::JointStateCallback, this,
                  std::placeholders::_1));

    // Create joint command publisher
    pub_ = this->create_publisher<aimdk_msgs::msg::JointCommandArray>(pub_topic,
                                                                      qos);
  }

private:
  // Ruckig trajectory planner variables
  ruckig::Ruckig<DOFs> ruckig;          // Trajectory planner instance
  ruckig::InputParameter<DOFs> input;   // Input parameters
  ruckig::OutputParameter<DOFs> output; // Output parameters
  bool ruckig_initialized_ = false;   // Trajectory planner initialization flag
  std::vector<JointInfo> joint_info_; // Joint information list

  // ROS communication variables
  rclcpp::Subscription<aimdk_msgs::msg::JointStateArray>::SharedPtr
      sub_; // State subscriber
  rclcpp::Publisher<aimdk_msgs::msg::JointCommandArray>::SharedPtr
      pub_; // Command publisher

  /**
   * @brief Joint state callback function
   * @param msg Joint state message
   */
  void
  JointStateCallback(const aimdk_msgs::msg::JointStateArray::SharedPtr msg) {
    // Initialize trajectory planner on first state reception
    if (!ruckig_initialized_) {
      for (int i = 0; i < DOFs; ++i) {
        input.current_position[i] = msg->joints[i].position;
        input.current_velocity[i] = msg->joints[i].velocity;
        input.current_acceleration[i] = 0.0;
      }
      ruckig_initialized_ = true;
      RCLCPP_INFO(this->get_logger(),
                  "Ruckig trajectory planner initialization complete");
    }
  }

public:
  /**
   * @brief Set target joint position
   * @param joint_name Joint name
   * @param target_position Target position
   * @return Whether the target position was successfully set
   */
  bool SetTargetPosition(std::string joint_name, double target_position) {
    if (!ruckig_initialized_) {
      RCLCPP_WARN(this->get_logger(),
                  "Ruckig trajectory planner not initialized");
      return false;
    }

    // Find target joint and set its position
    int target_joint = -1;
    for (int i = 0; i < DOFs; ++i) {
      if (joint_info_[i].name == joint_name) {
        // Check if target position is within limits
        if (target_position < joint_info_[i].lower_limit ||
            target_position > joint_info_[i].upper_limit) {
          RCLCPP_ERROR(
              this->get_logger(),
              "Target position %.3f exceeds limit for joint %s [%.3f, %.3f]",
              target_position, joint_name.c_str(), joint_info_[i].lower_limit,
              joint_info_[i].upper_limit);
          return false;
        }
        input.target_position[i] = target_position;
        input.target_velocity[i] = 0.0;
        input.target_acceleration[i] = 0.0;
        target_joint = i;
      } else {
        input.target_position[i] = input.current_position[i];
        input.target_velocity[i] = 0.0;
        input.target_acceleration[i] = 0.0;
      }
    }

    if (target_joint == -1) {
      RCLCPP_ERROR(this->get_logger(), "Joint %s not found",
                   joint_name.c_str());
      return false;
    }

    // Perform trajectory planning and send command using Ruckig
    const double tolerance = 1e-6;
    while (g_running && rclcpp::ok() && !g_emergency_stop) {
      auto result = ruckig.update(input, output);
      if (result != ruckig::Result::Working &&
          result != ruckig::Result::Finished) {
        RCLCPP_WARN(this->get_logger(), "Trajectory planning failed");
        break;
      }

      // Update current state
      for (int i = 0; i < DOFs; ++i) {
        input.current_position[i] = output.new_position[i];
        input.current_velocity[i] = output.new_velocity[i];
        input.current_acceleration[i] = output.new_acceleration[i];
      }

      // Check if target position is reached
      if (std::abs(output.new_position[target_joint] - target_position) <
          tolerance) {
        RCLCPP_INFO(this->get_logger(), "Joint %s reached target position",
                    joint_name.c_str());
        break;
      }

      // Create and send joint command
      aimdk_msgs::msg::JointCommandArray cmd;
      cmd.joints.resize(DOFs);
      for (int i = 0; i < DOFs; ++i) {
        auto &joint = joint_info_[i];
        cmd.joints[i].name = joint.name;
        cmd.joints[i].position = output.new_position[i];
        cmd.joints[i].velocity = output.new_velocity[i];
        cmd.joints[i].stiffness = joint.kp;
        cmd.joints[i].damping = joint.kd;
      }
      pub_->publish(cmd);

      // Short delay to avoid excessive CPU usage
      std::this_thread::sleep_for(std::chrono::milliseconds(2));
    }

    return true;
  }

  /**
   * @brief Safely stop all joints
   */
  void safe_stop() {
    if (!ruckig_initialized_) {
      RCLCPP_WARN(this->get_logger(), "Ruckig trajectory planner not "
                                      "initialized, cannot perform safe stop");
      return;
    }

    RCLCPP_INFO(this->get_logger(), "Performing safe stop...");

    // Set all joint target positions to current positions
    for (int i = 0; i < DOFs; ++i) {
      input.target_position[i] = input.current_position[i];
      input.target_velocity[i] = 0.0;
      input.target_acceleration[i] = 0.0;
    }

    // Send final command to ensure joints stop
    aimdk_msgs::msg::JointCommandArray cmd;
    cmd.joints.resize(DOFs);
    for (int i = 0; i < DOFs; ++i) {
      auto &joint = joint_info_[i];
      cmd.joints[i].name = joint.name;
      cmd.joints[i].position = input.current_position[i];
      cmd.joints[i].velocity = 0.0;
      cmd.joints[i].stiffness = joint.kp;
      cmd.joints[i].damping = joint.kd;
    }
    pub_->publish(cmd);

    RCLCPP_INFO(this->get_logger(), "Safe stop complete");
  }

  /**
   * @brief Emergency stop for all joints
   */
  void emergency_stop() {
    g_emergency_stop = true;
    safe_stop();
    RCLCPP_ERROR(this->get_logger(), "Emergency stop triggered");
  }
};

/**
 * @brief Main function
 */
int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);

  // Set up signal handling
  signal(SIGINT, signal_handler);
  signal(SIGTERM, signal_handler);

  try {
    // Create leg controller node
    auto leg_node = std::make_shared<JointControllerNode<12, JointArea::LEG>>(
        "leg_node", "/aima/hal/joint/leg/state", "/aima/hal/joint/leg/command");

    // Create timer node
    rclcpp::Node::SharedPtr timer_node =
        rclcpp::Node::make_shared("timer_node");
    double position = 0.8;

    // Create timer callback function
    auto timer = timer_node->create_wall_timer(std::chrono::seconds(3), [&]() {
      if (!g_running || g_emergency_stop)
        return; // If the program is shutting down or emergency stopped, do not
                // execute new actions
      position = -position;
      position = 1.3 + position;
      if (!leg_node->SetTargetPosition("left_knee_joint", position)) {
        RCLCPP_ERROR(rclcpp::get_logger("main"),
                     "Failed to set target position");
      }
    });

    // Create executor
    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(leg_node);
    executor.add_node(timer_node);

    // Main loop
    while (g_running && rclcpp::ok() && !g_emergency_stop) {
      executor.spin_once(std::chrono::milliseconds(100));
    }

    // Safely stop all joints
    RCLCPP_INFO(rclcpp::get_logger("main"), "Safely stopping all joints...");
    leg_node->safe_stop();

    // Wait a short time to ensure command transmission is complete
    std::this_thread::sleep_for(std::chrono::milliseconds(100));

    // Clean up resources
    RCLCPP_INFO(rclcpp::get_logger("main"), "Cleaning up resources...");
    leg_node.reset();
    timer_node.reset();

  } catch (const std::exception &e) {
    RCLCPP_ERROR(rclcpp::get_logger("main"), "Exception occurred: %s",
                 e.what());
    g_emergency_stop = true;
  } catch (...) {
    RCLCPP_ERROR(rclcpp::get_logger("main"), "Unknown exception occurred");
    g_emergency_stop = true;
  }

  RCLCPP_INFO(rclcpp::get_logger("main"), "Program exited safely");
  rclcpp::shutdown();
  return 0;
}

6.2.12 Keyboard Robot Control

This example enables controlling the robot’s forward, backward, and turning movements using PC keyboard input.

Use W A S D to control the walking direction, increase/decrease linear velocity (±0.2 m/s), use Q / E to increase/decrease angular velocity (±0.1 rad/s), ESC exits the program and releases terminal resources, and Space immediately resets the velocity to zero to perform an emergency stop.

Caution

Note: Before running this example, use the controller to switch the robot to Stable Standing Mode. (Standing Preparation Mode (Position-Control Standing Mode) / Locomotion Mode, press R2 + X; for other modes, refer to the mode routing diagram). Then, in the robot’s terminal, run aima em stop-app rc to disable the remote controller and prevent channel occupation.

Before enabling keyboard control, an input source must be registered (already implemented in this example).
The curse module needs to be installed before running:

  sudo apt install libncurses-dev

#include "aimdk_msgs/msg/common_request.hpp"
#include "aimdk_msgs/msg/common_response.hpp"
#include "aimdk_msgs/msg/common_state.hpp"
#include "aimdk_msgs/msg/common_task_response.hpp"
#include "aimdk_msgs/msg/mc_input_action.hpp"
#include "aimdk_msgs/msg/mc_locomotion_velocity.hpp"
#include "aimdk_msgs/msg/message_header.hpp"
#include "aimdk_msgs/srv/set_mc_input_source.hpp"

#include <algorithm>
#include <chrono>
#include <curses.h>
#include <rclcpp/rclcpp.hpp>

using aimdk_msgs::msg::McLocomotionVelocity;
using std::placeholders::_1;

class KeyboardVelocityController : public rclcpp::Node {
public:
  KeyboardVelocityController()
      : Node("keyboard_velocity_controller"), forward_velocity_(0.0),
        lateral_velocity_(0.0), angular_velocity_(0.0), step_(0.2),
        angular_step_(0.1) {
    pub_ = this->create_publisher<McLocomotionVelocity>(
        "/aima/mc/locomotion/velocity", 10);
    client_ = this->create_client<aimdk_msgs::srv::SetMcInputSource>(
        "/aimdk_5Fmsgs/srv/SetMcInputSource");
    // Register input source
    if (!register_input_source()) {
      RCLCPP_ERROR(this->get_logger(),
                   "❌ Input source registration failed, exiting");
      throw std::runtime_error("Input source registration failed");
    }
    // Initialize ncurses
    initscr();
    cbreak();
    noecho();
    keypad(stdscr, TRUE);
    nodelay(stdscr, TRUE);

    timer_ = this->create_wall_timer(
        std::chrono::milliseconds(50),
        std::bind(&KeyboardVelocityController::checkKeyAndPublish, this));

    RCLCPP_INFO(this->get_logger(),
                "Control started: W/S Forward/Backward | A/D Strafe Left/Right "
                "| Q/E Turn Left/Right | Space Stop | ESC Exit");
  }

  ~KeyboardVelocityController() {
    endwin(); // Restore terminal
  }

private:
  rclcpp::Publisher<McLocomotionVelocity>::SharedPtr pub_;
  rclcpp::Client<aimdk_msgs::srv::SetMcInputSource>::SharedPtr client_;
  rclcpp::TimerBase::SharedPtr timer_;

  float forward_velocity_, lateral_velocity_, angular_velocity_;
  const float step_, angular_step_;

  bool register_input_source() {
    const std::chrono::seconds srv_timeout(8);
    auto start_time = std::chrono::steady_clock::now();
    while (!client_->wait_for_service(std::chrono::seconds(2))) {
      if (std::chrono::steady_clock::now() - start_time > srv_timeout) {
        RCLCPP_ERROR(this->get_logger(), "Waiting for service timed out");
        return false;
      }
      RCLCPP_INFO(this->get_logger(), "Waiting for input source service...");
    }

    // Try ADD first; if it fails (likely already registered), fall back to
    // ENABLE
    if (call_input_source( // ADD (1001)
            aimdk_msgs::msg::McInputAction::INPUTACTION_ADD)) {
      RCLCPP_INFO(this->get_logger(), "✅ Input source registered (ADD).");
      return true;
    }
    RCLCPP_INFO(this->get_logger(),
                "ADD failed (likely already registered), trying ENABLE...");
    if (call_input_source( // ENABLE (2001)
            aimdk_msgs::msg::McInputAction::INPUTACTION_ENABLE)) {
      RCLCPP_INFO(this->get_logger(), "✅ Input source enabled.");
      return true;
    }
    RCLCPP_ERROR(this->get_logger(),
                 "❌ Failed to register input source via ADD or ENABLE.");
    return false;
  }

  bool call_input_source(int32_t action_value) {
    auto request =
        std::make_shared<aimdk_msgs::srv::SetMcInputSource::Request>();
    request->action.value = action_value;
    request->input_source.name = "node.keyboard";
    request->input_source.priority = 40;
    request->input_source.timeout = 1000;

    auto timeout = std::chrono::milliseconds(250);

    for (int i = 0; i < 8; i++) {
      request->request.header.stamp = this->now();
      auto future = client_->async_send_request(request);
      auto retcode = rclcpp::spin_until_future_complete(
          this->get_node_base_interface(), future, timeout);
      if (retcode != rclcpp::FutureReturnCode::SUCCESS) {
        // retry as remote peer is NOT handled well by ROS
        RCLCPP_INFO(
            this->get_logger(), "trying input source action %s (%d)... [%d]",
            action_value == aimdk_msgs::msg::McInputAction::INPUTACTION_ADD
                ? "ADD"
                : "ENABLE",
            action_value, i);
        continue;
      }
      auto response = future.get();
      return response->response.header.code == 0;
    }
    return false;
  }

  void checkKeyAndPublish() {
    int ch = getch(); // non-blocking read

    switch (ch) {
    case ' ': // Space key
      forward_velocity_ = 0.0;
      lateral_velocity_ = 0.0;
      angular_velocity_ = 0.0;
      break;
    case 'w':
      forward_velocity_ = std::min(forward_velocity_ + step_, 1.0f);
      break;
    case 's':
      forward_velocity_ = std::max(forward_velocity_ - step_, -1.0f);
      break;
    case 'a':
      lateral_velocity_ = std::min(lateral_velocity_ + step_, 1.0f);
      break;
    case 'd':
      lateral_velocity_ = std::max(lateral_velocity_ - step_, -1.0f);
      break;
    case 'q':
      angular_velocity_ = std::min(angular_velocity_ + angular_step_, 1.0f);
      break;
    case 'e':
      angular_velocity_ = std::max(angular_velocity_ - angular_step_, -1.0f);
      break;
    case 27: // ESC Key
      RCLCPP_INFO(this->get_logger(), "Exiting control");
      rclcpp::shutdown();
      return;
    }

    auto msg = std::make_unique<McLocomotionVelocity>();
    msg->header = aimdk_msgs::msg::MessageHeader();
    msg->header.stamp = this->now();
    msg->source = "node.keyboard";
    msg->forward_velocity = forward_velocity_;
    msg->lateral_velocity = lateral_velocity_;
    msg->angular_velocity = angular_velocity_;

    float fwd = forward_velocity_;
    float lat = lateral_velocity_;
    float ang = angular_velocity_;

    pub_->publish(std::move(msg));

    // Screen Output
    clear();
    mvprintw(0, 0,
             "W/S: Forward/Backward | A/D: Left/Right Strafe | Q/E: Turn "
             "Left/Right | Space: Stop | ESC: Exit");
    mvprintw(2, 0,
             "Speed Status: Forward: %.2f m/s | Lateral: %.2f m/s | Angular: "
             "%.2f rad/s",
             fwd, lat, ang);
    refresh();
  }
};

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  try {
    auto node = std::make_shared<KeyboardVelocityController>();
    rclcpp::spin(node);
  } catch (const std::exception &e) {
    RCLCPP_FATAL(rclcpp::get_logger("main"),
                 "Program exited with exception: %s", e.what());
  }
  rclcpp::shutdown();
  return 0;
}

6.2.13 Take Photo

This example uses take_photo. Before running the node, modify the camera topic from which to capture the image. After starting the node, an /images/ directory will be created, and the current frame will be saved into this directory.

#include <chrono>
#include <cv_bridge/cv_bridge.h>
#include <filesystem>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <string>

class SaveOneRaw : public rclcpp::Node {
public:
  SaveOneRaw() : Node("save_one_image"), saved_(false) {
    topic_ = declare_parameter<std::string>(
        "image_topic", "/aima/hal/sensor/stereo_head_front_left/rgb_image");

    std::filesystem::create_directories("images");

    auto qos = rclcpp::SensorDataQoS(); // BestEffort/Volatile
    sub_ = create_subscription<sensor_msgs::msg::Image>(
        topic_, qos, std::bind(&SaveOneRaw::cb, this, std::placeholders::_1));

    RCLCPP_INFO(get_logger(), "Subscribing (raw): %s", topic_.c_str());
  }

private:
  void cb(const sensor_msgs::msg::Image::SharedPtr msg) {
    if (saved_)
      return;

    try {
      // Obtain the Mat without copying by not specifying encoding
      cv_bridge::CvImageConstPtr cvp = cv_bridge::toCvShare(msg);
      cv::Mat img = cvp->image;

      // Convert to BGR for uniform saving
      if (msg->encoding == "rgb8") {
        cv::cvtColor(img, img, cv::COLOR_RGB2BGR);
      } else if (msg->encoding == "mono8") {
        cv::cvtColor(img, img, cv::COLOR_GRAY2BGR);
      } // bgr8 Use this directly; add more branches as needed to support
        // additional encodings.

      auto now = std::chrono::system_clock::now();
      auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(
                    now.time_since_epoch())
                    .count();
      std::string path = "images/frame_" + std::to_string(ms) + ".png";

      if (cv::imwrite(path, img)) {
        RCLCPP_INFO(get_logger(), "Saved: %s  (%dx%d)", path.c_str(), img.cols,
                    img.rows);
        saved_ = true;
        rclcpp::shutdown();
      } else {
        RCLCPP_ERROR(get_logger(), "cv::imwrite failed: %s", path.c_str());
      }
    } catch (const std::exception &e) {
      RCLCPP_ERROR(get_logger(), "raw decode failed: %s", e.what());
      // Do not set the saved flag; wait for the next frame
    }
  }

  std::string topic_;
  bool saved_;
  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_;
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<SaveOneRaw>());
  return 0;
}

6.2.14 Camera Streaming Example Collection

This example set provides multiple camera data subscription and processing functions, supporting data streams from depth cameras, stereo cameras, and monocular cameras.
These camera subscription examples do not provide actual application-level logic; they only print basic camera data information. If you are familiar with ROS2, you may notice that ros2 topic echo + ros2 topic hz can also achieve similar functionality. You may directly check the topic list in the SDK interface manual to start developing your own module, or you may use these camera examples as scaffolding to integrate your own business logic. All published sensor data is raw and unprocessed (e.g., without undistortion). If you need detailed sensor information (such as resolution, focal length, etc.), please refer to the camera_info topic.

Depth Camera Data Subscription

This example uses echo_camera_rgbd, subscribing to the /aima/hal/sensor/rgbd_head_front/ topic to receive depth camera data, supporting multiple data types including depth images, RGB images, compressed RGB images, and camera intrinsic parameters.

Features:

• Supports multiple data types (depth image, RGB image, compressed image, camera intrinsics)

• Real-time FPS statistics and data display

• Supports RGB video recording

• Configurable topic type selection

Supported Data Types:

• depth_image: Depth image (sensor_msgs/Image)

• rgb_image: RGB image (sensor_msgs/Image)

• rgb_image_compressed: Compressed RGB image (sensor_msgs/CompressedImage)

• camera_info: Camera intrinsic parameters (sensor_msgs/CameraInfo)

#include <algorithm>
#include <deque>
#include <iomanip>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/compressed_image.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sstream>
#include <string>
#include <vector>

// OpenCV headers for image/video writing
#include <cv_bridge/cv_bridge.h>
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/videoio.hpp>

/**
 * @brief Example of subscribing to multiple topics for the head depth camera
 *
 * You can select which topic type to subscribe to via the startup argument
 * --ros-args -p topic_type:=<type>:
 *   - depth_image: Depth image (sensor_msgs/Image)
 *   - 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)
 *   - depth_camera_info: Camera intrinsic parameters (sensor_msgs/CameraInfo)
 *
 * Examples:
 *   ros2 run examples echo_camera_rgbd --ros-args -p
 * topic_type:=rgb_image ros2
 *   run examples echo_camera_rgbd --ros-args -p
 * topic_type:=rgb_camera_info
 *
 * topic_type defaults to "rgb_image"
 *
 * See individual callbacks for more detailed comments
 */
class CameraTopicEcho : public rclcpp::Node {
public:
  CameraTopicEcho() : Node("camera_topic_echo") {
    // Select which topic type to subscribe to
    topic_type_ = declare_parameter<std::string>("topic_type", "rgb_image");
    dump_video_path_ = declare_parameter<std::string>("dump_video_path", "");
    dump_video_fps_ = declare_parameter<double>("dump_video_fps", 30.0);

    // Subscribed topics and their message layouts
    // 1. /aima/hal/sensor/rgbd_head_front/depth_image
    //    - topic_type: depth_image
    //    - message type: sensor_msgs::msg::Image
    //    - frame_id: rgbd_head_front
    //    - contents: depth image
    // 2. /aima/hal/sensor/rgbd_head_front/rgb_image
    //    - topic_type: rgb_image
    //    - message type: sensor_msgs::msg::Image
    //    - frame_id: rgbd_head_front
    //    - contents: RGB image
    // 3. /aima/hal/sensor/rgbd_head_front/rgb_image/compressed
    //    - topic_type: rgb_image_compressed
    //    - message type: sensor_msgs::msg::CompressedImage
    //    - frame_id: rgbd_head_front
    //    - contents: RGB compressed image
    // 4. /aima/hal/sensor/rgbd_head_front/rgb_camera_info
    //    - topic_type: camera_info
    //    - message type: sensor_msgs::msg::CameraInfo
    //    - frame_id: rgbd_head_front
    //    - contents: RGB camera intrinsic parameters
    // 5. /aima/hal/sensor/rgbd_head_front/depth_camera_info
    //    - topic_type: camera_info
    //    - message type: sensor_msgs::msg::CameraInfo
    //    - frame_id: rgbd_head_front
    //    - contents: RGB camera intrinsic parameters

    auto qos = rclcpp::SensorDataQoS();

    // Enable depth image subscription
    if (topic_type_ == "depth_image") {
      topic_name_ = "/aima/hal/sensor/rgbd_head_front/depth_image";
      sub_image_ = create_subscription<sensor_msgs::msg::Image>(
          topic_name_, qos,
          std::bind(&CameraTopicEcho::cb_image, this, std::placeholders::_1));
      RCLCPP_INFO(get_logger(), "✅ Subscribing Depth Image: %s",
                  topic_name_.c_str());

      // Enable RGB image subscription
    } else if (topic_type_ == "rgb_image") {
      topic_name_ = "/aima/hal/sensor/rgbd_head_front/rgb_image";
      sub_image_ = create_subscription<sensor_msgs::msg::Image>(
          topic_name_, qos,
          std::bind(&CameraTopicEcho::cb_image, this, std::placeholders::_1));
      RCLCPP_INFO(get_logger(), "✅ Subscribing RGB Image: %s",
                  topic_name_.c_str());
      if (!dump_video_path_.empty()) {
        RCLCPP_INFO(get_logger(), "📝 Will dump received images to video: %s",
                    dump_video_path_.c_str());
      }

      // Enable RGB compressed image subscription
    } else if (topic_type_ == "rgb_image_compressed") {
      topic_name_ = "/aima/hal/sensor/rgbd_head_front/rgb_image/compressed";
      sub_compressed_ = create_subscription<sensor_msgs::msg::CompressedImage>(
          topic_name_, qos,
          std::bind(&CameraTopicEcho::cb_compressed, this,
                    std::placeholders::_1));
      RCLCPP_INFO(get_logger(), "✅ Subscribing CompressedImage: %s",
                  topic_name_.c_str());

      // Enable rgb camera info subscription
    } else if (topic_type_ == "rgb_camera_info") {
      topic_name_ = "/aima/hal/sensor/rgbd_head_front/rgb_camera_info";
      // RGB-D CameraInfo subscriptions are different from other cameras.
      // The messages arrive at about 30Hz and SensorDataQoS is sufficient.
      sub_camerainfo_ = create_subscription<sensor_msgs::msg::CameraInfo>(
          topic_name_, qos,
          std::bind(&CameraTopicEcho::cb_camerainfo, this,
                    std::placeholders::_1));
      RCLCPP_INFO(get_logger(), "✅ Subscribing RGB CameraInfo: %s",
                  topic_name_.c_str());

      // Enable depth camera info subscription
    } else if (topic_type_ == "depth_camera_info") {
      topic_name_ = "/aima/hal/sensor/rgbd_head_front/depth_camera_info";
      // RGB-D CameraInfo subscriptions are different from other cameras.
      // The messages arrive at about 30Hz and SensorDataQoS is sufficient.
      sub_camerainfo_ = create_subscription<sensor_msgs::msg::CameraInfo>(
          topic_name_, qos,
          std::bind(&CameraTopicEcho::cb_camerainfo, this,
                    std::placeholders::_1));
      RCLCPP_INFO(get_logger(), "✅ Subscribing Depth CameraInfo: %s",
                  topic_name_.c_str());

      // Unknown topic_type error
    } else {
      RCLCPP_ERROR(get_logger(), "Unknown topic_type: %s", topic_type_.c_str());
      throw std::runtime_error("Unknown topic_type");
    }
  }

  ~CameraTopicEcho() override {
    if (video_writer_.isOpened()) {
      video_writer_.release();
      RCLCPP_INFO(get_logger(), "Video file closed.");
    }
  }

private:
  // Image callback (depth/RGB image)
  void cb_image(const sensor_msgs::msg::Image::SharedPtr msg) {
    update_arrivals();

    if (should_print()) {
      RCLCPP_INFO(get_logger(),
                  "📸 %s received\n"
                  "  • frame_id:        %s\n"
                  "  • stamp (sec):     %.6f\n"
                  "  • encoding:        %s\n"
                  "  • size (WxH):      %u x %u\n"
                  "  • step (bytes/row):%u\n"
                  "  • is_bigendian:    %u\n"
                  "  • recv FPS (1s):   %.1f",
                  topic_type_.c_str(), msg->header.frame_id.c_str(),
                  rclcpp::Time(msg->header.stamp).seconds(),
                  msg->encoding.c_str(), msg->width, msg->height, msg->step,
                  msg->is_bigendian, get_fps());
    }

    // Video dump is supported only for RGB images
    if (topic_type_ == "rgb_image" && !dump_video_path_.empty()) {
      dump_image_to_video(msg);
    }
  }

  // CompressedImage callback
  void cb_compressed(const sensor_msgs::msg::CompressedImage::SharedPtr msg) {
    update_arrivals();

    if (should_print()) {
      RCLCPP_INFO(get_logger(),
                  "🗜️  CompressedImage received\n"
                  "  • frame_id:        %s\n"
                  "  • stamp (sec):     %.6f\n"
                  "  • format:          %s\n"
                  "  • data size:       %zu\n"
                  "  • recv FPS (1s):   %.1f",
                  msg->header.frame_id.c_str(),
                  rclcpp::Time(msg->header.stamp).seconds(),
                  msg->format.c_str(), msg->data.size(), get_fps());
    }
  }

  // CameraInfo callback (camera intrinsic parameters)
  void cb_camerainfo(const sensor_msgs::msg::CameraInfo::SharedPtr msg) {
    // CameraInfo is typically published once; print it once
    std::ostringstream oss;
    oss << "📷 " << topic_type_ << " received\n"
        << "  • frame_id:        " << msg->header.frame_id << "\n"
        << "  • stamp (sec):     " << rclcpp::Time(msg->header.stamp).seconds()
        << "\n"
        << "  • width x height:  " << msg->width << " x " << msg->height << "\n"
        << "  • distortion_model:" << msg->distortion_model << "\n"
        << "  • D: [";
    for (size_t i = 0; i < msg->d.size(); ++i) {
      oss << msg->d[i];
      if (i + 1 < msg->d.size())
        oss << ", ";
    }
    oss << "]\n  • K: [";
    for (int i = 0; i < 9; ++i) {
      oss << msg->k[i];
      if (i + 1 < 9)
        oss << ", ";
    }
    oss << "]\n  • R: [";
    for (int i = 0; i < 9; ++i) {
      oss << msg->r[i];
      if (i + 1 < 9)
        oss << ", ";
    }
    oss << "]\n  • P: [";
    for (int i = 0; i < 12; ++i) {
      oss << msg->p[i];
      if (i + 1 < 12)
        oss << ", ";
    }
    oss << "]\n"
        << "  • binning_x: " << msg->binning_x << "\n"
        << "  • binning_y: " << msg->binning_y << "\n"
        << "  • 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 ? "true" : "false") << " }";
    RCLCPP_INFO(get_logger(), "%s", oss.str().c_str());
  }

  // Track arrival timestamps to compute FPS
  void update_arrivals() {
    const rclcpp::Time now = this->get_clock()->now();
    arrivals_.push_back(now);
    while (!arrivals_.empty() && (now - arrivals_.front()).seconds() > 1.0) {
      arrivals_.pop_front();
    }
  }
  double get_fps() const { return static_cast<double>(arrivals_.size()); }

  // Control printing frequency
  bool should_print() {
    const rclcpp::Time now = this->get_clock()->now();
    if ((now - last_print_).seconds() >= 1.0) {
      last_print_ = now;
      return true;
    }
    return false;
  }

  // Dump received images to a video file (RGB images only)
  void dump_image_to_video(const sensor_msgs::msg::Image::SharedPtr &msg) {
    cv::Mat image;
    try {
      // Obtain the Mat without copying by not specifying encoding
      cv_bridge::CvImageConstPtr cvp = cv_bridge::toCvShare(msg);
      image = cvp->image;
      // Convert to BGR for uniform saving
      if (msg->encoding == "rgb8") {
        cv::cvtColor(image, image, cv::COLOR_RGB2BGR);
      } else {
        RCLCPP_WARN(get_logger(), "image encoding not expected: %s",
                    msg->encoding.c_str());
        return;
      }
    } catch (const std::exception &e) {
      RCLCPP_WARN(get_logger(), "cv_bridge exception: %s", e.what());
      return;
    }

    // Initialize VideoWriter
    if (!video_writer_.isOpened()) {
      int fourcc = cv::VideoWriter::fourcc('m', 'p', '4', 'v');
      double fps = dump_video_fps_;
      bool ok = video_writer_.open(dump_video_path_, fourcc, fps,
                                   cv::Size(image.cols, image.rows), true);
      if (!ok) {
        RCLCPP_ERROR(get_logger(), "Failed to open video file: %s",
                     dump_video_path_.c_str());
        dump_video_path_.clear(); // stop trying
        return;
      }
      RCLCPP_INFO(get_logger(), "VideoWriter started: %s, size=%dx%d, fps=%.1f",
                  dump_video_path_.c_str(), image.cols, image.rows, fps);
    }
    video_writer_.write(image);
  }

  // Member variables
  std::string topic_type_;
  std::string topic_name_;
  std::string dump_video_path_;
  double dump_video_fps_ = 30.0;

  // Subscriptions
  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_image_;
  rclcpp::Subscription<sensor_msgs::msg::CompressedImage>::SharedPtr
      sub_compressed_;
  rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr sub_camerainfo_;

  // FPS statistics
  rclcpp::Time last_print_{0, 0, RCL_ROS_TIME};
  std::deque<rclcpp::Time> arrivals_;

  // Video writer
  cv::VideoWriter video_writer_;
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<CameraTopicEcho>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

Usage Instructions:

1. Subscribe to RGB image data:

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

2. Subscribe to camera intrinsic parameters:

   ros2 run examples echo_camera_rgbd --ros-args -p topic_type:=rgb_camera_info
   ros2 run examples echo_camera_rgbd --ros-args -p topic_type:=depth_camera_info
   

3. Record RGB video:

   # The value of dump_video_path can be changed to another path; make sure the directory exists beforehand
   ros2 run examples echo_camera_rgbd --ros-args -p topic_type:=rgb_image -p dump_video_path:=$PWD/output.mp4
   

Stereo Camera Data Subscription

This example uses echo_camera_stereo, subscribing to the /aima/hal/sensor/stereo_head_front_*/ topics to receive stereo camera data, supporting RGB images, compressed images, and camera intrinsic parameters from both the left and right cameras.

Features:

• Supports independent subscription for left and right cameras

• Real-time FPS statistics and data display

• Supports RGB video recording

• Configurable camera selection (left/right)

Supported Data Types:

• left_rgb_image: Left camera RGB image (sensor_msgs/Image)

• left_rgb_image_compressed: Left camera compressed RGB image (sensor_msgs/CompressedImage)

• left_camera_info: Left camera intrinsic parameters (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 intrinsic parameters (sensor_msgs/CameraInfo)

#include <algorithm>
#include <deque>
#include <iomanip>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/compressed_image.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sstream>
#include <string>
#include <vector>

// OpenCV headers for image/video writing
#include <cv_bridge/cv_bridge.h>
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/videoio.hpp>

/**
 * @brief Example of subscribing to multiple topics for the stereo head camera
 *
 * You can select which topic type to subscribe to via the startup argument
 * --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)
 *
 * Examples:
 *   ros2 run examples echo_camera_stereo --ros-args -p
 * topic_type:=left_rgb_image ros2 run examples echo_camera_stereo --ros-args -p
 * topic_type:=right_rgb_image ros2 run examples echo_camera_stereo --ros-args
 * -p topic_type:=left_camera_info
 *
 * topic_type defaults to "left_rgb_image"
 *
 * See individual callbacks for more detailed comments
 */
class StereoCameraTopicEcho : public rclcpp::Node {
public:
  StereoCameraTopicEcho() : Node("stereo_camera_topic_echo") {
    // Select which topic type to subscribe to
    topic_type_ =
        declare_parameter<std::string>("topic_type", "left_rgb_image");
    dump_video_path_ = declare_parameter<std::string>("dump_video_path", "");
    dump_video_fps_ = declare_parameter<double>("dump_video_fps", 10.0);

    // Subscribed topics and their message layouts
    // 1. /aima/hal/sensor/stereo_head_front_left/rgb_image
    //    - topic_type: left_rgb_image
    //    - message type: sensor_msgs::msg::Image
    //    - frame_id: stereo_head_front
    //    - child_frame_id: /
    //    - contents: left camera raw image
    // 2. /aima/hal/sensor/stereo_head_front_left/rgb_image/compressed
    //    - topic_type: left_rgb_image_compressed
    //    - message type: sensor_msgs::msg::CompressedImage
    //    - frame_id: stereo_head_front
    //    - contents: left camera compressed image
    // 3. /aima/hal/sensor/stereo_head_front_left/camera_info
    //    - topic_type: left_camera_info
    //    - message type: sensor_msgs::msg::CameraInfo
    //    - frame_id: stereo_head_front
    //    - contents: left camera intrinsic parameters
    // 4. /aima/hal/sensor/stereo_head_front_right/rgb_image
    //    - topic_type: right_rgb_image
    //    - message type: sensor_msgs::msg::Image
    //    - frame_id: stereo_head_front_right
    //    - child_frame_id: /
    //    - contents: right camera raw image
    // 5. /aima/hal/sensor/stereo_head_front_right/rgb_image/compressed
    //    - topic_type: right_rgb_image_compressed
    //    - message type: sensor_msgs::msg::CompressedImage
    //    - frame_id: stereo_head_front_right
    //    - contents: right camera compressed image
    // 6. /aima/hal/sensor/stereo_head_front_right/camera_info
    //    - topic_type: right_camera_info
    //    - message type: sensor_msgs::msg::CameraInfo
    //    - frame_id: stereo_head_front_right
    //    - contents: right camera intrinsic parameters

    // Set QoS parameters - use SensorData QoS
    auto qos = rclcpp::SensorDataQoS();

    // Enable left camera RGB image subscription
    if (topic_type_ == "left_rgb_image") {
      topic_name_ = "/aima/hal/sensor/stereo_head_front_left/rgb_image";
      sub_image_ = create_subscription<sensor_msgs::msg::Image>(
          topic_name_, qos,
          std::bind(&StereoCameraTopicEcho::cb_image, this,
                    std::placeholders::_1));
      RCLCPP_INFO(get_logger(), "✅ Subscribing Left RGB Image: %s",
                  topic_name_.c_str());
      if (!dump_video_path_.empty()) {
        RCLCPP_INFO(get_logger(), "📝 Will dump received images to video: %s",
                    dump_video_path_.c_str());
      }

      // Enable left camera RGB compressed image subscription
    } else if (topic_type_ == "left_rgb_image_compressed") {
      topic_name_ =
          "/aima/hal/sensor/stereo_head_front_left/rgb_image/compressed";
      sub_compressed_ = create_subscription<sensor_msgs::msg::CompressedImage>(
          topic_name_, qos,
          std::bind(&StereoCameraTopicEcho::cb_compressed, this,
                    std::placeholders::_1));
      RCLCPP_INFO(get_logger(), "✅ Subscribing Left CompressedImage: %s",
                  topic_name_.c_str());

      // Enable left camera info subscription
    } else if (topic_type_ == "left_camera_info") {

      topic_name_ = "/aima/hal/sensor/stereo_head_front_left/camera_info";
      // CameraInfo subscriptions must use reliable + transient_local
      // QoS in order to receive latched/history messages (even if only one
      // message was published). Here we use keep_last(1) + reliable
      // + transient_local.
      sub_camerainfo_ = create_subscription<sensor_msgs::msg::CameraInfo>(
          topic_name_,
          rclcpp::QoS(rclcpp::KeepLast(1)).reliable().transient_local(),
          std::bind(&StereoCameraTopicEcho::cb_camerainfo, this,
                    std::placeholders::_1));
      RCLCPP_INFO(get_logger(),
                  "✅ Subscribing Left CameraInfo (with transient_local): %s",
                  topic_name_.c_str());

      // Enable right camera RGB image subscription
    } else if (topic_type_ == "right_rgb_image") {
      topic_name_ = "/aima/hal/sensor/stereo_head_front_right/rgb_image";
      sub_image_ = create_subscription<sensor_msgs::msg::Image>(
          topic_name_, qos,
          std::bind(&StereoCameraTopicEcho::cb_image, this,
                    std::placeholders::_1));
      RCLCPP_INFO(get_logger(), "✅ Subscribing Right RGB Image: %s",
                  topic_name_.c_str());
      if (!dump_video_path_.empty()) {
        RCLCPP_INFO(get_logger(), "📝 Will dump received images to video: %s",
                    dump_video_path_.c_str());
      }

      // Enable right camera RGB compressed image subscription
    } else if (topic_type_ == "right_rgb_image_compressed") {
      topic_name_ =
          "/aima/hal/sensor/stereo_head_front_right/rgb_image/compressed";
      sub_compressed_ = create_subscription<sensor_msgs::msg::CompressedImage>(
          topic_name_, qos,
          std::bind(&StereoCameraTopicEcho::cb_compressed, this,
                    std::placeholders::_1));
      RCLCPP_INFO(get_logger(), "✅ Subscribing Right CompressedImage: %s",
                  topic_name_.c_str());

      // Enable right camera info subscription
    } else if (topic_type_ == "right_camera_info") {
      topic_name_ = "/aima/hal/sensor/stereo_head_front_right/camera_info";
      // CameraInfo subscriptions must use reliable + transient_local
      // QoS in order to receive latched/history messages (even if only one
      // message was published). Here we use keep_last(1) + reliable
      // + transient_local.
      sub_camerainfo_ = create_subscription<sensor_msgs::msg::CameraInfo>(
          topic_name_,
          rclcpp::QoS(rclcpp::KeepLast(1)).reliable().transient_local(),
          std::bind(&StereoCameraTopicEcho::cb_camerainfo, this,
                    std::placeholders::_1));
      RCLCPP_INFO(get_logger(),
                  "✅ Subscribing Right CameraInfo (with transient_local): %s",
                  topic_name_.c_str());

      // Unknown topic_type error
    } else {
      RCLCPP_ERROR(get_logger(), "Unknown topic_type: %s", topic_type_.c_str());
      throw std::runtime_error("Unknown topic_type");
    }
  }

  ~StereoCameraTopicEcho() override {
    if (video_writer_.isOpened()) {
      video_writer_.release();
      RCLCPP_INFO(get_logger(), "Video file closed.");
    }
  }

private:
  // Image callback (left/right RGB image)
  void cb_image(const sensor_msgs::msg::Image::SharedPtr msg) {
    update_arrivals();

    if (should_print()) {
      RCLCPP_INFO(get_logger(),
                  "📸 %s received\n"
                  "  • frame_id:        %s\n"
                  "  • stamp (sec):     %.6f\n"
                  "  • encoding:        %s\n"
                  "  • size (WxH):      %u x %u\n"
                  "  • step (bytes/row):%u\n"
                  "  • is_bigendian:    %u\n"
                  "  • recv FPS (1s):   %.1f",
                  topic_type_.c_str(), msg->header.frame_id.c_str(),
                  rclcpp::Time(msg->header.stamp).seconds(),
                  msg->encoding.c_str(), msg->width, msg->height, msg->step,
                  msg->is_bigendian, get_fps());
    }

    // Video dump is supported only for RGB images
    if ((topic_type_ == "left_rgb_image" || topic_type_ == "right_rgb_image") &&
        !dump_video_path_.empty()) {
      dump_image_to_video(msg);
    }
  }

  // CompressedImage callback (left/right RGB compressed image)
  void cb_compressed(const sensor_msgs::msg::CompressedImage::SharedPtr msg) {
    update_arrivals();

    if (should_print()) {
      RCLCPP_INFO(get_logger(),
                  "🗜️  %s received\n"
                  "  • frame_id:        %s\n"
                  "  • stamp (sec):     %.6f\n"
                  "  • format:          %s\n"
                  "  • data size:       %zu\n"
                  "  • recv FPS (1s):   %.1f",
                  topic_type_.c_str(), msg->header.frame_id.c_str(),
                  rclcpp::Time(msg->header.stamp).seconds(),
                  msg->format.c_str(), msg->data.size(), get_fps());
    }
  }

  // CameraInfo callback (left/right camera intrinsic parameters)
  void cb_camerainfo(const sensor_msgs::msg::CameraInfo::SharedPtr msg) {
    // CameraInfo is typically published once; print it once
    std::ostringstream oss;
    oss << "📷 " << topic_type_ << " received\n"
        << "  • frame_id:        " << msg->header.frame_id << "\n"
        << "  • stamp (sec):     " << rclcpp::Time(msg->header.stamp).seconds()
        << "\n"
        << "  • width x height:  " << msg->width << " x " << msg->height << "\n"
        << "  • distortion_model:" << msg->distortion_model << "\n"
        << "  • D: [";
    for (size_t i = 0; i < msg->d.size(); ++i) {
      oss << msg->d[i];
      if (i + 1 < msg->d.size())
        oss << ", ";
    }
    oss << "]\n  • K: [";
    for (int i = 0; i < 9; ++i) {
      oss << msg->k[i];
      if (i + 1 < 9)
        oss << ", ";
    }
    oss << "]\n  • R: [";
    for (int i = 0; i < 9; ++i) {
      oss << msg->r[i];
      if (i + 1 < 9)
        oss << ", ";
    }
    oss << "]\n  • P: [";
    for (int i = 0; i < 12; ++i) {
      oss << msg->p[i];
      if (i + 1 < 12)
        oss << ", ";
    }
    oss << "]\n"
        << "  • binning_x: " << msg->binning_x << "\n"
        << "  • binning_y: " << msg->binning_y << "\n"
        << "  • 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 ? "true" : "false") << " }";
    RCLCPP_INFO(get_logger(), "%s", oss.str().c_str());
  }

  // Track arrival timestamps to compute FPS
  void update_arrivals() {
    const rclcpp::Time now = this->get_clock()->now();
    arrivals_.push_back(now);
    while (!arrivals_.empty() && (now - arrivals_.front()).seconds() > 1.0) {
      arrivals_.pop_front();
    }
  }
  double get_fps() const { return static_cast<double>(arrivals_.size()); }

  // Control printing frequency
  bool should_print() {
    const rclcpp::Time now = this->get_clock()->now();
    if ((now - last_print_).seconds() >= 1.0) {
      last_print_ = now;
      return true;
    }
    return false;
  }

  // Dump received images to a video file (RGB images only)
  void dump_image_to_video(const sensor_msgs::msg::Image::SharedPtr &msg) {
    cv::Mat image;
    try {
      // Obtain the Mat without copying by not specifying encoding
      cv_bridge::CvImageConstPtr cvp = cv_bridge::toCvShare(msg);
      image = cvp->image;
      // Convert to BGR for uniform saving
      if (msg->encoding == "rgb8") {
        cv::cvtColor(image, image, cv::COLOR_RGB2BGR);
      } else {
        RCLCPP_WARN(get_logger(), "image encoding not expected: %s",
                    msg->encoding.c_str());
        return;
      }
    } catch (const std::exception &e) {
      RCLCPP_WARN(get_logger(), "cv_bridge exception: %s", e.what());
      return;
    }

    // Initialize VideoWriter
    if (!video_writer_.isOpened()) {
      int fourcc = cv::VideoWriter::fourcc('m', 'p', '4', 'v');
      double fps = dump_video_fps_;
      bool ok = video_writer_.open(dump_video_path_, fourcc, fps,
                                   cv::Size(image.cols, image.rows), true);
      if (!ok) {
        RCLCPP_ERROR(get_logger(), "Failed to open video file: %s",
                     dump_video_path_.c_str());
        dump_video_path_.clear(); // stop trying
        return;
      }
      RCLCPP_INFO(get_logger(), "VideoWriter started: %s, size=%dx%d, fps=%.1f",
                  dump_video_path_.c_str(), image.cols, image.rows, fps);
    }
    video_writer_.write(image);
  }

  // Member variables
  std::string topic_type_;
  std::string topic_name_;
  std::string dump_video_path_;
  double dump_video_fps_ = 10.0;

  // Subscriptions
  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_image_;
  rclcpp::Subscription<sensor_msgs::msg::CompressedImage>::SharedPtr
      sub_compressed_;
  rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr sub_camerainfo_;

  // FPS statistics
  rclcpp::Time last_print_{0, 0, RCL_ROS_TIME};
  std::deque<rclcpp::Time> arrivals_;

  // Video writer
  cv::VideoWriter video_writer_;
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<StereoCameraTopicEcho>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

Usage Instructions:

1. Subscribe to left camera RGB image:

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

2. Subscribe to right camera RGB image:

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

3. Subscribe to left camera intrinsic parameters:

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

4. Record left camera video:

   # The value of dump_video_path can be changed to another path; make sure the directory exists beforehand
   ros2 run examples echo_camera_stereo --ros-args -p topic_type:=left_rgb_image -p dump_video_path:=$PWD/left_camera.mp4
   

Rear Head Monocular Camera Data Subscription

This example uses echo_camera_head_rear, subscribing to the /aima/hal/sensor/rgb_head_rear/ topic to receive data from the robot’s rear head monocular camera, supporting RGB images (with/without mask), compressed images, and camera intrinsic parameters.

Features:

• Supports rear head camera data subscription

• Real-time FPS statistics and data display

• Supports RGB video recording with/without obstructed area masked

• Configurable topic type selection

• Detects installation orientation and applies rotation compensation

Supported Data Types:

• rgb_image: RGB image (sensor_msgs/Image)

• rgb_image_compressed: Compressed RGB image (sensor_msgs/CompressedImage)

• camera_info: Camera intrinsic parameters (sensor_msgs/CameraInfo)

#include <algorithm>
#include <deque>
#include <filesystem>
#include <fstream>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <regex>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/compressed_image.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sstream>
#include <string>
#include <vector>

#include <cv_bridge/cv_bridge.h>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/videoio.hpp>

/**
 * @brief Example of subscribing to multiple topics for the rear head monocular
 * camera
 *
 * You can select which topic type to subscribe to via the startup argument
 * --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)
 *
 * Examples:
 *   ros2 run examples echo_camera_head_rear --ros-args -p topic_type:=rgb_image
 *   ros2 run examples echo_camera_head_rear --ros-args -p
 * topic_type:=rgb_image_compressed ros2 run examples echo_camera_head_rear
 * --ros-args -p topic_type:=camera_info
 *
 * topic_type defaults to "rgb_image"
 *
 * Note: Some rear cameras are rotated by 90 degrees. This example
 * automatically detects them and applies rotation compensation
 * when needed.
 */
class HeadRearCameraTopicEcho : public rclcpp::Node {
public:
  HeadRearCameraTopicEcho() : Node("head_rear_camera_topic_echo") {
    topic_type_ = declare_parameter<std::string>("topic_type", "rgb_image");
    dump_video_path_ = declare_parameter<std::string>("dump_video_path", "");
    dump_video_fps_ = declare_parameter<double>("dump_video_fps", 10.0);
    with_mask_ = declare_parameter<bool>("with_mask", false);
    auto_rotate_ = declare_parameter<bool>("auto_rotate", true);

    auto qos = rclcpp::SensorDataQoS();

    if (auto_rotate_) {
      detect_rotate_from_file();
    }

    if (with_mask_ && !dump_video_path_.empty()) {
      std::string mask_file = need_rotation_ ? "rgb_head_rear_mask_t35.png"
                                             : "rgb_head_rear_mask.png";
      auto mask_path =
          std::filesystem::read_symlink("/proc/self/exe").parent_path() /
          "data" / mask_file;
      mask_image_ = cv::imread(mask_path, cv::IMREAD_GRAYSCALE);
      if (mask_image_.empty()) {
        RCLCPP_ERROR(get_logger(), "Failed to load mask file from %s",
                     mask_path.c_str());
        throw std::runtime_error("Failed to load mask file");
      }
      RCLCPP_INFO(get_logger(), "🎭 Loaded mask: %s", mask_file.c_str());
    }

    if (topic_type_ == "rgb_image") {
      topic_name_ = "/aima/hal/sensor/rgb_head_rear/rgb_image";
      sub_image_ = create_subscription<sensor_msgs::msg::Image>(
          topic_name_, qos,
          std::bind(&HeadRearCameraTopicEcho::cb_image, this,
                    std::placeholders::_1));
      RCLCPP_INFO(get_logger(), "✅ Subscribing RGB Image: %s",
                  topic_name_.c_str());
      if (!dump_video_path_.empty()) {
        RCLCPP_INFO(get_logger(),
                    "📝 Will dump received images %s mask %s to video: %s",
                    (with_mask_ ? "with" : "without"),
                    (need_rotation_ ? "with rotation compensation"
                                    : "without rotation"),
                    dump_video_path_.c_str());
      }
    }

    else if (topic_type_ == "rgb_image_compressed") {
      topic_name_ = "/aima/hal/sensor/rgb_head_rear/rgb_image/compressed";
      sub_compressed_ = create_subscription<sensor_msgs::msg::CompressedImage>(
          topic_name_, qos,
          std::bind(&HeadRearCameraTopicEcho::cb_compressed, this,
                    std::placeholders::_1));
      RCLCPP_INFO(get_logger(), "✅ Subscribing CompressedImage: %s",
                  topic_name_.c_str());

    } else if (topic_type_ == "camera_info") {
      topic_name_ = "/aima/hal/sensor/rgb_head_rear/camera_info";
      sub_camerainfo_ = create_subscription<sensor_msgs::msg::CameraInfo>(
          topic_name_,
          rclcpp::QoS(rclcpp::KeepLast(1)).reliable().transient_local(),
          std::bind(&HeadRearCameraTopicEcho::cb_camerainfo, this,
                    std::placeholders::_1));
      RCLCPP_INFO(get_logger(),
                  "✅ Subscribing CameraInfo (with transient_local): %s",
                  topic_name_.c_str());

    } else {
      RCLCPP_ERROR(get_logger(), "Unknown topic_type: %s", topic_type_.c_str());
      throw std::runtime_error("Unknown topic_type");
    }
  }

  ~HeadRearCameraTopicEcho() override {
    if (video_writer_.isOpened()) {
      video_writer_.release();
      RCLCPP_INFO(get_logger(), "Video file closed.");
    }
  }

private:
  void detect_rotate_from_file() {
    std::string hw_file = "/agibot/data/info/hardware_version";
    std::ifstream file(hw_file);

    if (file.is_open()) {
      std::getline(file, hw_version_);
      hw_version_.erase(hw_version_.find_last_not_of(" \n\r\t") + 1);
      RCLCPP_INFO(get_logger(), "📋 Hardware version from file: '%s'",
                  hw_version_.c_str());
      check_rotation(hw_version_);
    } else {
      RCLCPP_WARN(get_logger(), "⚠️  Hardware version file not found: %s",
                  hw_file.c_str());
    }
  }

  void check_rotation(const std::string &version) {
    std::regex rear_rotation_pattern("t3[d.]5", std::regex::icase);
    if (std::regex_search(version, rear_rotation_pattern)) {
      need_rotation_ = true;
      RCLCPP_INFO(get_logger(), "🔄 Rotation detected: Rear camera rotation "
                                "compensation enabled (90° counter-clockwise)");
    } else {
      RCLCPP_INFO(
          get_logger(),
          "✅ No rotation detected (%s): No rotation compensation needed",
          version.c_str());
    }
  }

  cv::Mat rotate_image_90_ccw(const cv::Mat &image) {
    cv::Mat rotated;
    cv::rotate(image, rotated, cv::ROTATE_90_COUNTERCLOCKWISE);
    return rotated;
  }

  void cb_image(const sensor_msgs::msg::Image::SharedPtr msg) {
    update_arrivals();

    if (should_print()) {
      std::string rotation_note = need_rotation_ ? " (rotated 90° CCW)" : "";
      RCLCPP_INFO(get_logger(),
                  "📸 %s received%s\n"
                  "  • frame_id:        %s\n"
                  "  • stamp (sec):     %.6f\n"
                  "  • encoding:        %s\n"
                  "  • size (WxH):      %u x %u\n"
                  "  • step (bytes/row):%u\n"
                  "  • is_bigendian:    %u\n"
                  "  • recv FPS (1s):   %.1f",
                  topic_type_.c_str(), rotation_note.c_str(),
                  msg->header.frame_id.c_str(),
                  rclcpp::Time(msg->header.stamp).seconds(),
                  msg->encoding.c_str(), msg->width, msg->height, msg->step,
                  msg->is_bigendian, get_fps());
    }

    if (topic_type_ == "rgb_image" && !dump_video_path_.empty()) {
      dump_image_to_video(msg);
    }
  }

  void cb_compressed(const sensor_msgs::msg::CompressedImage::SharedPtr msg) {
    update_arrivals();

    if (should_print()) {
      RCLCPP_INFO(get_logger(),
                  "🗜️  %s received\n"
                  "  • frame_id:        %s\n"
                  "  • stamp (sec):     %.6f\n"
                  "  • format:          %s\n"
                  "  • data size:       %zu\n"
                  "  • recv FPS (1s):   %.1f",
                  topic_type_.c_str(), msg->header.frame_id.c_str(),
                  rclcpp::Time(msg->header.stamp).seconds(),
                  msg->format.c_str(), msg->data.size(), get_fps());
    }
  }

  void cb_camerainfo(const sensor_msgs::msg::CameraInfo::SharedPtr msg) {
    std::ostringstream oss;
    oss << "📷 " << topic_type_ << " received\n"
        << "  • frame_id:        " << msg->header.frame_id << "\n"
        << "  • stamp (sec):     " << rclcpp::Time(msg->header.stamp).seconds()
        << "\n"
        << "  • width x height:  " << msg->width << " x " << msg->height << "\n"
        << "  • distortion_model:" << msg->distortion_model << "\n"
        << "  • D: [";
    for (size_t i = 0; i < msg->d.size(); ++i) {
      oss << msg->d[i];
      if (i + 1 < msg->d.size())
        oss << ", ";
    }
    oss << "]\n  • K: [";
    for (int i = 0; i < 9; ++i) {
      oss << msg->k[i];
      if (i + 1 < 9)
        oss << ", ";
    }
    oss << "]\n  • R: [";
    for (int i = 0; i < 9; ++i) {
      oss << msg->r[i];
      if (i + 1 < 9)
        oss << ", ";
    }
    oss << "]\n  • P: [";
    for (int i = 0; i < 12; ++i) {
      oss << msg->p[i];
      if (i + 1 < 12)
        oss << ", ";
    }
    oss << "]\n"
        << "  • binning_x: " << msg->binning_x << "\n"
        << "  • binning_y: " << msg->binning_y << "\n"
        << "  • 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 ? "true" : "false") << " }";
    RCLCPP_INFO(get_logger(), "%s", oss.str().c_str());
  }

  void update_arrivals() {
    const rclcpp::Time now = this->get_clock()->now();
    arrivals_.push_back(now);
    while (!arrivals_.empty() && (now - arrivals_.front()).seconds() > 1.0) {
      arrivals_.pop_front();
    }
  }
  double get_fps() const { return static_cast<double>(arrivals_.size()); }

  bool should_print() {
    const rclcpp::Time now = this->get_clock()->now();
    if ((now - last_print_).seconds() >= 1.0) {
      last_print_ = now;
      return true;
    }
    return false;
  }

  void dump_image_to_video(const sensor_msgs::msg::Image::SharedPtr &msg) {
    cv::Mat image;
    try {
      cv_bridge::CvImageConstPtr cvp = cv_bridge::toCvShare(msg);
      image = cvp->image;
      if (msg->encoding == "rgb8") {
        cv::cvtColor(image, image, cv::COLOR_RGB2BGR);
      } else {
        RCLCPP_WARN(get_logger(), "image encoding not expected: %s",
                    msg->encoding.c_str());
        return;
      }

      if (need_rotation_) {
        image = rotate_image_90_ccw(image);
      }

      if (with_mask_) {
        image.setTo(cv::Scalar(0, 0, 0), mask_image_ == 0);
      }
    } catch (const std::exception &e) {
      RCLCPP_WARN(get_logger(), "cv_bridge exception: %s", e.what());
      return;
    }

    if (!video_writer_.isOpened()) {
      int fourcc = cv::VideoWriter::fourcc('m', 'p', '4', 'v');
      double fps = dump_video_fps_;
      bool ok = video_writer_.open(dump_video_path_, fourcc, fps,
                                   cv::Size(image.cols, image.rows), true);
      if (!ok) {
        RCLCPP_ERROR(get_logger(), "Failed to open video file: %s",
                     dump_video_path_.c_str());
        dump_video_path_.clear();
        return;
      }
      RCLCPP_INFO(get_logger(), "VideoWriter started: %s, size=%dx%d, fps=%.1f",
                  dump_video_path_.c_str(), image.cols, image.rows, fps);
    }
    video_writer_.write(image);
  }

  std::string topic_type_;
  std::string topic_name_;
  std::string dump_video_path_;
  double dump_video_fps_ = 10.0;
  bool with_mask_;
  bool auto_rotate_;
  bool need_rotation_ = false;
  std::string hw_version_;
  cv::Mat mask_image_;

  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_image_;
  rclcpp::Subscription<sensor_msgs::msg::CompressedImage>::SharedPtr
      sub_compressed_;
  rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr sub_camerainfo_;

  rclcpp::Time last_print_{0, 0, RCL_ROS_TIME};
  std::deque<rclcpp::Time> arrivals_;

  cv::VideoWriter video_writer_;
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<HeadRearCameraTopicEcho>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

Usage Instructions:

1. Subscribe to RGB image data:

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

2. Subscribe to compressed image data:

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

3. Subscribe to camera intrinsic parameters:

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

4. Record video:

   # The value of dump_video_path can be changed to another path; make sure the directory exists beforehand
   ros2 run examples echo_camera_head_rear --ros-args -p topic_type:=rgb_image -p dump_video_path:=$PWD/rear_camera.mp4
   

5. Record video and mask obstructed area:

   # The value of dump_video_path can be changed to another path; make sure the directory exists beforehand
   ros2 run examples echo_camera_head_rear --ros-args -p topic_type:=rgb_image -p with_mask:=true -p dump_video_path:=$PWD/rear_camera.mp4
   

6. Disable auto rotation compensation (get original image):

   ros2 run examples echo_camera_head_rear --ros-args -p topic_type:=rgb_image -p auto_rotate:=false
   

Application Scenarios:

• Face recognition and tracking

• Object detection and recognition

• Visual SLAM

• Image processing and computer vision algorithm development

• Robot visual navigation

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

//
// Created by agiuser on 2026/1/23.
//

#include <aimdk_msgs/msg/touch_state.hpp>
#include <rclcpp/rclcpp.hpp>
#include <string>

class TouchStateSubscriber : public rclcpp::Node {
public:
  TouchStateSubscriber() : Node("touch_state_subscriber") {
    subscription_ = this->create_subscription<aimdk_msgs::msg::TouchState>(
        "/aima/hal/sensor/touch_head", 10,
        std::bind(&TouchStateSubscriber::touch_callback, this,
                  std::placeholders::_1));

    RCLCPP_INFO(this->get_logger(), "TouchState subscriber started, listening "
                                    "to /aima/hal/sensor/touch_head");
  }

private:
  void touch_callback(const aimdk_msgs::msg::TouchState::SharedPtr msg) {
    // print message info
    std::string event_str = get_event_type_string(msg->event_type);
    RCLCPP_INFO(this->get_logger(), "Timestamp: %d.%09d, Event: %s (%d)",
                msg->header.stamp.sec, msg->header.stamp.nanosec,
                event_str.c_str(), msg->event_type);
  }

  std::string get_event_type_string(uint8_t event_type) {
    switch (event_type) {
    case aimdk_msgs::msg::TouchState::UNKNOWN:
      return "UNKNOWN";
    case aimdk_msgs::msg::TouchState::IDLE:
      return "IDLE";
    case aimdk_msgs::msg::TouchState::TOUCH:
      return "TOUCH";
    default:
      return "INVALID";
    }
  }
  rclcpp::Subscription<aimdk_msgs::msg::TouchState>::SharedPtr subscription_;
};

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<TouchStateSubscriber>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

Usage Instructions:

ros2 run examples echo_head_touch_sensor

Output Example:

...
[INFO] [1779174857.614539691] [touch_state_subscriber]: Timestamp: 1779174857.613006485, Event: IDLE (1)
[INFO] [1779174857.623661452] [touch_state_subscriber]: Timestamp: 1779174857.623052293, Event: TOUCH (2)

6.2.16 LiDAR Data Subscription

This example uses echo_lidar_data, subscribing to the /aima/hal/sensor/lidar_chest_front/ topic to receive LiDAR data, supporting both point cloud data and IMU data types.

Features:

• Supports LiDAR point cloud data subscription

• Supports LiDAR IMU data subscription

• Real-time FPS statistics and data display

• Configurable topic type selection

• Detailed output of data field information

Supported Data Types:

• PointCloud2: LiDAR point cloud data (sensor_msgs/PointCloud2)

• Imu: LiDAR IMU data (sensor_msgs/Imu)

Technical Implementation:

• Uses SensorDataQoS configuration (BEST_EFFORT + VOLATILE)

• Supports parsing and displaying point cloud field information

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

• Provides detailed debugging log output

Application Scenarios:

• LiDAR data acquisition and analysis

• Point cloud data processing and visualization

• Robot navigation and localization

• SLAM algorithm development

• Environmental perception and mapping

#include <deque>
#include <iomanip>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sstream>
#include <string>
#include <vector>

/**
 * @brief Example for subscribing to chest LIDAR data
 *
 * Supports subscribing to the following topics:
 *   1. /aima/hal/sensor/lidar_chest_front/lidar_pointcloud
 *      - Data type: sensor_msgs::msg::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::msg::Imu
 *      - frame_id: lidar_imu_chest_front
 *      - Content: LIDAR IMU data
 *
 * You can select the topic type to subscribe to using the launch parameter
 * --ros-args -p topic_type:=<type>:
 *   - pointcloud: Subscribe to LIDAR point cloud
 *   - imu: Subscribe to LIDAR IMU
 * The default topic_type is pointcloud
 */
class LidarChestEcho : public rclcpp::Node {
public:
  LidarChestEcho() : Node("lidar_chest_echo") {
    topic_type_ = declare_parameter<std::string>("topic_type", "pointcloud");

    // SensorDataQoS: BEST_EFFORT + VOLATILE
    auto qos = rclcpp::SensorDataQoS();

    if (topic_type_ == "pointcloud") {
      topic_name_ = "/aima/hal/sensor/lidar_chest_front/lidar_pointcloud";
      sub_pointcloud_ = create_subscription<sensor_msgs::msg::PointCloud2>(
          topic_name_, qos,
          std::bind(&LidarChestEcho::cb_pointcloud, this,
                    std::placeholders::_1));
      RCLCPP_INFO(get_logger(), "✅ Subscribing LIDAR PointCloud2: %s",
                  topic_name_.c_str());
    } else if (topic_type_ == "imu") {
      topic_name_ = "/aima/hal/sensor/lidar_chest_front/imu";
      sub_imu_ = create_subscription<sensor_msgs::msg::Imu>(
          topic_name_, qos,
          std::bind(&LidarChestEcho::cb_imu, this, std::placeholders::_1));
      RCLCPP_INFO(get_logger(), "✅ Subscribing LIDAR IMU: %s",
                  topic_name_.c_str());
    } else {
      RCLCPP_ERROR(get_logger(), "Unknown topic_type: %s", topic_type_.c_str());
      throw std::runtime_error("Unknown topic_type");
    }
  }

private:
  // PointCloud2 callback
  void cb_pointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
    update_arrivals();

    if (should_print()) {
      std::ostringstream oss;
      oss << std::fixed << std::boolalpha << std::setfill('0');
      oss << "🟢 LIDAR PointCloud2 received\n"
          << "  • frame_id:        " << msg->header.frame_id << "\n"
          << "  • stamp (sec):     " << msg->header.stamp.sec << "."
          << std::setw(9) << msg->header.stamp.nanosec << "\n"
          << "  • width x height:  " << msg->width << " x " << msg->height
          << "\n"
          << "  • point_step:      " << msg->point_step << "\n"
          << "  • row_step:        " << msg->row_step << "\n"
          << "  • fields:          ";
      for (const auto &f : msg->fields)
        oss << f.name << "(" << (int)f.datatype << ") ";
      oss << "\n  • is_bigendian:    " << msg->is_bigendian
          << "\n  • is_dense:        " << msg->is_dense
          << "\n  • data size:       " << msg->data.size()
          << std::setprecision(1) // fps precision .1f
          << "\n  • recv FPS (1s):   " << get_fps();
      RCLCPP_INFO(get_logger(), "%s", oss.str().c_str());
    }
  }

  // IMU callback
  void cb_imu(const sensor_msgs::msg::Imu::SharedPtr msg) {
    update_arrivals();

    if (should_print()) {
      std::ostringstream oss;
      oss << std::fixed << std::setfill('0');
      oss << "🟢 LIDAR IMU received\n"
          << "  • frame_id:        " << msg->header.frame_id << "\n"
          << "  • stamp (sec):     " << msg->header.stamp.sec << "."
          << std::setw(9) << msg->header.stamp.nanosec << "\n"
          << std::setprecision(6) // data precision .6f
          << "  • orientation:     [" << msg->orientation.x << ", "
          << msg->orientation.y << ", " << msg->orientation.z << ", "
          << msg->orientation.w << "]\n"
          << "  • angular_velocity:[" << msg->angular_velocity.x << ", "
          << msg->angular_velocity.y << ", " << msg->angular_velocity.z << "]\n"
          << "  • linear_accel:    [" << msg->linear_acceleration.x << ", "
          << msg->linear_acceleration.y << ", " << msg->linear_acceleration.z
          << "]\n"
          << std::setprecision(1) // fps precision .1f
          << "  • recv FPS (1s):   " << get_fps();
      RCLCPP_INFO(get_logger(), "%s", oss.str().c_str());
    }
  }

  // Update FPS statistics
  void update_arrivals() {
    const rclcpp::Time now = this->get_clock()->now();
    arrivals_.push_back(now);
    while (!arrivals_.empty() && (now - arrivals_.front()).seconds() > 1.0) {
      arrivals_.pop_front();
    }
  }
  double get_fps() const { return static_cast<double>(arrivals_.size()); }

  // Control print frequency
  bool should_print() {
    const rclcpp::Time now = this->get_clock()->now();
    if ((now - last_print_).seconds() >= 1.0) {
      last_print_ = now;
      return true;
    }
    return false;
  }

  // Member variables
  std::string topic_type_;
  std::string topic_name_;

  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr
      sub_pointcloud_;
  rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr sub_imu_;

  rclcpp::Time last_print_{0, 0, RCL_ROS_TIME};
  std::deque<rclcpp::Time> arrivals_;
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<LidarChestEcho>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

Usage Instructions:

# Subscribe to LiDAR point cloud data
ros2 run examples echo_lidar_data --ros-args -p topic_type:=pointcloud

# Subscribe to LiDAR IMU data
ros2 run examples echo_lidar_data --ros-args -p topic_type:=imu

Output Example:

...
[INFO] [1779175640.420801950] [lidar_chest_echo]: 🟢 LIDAR PointCloud2 received
  • frame_id:        lidar_chest_front
  • stamp (sec):     1779175640.315327000
  • width x height:  24779 x 1
  • point_step:      32
  • row_step:        792928
  • fields:          x(7) y(7) z(7) intensity(7) ring(4) timestamp(8)
  • is_bigendian:    false
  • is_dense:        true
  • data size:       792928
  • recv FPS (1s):   11.0

6.2.17 IMU Data Subscription

This example uses echo_imu_data to subscribe to IMU topics for the robot’s orientation, angular velocity, and linear acceleration data, and prints the covariance diagonals of each field. It subscribes to the chest IMU by default and can be switched to the torso IMU, LiDAR IMU, or depth-camera IMU via a parameter.

Features:

• Subscribes to sensor_msgs/Imu data, outputting orientation quaternion, angular velocity, and linear acceleration

• Prints the covariance diagonals of orientation/angular velocity/acceleration (-1 means unknown)

• Measures the receive interval (recv_dt) to observe the actual arrival rate

• Switch between IMU sources via the imu_topic parameter

Supported Topics:

• /aima/hal/imu/chest/state: Chest IMU (default)

• /aima/hal/imu/torso/state: Torso IMU

• /aima/hal/sensor/lidar_chest_front/imu: LiDAR IMU

• /aima/hal/sensor/rgbd_head_front/imu: Depth-camera IMU

Technical Implementation:

• Uses SensorDataQoS (BEST_EFFORT + VOLATILE), consistent with the default QoS for sensor data

#include <chrono>
#include <iomanip>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <string>

class ImuPrinter : public rclcpp::Node {
public:
  ImuPrinter() : rclcpp::Node("imu_printer") {
    // Allow overriding the topic name via a parameter
    imu_topic_ = this->declare_parameter<std::string>(
        "imu_topic",
        "/aima/hal/imu/chest/state"); // Change to other IMU topics, e.g.
                                      // waist IMU: /aima/hal/imu/torso/state
                                      // lidar IMU:
                                      // /aima/hal/sensor/lidar_chest_front/imu

    auto qos = rclcpp::SensorDataQoS(); // BestEffort / Volatile
    sub_ = this->create_subscription<sensor_msgs::msg::Imu>(
        imu_topic_, qos,
        std::bind(&ImuPrinter::cb, this, std::placeholders::_1));

    RCLCPP_INFO(this->get_logger(), "Subscribing IMU: %s", imu_topic_.c_str());
  }

private:
  void cb(const sensor_msgs::msg::Imu::SharedPtr msg) {
    // Compute receive interval (based on wall time, for observation only)
    auto now = std::chrono::steady_clock::now();
    double dt_ms =
        last_recv_.time_since_epoch().count() == 0
            ? 0.0
            : std::chrono::duration<double, std::milli>(now - last_recv_)
                  .count();
    last_recv_ = now;

    const auto &q = msg->orientation;
    const auto &w = msg->angular_velocity;
    const auto &a = msg->linear_acceleration;

    // Print
    std::ostringstream oss;
    oss << std::fixed << std::setprecision(6);
    oss << "stamp=" << rclcpp::Time(msg->header.stamp).seconds()
        << "s  frame=" << msg->header.frame_id
        << "  recv_dt=" << std::setprecision(3) << dt_ms << "ms\n"
        << std::setprecision(6) << "  orientation (x,y,z,w): [" << q.x << ", "
        << q.y << ", " << q.z << ", " << q.w << "]\n"
        << "  angular_velocity (rad/s): [" << w.x << ", " << w.y << ", " << w.z
        << "]\n"
        << "  linear_accel (m/s^2): [" << a.x << ", " << a.y << ", " << a.z
        << "]";

    // Optional: print diagonal covariances (-1 means unknown)
    auto oc0 = msg->orientation_covariance[0];
    auto oc4 = msg->orientation_covariance[4];
    auto oc8 = msg->orientation_covariance[8];
    auto wc0 = msg->angular_velocity_covariance[0];
    auto wc4 = msg->angular_velocity_covariance[4];
    auto wc8 = msg->angular_velocity_covariance[8];
    auto ac0 = msg->linear_acceleration_covariance[0];
    auto ac4 = msg->linear_acceleration_covariance[4];
    auto ac8 = msg->linear_acceleration_covariance[8];

    oss << "\n  cov_diag(ori, ang, acc): [" << oc0 << "," << oc4 << "," << oc8
        << " | " << wc0 << "," << wc4 << "," << wc8 << " | " << ac0 << ","
        << ac4 << "," << ac8 << "]";

    RCLCPP_INFO(this->get_logger(), "%s", oss.str().c_str());
  }

  std::string imu_topic_;
  rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr sub_;
  std::chrono::steady_clock::time_point last_recv_{};
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<ImuPrinter>());
  rclcpp::shutdown();
  return 0;
}

Usage Instructions:

# Subscribe to the chest IMU (default)
ros2 run examples echo_imu_data

# Subscribe to the torso IMU
ros2 run examples echo_imu_data --ros-args -p imu_topic:=/aima/hal/imu/torso/state

# Subscribe to the LiDAR IMU
ros2 run examples echo_imu_data --ros-args -p imu_topic:=/aima/hal/sensor/lidar_chest_front/imu

Output Example:

[INFO] [1781764206.320174965] [imu_printer]: Subscribing IMU: /aima/hal/imu/chest/state
[INFO] [1781764206.318604000] [imu_printer]: stamp=1781764206.318604s  frame=base_link  recv_dt=2.039ms
  orientation (x,y,z,w): [0.087036, -0.264735, -0.019103, -0.960195]
  angular_velocity (rad/s): [-0.006531, 0.002874, 0.002391]
  linear_accel (m/s^2): [-5.013936, -1.548364, 8.280822]
  cov_diag(ori, ang, acc): [0.000000,0.000000,0.000000 | 0.000000,0.000000,0.000000 | 0.000000,0.000000,0.000000]

6.2.18 Play Video

This example uses play_video. Before running the node, you must upload the video file to the robot’s Interaction Computing Unit (PC3) (you may create a directory there to store videos, e.g. /var/tmp/videos/). Then, modify the video_path in the node program to the path of the video you want to play.

Attention

⚠️ Warning! 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 located at 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 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)

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 examples play_video  # Set video_path in the code to /var/tmp/videos/output.mp4

#include "aimdk_msgs/srv/play_video.hpp"
#include "aimdk_msgs/msg/common_request.hpp"
#include "rclcpp/rclcpp.hpp"
#include <algorithm>
#include <cctype>
#include <chrono>
#include <iostream>
#include <memory>
#include <signal.h>
#include <string>

// Global variable used for signal handling
std::shared_ptr<rclcpp::Node> g_node = nullptr;

// Signal handler function
void signal_handler(int signal) {
  if (g_node) {
    RCLCPP_INFO(g_node->get_logger(), "Received signal %d, shutting down...",
                signal);
    g_node.reset();
  }
  rclcpp::shutdown();
  exit(signal);
}

class PlayVideoClient : public rclcpp::Node {
public:
  PlayVideoClient() : Node("play_video_client") {
    client_ = this->create_client<aimdk_msgs::srv::PlayVideo>(
        "/aimdk_5Fmsgs/srv/PlayVideo");
    RCLCPP_INFO(this->get_logger(), "✅ PlayVideo client node started.");

    // Wait for the service to become available
    while (!client_->wait_for_service(std::chrono::seconds(2))) {
      RCLCPP_INFO(this->get_logger(), "⏳ Service unavailable, waiting...");
    }
    RCLCPP_INFO(this->get_logger(),
                "🟢 Service available, ready to send request.");
  }

  bool send_request(const std::string &video_path, uint8_t mode,
                    int32_t priority) {
    try {
      auto request = std::make_shared<aimdk_msgs::srv::PlayVideo::Request>();

      request->video_path = video_path;
      request->mode = mode;
      request->priority = priority;

      RCLCPP_INFO(this->get_logger(),
                  "📨 Sending request to play video: mode=%hhu video=%s", mode,
                  video_path.c_str());

      const std::chrono::milliseconds timeout(250);
      for (int i = 0; i < 8; i++) {
        request->header.header.stamp = this->now();
        auto future = client_->async_send_request(request);
        auto retcode = rclcpp::spin_until_future_complete(shared_from_this(),
                                                          future, timeout);
        if (retcode != rclcpp::FutureReturnCode::SUCCESS) {
          // retry as remote peer is NOT handled well by ROS
          RCLCPP_INFO(this->get_logger(), "trying ... [%d]", i);
          continue;
        }
        // future.done
        auto response = future.get();
        if (response->success) {
          RCLCPP_INFO(this->get_logger(),
                      "✅ Request to play video recorded successfully: %s",
                      response->message.c_str());
          return true;
        } else {
          RCLCPP_ERROR(this->get_logger(),
                       "❌ Failed to record play-video request: %s",
                       response->message.c_str());
          return false;
        }
      }
      RCLCPP_ERROR(this->get_logger(), "❌ Service call failed or timed out.");
      return false;
    } catch (const std::exception &e) {
      RCLCPP_ERROR(this->get_logger(), "Exception occurred: %s", e.what());
      return false;
    }
  }

private:
  rclcpp::Client<aimdk_msgs::srv::PlayVideo>::SharedPtr client_;
};

int main(int argc, char **argv) {
  try {
    rclcpp::init(argc, argv);

    // Set up signal handlers
    signal(SIGINT, signal_handler);
    signal(SIGTERM, signal_handler);

    std::string default_video_path =
        "/agibot/software/face_ui/bin/app/aarch64-generic/assets/mp4/"
        "emoji_x2_sleepy.mp4";
    std::string video_path;
    std::cout << "Enter video path (default: " << default_video_path << "): ";
    std::getline(std::cin, video_path);
    // Trim leading/trailing whitespace (parity with Python's .strip())
    auto not_space = [](int c) { return !std::isspace(c); };
    video_path.erase(
        video_path.begin(),
        std::find_if(video_path.begin(), video_path.end(), not_space));
    video_path.erase(
        std::find_if(video_path.rbegin(), video_path.rend(), not_space).base(),
        video_path.end());
    if (video_path.empty()) {
      video_path = default_video_path;
    }
    int32_t priority = 5;
    int mode = 2; // Loop playback
    std::cout << "Enter video play mode (1: once, 2: loop): ";
    std::cin >> mode;
    if (mode < 1 || mode > 2) {
      RCLCPP_ERROR(rclcpp::get_logger("main"), "Invalid play mode: %d", mode);
      rclcpp::shutdown();
      return 1;
    }

    g_node = std::make_shared<PlayVideoClient>();
    auto client = std::dynamic_pointer_cast<PlayVideoClient>(g_node);

    if (client) {
      client->send_request(video_path, mode, priority);
    }

    // Clean up resources
    g_node.reset();
    rclcpp::shutdown();

    return 0;
  } catch (const std::exception &e) {
    RCLCPP_ERROR(rclcpp::get_logger("main"),
                 "Program exited with exception: %s", e.what());
    return 1;
  }
}

6.2.19 Audio File Playback

This example uses play_audio, enabling playback of specified audio files, supporting WAV (PCM-encoded) and RAW PCM formats. Other formats such as MP3 are not supported.

Features:

• Supports WAV (PCM-encoded) and RAW PCM playback; other formats such as MP3 are not supported

• Supports priority control with configurable playback priority

• Supports custom file paths and playback parameters

• Provides comprehensive error handling and status feedback

Technical Implementation:

• Uses the PlayAudioFile service for audio file playback

• Supports priority settings (priority: 1–10, priority_weight: 1–100)

• Provides detailed playback status feedback

Application Scenarios:

• Audio file playback and media control

• Voice prompts and sound effect playback

• Multimedia application development

• Robot interaction audio feedback

Attention

⚠️ Warning! 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)

#include <aimdk_msgs/msg/common_state.hpp>
#include <aimdk_msgs/srv/play_audio_file.hpp>
#include <algorithm>
#include <cctype>
#include <filesystem>
#include <iostream>
#include <rclcpp/rclcpp.hpp>
#include <string>

using PlayAudioFile = aimdk_msgs::srv::PlayAudioFile;

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("play_audio_client_min");

  // 1) Service name
  const std::string service_name = "/aimdk_5Fmsgs/srv/PlayAudioFile";
  auto client = node->create_client<PlayAudioFile>(service_name);

  RCLCPP_INFO(node->get_logger(), "✅ PlayAudio client node created.");

  // 2) Input file path (prompt user if not provided as argument)
  std::string default_file = "/agibot/data/var/hal_audio/file/bingo.wav";
  std::string file_name;

  if (argc > 1) {
    file_name = argv[1];
  } else {
    std::cout << "Enter audio file path to play (default: " << default_file
              << "): ";
    if (!std::getline(std::cin, file_name) || !rclcpp::ok()) {
      std::cout << std::endl;
      rclcpp::shutdown();
      return 1;
    }
    // Trim leading/trailing whitespace (parity with Python's .strip())
    auto not_space = [](int c) { return !std::isspace(c); };
    file_name.erase(
        file_name.begin(),
        std::find_if(file_name.begin(), file_name.end(), not_space));
    file_name.erase(
        std::find_if(file_name.rbegin(), file_name.rend(), not_space).base(),
        file_name.end());
    if (file_name.empty()) {
      file_name = default_file;
    }
  }

  // 3) Build the request
  // Check if Ctrl+C was pressed during user input
  if (!rclcpp::ok()) {
    rclcpp::shutdown();
    return 1;
  }

  auto req = std::make_shared<PlayAudioFile::Request>();
  req->file.pkg_name = "demo_client";
  req->file.file_name = std::filesystem::path(file_name).filename().string();
  req->file.file_path =
      std::filesystem::path(file_name).parent_path().string() + "/";
  req->file.priority = 8;
  req->file.priority_weight = 0;

  // 4) Wait for service
  while (!client->wait_for_service(std::chrono::seconds(2))) {
    if (!rclcpp::ok()) {
      RCLCPP_ERROR(node->get_logger(),
                   "Interrupted while waiting for the service. Exiting.");
      rclcpp::shutdown();
      return 1;
    }
    RCLCPP_INFO(node->get_logger(), "⏳ Service unavailable, waiting...");
  }
  RCLCPP_INFO(node->get_logger(),
              "🟢 Service available, ready to send request.");

  RCLCPP_INFO(node->get_logger(), "📨 Sending request to play audio: %s",
              file_name.c_str());

  rclcpp::FutureReturnCode rc = rclcpp::FutureReturnCode::TIMEOUT;
  std::shared_future<PlayAudioFile::Response::SharedPtr> future;
  for (int i = 0; i < 5; ++i) {
    req->request.header.stamp = node->now();
    future = client->async_send_request(req);
    rc = rclcpp::spin_until_future_complete(node, future,
                                            std::chrono::seconds(1));
    if (rc == rclcpp::FutureReturnCode::SUCCESS) {
      break;
    }
    if (rc == rclcpp::FutureReturnCode::INTERRUPTED) {
      // Check if Ctrl+C was pressed during spin_until_future_complete
      if (rclcpp::ok()) {
        RCLCPP_WARN(node->get_logger(), "Interrupted while waiting");
      }
      rclcpp::shutdown();
      return 1;
    }
    // retry as remote peer is NOT handled well by ROS
    RCLCPP_INFO(node->get_logger(), "trying ... [%d]", i);
  }

  if (rc != rclcpp::FutureReturnCode::SUCCESS) {
    // Check if Ctrl+C was pressed, avoid using node after shutdown
    if (rclcpp::ok()) {
      RCLCPP_ERROR(node->get_logger(),
                   "❌ Service call not completed or timed out.");
    }
    rclcpp::shutdown();
    return 1;
  }

  // 5) Handle response (success is in reponse.status)
  try {
    const auto resp = future.get();
    bool success = resp->reponse.status.value ==
                   aimdk_msgs::msg::CommonState::SUCCESS; // SUCCESS (1)

    if (success) {
      RCLCPP_INFO(node->get_logger(),
                  "✅ Request to play audio file recorded successfully.");
    } else {
      // FAILURE(2): focus-apply failed; INVALID(5): invalid params;
      // other: see CommonState for details.
      const char *reason = "unknown";
      switch (resp->reponse.status.value) {
      case aimdk_msgs::msg::CommonState::FAILURE: // FAILURE (2)
        reason = "focus apply failed";
        break;
      case aimdk_msgs::msg::CommonState::INVALID: // INVALID (5)
        reason = "invalid params";
        break;
      default:
        break;
      }
      RCLCPP_ERROR(node->get_logger(),
                   "❌ Failed to record play-audio request (status=%d, %s): %s",
                   resp->reponse.status.value, reason,
                   resp->reponse.message.c_str());
    }
  } catch (const std::exception &e) {
    RCLCPP_ERROR(node->get_logger(), "Call exception: %s", e.what());
  }

  rclcpp::shutdown();
  return 0;
}

Usage Instructions:

# Play default audio file
ros2 run examples play_audio

# Play a specified audio file
# Replace /path/to/your/audio_file.wav with the actual file path on the interaction board
ros2 run examples play_audio /path/to/your/audio_file.wav

# Play TTS cached audio file
ros2 run examples play_audio /agibot/data/var/hal_audio/file/bingo.wav

Output Example:

[INFO] [1779184008.888874978] [play_audio_client_min]: ✅ PlayAudio client node created.
[INFO] [1779184008.895467698] [play_audio_client_min]: 🟢 Service available, ready to send request.
[INFO] [1779184008.897450608] [play_audio_client_min]: 📨 Sending request to play audio: /agibot/data/var/hal_audio/file/bingo.wav
[INFO] [1779184008.962054291] [play_audio_client_min]: ✅ Request to play audio file recorded successfully.

Notes:

• Ensure the audio file path is correct and the file exists

• Supported file formats: WAV (PCM-encoded), RAW PCM. Other formats such as MP3 are not supported

• Priority settings affect playback queue order

6.2.20 TTS (Text-to-Speech)

This example uses play_tts, enabling the robot to speak the provided text. Users can input any text depending on the scenario.

Features:

• Supports command-line arguments and interactive input

• Includes comprehensive service availability checks and error handling

• Supports priority control and interruption mechanism

• Provides detailed playback status feedback

Core Code

#include <aimdk_msgs/msg/tts_priority_level.hpp>
#include <aimdk_msgs/srv/play_tts.hpp>
#include <chrono>
#include <iostream>
#include <rclcpp/rclcpp.hpp>
#include <string>

using PlayTTS = aimdk_msgs::srv::PlayTts;

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("play_tts_client_min");

  const std::string service_name = "/aimdk_5Fmsgs/srv/PlayTts";
  auto client = node->create_client<PlayTTS>(service_name);

  RCLCPP_INFO(node->get_logger(), "✅ PlayTts client node created.");

  // Wait for the service to become available
  while (!client->wait_for_service(std::chrono::seconds(2))) {
    if (!rclcpp::ok()) {
      rclcpp::shutdown();
      return 1;
    }
    RCLCPP_INFO(node->get_logger(), "⏳ Service unavailable, waiting...");
  }
  RCLCPP_INFO(node->get_logger(),
              "🟢 Service available, ready to send request.");

  // Get text to speak
  std::string tts_text;
  if (argc > 1) {
    tts_text = argv[1];
  } else {
    std::cout << "Enter text to speak: ";
    if (!std::getline(std::cin, tts_text) || !rclcpp::ok()) {
      std::cout << std::endl;
      rclcpp::shutdown();
      return 1;
    }
    if (tts_text.empty()) {
      tts_text = "Hello, I am AgiBot X2.";
    }
  }

  if (!rclcpp::ok()) {
    rclcpp::shutdown();
    return 1;
  }

  auto req = std::make_shared<PlayTTS::Request>();
  req->tts_req.text = tts_text;
  req->tts_req.domain = "demo_client"; // Required: identifies the caller
  req->tts_req.trace_id =
      "demo"; // Optional: request identifier for the TTS request
  req->tts_req.is_interrupted =
      true; // Required: whether to interrupt same-priority playback
  req->tts_req.priority_weight = 0;
  req->tts_req.priority_level.value =
      aimdk_msgs::msg::TtsPriorityLevel::INTERACTION_L6; // L6 (6)

  RCLCPP_INFO(node->get_logger(), "📨 Sending request to play tts: text=%s",
              tts_text.c_str());

  std::shared_future<PlayTTS::Response::SharedPtr> future;
  rclcpp::FutureReturnCode rc = rclcpp::FutureReturnCode::TIMEOUT;
  for (int i = 0; i < 8; ++i) {
    req->header.header.stamp = node->now();
    future = client->async_send_request(req);
    rc = rclcpp::spin_until_future_complete(node, future,
                                            std::chrono::milliseconds(250));
    if (rc == rclcpp::FutureReturnCode::SUCCESS) {
      break;
    }
    if (!rclcpp::ok()) {
      rclcpp::shutdown();
      return 1;
    }
    // retry as remote peer is NOT handled well by ROS
    RCLCPP_INFO(node->get_logger(), "trying ... [%d]", i);
  }

  if (rc != rclcpp::FutureReturnCode::SUCCESS) {
    if (rclcpp::ok()) {
      RCLCPP_ERROR(node->get_logger(),
                   "❌ Service call not completed or timed out.");
    }
    rclcpp::shutdown();
    return 1;
  }

  const auto resp = future.get();
  if (resp->tts_resp.is_success) {
    RCLCPP_INFO(node->get_logger(), "✅ TTS sent successfully.");
  } else {
    RCLCPP_ERROR(node->get_logger(),
                 "❌ Failed to send TTS: error_code=%u, error_message=%s",
                 resp->tts_resp.error_code,
                 resp->tts_resp.error_message.c_str());
  }

  rclcpp::shutdown();
  return 0;
}

Usage Instructions

# Use command-line arguments to speak text (recommended)
ros2 run examples play_tts "Hello, I am the AgiBot X2 robot"

# Or run without arguments; the program will prompt for input
ros2 run examples play_tts

Output Example

[INFO] [1779181543.136645281] [play_tts_client_min]: ✅ PlayTts client node created.
[INFO] [1779181543.137467698] [play_tts_client_min]: 🟢 Service available, ready to send request.
[INFO] [1779181543.138450608] [play_tts_client_min]: 📨 Sending request to play tts: text=Hello
[INFO] [1779181543.146077262] [play_tts_client_min]: ✅ TTS sent successfully.

Notes

• Ensure the TTS service is running properly

• Supports both Chinese and English text-to-speech

• Priority settings affect playback queue order

• Interruption feature can stop the currently playing speech

Interface Reference

• Service: /aimdk_5Fmsgs/srv/PlayTts

• Message: aimdk_msgs/srv/PlayTts

6.2.21 Microphone Audio Reception

This example uses mic_receiver, subscribing to the /agent/process_audio_output topic to receive the robot’s noise-reduced audio stream. It supports both internal and external microphone audio streams, and automatically saves complete speech segments as PCM files based on VAD (Voice Activity Detection) states.

Features:

• Automatically detects speech start, processing, and end based on VAD state

• Automatically saves complete speech segments as PCM files (mono, 16 kHz sample rate, S16LE format)

• Stores recordings categorized by timestamp and audio stream

• Supports audio duration calculation and statistical output

Application Scenarios:

• Speech recognition and speech processing

• Audio data acquisition and analysis

• Real-time speech monitoring

• Audio quality evaluation

• Multi-microphone array data processing

#include <aimdk_msgs/msg/audio_vad_state_type.hpp>
#include <aimdk_msgs/msg/processed_audio_output.hpp>
#include <chrono>
#include <ctime>
#include <filesystem>
#include <fstream>
#include <iomanip>
#include <rclcpp/rclcpp.hpp>
#include <sstream>
#include <string>
#include <unordered_map>
#include <vector>

namespace fs = std::filesystem;

class AudioSubscriber : public rclcpp::Node {
public:
  AudioSubscriber() : rclcpp::Node("audio_subscriber") {
    // Audio buffers, stored separately by stream_id
    // stream_id -> buffer
    audio_buffers_ = {};
    recording_state_ = {};

    audio_output_dir_ = "audio_recordings";
    fs::create_directories(audio_output_dir_);

    // Note: deep queue to avoid missing data in a burst at start of VAD.
    auto qos = rclcpp::QoS(
        rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_sensor_data));
    qos.keep_last(500).best_effort();

    subscription_ =
        this->create_subscription<aimdk_msgs::msg::ProcessedAudioOutput>(
            "/agent/process_audio_output", qos,
            std::bind(&AudioSubscriber::audio_callback, this,
                      std::placeholders::_1));

    RCLCPP_INFO(this->get_logger(),
                "Start subscribing to noise-suppressed audio data...");
  }

private:
  void
  audio_callback(const aimdk_msgs::msg::ProcessedAudioOutput::SharedPtr msg) {
    try {
      uint32_t stream_id = msg->stream_id;
      uint8_t vad_state = msg->audio_vad_state.value;
      const std::vector<uint8_t> &audio_data = msg->audio_data;

      using V = aimdk_msgs::msg::AudioVadStateType;
      static const std::unordered_map<uint8_t, std::string> vad_state_names = {
          {V::AUDIO_VAD_STATE_BEGIN, "Speech start"},
          {V::AUDIO_VAD_STATE_PROCESSING, "Speech in progress"},
          {V::AUDIO_VAD_STATE_END, "Speech end"}};
      static const std::unordered_map<uint32_t, std::string> stream_names = {
          {1, "Built-in microphone"}, {2, "External microphone"}};

      RCLCPP_INFO(this->get_logger(),
                  "Received audio data: stream_id=%u, vad_state=%u(%s), "
                  "audio_size=%zu bytes",
                  stream_id, vad_state,
                  vad_state_names.count(vad_state)
                      ? vad_state_names.at(vad_state).c_str()
                      : "Unknown state",
                  audio_data.size());

      handle_vad_state(stream_id, vad_state, audio_data);
    } catch (const std::exception &e) {
      RCLCPP_ERROR(this->get_logger(), "Error processing audio message: %s",
                   e.what());
    }
  }

  void handle_vad_state(uint32_t stream_id, uint8_t vad_state,
                        const std::vector<uint8_t> &audio_data) {
    // Initialize the buffer for this stream_id (if it does not exist)
    if (audio_buffers_.count(stream_id) == 0) {
      audio_buffers_[stream_id] = std::vector<uint8_t>();
      recording_state_[stream_id] = false;
    }

    using V = aimdk_msgs::msg::AudioVadStateType;
    static const std::unordered_map<uint8_t, std::string> vad_state_names = {
        {V::AUDIO_VAD_STATE_BEGIN, "Speech start"},
        {V::AUDIO_VAD_STATE_PROCESSING, "Speech in progress"},
        {V::AUDIO_VAD_STATE_END, "Speech end"}};
    static const std::unordered_map<uint32_t, std::string> stream_names = {
        {1, "Built-in microphone"}, {2, "External microphone"}};

    RCLCPP_INFO(this->get_logger(), "[%s] VAD state: %s audio: %zu bytes",
                stream_names.count(stream_id)
                    ? stream_names.at(stream_id).c_str()
                    : ("Unknown Stream " + std::to_string(stream_id)).c_str(),
                vad_state_names.count(vad_state)
                    ? vad_state_names.at(vad_state).c_str()
                    : ("Unknown State" + std::to_string(vad_state)).c_str(),
                audio_data.size());

    if (vad_state == V::AUDIO_VAD_STATE_BEGIN) { // BEGIN (1)
      RCLCPP_INFO(this->get_logger(), "🎤 Speech start detected");
      if (recording_state_[stream_id] == false) {
        audio_buffers_[stream_id].clear();
        recording_state_[stream_id] = true;
      }
      if (!audio_data.empty()) {
        audio_buffers_[stream_id].insert(audio_buffers_[stream_id].end(),
                                         audio_data.begin(), audio_data.end());
      }

    } else if (vad_state == V::AUDIO_VAD_STATE_PROCESSING) { // PROCESSING (2)
      RCLCPP_INFO(this->get_logger(), "🔄 Speech in progress...");
      if (recording_state_[stream_id] && !audio_data.empty()) {
        audio_buffers_[stream_id].insert(audio_buffers_[stream_id].end(),
                                         audio_data.begin(), audio_data.end());
      }

    } else if (vad_state == V::AUDIO_VAD_STATE_END) { // END (3)
      RCLCPP_INFO(this->get_logger(), "✅ Speech end");
      if (recording_state_[stream_id] && !audio_data.empty()) {
        audio_buffers_[stream_id].insert(audio_buffers_[stream_id].end(),
                                         audio_data.begin(), audio_data.end());
      }
      if (recording_state_[stream_id] && !audio_buffers_[stream_id].empty()) {
        save_audio_segment(audio_buffers_[stream_id], stream_id);
      }
      recording_state_[stream_id] = false;

      // AUDIO_VAD_STATE_UNKNOWN
    } else {
      if (recording_state_[stream_id]) {
        RCLCPP_INFO(this->get_logger(), "⏹️ Reset recording state");
        recording_state_[stream_id] = false;
      }
    }

    // Output the current buffer status.
    size_t buffer_size = audio_buffers_[stream_id].size();
    bool recording = recording_state_[stream_id];
    RCLCPP_DEBUG(this->get_logger(),
                 "[Stream %u] Buffer size: %zu bytes, Recording state: %s",
                 stream_id, buffer_size, recording ? "true" : "false");
  }

  void save_audio_segment(const std::vector<uint8_t> &audio_data,
                          uint32_t stream_id) {
    if (audio_data.empty())
      return;

    // Get the current timestamp.
    auto now = std::chrono::system_clock::now();
    std::time_t t = std::chrono::system_clock::to_time_t(now);
    auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(
                  now.time_since_epoch()) %
              1000;

    std::ostringstream oss;
    oss << std::put_time(std::localtime(&t), "%Y%m%d_%H%M%S") << "_"
        << std::setw(3) << std::setfill('0') << ms.count();

    // Create a subdirectory by stream_id.
    fs::path stream_dir =
        fs::path(audio_output_dir_) / ("stream_" + std::to_string(stream_id));
    fs::create_directories(stream_dir);

    static const std::unordered_map<uint32_t, std::string> stream_names = {
        {1, "internal_mic"}, {2, "external_mic"}};
    std::string stream_name = stream_names.count(stream_id)
                                  ? stream_names.at(stream_id)
                                  : ("stream_" + std::to_string(stream_id));
    std::string filename = stream_name + "_" + oss.str() + ".pcm";
    fs::path filepath = stream_dir / filename;

    try {
      std::ofstream ofs(filepath, std::ios::binary);
      ofs.write(reinterpret_cast<const char *>(audio_data.data()),
                audio_data.size());
      ofs.close();
      RCLCPP_INFO(this->get_logger(),
                  "Audio segment saved: %s (size: %zu bytes)", filepath.c_str(),
                  audio_data.size());

      // Record audio file duration (assuming 16kHz, 16-bit, mono)
      int sample_rate = 16000;
      int bits_per_sample = 16;
      int channels = 1;
      int bytes_per_sample = bits_per_sample / 8;
      size_t total_samples = audio_data.size() / (bytes_per_sample * channels);
      double duration_seconds =
          static_cast<double>(total_samples) / sample_rate;

      RCLCPP_INFO(this->get_logger(), "Audio duration: %.2f s (%zu samples)",
                  duration_seconds, total_samples);
    } catch (const std::exception &e) {
      RCLCPP_ERROR(this->get_logger(), "Failed to save audio file: %s",
                   e.what());
    }
  }

  // Member variables
  std::unordered_map<uint32_t, std::vector<uint8_t>> audio_buffers_;
  std::unordered_map<uint32_t, bool> recording_state_;
  std::string audio_output_dir_;
  rclcpp::Subscription<aimdk_msgs::msg::ProcessedAudioOutput>::SharedPtr
      subscription_;
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<AudioSubscriber>();
  RCLCPP_INFO(
      node->get_logger(),
      "Listening for noise-suppressed audio data, press Ctrl+C to exit...");
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

Usage Instructions:

1. Run the program:

   # Build
   colcon build --packages-select examples
   
   # Subscribe to GNSS data
   ros2 run examples echo_gnss_data
   

   Press Ctrl+C to safely exit; the program automatically handles the start and end of audio streams.

2. Play saved PCM file:

   • After running the node, an audio_recordings directory will be created automatically

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

     • stream_1/: Built-in microphone audio

     • stream_2/: External microphone audio

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

     • internal_mic_20250909_133649_738.pcm (built-in microphone)

     • external_mic_20250909_133650_123.pcm (external microphone)

3. Play saved PCM file:

   # Play built-in microphone recording
   aplay -r 16000 -f S16_LE -c 1 audio_recordings/stream_1/internal_mic_20250909_133649_738.pcm
   

   Parameter explanation:

   • -r 16000 # Sampling rate 16 kHz

   • -f S16_LE # 16-bit little-endian format

   • -c 1 # Mono. You may also use other audio players (e.g., Audacity) to import and play raw PCM files.

4. Convert to WAV format (optional):

   # Use ffmpeg to convert to WAV format
   ffmpeg -f s16le -ar 16000 -ac 1 -i external_mic_20250909_133649_738.pcm output.wav
   

Output Example:

Normal startup and operation:

[INFO] [1779195658.978174026] [audio_subscriber]: Start subscribing to noise-suppressed audio data...
[INFO] [1779195658.978795319] [audio_subscriber]: Listening for noise-suppressed audio data, press Ctrl+C to exit...

Speech start detected:

[INFO] [1779195676.331873180] [audio_subscriber]: Received audio data: stream_id=1, vad_state=1(Speech start), audio_size=2560 bytes
[INFO] [1779195676.332029112] [audio_subscriber]: [Built-in microphone] VAD state: Speech start audio: 2560 bytes
[INFO] [1779195676.332061655] [audio_subscriber]: 🎤 Speech start detected

Speech processing:

[INFO] [1779195676.332590408] [audio_subscriber]: Received audio data: stream_id=1, vad_state=2(Speech in progress), audio_size=18560 bytes
[INFO] [1779195676.332690053] [audio_subscriber]: [Built-in microphone] VAD state: Speech in progress audio: 18560 bytes
[INFO] [1779195676.332720229] [audio_subscriber]: 🔄 Speech in progress...
...

Speech end and save:

[INFO] [1779195676.881115234] [audio_subscriber]: Received audio data: stream_id=1, vad_state=3(Speech end), audio_size=2560 bytes
[INFO] [1779195676.881210656] [audio_subscriber]: [Built-in microphone] VAD state: Speech end audio: 2560 bytes
[INFO] [1779195676.881234399] [audio_subscriber]: ✅ Speech end
[INFO] [1779195676.882756437] [audio_subscriber]: Audio segment saved: audio_recordings/stream_1/internal_mic_20260519_210116_881.pcm (size: 42880 bytes)
[INFO] [1779195676.882860210] [audio_subscriber]: Audio duration: 1.34 s (21440 samples)

6.2.22 Raw Microphone Audio Stream Receiver

This example uses mic_raw_receiver, which subscribes to the /aima/hal/audio/capture topic to receive raw audio data from the robot and automatically saves it to PCM files split by channel.

Features:

• Stores recordings categorized by timestamp and audio stream

• Automatically saves audio channel data as PCM files (mono, 16 kHz sample rate, S16LE format)

• Supports mic-source switch detection

/**
 * 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 audio
 * data as PCM files split by channel.
 *
 * Features:
 * - Automatically saves raw audio as PCM files
 * - Stores files categorized by timestamp and mic source
 */

#include <aimdk_msgs/msg/audio_capture.hpp>
#include <chrono>
#include <ctime>
#include <filesystem>
#include <fstream>
#include <iomanip>
#include <rclcpp/rclcpp.hpp>
#include <sstream>
#include <string>
#include <unordered_map>
#include <vector>

namespace fs = std::filesystem;

class RawAudioSubscriber : public rclcpp::Node {
public:
  RawAudioSubscriber() : rclcpp::Node("raw_audio_subscriber") {
    mic_channels_ = 0;
    ref_channels_ = 0;
    inited_ = false;

    // Create audio output directory named by current timestamp
    auto now = std::chrono::system_clock::now();
    auto time_t_now = std::chrono::system_clock::to_time_t(now);
    auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(
                  now.time_since_epoch()) %
              1000;
    std::ostringstream oss;
    oss << std::put_time(std::localtime(&time_t_now), "%Y%m%d_%H%M%S_")
        << std::setw(3) << std::setfill('0') << ms.count();
    audio_output_dir_ = "audio_recordings/" + oss.str();
    fs::create_directories(audio_output_dir_);

    // Note: deep queue to avoid missing data
    auto qos = rclcpp::QoS(rclcpp::KeepLast(500)).best_effort();

    subscription_ = this->create_subscription<aimdk_msgs::msg::AudioCapture>(
        "/aima/hal/audio/capture", qos,
        std::bind(&RawAudioSubscriber::audio_callback, this,
                  std::placeholders::_1));

    RCLCPP_INFO(this->get_logger(), "Start subscribing to raw audio data...");
  }

  void run() {
    timer_ = this->create_wall_timer(
        std::chrono::seconds(1),
        std::bind(&RawAudioSubscriber::save_audio_segments, this));
    rclcpp::spin(shared_from_this());
  }

private:
  void audio_callback(const aimdk_msgs::msg::AudioCapture::SharedPtr msg) {
    if (!inited_) {
      mic_channels_ = msg->mic_channels;
      ref_channels_ = msg->ref_channels;
      inited_ = true;
    } else if (mic_channels_ != msg->mic_channels ||
               ref_channels_ != msg->ref_channels) {
      // quit as MIC switched
      RCLCPP_ERROR(
          this->get_logger(),
          "MIC channels info changed (mic:%u ref:%u) -> (mic:%u ref:%u)",
          mic_channels_, ref_channels_, msg->mic_channels, msg->ref_channels);
      rclcpp::shutdown();
      return;
    }

    handle_audio_data(msg->data.data);
  }

  void handle_audio_data(const std::vector<uint8_t> &audio_data) {
    // Split S16LE data into channels
    int channels =
        static_cast<int>(ref_channels_) + static_cast<int>(mic_channels_);
    if (channels == 0)
      return;

    const int bytes_per_channel = 2;
    const int unit_size = channels * bytes_per_channel;
    size_t num_frames = audio_data.size() / unit_size;

    for (size_t k = 0; k < num_frames; ++k) {
      for (int i = 0; i < channels; ++i) {
        size_t base = k * unit_size + i * bytes_per_channel;
        audio_buffers_[i].push_back(audio_data[base]);
        audio_buffers_[i].push_back(audio_data[base + 1]);
      }
    }
  }

  void save_audio_segments() {
    RCLCPP_INFO(this->get_logger(), "Flushing cached audio data...");
    int total =
        static_cast<int>(mic_channels_) + static_cast<int>(ref_channels_);
    for (int i = 0; i < total; ++i) {
      bool is_ref = (i >= static_cast<int>(mic_channels_));
      save_audio_segment(i, is_ref);
    }
  }

  void save_audio_segment(int channel, bool is_ref) {
    auto &buf = audio_buffers_[channel];
    if (buf.empty())
      return;

    std::string channel_type = is_ref ? "ref" : "mic";
    std::string filename =
        "channel_" + std::to_string(channel) + "_" + channel_type + ".pcm";
    fs::path filepath = fs::path(audio_output_dir_) / filename;

    try {
      // Append mode (equivalent to Python 'ab')
      std::ofstream ofs(filepath, std::ios::binary | std::ios::app);
      ofs.write(reinterpret_cast<const char *>(buf.data()), buf.size());
      ofs.close();

      RCLCPP_INFO(this->get_logger(),
                  "Audio segment saved: %s appended: %zu bytes",
                  filepath.c_str(), buf.size());
      buf.clear();
    } catch (const std::exception &e) {
      RCLCPP_ERROR(this->get_logger(), "Failed to save audio file: %s",
                   e.what());
    }
  }

  bool inited_;
  uint8_t mic_channels_;
  uint8_t ref_channels_;
  std::string audio_output_dir_;
  std::unordered_map<int, std::vector<uint8_t>> audio_buffers_;
  rclcpp::Subscription<aimdk_msgs::msg::AudioCapture>::SharedPtr subscription_;
  rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<RawAudioSubscriber>();
  RCLCPP_INFO(node->get_logger(),
              "Listening to raw audio data, press Ctrl+C to exit...");
  node->run();
  rclcpp::shutdown();
  return 0;
}

Usage Instructions:

1. Run the program:

   # Build
   colcon build --packages-select examples
   
   # Subscribe to GNSS data
   ros2 run examples echo_gnss_data
   

2. Play saved PCM file:

   • After running the node, an audio_recordings directory will be created automatically

   • Audio files are stored in a directory named by the program’s startup timestamp:

     • 20250909_133650_123

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

     • channel_0_mic.pcm (microphone)

     • channel_4_ref.pcm (reference / echo-cancellation)

3. Play saved PCM file:

   • Play on a computer:

     aplay -r 16000 -f S16_LE -c 1 audio_recordings/20250909_133650_123/channel_0_mic.pcm
     

   • Play on the robot:

     # Use the raw audio stream playback example
     ros2 run examples play_audio_stream --ros-args -p raw_audio_path:=$PWD/audio_recordings/20250909_133650_123/channel_0_mic.pcm
     # Or use the media file playback example; note that the file must be transferred to the interaction compute unit first, replace the path below
     ros2 run examples play_audio please/use/path/on/PC3.pcm
     

6.2.23 Get Current Microphone

#include "aimdk_msgs/msg/common_request.hpp"
#include "aimdk_msgs/msg/common_state.hpp"
#include "aimdk_msgs/srv/get_mic_source_request.hpp"
#include "rclcpp/rclcpp.hpp"
#include <chrono>
#include <memory>
#include <signal.h>

std::shared_ptr<rclcpp::Node> g_node = nullptr;

void signal_handler(int signal) {
  if (g_node) {
    RCLCPP_INFO(g_node->get_logger(), "Received signal %d, shutting down...",
                signal);
    g_node.reset();
  }
  rclcpp::shutdown();
  exit(signal);
}

class GetMicSourceRequestClient : public rclcpp::Node {
public:
  GetMicSourceRequestClient() : Node("get_mic_source_request_client") {
    client_ = this->create_client<aimdk_msgs::srv::GetMicSourceRequest>(
        "/aimdk_5Fmsgs/srv/GetMicSourceRequest");

    RCLCPP_INFO(this->get_logger(),
                "✅ GetMicSourceRequest client node created.");

    // Wait for the service to become available
    while (!client_->wait_for_service(std::chrono::seconds(2))) {
      RCLCPP_INFO(this->get_logger(), "⏳ Service unavailable, waiting...");
    }
    RCLCPP_INFO(this->get_logger(),
                "🟢 Service available, ready to send request.");
  }

  void send_request() {
    try {
      auto request =
          std::make_shared<aimdk_msgs::srv::GetMicSourceRequest::Request>();
      request->header = aimdk_msgs::msg::CommonRequest();

      RCLCPP_INFO(this->get_logger(), "📨 Sending request to get MIC source");

      auto timeout = std::chrono::milliseconds(250);
      for (int i = 0; i < 8; i++) {
        request->header.header.stamp = this->now();
        auto future = client_->async_send_request(request);
        auto retcode = rclcpp::spin_until_future_complete(
            this->shared_from_this(), future, timeout);
        if (retcode != rclcpp::FutureReturnCode::SUCCESS) {
          // retry as remote peer is NOT handled well by ROS
          RCLCPP_INFO(this->get_logger(), "trying ... [%d]", i);
          continue;
        }
        // future.done
        auto response = future.get();
        if (response->header.status.value ==
            aimdk_msgs::msg::CommonState::SUCCESS) { // SUCCESS (1)
          RCLCPP_INFO(this->get_logger(), "✅ MIC source get successfully.");
          RCLCPP_INFO(this->get_logger(), "MIC id: %d", response->mic_source);
        } else {
          RCLCPP_ERROR(this->get_logger(), "❌ Failed to get MIC source: %s",
                       response->header.message.c_str());
        }
        return;
      }
      RCLCPP_ERROR(this->get_logger(), "❌ Service call failed or timed out.");
    } catch (const std::exception &e) {
      RCLCPP_ERROR(this->get_logger(), "Exception occurred: %s", e.what());
    }
  }

private:
  rclcpp::Client<aimdk_msgs::srv::GetMicSourceRequest>::SharedPtr client_;
};

int main(int argc, char *argv[]) {
  try {
    rclcpp::init(argc, argv);
    signal(SIGINT, signal_handler);
    signal(SIGTERM, signal_handler);

    g_node = std::make_shared<GetMicSourceRequestClient>();
    auto client = std::dynamic_pointer_cast<GetMicSourceRequestClient>(g_node);

    if (client) {
      client->send_request();
    }

    g_node.reset();
    rclcpp::shutdown();
    return 0;
  } catch (const std::exception &e) {
    RCLCPP_ERROR(rclcpp::get_logger("main"),
                 "Program exited with exception: %s", e.what());
    return 1;
  }
}

6.2.24 Switch Microphone

#include "aimdk_msgs/msg/common_request.hpp"
#include "aimdk_msgs/msg/common_state.hpp"
#include "aimdk_msgs/srv/set_mic_source_request.hpp"
#include "rclcpp/rclcpp.hpp"
#include <chrono>
#include <map>
#include <memory>
#include <signal.h>
#include <string>

std::shared_ptr<rclcpp::Node> g_node = nullptr;

void signal_handler(int signal) {
  if (g_node) {
    RCLCPP_INFO(g_node->get_logger(), "Received signal %d, shutting down...",
                signal);
    g_node.reset();
  }
  rclcpp::shutdown();
  exit(signal);
}

class SetMicSourceRequestClient : public rclcpp::Node {
public:
  SetMicSourceRequestClient() : Node("set_mic_source_request_client") {
    client_ = this->create_client<aimdk_msgs::srv::SetMicSourceRequest>(
        "/aimdk_5Fmsgs/srv/SetMicSourceRequest");

    RCLCPP_INFO(this->get_logger(),
                "✅ SetMicSourceRequest client node created.");

    // Wait for the service to become available
    while (!client_->wait_for_service(std::chrono::seconds(2))) {
      RCLCPP_INFO(this->get_logger(), "⏳ Service unavailable, waiting...");
    }
    RCLCPP_INFO(this->get_logger(),
                "🟢 Service available, ready to send request.");
  }

  void send_request(int mic_source) {
    try {
      auto request =
          std::make_shared<aimdk_msgs::srv::SetMicSourceRequest::Request>();
      request->header = aimdk_msgs::msg::CommonRequest();
      request->mic_source = mic_source;

      RCLCPP_INFO(this->get_logger(), "📨 Sending request to set mic source: %d",
                  mic_source);

      auto timeout = std::chrono::milliseconds(250);
      for (int i = 0; i < 8; i++) {
        request->header.header.stamp = this->now();
        auto future = client_->async_send_request(request);
        auto retcode = rclcpp::spin_until_future_complete(
            this->shared_from_this(), future, timeout);
        if (retcode != rclcpp::FutureReturnCode::SUCCESS) {
          // retry as remote peer is NOT handled well by ROS
          RCLCPP_INFO(this->get_logger(), "trying ... [%d]", i);
          continue;
        }
        // future.done
        auto response = future.get();
        if (response->header.status.value ==
            aimdk_msgs::msg::CommonState::SUCCESS) { // SUCCESS (1)
          RCLCPP_INFO(this->get_logger(), "✅ MIC source set successfully.");
        } else {
          RCLCPP_ERROR(this->get_logger(), "❌ Failed to set MIC source: %s",
                       response->header.message.c_str());
        }
        return;
      }
      RCLCPP_ERROR(this->get_logger(), "❌ Service call failed or timed out.");
    } catch (const std::exception &e) {
      RCLCPP_ERROR(this->get_logger(), "Exception occurred: %s", e.what());
    }
  }

private:
  rclcpp::Client<aimdk_msgs::srv::SetMicSourceRequest>::SharedPtr client_;
};

int main(int argc, char *argv[]) {
  // abbr -> mic_id mapping
  // 0: internal MIC, 1: external MIC
  const std::map<std::string, int> choices = {{"int", 0}, {"ext", 1}};

  try {
    rclcpp::init(argc, argv);
    signal(SIGINT, signal_handler);
    signal(SIGTERM, signal_handler);

    std::string mic_abbr;
    if (argc > 1) {
      mic_abbr = argv[1];
    } else {
      printf("%-4s - %-6s : %s\n", "abbr", "mic_id", "description");
      printf("%-4s - %-6d : %s\n", "int", 0, "internal MIC");
      printf("%-4s - %-6d : %s\n", "ext", 1, "external MIC");
      printf("Enter abbr of MIC source: ");
      std::cin >> mic_abbr;
    }

    auto it = choices.find(mic_abbr);
    if (it == choices.end()) {
      RCLCPP_ERROR(rclcpp::get_logger("main"), "Invalid abbr of MIC source: %s",
                   mic_abbr.c_str());
      return 1;
    }
    int mic_id = it->second;

    g_node = std::make_shared<SetMicSourceRequestClient>();
    auto client = std::dynamic_pointer_cast<SetMicSourceRequestClient>(g_node);

    if (client) {
      client->send_request(mic_id);
    }

    g_node.reset();
    rclcpp::shutdown();
    return 0;
  } catch (const std::exception &e) {
    RCLCPP_ERROR(rclcpp::get_logger("main"),
                 "Program exited with exception: %s", e.what());
    return 1;
  }
}

Usage Instructions

# Build
colcon build --packages-select examples

# Interactive run
ros2 run examples set_mic_source

# Run with argument
ros2 run examples set_mic_source ext  # ext - external mic, int - internal mic

6.2.25 Raw Audio Stream Playback

This example uses play_audio_stream, which first requests audio focus then publishes a PCM file at real-time rate to the /aima/hal/audio/playback topic; it exits automatically when audio focus is taken away.

/**
 * Raw audio stream playback example
 *
 * This example gets audio focus first and then publishes raw audio data at a
 * fixed rate to the `/aima/hal/audio/playback` topic. It stops playing when
 * audio focus is lost.
 *
 * Usage:
 *   ros2 run examples play_audio_stream --ros-args -p
 * raw_audio_path:=/path/to/file.pcm
 */

#include <aimdk_msgs/msg/audio_data.hpp>
#include <aimdk_msgs/msg/audio_info.hpp>
#include <aimdk_msgs/msg/audio_playback.hpp>
#include <aimdk_msgs/msg/common_state.hpp>
#include <aimdk_msgs/msg/focus_requester.hpp>
#include <aimdk_msgs/msg/focus_response.hpp>
#include <aimdk_msgs/srv/abandon_audio_focus.hpp>
#include <aimdk_msgs/srv/request_audio_focus.hpp>
#include <chrono>
#include <csignal>
#include <fstream>
#include <rclcpp/rclcpp.hpp>
#include <string>
#include <thread>
#include <vector>

// ---------------------------------------------------------------------------
// FakeMicDevice: simulates a real-time microphone from a raw PCM file
// ---------------------------------------------------------------------------
class FakeMicDevice {
public:
  explicit FakeMicDevice(const std::string &raw_audio_file)
      : channels_(1), sample_rate_(16000), bytes_per_sample_(2), index_(0),
        start_time_(std::chrono::steady_clock::time_point{}) {
    std::ifstream f(raw_audio_file, std::ios::binary);
    if (!f.is_open()) {
      throw std::runtime_error("Cannot open audio file: " + raw_audio_file);
    }
    audio_data_ = std::vector<uint8_t>(std::istreambuf_iterator<char>(f),
                                       std::istreambuf_iterator<char>());
  }

  int get_channel_count() const { return channels_; }
  int get_sample_rate() const { return sample_rate_; }

  std::vector<uint8_t> get_cached_data() {
    auto now = std::chrono::steady_clock::now();
    if (start_time_ == std::chrono::steady_clock::time_point{}) {
      // fake init: assume audio data started 1s ago
      start_time_ = now - std::chrono::seconds(1);
    }

    double elapsed = std::chrono::duration<double>(now - start_time_).count();
    size_t tail = static_cast<size_t>(elapsed * sample_rate_) * channels_ *
                  bytes_per_sample_;

    std::vector<uint8_t> data;
    if (tail < audio_data_.size()) {
      data = std::vector<uint8_t>(audio_data_.begin() + index_,
                                  audio_data_.begin() + tail);
      index_ = tail;
    } else {
      // loopback
      data =
          std::vector<uint8_t>(audio_data_.begin() + index_, audio_data_.end());
      index_ = 0;
      start_time_ = now;
    }
    return data;
  }

private:
  int channels_;
  int sample_rate_;
  int bytes_per_sample_;
  size_t index_;
  std::chrono::steady_clock::time_point start_time_;
  std::vector<uint8_t> audio_data_;
};

// ---------------------------------------------------------------------------
// AudioStreamPlayer node
// ---------------------------------------------------------------------------
using RequestAudioFocus = aimdk_msgs::srv::RequestAudioFocus;
using AbandonAudioFocus = aimdk_msgs::srv::AbandonAudioFocus;

class AudioStreamPlayer : public rclcpp::Node {
public:
  AudioStreamPlayer() : rclcpp::Node("audio_stream_player") {
    this->declare_parameter<std::string>("raw_audio_path", "");
    std::string raw_audio_path =
        this->get_parameter("raw_audio_path").as_string();

    mic_device_ = std::make_unique<FakeMicDevice>(raw_audio_path);

    pkg_name_ = "audio_stream_player" + std::to_string(getpid());
    focus_ = false;
    focus_force_ = true;

    RCLCPP_INFO(this->get_logger(), "local pkg name: %s", pkg_name_.c_str());

    // Create focus request/release clients
    request_client_ = this->create_client<RequestAudioFocus>(
        "/aimdk_5Fmsgs/srv/RequestAudioFocus");
    release_client_ = this->create_client<AbandonAudioFocus>(
        "/aimdk_5Fmsgs/srv/AbandonAudioFocus");

    // Wait for service to become available
    while (!request_client_->wait_for_service(std::chrono::seconds(2))) {
      RCLCPP_INFO(this->get_logger(), "⏳ Service unavailable, waiting...");
    }
    RCLCPP_INFO(this->get_logger(),
                "🟢 Service available, ready to send request.");

    // Subscriber: focus events
    auto focus_qos = rclcpp::QoS(rclcpp::KeepLast(10)).reliable();
    sub_ = this->create_subscription<aimdk_msgs::msg::FocusResponse>(
        "/aima/hal/audio/focus_response", focus_qos,
        std::bind(&AudioStreamPlayer::focus_event_callback, this,
                  std::placeholders::_1));

    // Publisher: raw audio playback
    pub_ = this->create_publisher<aimdk_msgs::msg::AudioPlayback>(
        "/aima/hal/audio/playback", 10);
  }

  void send_request(bool focus) {
    if (focus) {
      send_focus_request();
    } else {
      send_focus_release();
    }
  }

  bool is_holding_focus() const { return focus_ && focus_force_; }

  void spin_until_focus() {
    while (rclcpp::ok()) {
      focus_force_ = true;
      send_request(true);
      if (is_holding_focus())
        return;

      RCLCPP_INFO(this->get_logger(), "need retry to get focus");
      auto t0 = std::chrono::steady_clock::now();
      while (rclcpp::ok()) {
        auto t1 = std::chrono::steady_clock::now();
        if (std::chrono::duration<double>(t1 - t0).count() >= 1.0)
          break;
        rclcpp::spin_some(shared_from_this());
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
      }
    }
  }

  void run() {
    RCLCPP_INFO(this->get_logger(), "🟢 publishing audio data ...");
    while (rclcpp::ok() && is_holding_focus()) {
      run_once();
      rclcpp::spin_some(shared_from_this());
      std::this_thread::sleep_for(std::chrono::milliseconds(50));
    }
    if (!is_holding_focus()) {
      RCLCPP_INFO(this->get_logger(), "focus out, exiting...");
    }
  }

private:
  void send_focus_request() {
    auto req = std::make_shared<RequestAudioFocus::Request>();
    aimdk_msgs::msg::FocusRequester requester;
    requester.pkg_name = pkg_name_;
    requester.priority = kAudioPriority;
    req->focus_requester = requester;

    RCLCPP_INFO(this->get_logger(), "📨 Sending RequestAudioFocus request");
    std::shared_future<RequestAudioFocus::Response::SharedPtr> future;
    for (int i = 0; i < 8; ++i) {
      future = request_client_->async_send_request(req).future.share();
      auto status = rclcpp::spin_until_future_complete(
          shared_from_this(), future, std::chrono::milliseconds(250));
      if (status == rclcpp::FutureReturnCode::SUCCESS)
        break;
      // retry as remote peer is NOT handled well by ROS
      RCLCPP_INFO(this->get_logger(), "trying ... [%d]", i);
    }

    auto response = future.get();
    if (!response) {
      RCLCPP_ERROR(this->get_logger(), "❌ Service call failed or timed out.");
      return;
    }
    if (response->reponse.status.value ==
        aimdk_msgs::msg::CommonState::SUCCESS) { // SUCCESS (1)
      focus_ = response->focus_response.focus_gain;
      RCLCPP_INFO(this->get_logger(), "✅ RequestAudioFocus done: focus %s",
                  focus_ ? "true" : "false");
    } else {
      RCLCPP_ERROR(this->get_logger(),
                   "❌ Failed in response of RequestAudioFocus: %s",
                   response->reponse.message.c_str());
    }
  }

  void send_focus_release() {
    auto req = std::make_shared<AbandonAudioFocus::Request>();
    aimdk_msgs::msg::FocusRequester requester;
    requester.pkg_name = pkg_name_;
    requester.priority = kAudioPriority; // must match RequestAudioFocus
    req->focus_requester = requester;

    RCLCPP_INFO(this->get_logger(), "📨 Sending AbandonAudioFocus request");
    std::shared_future<AbandonAudioFocus::Response::SharedPtr> future;
    for (int i = 0; i < 8; ++i) {
      future = release_client_->async_send_request(req).future.share();
      auto status = rclcpp::spin_until_future_complete(
          shared_from_this(), future, std::chrono::milliseconds(250));
      if (status == rclcpp::FutureReturnCode::SUCCESS)
        break;
      // retry as remote peer is NOT handled well by ROS
      RCLCPP_INFO(this->get_logger(), "trying ... [%d]", i);
    }

    auto response = future.get();
    if (!response) {
      RCLCPP_ERROR(this->get_logger(), "❌ Service call failed or timed out.");
      return;
    }
    if (response->reponse.status.value ==
        aimdk_msgs::msg::CommonState::SUCCESS) { // SUCCESS (1)
      // always focus False after abandon
      focus_ = response->focus_response.focus_gain;
      RCLCPP_INFO(this->get_logger(), "✅ AbandonAudioFocus done: focus %s",
                  focus_ ? "true" : "false");
    } else {
      RCLCPP_ERROR(this->get_logger(),
                   "❌ Failed in response of AbandonAudioFocus: %s",
                   response->reponse.message.c_str());
    }
  }

  void
  focus_event_callback(const aimdk_msgs::msg::FocusResponse::SharedPtr msg) {
    if (msg->pkg_name == pkg_name_) {
      RCLCPP_INFO(this->get_logger(),
                  "receive focus out event: focus state: %s",
                  msg->focus_gain ? "true" : "false");
      focus_force_ = msg->focus_gain;
    }
  }

  void run_once() {
    aimdk_msgs::msg::AudioPlayback msg;
    msg.pkg_name = pkg_name_;
    msg.token_id = pkg_name_;

    aimdk_msgs::msg::AudioInfo info;
    info.channels = static_cast<uint8_t>(mic_device_->get_channel_count());
    info.sample_rate = static_cast<uint32_t>(mic_device_->get_sample_rate());

    aimdk_msgs::msg::AudioData audio_data;
    audio_data.data = mic_device_->get_cached_data();

    msg.info = info;
    msg.data = audio_data;
    pub_->publish(msg);
  }

  std::string pkg_name_;
  bool focus_;
  bool focus_force_;
  static constexpr uint32_t kAudioPriority = 8;
  std::unique_ptr<FakeMicDevice> mic_device_;
  rclcpp::Client<RequestAudioFocus>::SharedPtr request_client_;
  rclcpp::Client<AbandonAudioFocus>::SharedPtr release_client_;
  rclcpp::Subscription<aimdk_msgs::msg::FocusResponse>::SharedPtr sub_;
  rclcpp::Publisher<aimdk_msgs::msg::AudioPlayback>::SharedPtr pub_;
};

// ---------------------------------------------------------------------------
// Signal handling
// ---------------------------------------------------------------------------
static std::shared_ptr<AudioStreamPlayer> g_node;

void signal_handler(int sig) {
  if (g_node) {
    if (g_node->is_holding_focus()) {
      g_node->send_request(false);
    }
    RCLCPP_INFO(g_node->get_logger(),
                "Received signal %d, abandon audio focus and shutting down",
                sig);
  }
  rclcpp::shutdown();
}

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);

  try {
    auto node = std::make_shared<AudioStreamPlayer>();
    g_node = node;
    std::signal(SIGINT, signal_handler);
    std::signal(SIGTERM, signal_handler);

    node->spin_until_focus();
    node->run();

  } catch (const std::exception &e) {
    RCLCPP_ERROR(rclcpp::get_logger("main"),
                 "Program exited with exception: %s", e.what());
  }

  if (rclcpp::ok()) {
    rclcpp::shutdown();
  }
  return 0;
}

Usage Instructions:

# Replace /path/to/your/raw_audio_file.pcm with the actual PCM file path (mono, 16 kHz, S16LE)
ros2 run examples play_audio_stream --ros-args -p raw_audio_path:=/path/to/your/raw_audio_file.pcm

6.2.26 Emoji Control

This example uses play_emoji, which allows the robot to display a specified emoji. Users can choose an emoji from the available list; see the Emoji List for details.

#include "aimdk_msgs/srv/play_emoji.hpp"
#include "aimdk_msgs/msg/common_request.hpp"
#include "rclcpp/rclcpp.hpp"
#include <chrono>
#include <iostream>
#include <memory>
#include <signal.h>
#include <string>

// Global variable used for signal handling
std::shared_ptr<rclcpp::Node> g_node = nullptr;

// Signal handler function
void signal_handler(int signal) {
  if (g_node) {
    RCLCPP_INFO(g_node->get_logger(), "Received signal %d, shutting down...",
                signal);
    g_node.reset();
  }
  rclcpp::shutdown();
  exit(signal);
}

class PlayEmojiClient : public rclcpp::Node {
public:
  PlayEmojiClient() : Node("play_emoji_client") {
    client_ = this->create_client<aimdk_msgs::srv::PlayEmoji>(
        "/aimdk_5Fmsgs/srv/PlayEmoji");
    RCLCPP_INFO(this->get_logger(), "✅ PlayEmoji client node started.");

    // Wait for the service to become available
    while (!client_->wait_for_service(std::chrono::seconds(2))) {
      RCLCPP_INFO(this->get_logger(), "⏳ Service unavailable, waiting...");
    }
    RCLCPP_INFO(this->get_logger(),
                "🟢 Service available, ready to send request.");
  }

  bool send_request(uint8_t emoji, uint8_t mode, int32_t priority) {
    try {
      auto request = std::make_shared<aimdk_msgs::srv::PlayEmoji::Request>();

      request->emotion_id = emoji;
      request->mode = mode;
      request->priority = priority;

      RCLCPP_INFO(
          this->get_logger(),
          "📨 Sending request to play emoji: id=%hhu, mode=%hhu, priority=%d",
          emoji, mode, priority);

      const std::chrono::milliseconds timeout(250);
      for (int i = 0; i < 8; i++) {
        request->header.header.stamp = this->now();
        auto future = client_->async_send_request(request);
        auto retcode = rclcpp::spin_until_future_complete(shared_from_this(),
                                                          future, timeout);
        if (retcode != rclcpp::FutureReturnCode::SUCCESS) {
          // retry as remote peer is NOT handled well by ROS
          RCLCPP_INFO(this->get_logger(), "trying ... [%d]", i);
          continue;
        }
        // future.done
        auto response = future.get();
        if (response->success) {
          RCLCPP_INFO(this->get_logger(),
                      "✅ Request to play emoji recorded successfully: %s",
                      response->message.c_str());
          return true;
        } else {
          RCLCPP_ERROR(this->get_logger(),
                       "❌ Failed to record play-emoji request: %s",
                       response->message.c_str());
          return false;
        }
      }
      RCLCPP_ERROR(this->get_logger(), "❌ Service call failed or timed out.");
      return false;
    } catch (const std::exception &e) {
      RCLCPP_ERROR(this->get_logger(), "Exception occurred: %s", e.what());
      return false;
    }
  }

private:
  rclcpp::Client<aimdk_msgs::srv::PlayEmoji>::SharedPtr client_;
};

int main(int argc, char **argv) {
  try {
    rclcpp::init(argc, argv);

    // Set up signal handlers
    signal(SIGINT, signal_handler);
    signal(SIGTERM, signal_handler);

    int32_t priority = 10;

    int emotion = 1; // Expression type, 1 means Blink
    std::cout
        << "Enter expression ID: 1-Blink, 60-Bored, 70-Abnormal, 80-Sleeping, "
           "90-Happy, 190-Very Angry, 200-Adoration"
        << std::endl;
    std::cin >> emotion;

    int mode = 1; // Playback mode, 1 means play once, 2 means loop
    std::cout << "Enter play mode (1: once, 2: loop): ";
    std::cin >> mode;
    if (mode < 1 || mode > 2) {
      RCLCPP_ERROR(rclcpp::get_logger("main"), "Invalid play mode: %d", mode);
      rclcpp::shutdown();
      return 1;
    }

    g_node = std::make_shared<PlayEmojiClient>();
    auto client = std::dynamic_pointer_cast<PlayEmojiClient>(g_node);

    if (client) {
      client->send_request(emotion, mode, priority);
    }

    // Clean up resources
    g_node.reset();
    rclcpp::shutdown();

    return 0;
  } catch (const std::exception &e) {
    RCLCPP_ERROR(rclcpp::get_logger("main"),
                 "Program exited with exception: %s", e.what());
    return 1;
  }
}

6.2.27 Map Building

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

#include <iostream>
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <string>

// Class definition for publishing commands to control SLAM operations
class SlamCommandPublisher : public rclcpp::Node {
public:
  // Constructor for the SlamCommandPublisher class
  SlamCommandPublisher()
      : Node("slam_command_publisher") // Initialize the node with the name
                                       // "slam_command_publisher"
  {
    // Match SLAM subscriber QoS: TRANSIENT_LOCAL + RELIABLE
    rclcpp::QoS qos(10);
    qos.transient_local().reliable();
    publisher_ = this->create_publisher<std_msgs::msg::String>(
        "/integrated_command", qos);
  }

  // Method to publish the "start_mapping" command
  void publish_start_mapping() {
    auto message = std_msgs::msg::String();
    message.data = "start_mapping";
    RCLCPP_INFO(this->get_logger(), "Publishing: [%s]", message.data.c_str());
    publisher_->publish(message);
  }

  // Method to publish the "stop_mapping" command with a specified map name
  void publish_stop_mapping(const std::string &map_name) {
    auto message = std_msgs::msg::String();
    message.data = "stop_mapping:" + map_name;
    RCLCPP_INFO(this->get_logger(), "Publishing: [%s]", message.data.c_str());
    publisher_->publish(message);
  }

private:
  // Publisher object to publish messages to the "/integrated_command" topic
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
};

int main(int argc, char *argv[]) {
  // Initialize the ROS 2 communication system
  rclcpp::init(argc, argv);
  // Create an instance of the SlamCommandPublisher node
  auto node = std::make_shared<SlamCommandPublisher>();

  int input;
  std::string map_name;

  // Infinite loop to continuously accept user input
  while (rclcpp::ok()) {
    std::cout << "Enter 1 to start mapping, 2 to stop mapping: ";
    std::cin >> input;

    if (input == 1) {
      // Publish the "start_mapping" command
      node->publish_start_mapping();
    } else if (input == 2) {
      // Prompt the user to enter the map name
      std::cout << "Enter map name: ";
      std::cin >> map_name;
      // Publish the "stop_mapping" command with the specified map name
      node->publish_stop_mapping(map_name);
      break;
    } else {
      // Handle invalid input
      std::cout << "Invalid input. Please enter 1 or 2." << std::endl;
    }
  }

  // Shutdown the ROS 2 communication system
  rclcpp::shutdown();
  return 0;
}

Usage Instructions:

ros2 run examples slam
Enter 1 to start mapping, 2 to stop mapping: 1
[INFO] [1772528788.353934160] [slam_command_publisher]: Publishing: [start_mapping]
Enter 1 to start mapping, 2 to stop mapping: 2
Enter map name: testmap
[INFO] [1772528791.900924187] [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.2.28 Relocalization Using a Map

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

#include <chrono>
#include <functional>
#include <geometry_msgs/msg/pose.hpp>
#include <iostream>
#include <nav_msgs/msg/odometry.hpp>
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <string>

using namespace std::chrono_literals;

class RelocalizationNode : public rclcpp::Node {
public:
  RelocalizationNode(uint64_t map_id, double init_x, double init_y)
      : Node("relocalization_node"), map_id_(map_id), init_x_(init_x),
        init_y_(init_y) {
    // /integrated_command subscriber requires TRANSIENT_LOCAL durability
    auto command_qos = rclcpp::QoS(rclcpp::KeepLast(10));
    command_qos.transient_local();

    integrated_command_pub_ = this->create_publisher<std_msgs::msg::String>(
        "/integrated_command", command_qos);
    relocalization_pose_pub_ = this->create_publisher<geometry_msgs::msg::Pose>(
        "/relocalization_pose", 10);

    // Create subscriber with BEST_EFFORT QoS
    auto lidar_loc_qos = rclcpp::QoS(rclcpp::KeepLast(10));
    lidar_loc_qos.best_effort();

    odometry_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
        "/slam/lidar_odom", lidar_loc_qos,
        std::bind(&RelocalizationNode::odometryCallback, this,
                  std::placeholders::_1));

    // Wait for subscribers before publishing
    publish_timer_ = this->create_wall_timer(200ms, [this]() {
      // Wait until both publishers have at least one subscriber
      if (integrated_command_pub_->get_subscription_count() == 0 ||
          relocalization_pose_pub_->get_subscription_count() == 0) {
        RCLCPP_INFO_THROTTLE(
            this->get_logger(), *this->get_clock(), 2000,
            "Waiting for subscribers... (command: %zu, pose: %zu)",
            integrated_command_pub_->get_subscription_count(),
            relocalization_pose_pub_->get_subscription_count());
        return;
      }
      publish_timer_->cancel();

      // Publish integrated_command
      auto integrated_command_msg = std_msgs::msg::String();
      integrated_command_msg.data =
          "start_relocalization:" + std::to_string(map_id_);
      integrated_command_pub_->publish(integrated_command_msg);
      RCLCPP_INFO(this->get_logger(), "Published integrated_command");

      // Schedule relocalization_pose publication after delay
      this->relocalization_pose_timer_ = this->create_wall_timer(1s, [this]() {
        this->relocalization_pose_timer_->cancel();

        auto relocalization_pose_msg = geometry_msgs::msg::Pose();
        relocalization_pose_msg.position.x = init_x_;
        relocalization_pose_msg.position.y = init_y_;
        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;

        relocalization_pose_pub_->publish(relocalization_pose_msg);
        RCLCPP_INFO(this->get_logger(), "Published relocalization_pose");

        // Start timeout timer (30 seconds)
        this->timeout_timer_ = this->create_wall_timer(30s, [this]() {
          if (!success_received_) {
            RCLCPP_ERROR(this->get_logger(),
                         "Timeout reached - Relocalization failed!");
            rclcpp::shutdown();
          }
        });

        RCLCPP_INFO(this->get_logger(),
                    "Waiting for robot pose data (timeout: 30s)...");
      });
    });
  }

private:
  // get robot pose here
  void odometryCallback(const nav_msgs::msg::Odometry::SharedPtr msg) {
    if (!success_received_) {
      success_received_ = true;
      if (timeout_timer_) {
        timeout_timer_->cancel();
      }
      RCLCPP_INFO(this->get_logger(),
                  "Received odometry data - Relocalization successful!");
      rclcpp::shutdown();
    }
  }

  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr integrated_command_pub_;
  rclcpp::Publisher<geometry_msgs::msg::Pose>::SharedPtr
      relocalization_pose_pub_;
  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odometry_sub_;

  // Timers
  rclcpp::TimerBase::SharedPtr publish_timer_;
  rclcpp::TimerBase::SharedPtr relocalization_pose_timer_;
  rclcpp::TimerBase::SharedPtr timeout_timer_;

  uint64_t map_id_;
  double init_x_;
  double init_y_;
  bool success_received_ = false;
};

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);

  // Inputs needed for relocalization. To find them, see:
  //   map_id        -> sqlite3 /agibot/data/var/MapManagerModule/map.db
  //                    "SELECT * FROM map;"
  //   init_x/init_y -> origin (u, v) in
  //                    /agibot/data/var/MapManagerModule/<map_id>/grid_map_info.txt
  //                    (line 2; pixel coordinates)
  uint64_t map_id = 0;
  double init_x = 0.0;
  double init_y = 0.0;
  try {
    std::string line;
    std::cout << "Enter map_id: ";
    std::getline(std::cin, line);
    map_id = std::stoull(line);
    std::cout << "Enter initial position.x (pixel u): ";
    std::getline(std::cin, line);
    init_x = std::stod(line);
    std::cout << "Enter initial position.y (pixel v): ";
    std::getline(std::cin, line);
    init_y = std::stod(line);
  } catch (const std::exception &e) {
    std::cerr << "Invalid input, aborted." << std::endl;
    rclcpp::shutdown();
    return 1;
  }

  auto node = std::make_shared<RelocalizationNode>(map_id, init_x, init_y);
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

Usage Instructions:

ros2 run examples relocate
Enter map_id: 1774430080403
Enter initial position.x (pixel u): 273
Enter initial position.y (pixel v): 200
[INFO] [1772528939.129777359] [relocalization_node]: Published integrated_command
[INFO] [1772528940.130067255] [relocalization_node]: Published relocalization_pose
[INFO] [1772528940.130129806] [relocalization_node]: Waiting for robot pose data (timeout: 30s)...
[INFO] [1772528945.130242770] [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.2.29 Retrieve a Stored Map

In this example, get_map is used to retrieve stored map data by name, including the map path, resolution, dimensions, and map ID.

//
// Created by agiuser on 2026/3/2.
//

#include "aimdk_msgs/srv/get_stored_map_by_name.hpp"
#include "rclcpp/rclcpp.hpp"
#include <chrono>
#include <cinttypes>
#include <iostream>
#include <memory>
#include <string>

class MapClient : public rclcpp::Node {
public:
  MapClient() : Node("get_map_client") {
    client_ = this->create_client<aimdk_msgs::srv::GetStoredMapByName>(
        "/aimdk_5Fmsgs/srv/GetStoredMapByName");
  }

  void send_request(const std::string &map_name) {
    while (!client_->wait_for_service(std::chrono::seconds(1))) {
      if (!rclcpp::ok()) {
        RCLCPP_ERROR(this->get_logger(),
                     "Interrupted while waiting for service.");
        return;
      }
      RCLCPP_INFO(this->get_logger(),
                  "Service not available, waiting again...");
    }

    auto request =
        std::make_shared<aimdk_msgs::srv::GetStoredMapByName::Request>();

    request->header.stamp = this->now();
    request->header.frame_id = "";

    request->map_name = map_name;

    auto result_future = client_->async_send_request(request);

    // Use a fresh executor to spin, avoiding any pre-existing state.
    rclcpp::executors::SingleThreadedExecutor exec;
    exec.add_node(shared_from_this());
    if (exec.spin_until_future_complete(result_future,
                                        std::chrono::seconds(60)) !=
        rclcpp::FutureReturnCode::SUCCESS) {
      RCLCPP_ERROR(this->get_logger(), "Service call failed or timed out.");
      return;
    }

    auto result = result_future.get();

    if (result->code == 0) {
      RCLCPP_INFO(this->get_logger(), "Service call succeeded");
      RCLCPP_INFO(this->get_logger(), "Map path: %s", result->map_path.c_str());
      RCLCPP_INFO(this->get_logger(), "Map resolution: %f",
                  result->map_info.resolution);
      RCLCPP_INFO(this->get_logger(), "Map width: %u", result->map_info.width);
      RCLCPP_INFO(this->get_logger(), "Map height: %u",
                  result->map_info.height);
      RCLCPP_INFO(this->get_logger(), "Map id: %" PRIu64, result->map_id);
    } else {
      RCLCPP_ERROR(this->get_logger(),
                   "Service call failed with code: %" PRIu64, result->code);
    }
  }

private:
  rclcpp::Client<aimdk_msgs::srv::GetStoredMapByName>::SharedPtr client_;
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);

  auto client = std::make_shared<MapClient>();

  // Map name (NOT map_id) is required. To list stored maps, run:
  //   sqlite3 /agibot/data/var/MapManagerModule/map.db "SELECT * FROM map;"
  std::cout << "Enter map name: ";
  std::string map_name;
  std::getline(std::cin, map_name);
  if (map_name.empty()) {
    RCLCPP_ERROR(client->get_logger(), "Map name is empty, aborted.");
    rclcpp::shutdown();
    return 1;
  }
  client->send_request(map_name);

  rclcpp::shutdown();
  return 0;
}

Usage Instructions:

ros2 run examples get_map
Enter map name: test_map
[INFO] [get_map_client]: Service call succeeded
[INFO] [get_map_client]: Map path: /path/to/test_map.png
[INFO] [get_map_client]: Map resolution: 0.050000
[INFO] [get_map_client]: Map width: 200
[INFO] [get_map_client]: Map height: 200
[INFO] [get_map_client]: Map id: 1774430080403

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.2.30 Navigation

This example provides an interactive terminal menu for starting, pausing, resuming, and stopping navigation tasks.

#include <chrono>
#include <cinttypes>
#include <cstring>
#include <iostream>
#include <string>
#include <thread>

#include <aimdk_msgs/msg/message_header.hpp>
#include <aimdk_msgs/msg/pnc_task_request.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <rclcpp/rclcpp.hpp>

using namespace std::chrono_literals;

// task_request enum
constexpr uint8_t TASK_REQUEST_START = 1;
constexpr uint8_t TASK_REQUEST_STOP = 2;
constexpr uint8_t TASK_REQUEST_PAUSE = 3;
constexpr uint8_t TASK_REQUEST_RESUME = 4;

class NavigationNode : public rclcpp::Node {
public:
  NavigationNode() : Node("navigation_example") {
    publisher_ = this->create_publisher<aimdk_msgs::msg::PncTaskRequest>(
        "/aima/te/pnc_task_request", 10);

    auto now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
                      std::chrono::system_clock::now().time_since_epoch())
                      .count();
    task_id_ = static_cast<uint64_t>(now_ms);
    map_id_ = 0;

    RCLCPP_INFO(this->get_logger(),
                "Navigation example node started, task_id=%" PRIu64, task_id_);
  }

  void send_start(uint64_t map_id, double x, double y, double z, double ox,
                  double oy, double oz, double ow, double radius,
                  uint8_t pnc_mode, double max_speed) {
    map_id_ = map_id;
    auto msg = aimdk_msgs::msg::PncTaskRequest();
    msg.header = make_header();
    msg.task_type = 2;
    msg.task_request = TASK_REQUEST_START;
    msg.pnc_mode = pnc_mode;
    msg.max_forward_speed = max_speed;
    msg.task_id = task_id_;
    msg.map_id = map_id;
    msg.target_pose_radius = radius;
    std::memset(msg.reserve_info.data(), 0, msg.reserve_info.size());

    auto &target = msg.target_pose;
    target.header.stamp = this->get_clock()->now();
    target.header.frame_id = "map";
    target.pose.position.x = x;
    target.pose.position.y = y;
    target.pose.position.z = z;
    target.pose.orientation.x = ox;
    target.pose.orientation.y = oy;
    target.pose.orientation.z = oz;
    target.pose.orientation.w = ow;

    RCLCPP_INFO(this->get_logger(),
                "[Start] task_id=%" PRIu64 ", map_id=%" PRIu64 ", "
                "target=(%.2f, %.2f, %.2f), radius=%.2f",
                task_id_, map_id, x, y, z, radius);
    start_publishing(msg);
  }

  void send_pause() {
    auto msg = make_simple_request(TASK_REQUEST_PAUSE, 0);
    RCLCPP_INFO(this->get_logger(), "[Pause] task_id=%" PRIu64, task_id_);
    start_publishing(msg);
  }

  void send_resume() {
    auto msg = make_simple_request(TASK_REQUEST_RESUME, map_id_);
    RCLCPP_INFO(this->get_logger(),
                "[Resume] task_id=%" PRIu64 ", map_id=%" PRIu64, task_id_,
                map_id_);
    start_publishing(msg);
  }

  void send_stop() {
    auto msg = make_simple_request(TASK_REQUEST_STOP, map_id_);
    RCLCPP_INFO(this->get_logger(),
                "[Stop] task_id=%" PRIu64 ", map_id=%" PRIu64, task_id_,
                map_id_);
    start_publishing(msg);
  }

  void stop_publishing() {
    if (timer_) {
      timer_->cancel();
      timer_.reset();
      RCLCPP_INFO(this->get_logger(), "Stopped continuous publishing");
    }
  }

  uint64_t task_id() const { return task_id_; }

private:
  void start_publishing(const aimdk_msgs::msg::PncTaskRequest &msg) {
    current_msg_ = msg;
    if (timer_) {
      timer_->cancel();
    }
    publisher_->publish(current_msg_);
    timer_ = this->create_wall_timer(1s, [this]() {
      current_msg_.header.stamp = this->get_clock()->now();
      current_msg_.header.meas_stamp = this->get_clock()->now();
      current_msg_.target_pose.header.stamp = this->get_clock()->now();
      publisher_->publish(current_msg_);
    });
    RCLCPP_INFO(this->get_logger(), "Continuous publishing at 1Hz");
  }

  aimdk_msgs::msg::MessageHeader make_header() {
    auto header = aimdk_msgs::msg::MessageHeader();
    auto now = this->get_clock()->now();
    header.stamp = now;
    header.meas_stamp = now;
    header.frame_id = "map";
    header.sequence = 0;
    return header;
  }

  aimdk_msgs::msg::PncTaskRequest make_simple_request(uint8_t task_request,
                                                      uint64_t map_id) {
    auto msg = aimdk_msgs::msg::PncTaskRequest();
    msg.header = make_header();
    msg.task_type = 2;
    msg.task_request = task_request;
    msg.pnc_mode = 0;
    msg.max_forward_speed = 0.5;
    msg.task_id = task_id_;
    msg.map_id = map_id;
    msg.target_pose_radius = 0.0;
    std::memset(msg.reserve_info.data(), 0, msg.reserve_info.size());
    msg.target_pose.pose.orientation.w = 1.0;
    return msg;
  }

  rclcpp::Publisher<aimdk_msgs::msg::PncTaskRequest>::SharedPtr publisher_;
  rclcpp::TimerBase::SharedPtr timer_;
  aimdk_msgs::msg::PncTaskRequest current_msg_;
  uint64_t task_id_;
  uint64_t map_id_;
};

template <typename T> T input_value(const std::string &prompt, T default_val) {
  std::cout << prompt << " [" << default_val << "]: ";
  std::string line;
  std::getline(std::cin, line);
  if (line.empty()) {
    return default_val;
  }
  if constexpr (std::is_same_v<T, uint64_t>) {
    return std::stoull(line);
  } else if constexpr (std::is_same_v<T, uint8_t>) {
    return static_cast<uint8_t>(std::stoi(line));
  } else if constexpr (std::is_integral_v<T>) {
    return static_cast<T>(std::stoll(line));
  } else {
    return static_cast<T>(std::stod(line));
  }
}

void print_menu(uint64_t task_id) {
  std::cout << "\n========================================\n"
            << "  Navigation Control  (task_id=" << task_id << ")\n"
            << "========================================\n"
            << "  1. Start navigation\n"
            << "  2. Pause navigation\n"
            << "  3. Resume navigation\n"
            << "  4. Stop navigation\n"
            << "  q. Quit\n"
            << "========================================\n";
}

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<NavigationNode>();

  std::thread spin_thread([node]() { rclcpp::spin(node); });

  RCLCPP_INFO(node->get_logger(),
              "Waiting for node communication ready (5s)...");
  std::this_thread::sleep_for(5s);

  std::string choice;
  while (rclcpp::ok()) {
    print_menu(node->task_id());
    std::cout << "Select an option: ";
    std::getline(std::cin, choice);

    if (choice == "1") {
      std::cout << "\n--- Start Navigation Parameters ---\n";
      auto map_id = input_value<uint64_t>("Map ID (map_id)", 1773113429735ULL);
      auto x = input_value<double>("Target x", 1.0);
      auto y = input_value<double>("Target y", 2.0);
      auto z = input_value<double>("Target z", 0.0);
      auto ox = input_value<double>("Orientation x", 0.0);
      auto oy = input_value<double>("Orientation y", 0.0);
      auto oz = input_value<double>("Orientation z", 0.0);
      auto ow = input_value<double>("Orientation w", 1.0);
      auto radius = input_value<double>("Target pose radius", 0.5);
      auto pnc_mode = input_value<uint8_t>("pnc_mode", 0);
      auto max_speed = input_value<double>("Max forward speed", 0.5);
      node->send_start(map_id, x, y, z, ox, oy, oz, ow, radius, pnc_mode,
                       max_speed);
    } else if (choice == "2") {
      node->send_pause();
    } else if (choice == "3") {
      node->send_resume();
    } else if (choice == "4") {
      node->send_stop();
    } else if (choice == "q" || choice == "Q") {
      node->stop_publishing();
      std::cout << "Exiting navigation control.\n";
      break;
    } else {
      std::cout << "Invalid input, please try again.\n";
    }
  }

  rclcpp::shutdown();
  spin_thread.join();
  return 0;
}

Usage Instructions:

ros2 run examples navigation

Output Example:

[INFO] [navigation_example]: Navigation example node started, task_id=1709123456789
[INFO] [navigation_example]: Waiting for node communication ready (5s)...

========================================
  Navigation Control  (task_id=1709123456789)
========================================
  1. Start navigation
  2. Pause navigation
  3. Resume navigation
  4. Stop navigation
  q. Quit
========================================
Select an option: 1

--- Start Navigation Parameters ---
Map ID (map_id) [1773113429735]:
Target x [1]:
Target y [2]:
...
[INFO] [navigation_example]: [Start] task_id=1709123456789, map_id=1773113429735, target=(1.00, 2.00, 0.00), radius=0.50

When starting navigation, you need to enter parameters such as map ID, target pose, and arrival precision. Each has a default value; press Enter directly to use the default.

How to obtain these parameters: to obtain map_id, see Obtaining Map Identifiers and Coordinates; target_x / target_y are the target point in metric coordinates (map frame), different from the pixel coordinates used for relocalization — specify them according to the actual task.

6.2.31 Play LinkCraft Action

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

//
// Created by agiuser on 2026/3/3.
//

#include <aimdk_msgs/srv/execute_action_resource.hpp>
#include <aimdk_msgs/srv/get_robot_resources.hpp>
#include <cinttypes>
#include <iostream>
#include <rclcpp/rclcpp.hpp>
#include <string>
#include <vector>

using namespace std::chrono_literals;

class RobotResourceClient : public rclcpp::Node {
public:
  RobotResourceClient() : Node("robot_resource_client") {
    get_resource_client_ =
        this->create_client<aimdk_msgs::srv::GetRobotResources>(
            "/aimdk_5Fmsgs/srv/GetRobotResources");
    play_resource_client_ =
        this->create_client<aimdk_msgs::srv::ExecuteActionResource>(
            "/aimdk_5Fmsgs/srv/ExecuteActionResource");
  }

  bool get_robot_resources() {
    while (!get_resource_client_->wait_for_service(1s)) {
      if (!rclcpp::ok()) {
        return false;
      }
      RCLCPP_INFO(this->get_logger(),
                  "Service not available, waiting again...");
    }

    if (!rclcpp::ok()) {
      return false;
    }

    auto request =
        std::make_shared<aimdk_msgs::srv::GetRobotResources::Request>();

    auto future_result = get_resource_client_->async_send_request(request);

    if (rclcpp::spin_until_future_complete(this->get_node_base_interface(),
                                           future_result) !=
        rclcpp::FutureReturnCode::SUCCESS) {
      if (rclcpp::ok()) {
        RCLCPP_ERROR(this->get_logger(), "Failed to call GetRobotResources");
      }
      return false;
    }

    auto response = future_result.get();
    resources_ = response->robot_resources;
    RCLCPP_INFO(this->get_logger(), "Found %zu resources", resources_.size());
    return true;
  }

  void print_resource_menu() {
    if (resources_.empty()) {
      std::cout << "(no resources)\n";
      return;
    }

    std::cout << "\n=== Available Robot Resources ===\n";
    for (size_t i = 0; i < resources_.size(); ++i) {
      const auto &ver = resources_[i].current_version;
      std::cout << "[" << i << "] "
                << (ver.name.empty() ? "<no name>" : ver.name) << "\n";
      std::cout << "     key:     " << resources_[i].resource_key << "\n";
      std::cout << "     version: " << ver.version << "\n";
    }
    std::cout << "=================================\n\n";
  }

  int pick_resource() {
    if (resources_.empty()) {
      return -1;
    }

    std::string line;
    while (true) {
      std::cout << "Select resource index [0-" << resources_.size() - 1
                << "] (q to quit): ";
      if (!std::getline(std::cin, line)) {
        return -1;
      }
      if (line == "q" || line == "quit" || line == "exit") {
        return -1;
      }
      try {
        int idx = std::stoi(line);
        if (idx >= 0 && idx < static_cast<int>(resources_.size())) {
          return idx;
        }
        std::cout << "Index out of range (0-" << resources_.size() - 1
                  << ").\n";
      } catch (...) {
        std::cout << "Please enter a number.\n";
      }
    }
  }

  void play_resource(int idx) {
    while (!play_resource_client_->wait_for_service(1s)) {
      if (!rclcpp::ok()) {
        RCLCPP_ERROR(this->get_logger(),
                     "Interrupted while waiting for service.");
        return;
      }
      RCLCPP_INFO(this->get_logger(), "Service not available, waiting...");
    }

    if (!rclcpp::ok()) {
      return;
    }

    const auto &resource = resources_[idx];
    auto request =
        std::make_shared<aimdk_msgs::srv::ExecuteActionResource::Request>();

    request->resource_key = resource.resource_key;
    request->resource_version = resource.current_version.version;

    if (request->resource_key.find("onnx") != std::string::npos) {
      request->meta = R"({"resource_type": "BODY_MONTION"})";
    } else {
      request->meta = R"({"resource_type": "ARM_MONTION"})";
    }

    RCLCPP_INFO(this->get_logger(), "Sending request:");
    RCLCPP_INFO(this->get_logger(), "  resource_key: %s",
                request->resource_key.c_str());
    RCLCPP_INFO(this->get_logger(), "  resource_version: %s",
                request->resource_version.c_str());
    RCLCPP_INFO(this->get_logger(), "  meta: %s", request->meta.c_str());

    auto future = play_resource_client_->async_send_request(request);

    if (rclcpp::spin_until_future_complete(this->get_node_base_interface(),
                                           future) ==
        rclcpp::FutureReturnCode::SUCCESS) {
      handle_response(future.get());
    } else {
      if (rclcpp::ok()) {
        RCLCPP_ERROR(this->get_logger(),
                     "Failed to call ExecuteActionResource");
      }
    }
  }

private:
  rclcpp::Client<aimdk_msgs::srv::GetRobotResources>::SharedPtr
      get_resource_client_;
  rclcpp::Client<aimdk_msgs::srv::ExecuteActionResource>::SharedPtr
      play_resource_client_;
  std::vector<aimdk_msgs::msg::RobotResource> resources_;

  void handle_response(
      const aimdk_msgs::srv::ExecuteActionResource::Response::SharedPtr
          response) {
    RCLCPP_INFO(this->get_logger(), "Service response received:");
    RCLCPP_INFO(this->get_logger(), "Response Header:");
    RCLCPP_INFO(this->get_logger(), "  stamp: %d.%09u",
                response->header.header.stamp.sec,
                response->header.header.stamp.nanosec);
    RCLCPP_INFO(this->get_logger(), "  code: %" PRId64,
                response->header.header.code);
    RCLCPP_INFO(this->get_logger(), "Status:");
    RCLCPP_INFO(this->get_logger(), "  value: %d",
                response->header.status.value);
    RCLCPP_INFO(this->get_logger(), "  message: %s",
                response->header.message.c_str());
  }
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto client_node = std::make_shared<RobotResourceClient>();

  if (!client_node->get_robot_resources()) {
    rclcpp::shutdown();
    return 1;
  }

  client_node->print_resource_menu();
  int chosen = client_node->pick_resource();
  if (chosen < 0) {
    RCLCPP_INFO(client_node->get_logger(), "No resource selected, exiting.");
    rclcpp::shutdown();
    return 0;
  }

  client_node->play_resource(chosen);
  rclcpp::shutdown();
  return 0;
}

Usage Instructions:

ros2 run examples play_linkcraft
[INFO] [1772529153.517267476] [robot_resource_client]: Found 1 resources

=== Available Robot Resources ===
[0] resource_key: linkcraft_resource_onnx_01KBM2BHNFM93Z0DK6ES9F6DJA
     key:     linkcraft_resource_onnx_01KBM2BHNFM93Z0DK6ES9F6DJA
     version: 0.0.1
=================================

Select resource index [0-0]: 0
[INFO] [1772529153.517488347] [robot_resource_client]: Sending request:
[INFO] [1772529153.517497834] [robot_resource_client]:   resource_key: linkcraft_resource_onnx_01KBM2BHNFM93Z0DK6ES9F6DJA
[INFO] [1772529153.517505889] [robot_resource_client]:   resource_version: 0.0.1
[INFO] [1772529153.517473238] [robot_resource_client]:   meta: {"resource_type": "BODY_MONTION"}
[INFO] [1772529153.553673433] [robot_resource_client]: Service response received:
[INFO] [1772529153.553697021] [robot_resource_client]: Response Header:
[INFO] [1772529153.553700410] [robot_resource_client]:   stamp: 0.000000000
[INFO] [1772529153.553704130] [robot_resource_client]:   code: 0
[INFO] [1772529153.553707001] [robot_resource_client]: Status:
[INFO] [1772529153.553709853] [robot_resource_client]:   value: 1
[INFO] [1772529153.553712662] [robot_resource_client]:   message: 1/1 task(s) failed. Details: localhost: failed (status code: 400) - Failed with status code: 400

6.2.32 Play LinkCraft Action (Multi-Robot)

In this example, multi_play_linkcraft is used to synchronize time across multiple robots and perform group-controlled LinkCraft action playback.

//
// Created by agiuser on 2026/3/3.
//

#include <aimdk_msgs/msg/slave_device.hpp>
#include <aimdk_msgs/srv/execute_action_resource.hpp>
#include <aimdk_msgs/srv/get_robot_resources.hpp>
#include <aimdk_msgs/srv/group_control_time_sync.hpp>
#include <cinttypes>
#include <iostream>
#include <rclcpp/rclcpp.hpp>
#include <string>
#include <vector>

using namespace std::chrono_literals;

class MultiRobotResourceClient : public rclcpp::Node {
public:
  MultiRobotResourceClient() : Node("robot_resource_client") {
    get_resource_client_ =
        this->create_client<aimdk_msgs::srv::GetRobotResources>(
            "/aimdk_5Fmsgs/srv/GetRobotResources");
    play_resource_client_ =
        this->create_client<aimdk_msgs::srv::ExecuteActionResource>(
            "/aimdk_5Fmsgs/srv/ExecuteActionResource");
    time_sync_client_ =
        this->create_client<aimdk_msgs::srv::GroupControlTimeSync>(
            "/aimdk_5Fmsgs/srv/GroupControlTimeSync");
  }

  bool get_robot_resources() {
    while (!get_resource_client_->wait_for_service(1s)) {
      if (!rclcpp::ok()) {
        return false;
      }
      RCLCPP_INFO(this->get_logger(),
                  "Service not available, waiting again...");
    }

    if (!rclcpp::ok()) {
      return false;
    }

    auto request =
        std::make_shared<aimdk_msgs::srv::GetRobotResources::Request>();

    auto future_result = get_resource_client_->async_send_request(request);

    if (rclcpp::spin_until_future_complete(this->get_node_base_interface(),
                                           future_result) !=
        rclcpp::FutureReturnCode::SUCCESS) {
      if (rclcpp::ok()) {
        RCLCPP_ERROR(this->get_logger(), "Failed to call GetRobotResources");
      }
      return false;
    }

    auto response = future_result.get();
    resources_ = response->robot_resources;
    RCLCPP_INFO(this->get_logger(), "Found %zu resources", resources_.size());
    return true;
  }

  void print_resource_menu() {
    if (resources_.empty()) {
      std::cout << "(no resources)\n";
      return;
    }

    std::cout << "\n=== Available Robot Resources ===\n";
    for (size_t i = 0; i < resources_.size(); ++i) {
      const auto &ver = resources_[i].current_version;
      std::cout << "[" << i << "] "
                << (ver.name.empty() ? "<no name>" : ver.name) << "\n";
      std::cout << "     key:     " << resources_[i].resource_key << "\n";
      std::cout << "     version: " << ver.version << "\n";
    }
    std::cout << "=================================\n\n";
  }

  int pick_resource() {
    if (resources_.empty()) {
      return -1;
    }

    std::string line;
    while (true) {
      std::cout << "Select resource index [0-" << resources_.size() - 1
                << "] (q to quit): ";
      if (!std::getline(std::cin, line)) {
        return -1;
      }
      if (line == "q" || line == "quit" || line == "exit") {
        return -1;
      }
      try {
        int idx = std::stoi(line);
        if (idx >= 0 && idx < static_cast<int>(resources_.size())) {
          return idx;
        }
        std::cout << "Index out of range (0-" << resources_.size() - 1
                  << ").\n";
      } catch (...) {
        std::cout << "Please enter a number.\n";
      }
    }
  }

  void play_resource(int idx) {
    while (!play_resource_client_->wait_for_service(1s)) {
      if (!rclcpp::ok()) {
        RCLCPP_ERROR(this->get_logger(),
                     "Interrupted while waiting for service.");
        return;
      }
      RCLCPP_INFO(this->get_logger(), "Service not available, waiting...");
    }

    if (!rclcpp::ok()) {
      return;
    }

    const auto &resource = resources_[idx];
    auto request =
        std::make_shared<aimdk_msgs::srv::ExecuteActionResource::Request>();

    request->resource_key = resource.resource_key;
    request->resource_version = resource.current_version.version;

    for (const auto &ip : slave_ips_) {
      aimdk_msgs::msg::SlaveDevice slave;
      slave.ip = ip;
      request->slaves.push_back(slave);
    }

    if (request->resource_key.find("onnx") != std::string::npos) {
      request->meta =
          R"({"control_type": "GROUP_CONTROL","resource_type": "BODY_MONTION"})";
    } else {
      request->meta =
          R"({"control_type": "GROUP_CONTROL","resource_type": "ARM_MONTION"})";
    }

    RCLCPP_INFO(this->get_logger(), "Sending request:");
    RCLCPP_INFO(this->get_logger(), "  resource_key: %s",
                request->resource_key.c_str());
    RCLCPP_INFO(this->get_logger(), "  resource_version: %s",
                request->resource_version.c_str());
    RCLCPP_INFO(this->get_logger(), "  meta: %s", request->meta.c_str());
    for (size_t i = 0; i < request->slaves.size(); ++i) {
      RCLCPP_INFO(this->get_logger(), "  slaves[%zu] ip: %s", i,
                  request->slaves[i].ip.c_str());
    }

    auto future = play_resource_client_->async_send_request(request);

    if (rclcpp::spin_until_future_complete(this->get_node_base_interface(),
                                           future) ==
        rclcpp::FutureReturnCode::SUCCESS) {
      handle_response(future.get());
    } else {
      if (rclcpp::ok()) {
        RCLCPP_ERROR(this->get_logger(),
                     "Failed to call ExecuteActionResource");
      }
    }
  }

  bool sync_time(const std::vector<std::string> &ips) {
    slave_ips_ = ips;

    while (!time_sync_client_->wait_for_service(1s)) {
      if (!rclcpp::ok()) {
        return false;
      }
      RCLCPP_INFO(this->get_logger(),
                  "TimeSync service not available, waiting again...");
    }

    if (!rclcpp::ok()) {
      return false;
    }

    auto request =
        std::make_shared<aimdk_msgs::srv::GroupControlTimeSync::Request>();

    for (const auto &ip : ips) {
      aimdk_msgs::msg::SlaveDevice slave;
      slave.ip = ip;
      request->slaves.push_back(slave);
    }

    RCLCPP_INFO(this->get_logger(), "Sending GroupControlTimeSync request:");
    for (const auto &slave : request->slaves) {
      RCLCPP_INFO(this->get_logger(), "  slave ip: %s", slave.ip.c_str());
    }

    auto future = time_sync_client_->async_send_request(request);

    if (rclcpp::spin_until_future_complete(this->get_node_base_interface(),
                                           future) !=
        rclcpp::FutureReturnCode::SUCCESS) {
      if (rclcpp::ok()) {
        RCLCPP_ERROR(this->get_logger(), "Failed to call GroupControlTimeSync");
      }
      return false;
    }

    auto response = future.get();
    RCLCPP_INFO(this->get_logger(), "GroupControlTimeSync response:");
    RCLCPP_INFO(this->get_logger(), "  error_code: %d", response->error_code);
    RCLCPP_INFO(this->get_logger(), "  error_msg: %s",
                response->error_msg.c_str());
    return response->error_code == 0;
  }

private:
  rclcpp::Client<aimdk_msgs::srv::GetRobotResources>::SharedPtr
      get_resource_client_;
  rclcpp::Client<aimdk_msgs::srv::ExecuteActionResource>::SharedPtr
      play_resource_client_;
  rclcpp::Client<aimdk_msgs::srv::GroupControlTimeSync>::SharedPtr
      time_sync_client_;
  std::vector<aimdk_msgs::msg::RobotResource> resources_;
  std::vector<std::string> slave_ips_;

  void handle_response(
      const aimdk_msgs::srv::ExecuteActionResource::Response::SharedPtr
          response) {
    RCLCPP_INFO(this->get_logger(), "Service response received:");
    RCLCPP_INFO(this->get_logger(), "Response Header:");
    RCLCPP_INFO(this->get_logger(), "  stamp: %d.%09u",
                response->header.header.stamp.sec,
                response->header.header.stamp.nanosec);
    RCLCPP_INFO(this->get_logger(), "  code: %" PRId64,
                response->header.header.code);
    RCLCPP_INFO(this->get_logger(), "Status:");
    RCLCPP_INFO(this->get_logger(), "  value: %d",
                response->header.status.value);
    RCLCPP_INFO(this->get_logger(), "  message: %s",
                response->header.message.c_str());
  }
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto client_node = std::make_shared<MultiRobotResourceClient>();

  if (!client_node->get_robot_resources()) {
    rclcpp::shutdown();
    return 1;
  }

  // [USER_CONFIG] Replace the IPs below with your slave robot IPs.
  std::vector<std::string> slave_ips = {"172.22.133.178", "172.22.135.45"};
  if (!client_node->sync_time(slave_ips)) {
    RCLCPP_WARN(client_node->get_logger(),
                "Time sync failed or returned error.");
  }

  client_node->print_resource_menu();
  int chosen = client_node->pick_resource();
  if (chosen < 0) {
    RCLCPP_INFO(client_node->get_logger(), "No resource selected, exiting.");
    rclcpp::shutdown();
    return 0;
  }

  client_node->play_resource(chosen);
  rclcpp::shutdown();
  return 0;
}

Usage Instructions:

ros2 run examples multi_play_linkcraft
[INFO] [1772529153.517267476] [robot_resource_client]: Found 1 resources
[INFO] [1772529153.517473238] [robot_resource_client]: Sending GroupControlTimeSync request:
[INFO] [1772529153.517488347] [robot_resource_client]:   slave ip: 172.22.133.178
[INFO] [1772529153.517497834] [robot_resource_client]:   slave ip: 172.22.135.45
[INFO] [1772529153.517505889] [robot_resource_client]: GroupControlTimeSync response:
[INFO] [1772529153.517506000] [robot_resource_client]:   error_code: 0
[INFO] [1772529153.517506100] [robot_resource_client]:   error_msg: ok
[INFO] [1772529153.517473238] [robot_resource_client]: Sending request:
[INFO] [1772529153.517488347] [robot_resource_client]:   resource_key: linkcraft_resource_onnx_01KBM2BHNFM93Z0DK6ES9F6DJA
[INFO] [1772529153.517497834] [robot_resource_client]:   resource_version: 0.0.1
[INFO] [1772529153.517505889] [robot_resource_client]:   meta: {"control_type": "GROUP_CONTROL","resource_type": "BODY_MONTION"}
[INFO] [1772529153.517506200] [robot_resource_client]:   slaves[0] ip: 172.22.133.178
[INFO] [1772529153.517506300] [robot_resource_client]:   slaves[1] ip: 172.22.135.45
[INFO] [1772529153.553673433] [robot_resource_client]: Service response received:
[INFO] [1772529153.553697021] [robot_resource_client]: Response Header:
[INFO] [1772529153.553700410] [robot_resource_client]:   stamp: 0.000000000
[INFO] [1772529153.553704130] [robot_resource_client]:   code: 0
[INFO] [1772529153.553707001] [robot_resource_client]: Status:
[INFO] [1772529153.553709853] [robot_resource_client]:   value: 1
[INFO] [1772529153.553712662] [robot_resource_client]:   message: 1/1 task(s) failed. Details: localhost: failed (status code: 400) - Failed with status code: 400

6.2.33 LED Strip Control

Function Description: Demonstrates how to control the robot’s LED strip, supporting multiple display modes and customizable 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:

#include <aimdk_msgs/msg/common_request.hpp>
#include <aimdk_msgs/srv/set_pmu_led.hpp>
#include <algorithm>
#include <cctype>
#include <chrono>
#include <iostream>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <signal.h>
#include <string>

std::shared_ptr<rclcpp::Node> g_node = nullptr;

void signal_handler(int signal) {
  if (g_node) {
    RCLCPP_INFO(g_node->get_logger(), "Received signal %d, shutting down...",
                signal);
    g_node.reset();
  }
  rclcpp::shutdown();
  exit(signal);
}

class PlayLightsClient : public rclcpp::Node {
public:
  PlayLightsClient() : Node("play_lights_client") {
    client_ = this->create_client<aimdk_msgs::srv::SetPmuLed>(
        "/aimdk_5Fmsgs/srv/SetPmuLed");
    RCLCPP_INFO(this->get_logger(), "✅ PlayLights client node started.");

    // Wait for the service to become available
    while (!client_->wait_for_service(std::chrono::seconds(2))) {
      RCLCPP_INFO(this->get_logger(), "⏳ Service unavailable, waiting...");
    }
    RCLCPP_INFO(this->get_logger(),
                "🟢 Service available, ready to send request.");
  }

  bool send_request(uint8_t led_mode, uint8_t r, uint8_t g, uint8_t b,
                    int32_t priority = 6, bool reset_priority = false) {
    try {
      auto request = std::make_shared<aimdk_msgs::srv::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";

      RCLCPP_INFO(this->get_logger(),
                  "📨 Sending request to control led strip: mode=%hhu, "
                  "RGB=(%hhu, %hhu, %hhu), priority=%d, reset_priority=%s",
                  led_mode, r, g, b, priority,
                  reset_priority ? "true" : "false");

      // LED strip is slow to response (up to ~5s)
      const std::chrono::milliseconds timeout(5000);
      for (int i = 0; i < 4; i++) {
        request->request.header.stamp = this->now();
        auto future = client_->async_send_request(request);
        auto retcode = rclcpp::spin_until_future_complete(
            this->shared_from_this(), future, timeout);

        if (retcode != rclcpp::FutureReturnCode::SUCCESS) {
          // retry as remote peer is NOT handled well by ROS
          RCLCPP_INFO(this->get_logger(), "trying ... [%d]", i);
          continue;
        }
        // future.done
        auto response = future.get();
        if (response->status_code == 0) {
          RCLCPP_INFO(this->get_logger(),
                      "✅ LED strip command sent successfully.");
          return true;
        } else {
          // 0x1024: priority insufficient (priority < current threshold);
          // use a higher priority or set reset_priority=true to reset the
          // threshold to 0.
          RCLCPP_ERROR(this->get_logger(),
                       "❌ LED strip command failed with status: 0x%X (%s)",
                       response->status_code,
                       response->status_code == 0x1024 ? "priority insufficient"
                                                       : "failure");
          return false;
        }
      }
      RCLCPP_ERROR(this->get_logger(), "❌ Service call failed or timed out.");
      return false;
    } catch (const std::exception &e) {
      RCLCPP_ERROR(this->get_logger(), "Exception occurred: %s", e.what());
      return false;
    }
  }

private:
  rclcpp::Client<aimdk_msgs::srv::SetPmuLed>::SharedPtr client_;
};

int main(int argc, char **argv) {
  try {
    rclcpp::init(argc, argv);
    signal(SIGINT, signal_handler);
    signal(SIGTERM, signal_handler);

    g_node = std::make_shared<PlayLightsClient>();
    auto client_node = std::dynamic_pointer_cast<PlayLightsClient>(g_node);

    int led_mode = 0;          // LED Strip Mode
    int r = 255, g = 0, b = 0; // RGB values
    int priority = 6;          // Priority
    bool reset_priority = false;

    // Parse command-line arguments: mode R G B priority reset_priority
    // e.g. ros2 run examples play_lights 1 255 0 0 6 false
    if (argc > 6) {
      led_mode = std::stoi(argv[1]);
      if (led_mode < 0 || led_mode > 3) {
        throw std::runtime_error("invalid mode");
      }
      r = std::stoi(argv[2]);
      if (r < 0 || r > 255) {
        throw std::runtime_error("invalid R value");
      }
      g = std::stoi(argv[3]);
      if (g < 0 || g > 255) {
        throw std::runtime_error("invalid G value");
      }
      b = std::stoi(argv[4]);
      if (b < 0 || b > 255) {
        throw std::runtime_error("invalid B value");
      }
      priority = std::stoi(argv[5]);
      if (priority < 0) {
        throw std::runtime_error("invalid priority value");
      }
      std::string reset_str = argv[6];
      std::transform(reset_str.begin(), reset_str.end(), reset_str.begin(),
                     ::tolower);
      reset_priority = (reset_str == "true" || reset_str == "1" ||
                        reset_str == "yes" || reset_str == "y");
    } else {
      std::cout << "=== LED strip control example ===" << std::endl;
      std::cout << "Select LED strip mode:" << std::endl;
      std::cout << "0 - Steady on" << std::endl;
      std::cout << "1 - Breathing (4s cycle, sine brightness)" << std::endl;
      std::cout << "2 - Blinking (1s cycle, 0.5s on, 0.5s off)" << std::endl;
      std::cout << "3 - Flowing (2s cycle, light up from left to right)"
                << std::endl;
      std::cout << "Enter mode (0-3): ";
      std::cin >> led_mode;
      if (led_mode < 0 || led_mode > 3) {
        throw std::runtime_error("invalid mode");
      }

      std::cout << "\nSet RGB color values (0-255):" << std::endl;
      std::cout << "Red (R): ";
      std::cin >> r;
      if (r < 0 || r > 255) {
        throw std::runtime_error("invalid R value");
      }
      std::cout << "Green (G): ";
      std::cin >> g;
      if (g < 0 || g > 255) {
        throw std::runtime_error("invalid G value");
      }
      std::cout << "Blue (B): ";
      std::cin >> b;
      if (b < 0 || b > 255) {
        throw std::runtime_error("invalid B value");
      }

      std::cout
          << "\nSet priority (higher value = higher priority, default=6): ";
      std::cin >> priority;
      if (priority < 0) {
        throw std::runtime_error("invalid priority value");
      }

      std::string reset_input;
      std::cout << "Reset priority after command? (y/n, default=n): ";
      std::cin >> reset_input;
      std::transform(reset_input.begin(), reset_input.end(),
                     reset_input.begin(), ::tolower);
      reset_priority = (reset_input == "true" || reset_input == "1" ||
                        reset_input == "yes" || reset_input == "y");
    }

    if (client_node) {
      client_node->send_request(led_mode, r, g, b, priority, reset_priority);
    }

    g_node.reset();
    rclcpp::shutdown();

    return 0;
  } catch (const std::exception &e) {
    RCLCPP_ERROR(rclcpp::get_logger("main"),
                 "Program terminated with exception: %s", e.what());
    return 1;
  }
}

Usage Instructions:

# Build
colcon build --packages-select examples

# Run interactively
ros2 run examples play_lights

# Run with command-line parameters: <mode> <R> <G> <B> <priority> <reset_priority(true/false)>
ros2 run examples play_lights 1 255 0 0 6 false  # Mode 1, red, priority 6
ros2 run examples set_mic_source

# Run with argument
ros2 run examples set_mic_source ext  # ext - external mic, int - internal mic

Output Example:

=== LED strip control example ===
Select LED strip mode:
0 - Steady on
1 - Breathing (4s cycle, sine brightness)
2 - Blinking (1s cycle, 0.5s on, 0.5s off)
3 - Flowing (2s cycle, light up from left to right)
Enter mode (0-3): 1

Set RGB color values (0-255):
Red (R): 255
Green (G): 0
Blue (B): 0

Set priority (higher value = higher priority, default=6): 6
Reset priority after command? (y/n, default=n): n

[INFO] [play_lights_client]: 📨 Sending request to control led strip: mode=1, RGB=(255, 0, 0), priority=6, reset_priority=false
[INFO] [play_lights_client]: ✅ LED strip command sent successfully.

Technical Features:

• Supports 4 LED display modes

• Customizable RGB colors

• Priority control (higher value = higher priority)

• Priority reset support

• Asynchronous service calls

• Command-line parameter support

• Input parameter validation

• User-friendly interactive interface

6.2.34 GNSS Positioning Data Subscription

This example uses echo_gnss_data to subscribe to GNSS topics for positioning data, outputting latitude, longitude, altitude, and covariance information.

Features:

• Subscribes to NavSatFix positioning data (latitude, longitude, altitude)

• Real-time display of positioning status and covariance

Topics:

• /aima/hal/sensor/gnss: Positioning data (sensor_msgs/NavSatFix), published automatically by the system

#include <chrono>
#include <iomanip>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/nav_sat_fix.hpp>
#include <sstream>
#include <string>

class GnssPrinter : public rclcpp::Node {
public:
  GnssPrinter() : rclcpp::Node("gnss_printer") {
    constexpr char kNavSatTopic[] = "/aima/hal/sensor/gnss";

    auto qos = rclcpp::QoS(rclcpp::KeepLast(10))
                   .reliability(rclcpp::ReliabilityPolicy::Reliable)
                   .durability(rclcpp::DurabilityPolicy::TransientLocal);

    navsat_sub_ = this->create_subscription<sensor_msgs::msg::NavSatFix>(
        kNavSatTopic, qos,
        std::bind(&GnssPrinter::navsat_cb, this, std::placeholders::_1));

    RCLCPP_INFO(this->get_logger(), "Subscribing NavSatFix: %s", kNavSatTopic);
  }

private:
  void navsat_cb(const sensor_msgs::msg::NavSatFix::SharedPtr msg) {
    auto now = std::chrono::steady_clock::now();
    double dt_ms =
        last_navsat_recv_.time_since_epoch().count() == 0
            ? 0.0
            : std::chrono::duration<double, std::milli>(now - last_navsat_recv_)
                  .count();
    last_navsat_recv_ = now;

    std::ostringstream oss;
    oss << std::fixed << std::setprecision(6);
    oss << "\n========== NavSatFix ==========\n";
    oss << "stamp=" << rclcpp::Time(msg->header.stamp).seconds() << "s\n";
    oss << "frame=" << msg->header.frame_id << "\n";
    oss << "recv_dt=" << std::setprecision(3) << dt_ms << "ms\n";
    oss << std::setprecision(8);
    oss << "latitude:  " << msg->latitude << " deg\n";
    oss << "longitude: " << msg->longitude << " deg\n";
    oss << std::setprecision(3);
    oss << "altitude:  " << msg->altitude << " m\n";

    const char *status_str = "UNKNOWN";
    switch (msg->status.status) {
    case sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX:
      status_str = "NO_FIX";
      break;
    case sensor_msgs::msg::NavSatStatus::STATUS_FIX:
      status_str = "FIX";
      break;
    }
    oss << "status: " << status_str << "\n";
    oss << "service: " << msg->status.service << "\n";

    oss << std::setprecision(4);
    oss << "position_covariance: [";
    for (size_t i = 0; i < 9; ++i) {
      if (i > 0)
        oss << ", ";
      oss << msg->position_covariance[i];
    }
    oss << "]\n";
    oss << "position_covariance_type: "
        << static_cast<int>(msg->position_covariance_type);

    RCLCPP_INFO(this->get_logger(), "%s", oss.str().c_str());
  }

  rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr navsat_sub_;
  std::chrono::steady_clock::time_point last_navsat_recv_{};
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<GnssPrinter>());
  rclcpp::shutdown();
  return 0;
}

Usage Instructions:

# Build
colcon build --packages-select examples

# Subscribe to GNSS data
ros2 run examples echo_gnss_data

Output Example:

[INFO] [gnss_printer]: Subscribing NavSatFix: /aima/hal/sensor/gnss

========== NavSatFix ==========
stamp=1234567890.123456s
frame=gnss
recv_dt=1000.000ms
latitude:  31.84537000 deg
longitude: 117.19882000 deg
altitude:  30.000 m
status: FIX
service: 1
position_covariance: [1.0000, 0.0000, 0.0000, 0.0000, 1.0000, 0.0000, 0.0000, 0.0000, 1.0000]
position_covariance_type: 2

Notes:

• Positioning status: NO_FIX (no fix), FIX (single point fix)

• Indoors, status is usually NO_FIX; move to an open outdoor area and wait for a fix

6.2.35 Upper Body Control

Note

Switch to the corresponding mode before running. See Upper Body Control.

// Upper body control example.
//
// Usage:
//   ros2 run examples upper_body_control <mode>
//
//   mode:
//     head    — head center pose,          requires HEAD_ONLY mode
//     claw    — claw half-open,            requires UPPERBODY_REMOTE_SPLIT mode
//     joint   — dexterous hand half-open,  requires UPPERBODY_REMOTE_SPLIT mode
//     gesture — dexterous hand gesture 1,  requires UPPERBODY_REMOTE_SPLIT mode
//
// Switch to the desired MC mode before running, e.g. for head:
//   ros2 run examples set_mc_action SD     # → STAND_DEFAULT
//   ros2 run examples set_mc_action HO     # → HEAD_ONLY
// or for claw / joint / gesture:
//   ros2 run examples set_mc_action URS    # → UPPERBODY_REMOTE_SPLIT

#include <array>
#include <chrono>
#include <cstdint>
#include <cstdio>
#include <map>
#include <memory>
#include <string>
#include <vector>

#include "aimdk_msgs/msg/message_header.hpp"
#include "aimdk_msgs/msg/upper_body_command_array.hpp"
#include "rclcpp/rclcpp.hpp"

using namespace std::chrono_literals;
using Msg = aimdk_msgs::msg::UpperBodyCommandArray;

// hand_sub_mode, head_pos, arm_pos, hand_pos
struct ModeConfig {
  uint8_t hand_sub_mode;
  std::array<double, 2> head_pos;
  std::array<double, 14> arm_pos;
  std::vector<double> hand_pos;
};

static const std::map<std::string, ModeConfig> &modes() {
  static const std::map<std::string, ModeConfig> kModes = {
      // head_yaw=0, head_pitch=0 (center)
      {"head",
       {0,
        {{0.0, 0.0}},
        {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
          0.0}},
        {}}},
      // left_open=0.5, right_open=0.5
      {"claw",
       {Msg::HAND_CLAW_OPEN_CLOSE, // claw (1)
        {{0.0, 0.0}},
        {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
          0.0}},
        {0.5, 0.5}}},
      // left 10 joints + right 10 joints, in rad, all half-open
      // (each joint at half its URDF range limit). Right is the mirror of left.
      // Joint order: [thumb_roll, thumb_abad, thumb_mcp, index_abad, index_pip,
      //               middle_pip, ring_abad, ring_pip, pinky_abad, pinky_pip]
      {"joint",
       {Msg::HAND_DEXTEROUS_JOINT, // dexterous joint (2)
        {{0.0, 0.0}},
        {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
          0.0}},
        {-0.6, 0.8,  -0.4, 0.1,  0.7, 0.7, -0.1, 0.7, -0.1, 0.7,
         0.6,  -0.8, 0.4,  -0.1, 0.7, 0.7, 0.1,  0.7, 0.1,  0.7}}},
      // left_gesture=1, left_open=1.0, right_gesture=1, right_open=1.0
      {"gesture",
       {Msg::HAND_DEXTEROUS_GESTURE, // dexterous gesture (3)
        {{0.0, 0.0}},
        {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
          0.0}},
        {1.0, 1.0, 1.0, 1.0}}},
  };
  return kModes;
}

class UpperBodyControlNode : public rclcpp::Node {
public:
  UpperBodyControlNode(const std::string &mode)
      : Node("upper_body_control"), seq_(0) {
    const auto &cfg = modes().at(mode);
    hand_sub_mode_ = cfg.hand_sub_mode;
    head_pos_ = cfg.head_pos;
    arm_pos_ = cfg.arm_pos;
    hand_pos_ = cfg.hand_pos;
    pub_ = create_publisher<Msg>("/mc/upper_body_command", 10);
    timer_ = create_wall_timer(20ms, [this]() { publish(); });
    RCLCPP_INFO(get_logger(), "mode=%s  hand_sub_mode=%d", mode.c_str(),
                hand_sub_mode_);
  }

private:
  void publish() {
    Msg msg;
    auto t = this->now();
    msg.header.stamp.sec = static_cast<int32_t>(t.seconds());
    msg.header.stamp.nanosec =
        static_cast<uint32_t>(t.nanoseconds() % 1000000000);
    msg.header.frame_id = "mc_upper_body";
    msg.header.sequence = seq_++;
    msg.source = "upper_body_example";
    msg.hand_sub_mode = hand_sub_mode_;
    msg.head_pos = head_pos_;
    msg.arm_pos = arm_pos_;
    msg.hand_pos = hand_pos_;
    pub_->publish(msg);
  }

  rclcpp::Publisher<Msg>::SharedPtr pub_;
  rclcpp::TimerBase::SharedPtr timer_;
  uint32_t seq_;
  uint8_t hand_sub_mode_;
  std::array<double, 2> head_pos_;
  std::array<double, 14> arm_pos_;
  std::vector<double> hand_pos_;
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);

  std::string mode;
  if (argc >= 2)
    mode = argv[1];

  if (modes().find(mode) == modes().end()) {
    std::string usage = "Usage: upper_body_control <";
    bool first = true;
    for (const auto &[k, _] : modes()) {
      if (!first)
        usage += "|";
      usage += k;
      first = false;
    }
    usage += ">";
    std::printf("%s\n", usage.c_str());
    rclcpp::shutdown();
    return 1;
  }

  rclcpp::spin(std::make_shared<UpperBodyControlNode>(mode));
  rclcpp::shutdown();
  return 0;
}

Usage Instructions:

colcon build --packages-select examples

# Head control (HEAD_ONLY mode)
ros2 run examples upper_body_control head

# Claw (gripper) control (UPPERBODY_REMOTE_SPLIT mode)
ros2 run examples upper_body_control claw

# Dexterous hand joints (UPPERBODY_REMOTE_SPLIT mode)
ros2 run examples upper_body_control joint

# Dexterous hand gesture (UPPERBODY_REMOTE_SPLIT mode)
ros2 run examples upper_body_control gesture

6.2.36 Subscribe to HDS Diagnostics / Query Historical Events

Subscribe to /aima/hds/diag_code_list, /aima/hds/alert_code_list, and /aima/hds/post_proc/signal topics to receive diagnostic codes, alert codes, and post-processing signals in real time; also supports calling the GetHdsEventHistory service to query historical events. For the full list of diagnostic codes, see Diagnostic Code Reference.

// HDS Diagnostics Tool — Subscribe to HDS topics or query history events.
//
// See docs/cn/example/Cpp.md section "Subscribe HDS diagnostics /
// query history events" for full usage and field descriptions.

#include <chrono>
#include <cstdint>
#include <functional>
#include <iomanip>
#include <iostream>
#include <memory>
#include <sstream>
#include <string>
#include <vector>

#include <rclcpp/rclcpp.hpp>

#include "aimdk_msgs/msg/alert_code_array.hpp"
#include "aimdk_msgs/msg/diagnostic_info_array.hpp"
#include "aimdk_msgs/msg/diagnostic_type.hpp"
#include "aimdk_msgs/msg/hds_event_query.hpp"
#include "aimdk_msgs/msg/post_proc_sig_array.hpp"
#include "aimdk_msgs/srv/get_hds_event_history.hpp"

using namespace std::chrono_literals;

namespace {

std::string DiagTypeName(int32_t value) {
  switch (value) {
  case aimdk_msgs::msg::DiagnosticType::NORMAL:
    return "NORMAL";
  case aimdk_msgs::msg::DiagnosticType::TRIGGER_APPEAR:
    return "APPEAR";
  case aimdk_msgs::msg::DiagnosticType::TRIGGER_DISAPPEAR:
    return "DISAPPEAR";
  default:
    return "UNKNOWN(" + std::to_string(value) + ")";
  }
}

std::string StateName(uint8_t value) {
  switch (value) {
  case aimdk_msgs::msg::HdsEventQuery::STATE_ALL:
    return "ALL";
  case aimdk_msgs::msg::HdsEventQuery::STATE_ACTIVE:
    return "ACTIVE";
  case aimdk_msgs::msg::HdsEventQuery::STATE_RECOVERED:
    return "RECOVERED";
  default:
    return "UNKNOWN";
  }
}

std::string HexCode(uint64_t code, int width = 10) {
  std::ostringstream oss;
  oss << "0x" << std::hex << std::uppercase << std::setw(width)
      << std::setfill('0') << code;
  return oss.str();
}

uint64_t ParseHex(const std::string &s) { return std::stoull(s, nullptr, 0); }

// ---------- echo mode ----------

class HdsDiagnosticsEcho : public rclcpp::Node {
public:
  HdsDiagnosticsEcho() : Node("hds_diagnostics_echo") {
    // HDS publishers use BEST_EFFORT; match it to receive messages.
    auto qos = rclcpp::QoS(10).best_effort().durability_volatile();

    sub_diag_ = create_subscription<aimdk_msgs::msg::DiagnosticInfoArray>(
        "/aima/hds/diag_code_list", qos,
        std::bind(&HdsDiagnosticsEcho::OnDiagnostics, this,
                  std::placeholders::_1));

    sub_alert_ = create_subscription<aimdk_msgs::msg::AlertCodeArray>(
        "/aima/hds/alert_code_list", qos,
        std::bind(&HdsDiagnosticsEcho::OnAlerts, this, std::placeholders::_1));

    sub_post_ = create_subscription<aimdk_msgs::msg::PostProcSigArray>(
        "/aima/hds/post_proc/signal", qos,
        std::bind(&HdsDiagnosticsEcho::OnPostProc, this,
                  std::placeholders::_1));

    RCLCPP_INFO(get_logger(), "Subscribed: diag_code_list / alert_code_list / "
                              "post_proc/signal");
  }

private:
  void
  OnDiagnostics(const aimdk_msgs::msg::DiagnosticInfoArray::SharedPtr msg) {
    if (msg->diagnostics.empty())
      return;
    std::ostringstream oss;
    oss << "[Diagnostics] count=" << msg->diagnostics.size();
    for (const auto &d : msg->diagnostics) {
      oss << "\n  diag_code=" << HexCode(d.diag_code)
          << "  type=" << DiagTypeName(d.type.value)
          << "  module_id=" << static_cast<int>(d.module_id)
          << "  ts=" << d.timestamp << "  info=\"" << d.info << "\"";
    }
    RCLCPP_INFO(get_logger(), "%s", oss.str().c_str());
  }

  void OnAlerts(const aimdk_msgs::msg::AlertCodeArray::SharedPtr msg) {
    if (msg->list.empty())
      return;
    std::ostringstream oss;
    oss << "[Alerts] count=" << msg->list.size();
    for (const auto &a : msg->list) {
      oss << "\n  code=\"" << a.code << "\"  ts=" << a.timestamp
          << "  first_ts=" << a.first_timestamp;
    }
    RCLCPP_INFO(get_logger(), "%s", oss.str().c_str());
  }

  void OnPostProc(const aimdk_msgs::msg::PostProcSigArray::SharedPtr msg) {
    if (msg->post_proc_sig.empty())
      return;
    std::ostringstream oss;
    oss << "[PostProc] count=" << msg->post_proc_sig.size();
    for (const auto &p : msg->post_proc_sig) {
      const auto &s = p.sig_set;
      oss << "\n  diag_code=" << HexCode(p.diag_code) << "  ts=" << p.timestamp
          << "  robot=" << s.robot_sig
          << " motion=" << static_cast<int>(s.motion_sig)
          << " sound=" << static_cast<int>(s.sound_sig)
          << " led=" << static_cast<int>(s.led_sig)
          << " facial=" << static_cast<int>(s.facial_sig)
          << " funcdisable=" << HexCode(s.funcdisable_sig, 16);
    }
    RCLCPP_INFO(get_logger(), "%s", oss.str().c_str());
  }

  rclcpp::Subscription<aimdk_msgs::msg::DiagnosticInfoArray>::SharedPtr
      sub_diag_;
  rclcpp::Subscription<aimdk_msgs::msg::AlertCodeArray>::SharedPtr sub_alert_;
  rclcpp::Subscription<aimdk_msgs::msg::PostProcSigArray>::SharedPtr sub_post_;
};

int RunEcho(int argc, char **argv) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<HdsDiagnosticsEcho>());
  rclcpp::shutdown();
  return 0;
}

// ---------- history mode ----------

struct HistoryArgs {
  uint64_t start_ns = 0;
  uint64_t end_ns = 0;
  std::vector<uint64_t> diag_codes;
  std::vector<std::string> alert_codes;
  uint8_t state = aimdk_msgs::msg::HdsEventQuery::STATE_ALL;
  bool desc = false;
  uint32_t offset = 0;
  uint32_t limit = 20;
};

bool ParseHistoryArgs(int argc, char **argv, HistoryArgs *args) {
  // argv[0] = exe, argv[1] = "history", real options start at 2.
  for (int i = 2; i < argc; ++i) {
    std::string a = argv[i];
    auto need_value = [&](const char *flag) -> const char * {
      if (i + 1 >= argc) {
        std::cerr << "Missing value for " << flag << std::endl;
        return nullptr;
      }
      return argv[++i];
    };

    if (a == "--start-ns") {
      const char *v = need_value("--start-ns");
      if (!v)
        return false;
      args->start_ns = std::stoull(v);
    } else if (a == "--end-ns") {
      const char *v = need_value("--end-ns");
      if (!v)
        return false;
      args->end_ns = std::stoull(v);
    } else if (a == "--state") {
      const char *v = need_value("--state");
      if (!v)
        return false;
      args->state = static_cast<uint8_t>(std::stoi(v));
    } else if (a == "--offset") {
      const char *v = need_value("--offset");
      if (!v)
        return false;
      args->offset = static_cast<uint32_t>(std::stoul(v));
    } else if (a == "--limit") {
      const char *v = need_value("--limit");
      if (!v)
        return false;
      args->limit = static_cast<uint32_t>(std::stoul(v));
    } else if (a == "--desc") {
      args->desc = true;
    } else if (a == "--diag-codes") {
      while (i + 1 < argc && std::string(argv[i + 1]).rfind("--", 0) != 0) {
        args->diag_codes.push_back(ParseHex(argv[++i]));
      }
    } else if (a == "--alert-codes") {
      while (i + 1 < argc && std::string(argv[i + 1]).rfind("--", 0) != 0) {
        args->alert_codes.emplace_back(argv[++i]);
      }
    } else {
      std::cerr << "Unknown option: " << a << std::endl;
      return false;
    }
  }
  return true;
}

void PrintRecords(
    const aimdk_msgs::srv::GetHdsEventHistory::Response &response) {
  std::cout << "total_count = " << response.total_count
            << ", returned = " << response.records.size()
            << ", has_more = " << (response.has_more ? "true" : "false")
            << std::endl;
  if (response.records.empty())
    return;

  std::string sep(80, '-');
  std::cout << sep << std::endl;
  for (const auto &r : response.records) {
    double duration_s = static_cast<double>(r.duration_ns) / 1e9;
    std::cout << "id=" << r.id << "  diag=" << HexCode(r.diag_code)
              << "  alert=\"" << r.alert_code << "\"" << std::endl;
    std::cout << "  first_ts=" << r.first_timestamp
              << "  last_ts=" << r.last_timestamp << std::endl;
    std::cout << "  recovered=" << (r.recovered ? "YES" : "NO")
              << "  recover_ts=" << r.recover_timestamp
              << "  duration=" << std::fixed << std::setprecision(3)
              << duration_s << "s" << std::endl;
    if (!r.alert_location.empty()) {
      std::cout << "  location=\"" << r.alert_location << "\"" << std::endl;
    }
    std::cout << sep << std::endl;
  }
}

int RunHistory(int argc, char **argv) {
  HistoryArgs args;
  if (!ParseHistoryArgs(argc, argv, &args))
    return 1;

  rclcpp::init(argc, argv);
  auto node = std::make_shared<rclcpp::Node>("hds_history_client");
  auto client = node->create_client<aimdk_msgs::srv::GetHdsEventHistory>(
      "/aimdk_5Fmsgs/srv/GetHdsEventHistory");

  if (!client->wait_for_service(5s)) {
    RCLCPP_ERROR(node->get_logger(),
                 "Service /aimdk_5Fmsgs/srv/GetHdsEventHistory not available");
    rclcpp::shutdown();
    return 1;
  }

  auto request =
      std::make_shared<aimdk_msgs::srv::GetHdsEventHistory::Request>();
  auto now = node->now();
  request->header.stamp.sec = now.seconds();
  request->header.stamp.nanosec = now.nanoseconds() % 1000000000;
  request->query.start_time = args.start_ns;
  request->query.end_time = args.end_ns;
  request->query.diag_codes = args.diag_codes;
  request->query.alert_codes = args.alert_codes;
  request->query.state = args.state;
  request->query.order_desc = args.desc;
  request->query.offset = args.offset;
  request->query.limit = args.limit;

  RCLCPP_INFO(node->get_logger(),
              "Querying history: state=%s, offset=%u, limit=%u, desc=%s",
              StateName(args.state).c_str(), args.offset, args.limit,
              args.desc ? "true" : "false");

  auto future = client->async_send_request(request);
  if (rclcpp::spin_until_future_complete(node, future, 10s) !=
      rclcpp::FutureReturnCode::SUCCESS) {
    RCLCPP_ERROR(node->get_logger(), "Service call timeout");
    rclcpp::shutdown();
    return 1;
  }

  PrintRecords(*future.get());
  rclcpp::shutdown();
  return 0;
}

void PrintUsage(const char *prog) {
  std::cout << "Usage:\n"
            << "  " << prog << " echo\n"
            << "  " << prog
            << " history [--state N] [--limit N] [--offset N] [--desc]\n"
            << "                  [--start-ns N] [--end-ns N]\n"
            << "                  [--diag-codes 0xAAAA 0xBBBB ...]\n"
            << "                  [--alert-codes \"code1\" \"code2\" ...]\n";
}

} // namespace

int main(int argc, char **argv) {
  if (argc < 2) {
    PrintUsage(argv[0]);
    return 1;
  }

  std::string mode = argv[1];
  if (mode == "echo")
    return RunEcho(argc, argv);
  if (mode == "history")
    return RunHistory(argc, argv);

  PrintUsage(argv[0]);
  return 1;
}

Usage Instructions

Real-time subscription (echo):

# Subscribe to all three topics, print diagnostics, alerts, and post-proc signals in real time
ros2 run examples hds_diagnostics echo

Topic	Message Type
/aima/hds/diag_code_list	aimdk_msgs/msg/DiagnosticInfoArray
/aima/hds/alert_code_list	aimdk_msgs/msg/AlertCodeArray
/aima/hds/post_proc/signal	aimdk_msgs/msg/PostProcSigArray

Query historical events (history) — all filter options correspond to HdsEventQuery fields. Usage examples:

# Query latest 20 records (default limit=20, ascending)
ros2 run examples hds_diagnostics history

# Query by time range (start_time / end_time)
ros2 run examples hds_diagnostics history \
  --start-ns 1716000000000000000 \
  --end-ns   1716100000000000000

# Only active alerts (state = STATE_ACTIVE)
ros2 run examples hds_diagnostics history --state 1

# Filter by diagnostic codes (diag_codes, hex supported)
ros2 run examples hds_diagnostics history --diag-codes 0x6200000001 0x6200000002

# Filter by alert codes (alert_codes)
ros2 run examples hds_diagnostics history --alert-codes MOTOR_OVERHEAT COMM_LOST

# Pagination + descending (limit / offset / order_desc, newest first)
ros2 run examples hds_diagnostics history --limit 50 --offset 0 --desc

# Combined: only recovered, 10 per page, newest first
ros2 run examples hds_diagnostics history --state 2 --limit 10 --desc

Response fields correspond to HdsEventRecord.

echo Mode Output Example

[INFO] [hds_diagnostics_echo]: Subscribed: diag_code_list / alert_code_list / post_proc/signal
[INFO] [hds_diagnostics_echo]: [Diagnostics] count=1
  diag_code=0x6200000007  type=APPEAR  module_id=98  ts=1770125762202825297  info="Unable to get robot localization"
[INFO] [hds_diagnostics_echo]: [Alerts] count=1
  code="LOCALIZATION_LOST"  ts=1770125762202825297  first_ts=1770125762202825297
[INFO] [hds_diagnostics_echo]: [PostProc] count=1
  diag_code=0x6200000007  ts=1770125762202825297  robot=1 motion=2 sound=1 led=3 facial=0 funcdisable=0x0000000000000004

history Mode Output Example

[INFO] [hds_history_client]: Querying history: state=ALL, offset=0, limit=20, desc=true
total_count = 5, returned = 5, has_more = false
--------------------------------------------------------------------------------
id=42  diag=0x6200000007  alert="LOCALIZATION_LOST"
  first_ts=1770125762202825297  last_ts=1770125765310145000
  recovered=YES  recover_ts=1770125770000000000  duration=7.797s
--------------------------------------------------------------------------------

Interface Reference

• Topics: /aima/hds/diag_code_list, /aima/hds/alert_code_list, /aima/hds/post_proc/signal

• Service: /aimdk_5Fmsgs/srv/GetHdsEventHistory

• Messages: aimdk_msgs/msg/DiagnosticInfoArray, aimdk_msgs/msg/AlertCodeArray, aimdk_msgs/msg/PostProcSigArray