6.2 C++接口使用示例

本章节将带您实现索引所示若干功能

构建 & 运行说明

• 进入到 AimDK 顶层目录(机器人上为 ~/aimdk)，执行以下命令

  source /opt/ros/humble/setup.bash
  colcon build
  source install/local_setup.bash
  ros2 run examples '对应功能名称如: get_mc_action'
  

📝 代码说明

完整的代码实现包含完整的错误处理、信号处理、超时处理等机制，确保程序的健壮性。请您在 examples 目录下查看/修改

小心

ROS的服务(service)机制在跨板通信时存在一些待优化问题, 二次开发时请参考例程添加异常处理、快速重试等保护机制

机器人处于稳定站立 / 走跑模式时, 请勿批量快速启动 ROS 节点(建议不超过 2 个/秒), 短时间内大量节点参与 DDS 发现会造成通信拥塞、影响运控实时性, 存在机器人失稳摔倒风险

6.2.1 进入开发者模式

通过调用'GetSystemState'服务获取机器人当前的系统状态，并且通过'MigrateSystemState'进入对应的开发者模式。

开发者模式定义

#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;
}

使用说明

ros2 run examples migrate_system_state Develop_MC

输出示例

[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 获取机器人模式

通过调用GetMcAction服务获取机器人当前的运行模式，包括名称和状态信息。

运动模式定义

#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;
  }
}

使用说明

ros2 run examples get_mc_action

输出示例

[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

接口参考

• 服务：/aimdk_5Fmsgs/srv/GetMcAction

• 消息：aimdk_msgs/srv/GetMcAction

6.2.3 设置机器人模式

该示例中用到了SetMcAction服务，运行节点程序后终端输入模式对应的字段值，机器人会尝试进入相应的运动模式。
切入稳定站立(STAND_DEFAULT)模式前，确保机器人已立起且双脚着地。
模式切换需遵循模式转换图, 跨模式转换不能成功
走跑模式(LOCOMOTION_DEFAULT)和稳定站立模式一体化自动切换，按模式转换图流转到两者中较近的即可

#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;
  }
}

使用说明

# 使用命令行参数设置模式（推荐）
ros2 run examples set_mc_action JD  # 零力矩模式>>站姿预备（位控站立）
ros2 run examples set_mc_action SD  # 机器人已立起且脚着地后方可执行，站姿预备>>稳定站立
# 稳定站立>>走跑模式 自动切换，无需手动切换

# 或者不带参数运行，程序会提示用户输入双字母缩写代码
ros2 run examples set_mc_action

输出示例

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

注意事项

• 切入STAND_DEFAULT模式前，确保机器人已立起且双脚着地

• 模式切换可能需要几秒钟时间完成

接口参考

• 服务：/aimdk_5Fmsgs/srv/SetMcAction

• 消息：aimdk_msgs/srv/SetMcAction

6.2.4 设置机器人动作

该示例中用到了preset_motion_client，进入稳定站立模式后，启动节点程序，输入相应的字段值可实现左手（右手）的握手（举手、挥手、飞吻）动作。

可供调用的参数见预设动作表

#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 夹爪控制

该示例中用到了hand_control，通过往话题/aima/hal/joint/hand/command发布消息来控制夹爪的运动

注意

注意⚠️ :运行本示例前需要在机器人运控计算单元（PC1）使用aima em stop-app mc关闭原生运控模块，获取机器人控制权。注意确保机器人安全

#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 灵巧手控制

该示例中用到了omnihand_control，通过往话题/aima/hal/joint/hand/command发布消息来控制灵巧手的运动

注意

注意⚠️ :运行本示例前需要在机器人运控计算单元（PC1）使用aima em stop-app mc关闭原生运控模块，获取机器人控制权。注意确保机器人安全

#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 灵巧手触觉数据（T2.1支持）

该示例中用到了 omnihand_touch，通过订阅话题/aima/hal/joint/hand/state来接收灵巧手指尖压力传感器的数据

注意

注意⚠️ :运行本示例前需要在机器人运控计算单元（PC1）使用aima em stop-app mc关闭原生运控模块，获取机器人控制权。注意确保机器人安全

//
// 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;
}

压力传感器位于五个手指的指尖、手心、手背。需要给灵巧手发送至少一个运动指令，才能获得对应的压力数据上报。

6.2.8 注册二开输入源

对于v0.7之后的版本，在实现对MC的控制前，需要先注册输入源。该示例中通过/aimdk_5Fmsgs/srv/SetMcInputSource服务来注册二开的输入源，让MC感知到，只有注册过输入源后才能实现机器人的速度控制

#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 获取当前输入源

该示例中用到了GetCurrentInputSource服务，用于获取当前已注册的输入源信息，包括输入源名称、优先级和超时时间等信息。

#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;
  }
}

使用说明

# 获取当前输入源信息
ros2 run examples get_current_input_source

输出示例

[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

注意事项

• 确保GetCurrentInputSource服务正常运行

• 需要在注册输入源之后才能获取到有效信息

• 状态码为0表示查询成功

6.2.10 机器人走跑控制

该示例中用到了mc_locomotion_velocity，以下示例通过发布/aima/mc/locomotion/velocity话题控制机器人的行走,对于v0.7之后的版本，实现速度控制前需要注册输入源（该示例已实现注册输入源），具体注册步骤可参考代码。

进入稳定站立模式后执行本程序

#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 关节电机控制

本示例展示了如何使用ROS2和Ruckig库来控制机器人的关节运动。

注意

注意⚠️ :运行本示例前需要在机器人运控计算单元（PC1）使用aima em stop-app mc关闭原生运控模块，获取机器人控制权。注意确保机器人安全

！该示例代码直接对底层电机（即 HAL 层）进行控制，在运行程序前请检查代码中对关节的安全限位是否与所用机器人相符, 并确保安全！

机器人关节控制示例

这个示例展示了如何使用ROS2和Ruckig库来控制机器人的关节运动。示例实现了以下功能：

1. 机器人关节模型定义

2. 基于Ruckig的轨迹插补

3. 多关节协同控制

4. 实时位置、速度和加速度控制

依赖项

• ROS2

• Ruckig库

• aimdk_msgs包

构建说明

1. 将代码放入ROS2工作空间的src目录

2. 在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. 在package.xml中添加依赖：

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

示例功能说明

1. 创建了四个控制器节点，分别控制：

   • 腿部x2（12个关节）

   • 腰部x1（3个关节）

   • 手臂x2（14个关节）

   • 头部x1（2个关节）

2. 演示功能：

   • 每10秒让指定关节在正负0.5弧度之间摆动

   • 使用Ruckig库实现平滑的运动轨迹

   • 实时发布关节控制命令

自定义使用

2. 添加新的控制逻辑：

   • 修改SetTargetPosition函数

   • 添加新的控制回调函数

3. 调整控制频率：

   • 修改timer的周期（当前为3s）和ruckig的dt（当前为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 键盘控制机器人

本示例实现了通过PC的键盘输入控制机器人的前进后退转弯的功能。

通过WASD控制机器人行走方向，增加/减少速度（±0.2 m/s）,Q/E增加/减少角速度（±0.1 rad/s）,ESC退出程序并释放终端资源，Space立即将速度归零，执行急停。

小心

注意:运行本示例前需要先使用手柄，将机器人切入稳定站立模式。（位控站立/走跑模式时按R2+X, 其他模式详见模式流转图进行操作），然后在机器人的终端界面使用：aima em stop-app rc关闭遥控器，防止通道占用。

实现键盘控制前需要注册输入源（该示例已实现注册输入源）
运行前需要安装curse模块:

  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，在运行节点程序前，修改需要拍照的相机话题，启动节点程序后，会创建/images/目录，将当前帧图像保存在这个目录下。

#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 相机推流示例集

该示例集提供了多种相机数据订阅和处理功能，支持深度相机、双目相机和单目相机的数据流订阅。
这些相机数据订阅 example 并没有实际的业务用途， 仅提供相机数据基础信息的打印；若您比较熟悉 ros2 使用，会发现 ros2 topic echo + ros2 topic hz 也能够实现 example 提供的功能。 您可以选择快速查阅 SDK 接口手册中话题列表直接快速进入自己模块的开发，也可以使用相机 example 作为脚手架加入自己的业务逻辑。 我们发布的传感器数据均为未经预处理(去畸变等)的原始数据，如果您需要查询传感器的详细信息（如分辨率、焦距等），请关注相应的内参（camera_info）话题。

深度相机数据订阅

该示例中用到了echo_camera_rgbd，通过订阅/aima/hal/sensor/rgbd_head_front/话题来接收机器人的深度相机数据，支持深度图、RGB图、压缩RGB图和相机内参等多种数据类型。

功能特点：

• 支持多种数据类型订阅（深度图、RGB图、压缩图、相机内参）

• 实时FPS统计和数据显示

• 支持RGB图视频录制功能

• 可配置的topic类型选择

支持的数据类型：

• depth_image: 深度图像 (sensor_msgs/Image)

• rgb_image: RGB图像 (sensor_msgs/Image)

• rgb_image_compressed: 压缩RGB图像 (sensor_msgs/CompressedImage)

• camera_info: 相机内参 (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;
}

使用说明：

1. 订阅RGB图像数据：

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

2. 订阅相机内参：

   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. 录制RGB视频：

   # dump_video_path的值可改为其他路径, 注意提前创建该文件所在目录才能保存
   ros2 run examples echo_camera_rgbd --ros-args -p topic_type:=rgb_image -p dump_video_path:=$PWD/output.mp4
   

双目相机数据订阅

该示例中用到了echo_camera_stereo，通过订阅/aima/hal/sensor/stereo_head_front_*/话题来接收机器人的双目相机数据，支持左右相机的RGB图、压缩图和相机内参数据。

功能特点：

• 支持左右相机独立数据订阅

• 实时FPS统计和数据显示

• 支持RGB图视频录制功能

• 可配置的相机选择（左/右）

支持的数据类型：

• left_rgb_image: 左相机RGB图像 (sensor_msgs/Image)

• left_rgb_image_compressed: 左相机压缩RGB图像 (sensor_msgs/CompressedImage)

• left_camera_info: 左相机内参 (sensor_msgs/CameraInfo)

• right_rgb_image: 右相机RGB图像 (sensor_msgs/Image)

• right_rgb_image_compressed: 右相机压缩RGB图像 (sensor_msgs/CompressedImage)

• right_camera_info: 右相机内参 (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;
}

使用说明：

1. 订阅左相机RGB图像：

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

2. 订阅右相机RGB图像：

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

3. 订阅左相机内参：

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

4. 录制左相机视频：

   # dump_video_path的值可改为其他路径, 注意提前创建该文件所在目录才能保存
   ros2 run examples echo_camera_stereo --ros-args -p topic_type:=left_rgb_image -p dump_video_path:=$PWD/left_camera.mp4
   

头部后置单目相机数据订阅

该示例中用到了echo_camera_head_rear，通过订阅/aima/hal/sensor/rgb_head_rear/话题来接收机器人的头部后置单目相机数据，支持RGB图(及其mask处理)、压缩图和相机内参数据。

功能特点：

• 支持头部后置相机数据订阅

• 实时FPS统计和数据显示

• 支持RGB图视频录制功能及把手遮挡区域mask处理

• 可配置的topic类型选择

• 基于软件标记识别相机安装方向，对图像旋转补偿

支持的数据类型：

• rgb_image: RGB图像 (sensor_msgs/Image)

• rgb_image_compressed: 压缩RGB图像 (sensor_msgs/CompressedImage)

• camera_info: 相机内参 (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;
}

使用说明：

1. 订阅RGB图像数据：

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

2. 订阅压缩图像数据：

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

3. 订阅相机内参：

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

4. 录制视频：

   # dump_video_path的值可改为其他路径, 注意提前创建该文件所在目录才能保存
   ros2 run examples echo_camera_head_rear --ros-args -p topic_type:=rgb_image -p dump_video_path:=$PWD/rear_camera.mp4
   

5. 录制视频并对把手遮挡区域mask处理：

   # dump_video_path的值可改为其他路径, 注意提前创建该文件所在目录才能保存
   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. 禁用自动旋转补偿（获取原始图像）：

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

应用场景：

• 人脸识别和追踪

• 目标检测和识别

• 视觉SLAM

• 图像处理和计算机视觉算法开发

• 机器人视觉导航

6.2.15 头部触摸传感器数据订阅

该示例中用到了 echo_head_touch_sensor，通过订阅/aima/hal/sensor/touch_head话题来接收机器人的头部触摸传感器的反馈数据。

功能特点：

• 订阅了头部传感器的反馈数据，当头部被触摸时候，输出会从 IDLE->TOUCH

//
// 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;
}

使用说明：

ros2 run examples echo_head_touch_sensor

输出示例：

...
[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 激光雷达数据订阅

该示例中用到了echo_lidar_data，通过订阅/aima/hal/sensor/lidar_chest_front/话题来接收机器人的激光雷达数据，支持点云数据和IMU数据两种数据类型。

功能特点：

• 支持激光雷达点云数据订阅

• 支持激光雷达IMU数据订阅

• 实时FPS统计和数据显示

• 可配置的topic类型选择

• 详细的数据字段信息输出

支持的数据类型：

• PointCloud2: 激光雷达点云数据 (sensor_msgs/PointCloud2)

• Imu: 激光雷达IMU数据 (sensor_msgs/Imu)

技术实现：

• 使用SensorDataQoS配置（BEST_EFFORT + VOLATILE）

• 支持点云字段信息解析和显示

• 支持IMU四元数、角速度和线性加速度数据

• 提供详细的调试日志输出

应用场景：

• 激光雷达数据采集和分析

• 点云数据处理和可视化

• 机器人导航和定位

• SLAM算法开发

• 环境感知和建图

#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;
}

使用说明：

# 订阅激光雷达点云数据
ros2 run examples echo_lidar_data --ros-args -p topic_type:=pointcloud

# 订阅激光雷达IMU数据
ros2 run examples echo_lidar_data --ros-args -p topic_type:=imu

输出示例：

...
[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数据订阅

该示例中用到了echo_imu_data，通过订阅IMU话题接收机器人的姿态、角速度与线性加速度数据，并打印各字段的协方差对角线信息。默认订阅胸部IMU，可通过参数切换到胯部IMU、Lidar IMU或深度相机IMU。

功能特点：

• 订阅 sensor_msgs/Imu 数据，输出姿态四元数、角速度、线性加速度

• 打印姿态/角速度/加速度的协方差对角线（-1 表示未知）

• 统计接收间隔（recv_dt），便于观察实际到达频率

• 通过 imu_topic 参数可切换不同 IMU 源

支持的话题：

• /aima/hal/imu/chest/state：胸部IMU（默认）

• /aima/hal/imu/torso/state：胯部IMU

• /aima/hal/sensor/lidar_chest_front/imu：Lidar IMU

• /aima/hal/sensor/rgbd_head_front/imu：深度相机 IMU

技术实现：

• 使用 SensorDataQoS（BEST_EFFORT + VOLATILE），与传感器数据默认 QoS 一致

#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;
}

使用说明：

# 订阅胸部IMU（默认）
ros2 run examples echo_imu_data

# 订阅胯部IMU
ros2 run examples echo_imu_data --ros-args -p imu_topic:=/aima/hal/imu/torso/state

# 订阅Lidar IMU
ros2 run examples echo_imu_data --ros-args -p imu_topic:=/aima/hal/sensor/lidar_chest_front/imu

输出示例：

[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，在运行节点程序前，需要先将视频上传到机器人的**交互计算单元（PC3）**上（用户可在其上创建一个用来存储视频的目录，如/var/tmp/videos/），然后将节点程序中的video_path改为需要播放视频的路径。

注意

⚠️ 请注意！交互计算单元（PC3）独立于二开程序所在的开发计算单元（PC2）, 音视频文件务必存入交互计算单元（IP: 10.0.1.42）。
⚠️ 音视频文件夹及该文件夹所有父目录应当为所有用户可访问读取(建议在/var/tmp/下创建子目录存放)

注意

使用本功能前，请在运控计算单元（PC1）上执行以下命令关闭 task_manager 模块，避免其与二开程序争抢显示屏控制权：

aima em stop-app task_manager

功能说明 通过调用PlayVideo服务，可以让机器人在屏幕上播放指定路径的视频文件。请确保视频文件已上传到交互计算单元，否则播放会失败。

💡 提示：视频文件的准备方式

您可以使用 echo_camera_rgbd 示例将机器人的深度相机 RGB 图像录制为 .mp4 视频文件，再将其作为 play_video 的输入：

第一步：在机器人开发计算单元（PC2）上录制视频

ros2 run examples echo_camera_rgbd --ros-args -p topic_type:=rgb_image -p dump_video_path:=$PWD/output.mp4
# 录制完成后按 Ctrl+C 停止，视频保存为 output.mp4

第二步：将视频传输到交互计算单元（PC3，IP: 10.0.1.42）

scp output.mp4 user@10.0.1.42:/var/tmp/videos/output.mp4

第三步：运行 play_video 播放

ros2 run examples play_video  # 修改代码中 video_path 为 /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 音频文件播放

该示例中用到了play_audio，通过该节点可实现播放指定的音频文件，支持WAV（PCM编码）、RAW PCM格式的音频文件播放，不支持MP3等其他格式。

功能特点：

• 支持WAV（PCM编码）、RAW PCM格式播放，不支持MP3等其他格式

• 支持优先级控制，可设置播放优先级

• 支持自定义文件路径和播放参数

• 提供完整的错误处理和状态反馈

技术实现：

• 使用PlayAudioFile服务进行音频文件播放

• 支持优先级设置（priority: 1~10，priority_weight: 1~100）

• 提供详细的播放状态反馈

应用场景：

• 音频文件播放和媒体控制

• 语音提示和音效播放

• 多媒体应用开发

• 机器人交互音频反馈

注意

⚠️ 请注意！交互计算单元（PC3）独立于二开程序所在的开发计算单元（PC2）, 音视频文件务必存入交互计算单元（IP: 10.0.1.42）。
⚠️ 音视频文件夹及该文件夹所有父目录应当为所有用户可访问读取(建议在/var/tmp/下创建子目录存放)

#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;
}

使用说明：

# 播放默认音频文件
ros2 run examples play_audio

# 播放指定音频文件
# 注意替换/path/to/your/audio_file.wav为交互板上实际文件路径
ros2 run examples play_audio /path/to/your/audio_file.wav

# 播放TTS缓存文件
ros2 run examples play_audio /agibot/data/var/hal_audio/file/bingo.wav

输出示例：

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

注意事项：

• 确保音频文件路径正确且文件存在

• 支持的文件格式：WAV（PCM编码）、RAW PCM，不支持MP3等其他格式

• 优先级设置影响播放队列顺序

6.2.20 TTS (文字转语音)

该示例中用到了play_tts，通过该节点可实现语音播放输入的文字，用户可根据不同的场景输入相应的文本。

功能特点：

• 支持命令行参数和交互式输入

• 完整的服务可用性检查和错误处理

• 支持优先级控制和打断机制

• 提供详细的播放状态反馈

核心代码

#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;
}

使用说明

# 使用命令行参数播报文本（推荐）
ros2 run examples play_tts "你好，我是灵犀X2机器人"

# 或者不带参数运行，程序会提示用户输入
ros2 run examples play_tts

输出示例

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

注意事项

• 确保TTS服务正常运行

• 支持中文和英文文本播报

• 优先级设置影响播放队列顺序

• 打断功能可中断当前播放的语音

接口参考

• 服务：/aimdk_5Fmsgs/srv/PlayTts

• 消息：aimdk_msgs/srv/PlayTts

6.2.21 麦克风数据接收

该示例中用到了mic_receiver，通过订阅/agent/process_audio_output话题来接收机器人的降噪音频数据，支持内置麦克风和外置麦克风两种音频流，并根据VAD（语音活动检测）状态自动保存完整的语音片段为PCM文件。

功能特点：

• 基于VAD状态自动检测语音开始、处理中、结束

• 自动保存完整语音片段为PCM文件（单声道，采样率16kHz，格式S16LE）

• 按时间戳和音频流分类存储

• 支持音频时长计算和统计信息输出

应用场景：

• 语音识别和语音处理

• 音频数据采集和分析

• 实时语音监控

• 音频质量检测

• 多麦克风阵列数据处理

#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;
}

使用说明：

1. 运行程序：

   # 构建
   colcon build --packages-select examples
   
   # 运行麦克风接收程序, 配合唤醒词触发VAD
   ros2 run examples mic_receiver
   

   按 Ctrl+C 安全退出程序，程序会自动处理音频流的开始和结束。

2. 查找录制的PCM文件：

   • 运行节点后会自动创建audio_recordings目录

   • 音频文件按stream_id分类存储（暂不区分, 一律使用stream_1）：

     • stream_1/: 内置麦克风音频

     • stream_2/: 外置麦克风音频

   • 文件命名格式：{stream_name}_{timestamp}.pcm

     • internal_mic_20250909_133649_738.pcm (内置麦克风)

     • external_mic_20250909_133650_123.pcm (外置麦克风)

3. 播放保存的PCM文件：

   # 播放内置麦克风录音
   aplay -r 16000 -f S16_LE -c 1 audio_recordings/stream_1/internal_mic_20250909_133649_738.pcm
   

   参数说明：

   • -r 16000 # 采样率16kHz

   • -f S16_LE # 16位小端格式

   • -c 1 # 单声道 你也可以用其他音频播放器（如Audacity）以原始PCM格式导入并播放。

4. 转换为WAV格式（可选）：

   # 使用ffmpeg转换为WAV格式
   ffmpeg -f s16le -ar 16000 -ac 1 -i external_mic_20250909_133649_738.pcm output.wav
   

输出示例：

正常启动和运行：

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

检测到语音开始：

[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

语音处理过程：

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

语音结束和保存：

[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 麦克风原始流式数据接收

该示例中用到了mic_raw_receiver，通过订阅/aima/hal/audio/capture话题来接收机器人的原始音频数据，识别麦克风/回采，自动按声道保存为PCM文件。

功能特点：

• 按时间戳和声道分类存储

• 自动保存声道音频数据为PCM文件（单声道，采样率16kHz，格式S16LE）

• 支持麦克风切换检测

/**
 * 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;
}

使用说明：

1. 运行程序：

   # 构建
   colcon build --packages-select examples
   
   # 运行麦克风接收程序
   ros2 run examples mic_raw_receiver
   

2. 查找录制的PCM文件：

   • 运行节点后会自动创建audio_recordings目录

   • 音频文件按录制程序本次启动时间归集存储至如下格式的目录:

     • 20250909_133650_123

   • 文件命名格式：channel_{index}_{type}.pcm，如

     • channel_0_mic.pcm（麦克风）

     • channel_4_ref.pcm（回采）

3. 播放保存的PCM文件：

   • 转到计算机上播放：

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

   • 机器人内播放：

     # 使用原始流式音频播放例程
     ros2 run examples play_audio_stream --ros-args -p raw_audio_path:=$PWD/audio_recordings/20250909_133650_123/channel_0_mic.pcm
     # 或使用媒体文件播放例程, 注意提前将文件转存到交互板内, 并使用其路径替换下方pcm路径
     ros2 run examples play_audio please/use/path/on/PC3.pcm
     

6.2.23 获取当前麦克风

#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 切换麦克风

#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;
  }
}

使用说明

# 构建
colcon build --packages-select examples

# 交互式运行
ros2 run examples set_mic_source

# 命令行参数运行
ros2 run examples set_mic_source ext  # ext - 外置麦, int - 内置麦

6.2.25 原始流式音频播放

该示例中用到了play_audio_stream，先申请音频焦点，再将PCM文件以实时速率发布到/aima/hal/audio/playback话题进行播放；焦点被抢时自动退出。

/**
 * 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;
}

使用说明：

# 注意替换/path/to/your/raw_audio_file.pcm为原始音频文件路径，单通道，采样率16kHz，格式S16LE
ros2 run examples play_audio_stream --ros-args -p raw_audio_path:=/path/to/your/raw_audio_file.pcm

6.2.26 表情控制

该示例中用到了play_emoji，通过该节点可实现表达指定的表情，用户可根据已有的表情列表来选择要变化的表情，具体表情列表可参考表情列表

#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 地图构建

该示例中用到了slam，通过该节点给建图服务发送消息，实现建图功能

#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;
}

使用说明:

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]

开始建图之后，推动机器，或者控制机器人移动，完成建图

6.2.28 使用地图进行重定位

该示例中用到地图构建步骤完成构建的地图进行定位

#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;
}

使用说明:

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!

需要将机器人移动到地图构建的起始点，然后运行重定位程序

参数获取方式：map_id 与起点像素坐标（position.x / position.y）的获取见 地图标识与坐标的获取。

6.2.29 获取已存储的地图

该示例中用到了get_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;
}

使用说明:

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

参数获取方式：输入的是地图名称（map_name），不是 map_id；map_name 的获取见 地图标识与坐标的获取。

6.2.30 导航

该示例提供了交互式终端菜单，支持导航任务的开始、暂停、恢复和停止操作。

#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;
}

使用说明:

ros2 run examples navigation

输出示例:

[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

开始导航时需输入地图ID、目标位姿、到点精度等参数，每项都有默认值，直接回车即可使用默认值。

参数获取方式：map_id 的获取见 地图标识与坐标的获取；target_x / target_y 为目标点米制坐标（map 坐标系），与重定位用的像素坐标不同，根据实际任务直接指定

6.2.31 播放灵创动作

该示例中用到了play_linkcraft，列出机器人上的灵创动作，并且选择一条进行播放

//
// 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;
}

使用说明:

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 群控播放灵创动作

该示例中用到了 multi_play_linkcraft，在多台机器人之间进行时间同步，并群控播放灵创动作。

//
// 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;
}

使用说明:

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灯带控制

功能说明：演示如何控制机器人的LED灯带，支持多种显示模式和自定义颜色。

注意

使用本功能前，请在运控计算单元（PC1）上执行以下命令关闭 task_manager 模块，避免其与二开程序争抢灯带控制权：

aima em stop-app task_manager

核心代码：

#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;
  }
}

使用说明：

# 构建
colcon build --packages-select examples

# 交互式运行
ros2 run examples play_lights

# 命令行参数运行: <模式> <R> <G> <B> <优先级> <重置优先级(true/false)>
ros2 run examples play_lights 1 255 0 0 6 false  # 模式1，红色，优先级6

输出示例：

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

技术特点：

• 支持4种LED显示模式

• RGB颜色自定义

• 优先级控制（值越大优先级越高）

• 优先级重置功能

• 异步服务调用

• 命令行参数支持

• 输入参数验证

• 友好的用户交互界面

6.2.34 GNSS定位数据订阅

该示例中用到了echo_gnss_data，通过订阅GNSS话题获取定位数据，输出经纬度、海拔和协方差信息。

功能特点：

• 订阅NavSatFix定位数据（经纬度、海拔）

• 实时显示定位状态和协方差

话题说明：

• /aima/hal/sensor/gnss: 定位数据 (sensor_msgs/NavSatFix)，由系统自动发布

#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;
}

使用说明：

# 构建
colcon build --packages-select examples

# 订阅GNSS数据
ros2 run examples echo_gnss_data

输出示例：

[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

注意事项：

• 定位状态说明：NO_FIX(无定位)、FIX(单点定位)

• 室内环境下 status 通常为 NO_FIX，需到室外开阔处等待定位

6.2.35 上肢控制

备注

运行前需先切换到对应模式，详见 上肢控制。

// 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;
}

使用说明：

colcon build --packages-select examples

# 头部控制（HEAD_ONLY 模式）
ros2 run examples upper_body_control head

# 夹爪控制（claw，UPPERBODY_REMOTE_SPLIT 模式）
ros2 run examples upper_body_control claw

# 灵巧手关节（UPPERBODY_REMOTE_SPLIT 模式）
ros2 run examples upper_body_control joint

# 灵巧手手势（UPPERBODY_REMOTE_SPLIT 模式）
ros2 run examples upper_body_control gesture

6.2.36 订阅 HDS 诊断信息 / 查询历史事件

订阅 /aima/hds/diag_code_list、/aima/hds/alert_code_list、/aima/hds/post_proc/signal 三个话题实时获取诊断码、告警码与后处理信号；同时支持调用 GetHdsEventHistory 服务查询历史事件。完整的诊断码清单见 诊断码参考表。

// 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;
}

使用说明

实时订阅 (echo)：

# 直接订阅三个话题，实时打印诊断码、告警码、后处理信号
ros2 run examples hds_diagnostics echo

话题	数据类型
/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

查询历史事件 (history)，所有过滤条件均对应 HdsEventQuery 字段，使用示例：

# 查最近 20 条 (默认 limit=20，正序)
ros2 run examples hds_diagnostics history

# 按时间范围查询 (对应 start_time / end_time)
ros2 run examples hds_diagnostics history \
  --start-ns 1716000000000000000 \
  --end-ns   1716100000000000000

# 只看活跃中的告警 (对应 state = STATE_ACTIVE)
ros2 run examples hds_diagnostics history --state 1

# 按诊断码过滤 (对应 diag_codes，支持十六进制)
ros2 run examples hds_diagnostics history --diag-codes 0x6200000001 0x6200000002

# 按告警码过滤 (对应 alert_codes)
ros2 run examples hds_diagnostics history --alert-codes MOTOR_OVERHEAT COMM_LOST

# 分页 + 倒序 (对应 limit / offset / order_desc，最新的先显示)
ros2 run examples hds_diagnostics history --limit 50 --offset 0 --desc

# 组合使用：只看已恢复的，每次取 10 条，最新的先显示
ros2 run examples hds_diagnostics history --state 2 --limit 10 --desc

返回字段对应 HdsEventRecord。

echo 模式输出示例

[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="获取不到机器人定位"
[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 模式输出示例

[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
--------------------------------------------------------------------------------

接口参考

• 话题：/aima/hds/diag_code_list、/aima/hds/alert_code_list、/aima/hds/post_proc/signal

• 服务：/aimdk_5Fmsgs/srv/GetHdsEventHistory

• 消息：aimdk_msgs/msg/DiagnosticInfoArray、aimdk_msgs/msg/AlertCodeArray、aimdk_msgs/msg/PostProcSigArray