6.2 C++接口使用示例

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

构建 & 运行说明

• 进入到解压后的 sdk 顶层目录，执行以下命令

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

📝 代码说明

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

小心

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

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) {
              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)
    // 2. System status equals 1 (IN_READY)
    bool state_match = (current_state_lower == target_state_lower);
    bool status_match = (response->curr_status.value ==
                         aimdk_msgs::msg::SystemStatus::IN_READY);

    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);
    }
    if (state_match && !state_match) {
      RCLCPP_ERROR(this->get_logger(), "State Migrate Fail");
      exit(0);
    }

    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...
[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] [1764066631.021247791] [get_mc_action_client]: Current robot mode:
[INFO] [1764066631.021832667] [get_mc_action_client]: Mode name: PASSIVE_DEFAULT
[INFO] [1764066631.022396136] [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 <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 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) {
          RCLCPP_INFO(this->get_logger(), "✅ Robot mode set successfully.");
          return true;
        } else {
          RCLCPP_ERROR(this->get_logger(), "❌ Failed to set robot mode: %s",
                       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)"}},
};

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_preset_motion.hpp"
#include "aimdk_msgs/msg/request_header.hpp"
#include "aimdk_msgs/srv/set_mc_preset_motion.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 PresetMotionClient : public rclcpp::Node {
public:
  PresetMotionClient() : Node("preset_motion_client") {
    const std::chrono::seconds timeout(8);

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

    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 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();

      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: %lu",
                      response->response.task_id);
          return true;
        } else if (response->response.state.value ==
                   aimdk_msgs::msg::CommonState::RUNNING) {
          RCLCPP_INFO(this->get_logger(), "⏳ Preset motion executing: %lu",
                      response->response.task_id);
          return true;
        } else {
          RCLCPP_WARN(this->get_logger(), "❌ Failed to set preset motion: %lu",
                      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::SetMcPresetMotion>::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<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) {
      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 <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 = 2;
    msg->right_hand_type.value = 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 <chrono>
#include <cmath>
#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));
  }

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

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

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

    // Create left hand command array
    message.left_hands.resize(10);

    // Set left thumb
    message.left_hands[0].name = "left_thumb";
    message.left_hands[0].position = 0.0;
    message.left_hands[0].velocity = 0.1;
    message.left_hands[0].acceleration = 0.0;
    message.left_hands[0].deceleration = 0.0;
    message.left_hands[0].effort = 0.0;
    // Set other left fingers
    for (int i = 1; i < 10; i++) {
      message.left_hands[i].name = "left_index";
      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);

    // Set right thumb
    message.right_hands[0].name = "right_thumb";
    message.right_hands[0].position = 0.0;
    message.right_hands[0].velocity = 0.1;
    message.right_hands[0].acceleration = 0.0;
    message.right_hands[0].deceleration = 0.0;
    message.right_hands[0].effort = 0.0;

    // Set other right fingers (pinky)
    for (int i = 1; i < 10; i++) {
      message.right_hands[i].name = "right_pinky";
      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;
    }

    if (target_finger <= 10) {
      message.right_hands[target_finger].position = 0.8;
    } else {
      int target_finger_ = target_finger - 10;
      double target_position = 0.8;
      if (target_finger_ < 3) {
        // The three thumb motors on the left hand need their signs inverted to
        // mirror the right hand's motion
        target_position = -target_position;
      }
      message.left_hands[target_finger_].position = target_position;
    }

    // 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 <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 25 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 hander
    message.header.stamp = this->now();
    message.header.frame_id = "hand_command";

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

    // Create left hand command array
    message.left_hands.resize(10);

    // Set left thumb
    message.left_hands[0].name = "left_thumb";
    message.left_hands[0].position = 0.0;
    message.left_hands[0].velocity = 0.1;
    message.left_hands[0].acceleration = 0.0;
    message.left_hands[0].deceleration = 0.0;
    message.left_hands[0].effort = 0.0;
    // Set other left fingers
    for (int i = 1; i < 10; i++) {
      message.left_hands[i].name = "left_index";
      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);

    // Set right thumb
    message.right_hands[0].name = "right_thumb";
    message.right_hands[0].position = 0.0;
    message.right_hands[0].velocity = 0.1;
    message.right_hands[0].acceleration = 0.0;
    message.right_hands[0].deceleration = 0.0;
    message.right_hands[0].effort = 0.0;

    // Set other right fingers (pinky)
    for (int i = 1; i < 10; i++) {
      message.right_hands[i].name = "right_pinky";
      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 <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 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 = 1001;         // Add new input source
      request->input_source.name = "node";  // Set message source
      request->input_source.priority = 40;  // Set priority
      request->input_source.timeout = 1000; // Set timeout (ms)

      RCLCPP_INFO(this->get_logger(), "📨 Sending input source request: (ID=%d)",
                  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=%lu",
                      response->response.task_id);
          return true;
        } else {
          RCLCPP_ERROR(
              this->get_logger(),
              "❌ Input source set failed. ret_code=%ld, task_id=%lu "
              "(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 <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: %ld",
                      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] [get_current_input_source_client]: 当前输入源: node
[INFO] [get_current_input_source_client]: 优先级: 40
[INFO] [get_current_input_source_client]: 超时时间: 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 <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...");
    }

    auto request =
        std::make_shared<aimdk_msgs::srv::SetMcInputSource::Request>();
    request->action.value = 1001;
    request->input_source.name = "node";
    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 to register input source... [%d]", i);
        continue;
      }
      // future.done
      auto response = future.get();
      int state = response->response.state.value;
      RCLCPP_INFO(this->get_logger(),
                  "Set input source succeeded: state=%d, task_id=%lu", state,
                  response->response.task_id);
      return true;
    }
    RCLCPP_ERROR(this->get_logger(), "Service call failed or timed out");
    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"; // 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) {
    if (abs(forward) < 0.005) {
      forward_velocity_ = 0.0;
      return true;
    } else if ((abs(forward) > max_forward_speed_) ||
               (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) {
    if (abs(lateral) < 0.005) {
      lateral_velocity_ = 0.0;
      return true;
    } else if ((abs(lateral) > max_lateral_speed_) ||
               (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) {
    if (abs(angular) < 0.005) {
      angular_velocity_ = 0.0;
      return true;
    } else if ((abs(angular) > max_angular_speed_) ||
               (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. 调整控制频率：

   • 修改control_timer_的周期（当前为2ms）

#include <aimdk_msgs/msg/joint_command_array.hpp>
#include <aimdk_msgs/msg/joint_state_array.hpp>
#include <atomic>
#include <cstdlib>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <ruckig/ruckig.hpp>
#include <signal.h>
#include <string>
#include <unordered_map>
#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 <csignal>
#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...");
    }

    auto request =
        std::make_shared<aimdk_msgs::srv::SetMcInputSource::Request>();
    request->action.value = 1001;
    request->input_source.name = "node";
    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 to register input source... [%d]", i);
        continue;
      }
      // future.done
      auto response = future.get();
      int state = response->response.state.value;
      RCLCPP_INFO(this->get_logger(),
                  "Set input source succeeded: state=%d, task_id=%lu", state,
                  response->response.task_id);
      return true;
    }
    RCLCPP_ERROR(this->get_logger(), "Service call failed or timed out");
    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";
    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/opencv.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 <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/opencv.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", "");

    // 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 is different with other cameras.
      // The messages arrive in about 10Hz and SensorDataQoS is enough.
      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 is different with other cameras.
      // The messages arrive in about 10Hz and SensorDataQoS is enough.
      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 = std::max(1.0, get_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_;

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

双目相机数据订阅

该示例中用到了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 <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/opencv.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", "");

    // 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', 'J', 'P', 'G');
      double fps = std::max(1.0, get_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_;

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

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

该示例中用到了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 <deque>
#include <filesystem>
#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/opencv.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"
 *
 * See individual callbacks for more detailed comments
 */
class HeadRearCameraTopicEcho : public rclcpp::Node {
public:
  HeadRearCameraTopicEcho() : Node("head_rear_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", "");
    with_mask_ = declare_parameter<bool>("with_mask", false);

    // Subscribed topics and their message layouts
    // 1. /aima/hal/sensor/rgb_head_rear/rgb_image
    //    - topic_type: rgb_image
    //    - message type: sensor_msgs::msg::Image
    //    - frame_id: rgb_head_rear
    //    - child_frame_id: /
    //    - contents: raw image data
    // 2. /aima/hal/sensor/rgb_head_rear/rgb_image/compressed
    //    - topic_type: rgb_image_compressed
    //    - message type: sensor_msgs::msg::CompressedImage
    //    - frame_id: rgb_head_rear
    //    - contents: compressed image data
    // 3. /aima/hal/sensor/rgb_head_rear/camera_info
    //    - topic_type: camera_info
    //    - message type: sensor_msgs::msg::CameraInfo
    //    - frame_id: rgb_head_rear
    //    - contents: camera intrinsic parameters

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

    if (with_mask_ && !dump_video_path_.empty()) {
      auto mask_path =
          std::filesystem::read_symlink("/proc/self/exe").parent_path() /
          "data" / "rgb_head_rear_mask.png";
      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");
      }
    }

    // Enable RGB image subscription
    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 to video: %s",
            (with_mask_ ? "with" : "without"), dump_video_path_.c_str());
      }
    }

    // Enable RGB compressed image subscription
    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());

      // Enable camera info subscription
    } else if (topic_type_ == "camera_info") {
      topic_name_ = "/aima/hal/sensor/rgb_head_rear/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(&HeadRearCameraTopicEcho::cb_camerainfo, this,
                    std::placeholders::_1));
      RCLCPP_INFO(get_logger(),
                  "✅ Subscribing 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");
    }
  }

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

private:
  // Image callback (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 (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 (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;
      }
      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;
    }

    // Initialize VideoWriter
    if (!video_writer_.isOpened()) {
      int fourcc = cv::VideoWriter::fourcc('M', 'J', 'P', 'G');
      double fps = std::max(1.0, get_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_;
  bool with_mask_;
  cv::Mat mask_image_;

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

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

应用场景：

• 人脸识别和追踪

• 目标检测和识别

• 视觉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>

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
    RCLCPP_INFO(this->get_logger(), "Received TouchState message:");
    RCLCPP_INFO(this->get_logger(), "  Timestamp: %d.%09d",
                msg->header.stamp.sec, msg->header.stamp.nanosec);

    std::string event_str = get_event_type_string(msg->event_type);
    RCLCPP_INFO(this->get_logger(), "  Event Type: %s (%d)", 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";
    case aimdk_msgs::msg::TouchState::SLIDE:
      return "SLIDE";
    case aimdk_msgs::msg::TouchState::PAT_ONCE:
      return "PAT_ONCE";
    case aimdk_msgs::msg::TouchState::PAT_TWICE:
      return "PAT_TWICE";
    case aimdk_msgs::msg::TouchState::PAT_TRIPLE:
      return "PAT_TRIPLE";
    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] [1769162721.359354722] [touch_state_subscriber]:   Timestamp: 1769162726.863282315
[INFO] [1769162721.359361643] [touch_state_subscriber]:   Event Type: IDLE (1)
[INFO] [1769167184.142143492] [touch_state_subscriber]:   Timestamp: 1769167189.364879133
[INFO] [1769167184.142147126] [touch_state_subscriber]:   Event Type: 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");

    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 << "🟢 LIDAR PointCloud2 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"
          << "  • 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()
          << "\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 << "🟢 LIDAR IMU received\n"
          << "  • frame_id:        " << msg->header.frame_id << "\n"
          << "  • stamp (sec):     "
          << rclcpp::Time(msg->header.stamp).seconds() << "\n"
          << "  • 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"
          << "  • 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] [lidar_chest_echo]: ✅ Subscribing LIDAR PointCloud2: /aima/hal/sensor/lidar_chest_front/lidar_pointcloud
[INFO] [lidar_chest_echo]: 🟢 LIDAR PointCloud2 received
  • frame_id:        lidar_chest_front
  • stamp (sec):     1234567890.123456
  • width x height:  1 x 36000
  • point_step:      16
  • row_step:        16
  • fields:          x(7) y(7) z(7) intensity(7)
  • is_bigendian:    False
  • is_dense:        True
  • data size:       576000
  • recv FPS (1s):   10.0

6.2.17 播放视频

该示例中用到了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 <chrono>
#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>(
        "/face_ui_proxy/play_video");
    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 video_path =
        "/agibot/data/home/agi/zhiyuan.mp4"; // Default video path; modify as
                                             // needed
    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.18 音频文件播放

该示例中用到了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/srv/play_audio_file.hpp>
#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_file_client_min");

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

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

  if (argc > 1) {
    file_name = argv[1];
  } else {
    std::cout << "Enter the media file path to play (default: " << default_file
              << "): ";
    if (!std::getline(std::cin, file_name) || !rclcpp::ok()) {
      std::cout << std::endl;
      rclcpp::shutdown();
      return 1;
    }
    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 = 6;
  req->file.priority_weight = 0;

  // 4) Wait for service and call
  RCLCPP_INFO(rclcpp::get_logger("main"), "Waiting for service: %s",
              service_name.c_str());
  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::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(), "Call timed out or did not complete");
    }
    rclcpp::shutdown();
    return 1;
  }

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

    if (success) {
      RCLCPP_INFO(node->get_logger(), "✅ Audio file play request succeeded: %s",
                  file_name.c_str());
    } else {
      RCLCPP_ERROR(node->get_logger(), "❌ Audio file play request failed: %s",
                   file_name.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/interaction/tts_cache/normal/demo.wav

输出示例：

[INFO] [play_audio_file_client_min]: ✅ Audio file play request succeeded

注意事项：

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

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

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

6.2.19 TTS (文字转语音)

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

功能特点：

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

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

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

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

核心代码

#include <aimdk_msgs/msg/tts_priority_level.hpp>
#include <aimdk_msgs/srv/play_tts.hpp>
#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);

  // 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->header.header.stamp = node->now();
  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 = 6;

  if (!client->wait_for_service(
          std::chrono::duration_cast<std::chrono::seconds>(
              std::chrono::seconds(5)))) {
    if (rclcpp::ok()) {
      RCLCPP_ERROR(node->get_logger(), "Service unavailable: %s",
                   service_name.c_str());
    }
    rclcpp::shutdown();
    return 1;
  }

  auto future = client->async_send_request(req);
  if (rclcpp::spin_until_future_complete(
          node, future,
          std::chrono::duration_cast<std::chrono::seconds>(
              std::chrono::seconds(10))) != rclcpp::FutureReturnCode::SUCCESS) {
    if (rclcpp::ok()) {
      RCLCPP_ERROR(node->get_logger(), "Call timed out");
    }
    rclcpp::shutdown();
    return 1;
  }

  const auto resp = future.get();
  if (resp->tts_resp.is_success) {
    RCLCPP_INFO(node->get_logger(), "✅ TTS play request succeeded");
  } else {
    RCLCPP_ERROR(node->get_logger(), "❌ TTS play request failed");
  }

  rclcpp::shutdown();
  return 0;
}

使用说明

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

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

输出示例

[INFO] [play_tts_client_min]: ✅ 播报请求成功

注意事项

• 确保TTS服务正常运行

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

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

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

接口参考

• 服务：/aimdk_5Fmsgs/srv/PlayTts

• 消息：aimdk_msgs/srv/PlayTts

6.2.20 麦克风数据接收

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

功能特点：

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

• 自动保存完整语音片段为PCM格式文件

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

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

VAD状态说明：

• 0: 无语音

• 1: 语音开始

• 2: 语音处理中

• 3: 语音结束

#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(),
                "Starting to subscribe to denoised 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;

      static const std::unordered_map<uint8_t, std::string> vad_state_names = {
          {0, "No Speech"},
          {1, "Speech Start"},
          {2, "Speech Processing"},
          {3, "Speech End"}};
      static const std::unordered_map<uint32_t, std::string> stream_names = {
          {1, "Internal Microphone"}, {2, "External Microphone"}};

      RCLCPP_INFO(this->get_logger(),
                  "Audio data received: 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;
    }

    static const std::unordered_map<uint8_t, std::string> vad_state_names = {
        {0, "No Speech"},
        {1, "Speech Start"},
        {2, "Speech Processing"},
        {3, "Speech End"}};
    static const std::unordered_map<uint32_t, std::string> stream_names = {
        {1, "Internal Microphone"}, {2, "External Microphone"}};

    RCLCPP_INFO(this->get_logger(), "[%s] VAD Atate: %s Audio Data: %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());

    // AUDIO_VAD_STATE_BEGIN
    if (vad_state == 1) {
      RCLCPP_INFO(this->get_logger(), "🎤 Speech detected - Start");
      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());
      }

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

      // AUDIO_VAD_STATE_END
    } else if (vad_state == 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_NONE
    } else if (vad_state == 0) {
      if (recording_state_[stream_id]) {
        RCLCPP_INFO(this->get_logger(), "⏹️ Recording state reset");
        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 seconds (%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 denoised audio data, press Ctrl+C to exit...");
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

使用说明：

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

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

   • stream_1/: 内置麦克风音频

   • stream_2/: 外置麦克风音频

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

4. 音频格式：16kHz, 16位, 单声道PCM

5. 可通过以下命令播放保存的PCM文件：

   aplay -r 16000 -f S16_LE -c 1 external_mic_20250909_133649_738.pcm
   

6. 配合唤醒词触发VAD

输出示例：

[INFO] 开始订阅降噪音频数据...
[INFO] 收到音频数据: stream_id=2, vad_state=1(语音开始), audio_size=320 bytes
[INFO] [外置麦克风] VAD状态: 语音开始 音频数据: 320 bytes
[INFO] 🎤 检测到语音开始
[INFO] 收到音频数据: stream_id=2, vad_state=2(语音处理中), audio_size=320 bytes
[INFO] [外置麦克风] VAD状态: 语音处理中 音频数据: 320 bytes
[INFO] 🔄 语音处理中...
[INFO] 收到音频数据: stream_id=2, vad_state=3(语音结束), audio_size=320 bytes
[INFO] [外置麦克风] VAD状态: 语音结束 音频数据: 320 bytes
[INFO] ✅ 语音结束
[INFO] 音频段已保存: audio_recordings/stream_2/external_mic_20250909_133649_738.pcm (大小: 960 bytes)
[INFO] 音频时长: 0.06 秒 (480 样本)

播放PCM音频文件示例 (Linux下使用aplay命令)

假设你已经录制并保存了音频文件 external_mic_20250909_151117_223.pcm， 可以通过如下命令进行播放：

aplay -r 16000 -f S16_LE -c 1 audio_recordings/stream_2/external_mic_20250909_151117_223.pcm

参数说明：

• -r 16000 # 采样率16kHz

• -f S16_LE # 16位小端格式

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

注意：如果你保存的是内置麦克风音频，路径应为 audio_recordings/stream_1/internal_mic_xxx.pcm

6.2.21 麦克风原始流式数据接收

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

功能特点：

• 自动按时间戳创建输出目录

• 将多通道音频数据按通道拆分保存（麦克风通道/回采通道）

• 每秒刷新一次缓存数据到磁盘

• 支持麦克风切换检测

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

• channel_0_mic.pcm（麦克风）

• channel_4_ref.pcm（回采）

/**
 * 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(), "Flashing 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 append: %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;
}

使用说明：

ros2 run examples mic_raw_receiver

播放保存的PCM文件：

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

6.2.22 原始流式音频播放

该示例中用到了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 = 6;
    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;
      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) {
      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_;
    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;
      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) {
      // 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_;
  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.23 表情控制

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

#include "aimdk_msgs/srv/play_emoji.hpp"
#include "aimdk_msgs/msg/common_request.hpp"
#include "rclcpp/rclcpp.hpp"
#include <chrono>
#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>(
        "/face_ui_proxy/play_emoji");
    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.24 地图构建

该示例中用到了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.25 使用地图进行重定位

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

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

using namespace std::chrono_literals;

class RelocalizationNode : public rclcpp::Node {
public:
  RelocalizationNode() : Node("relocalization_node") {
    // /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:1774430080403";
      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 = 273;
        relocalization_pose_msg.position.y = 200;
        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 (5 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_;

  bool success_received_ = false;
};

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

使用说明:

ros2 run examples relocate 
[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: 5s)...
[INFO] [1772528945.130242770] [relocalization_node]: Received odometry data - Relocalization successful!

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

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

6.2.26 获取已存储的地图

该示例中用到了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 <memory>

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(10)) !=
        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: %ld", result->map_id);
    } else {
      RCLCPP_ERROR(this->get_logger(), "Service call failed with code: %lu",
                   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>();
  client->send_request("map_name");

  rclcpp::shutdown();
  return 0;
}

使用说明:

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

示例默认请求名称为 test_map 的地图，可在代码中修改 send_request 的参数以请求其他地图。

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

6.2.27 播放灵创动作

该示例中用到了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 <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: %ld",
                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]: Service call succeeded
[INFO] [1772529153.517416880] [robot_resource_client]: Robot Resources (0):
[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: {"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.28 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 <chrono>
#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 {
          RCLCPP_ERROR(this->get_logger(),
                       "❌ LED strip command failed with status: %d",
                       response->status_code);
          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
    int reset_priority_input = 0; // Reset priority flag

    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 period, sinusoidal brightness)"
              << std::endl;
    std::cout << "2 - Blinking (1s period, 0.5s on, 0.5s off)" << std::endl;
    std::cout << "3 - Flow (2s period, lights turn on left to right)"
              << std::endl;
    std::cout << "Enter mode (0-3): ";
    std::cin >> led_mode;

    std::cout << "\nSet RGB color values (0-255):" << std::endl;
    std::cout << "Red component (R): ";
    std::cin >> r;
    std::cout << "Green component (G): ";
    std::cin >> g;
    std::cout << "Blue component (B): ";
    std::cin >> b;

    std::cout << "\nSet priority (higher value = higher priority, default=6): ";
    std::cin >> priority;

    std::cout << "Reset priority after command? (1=yes, 0=no, default=0): ";
    std::cin >> reset_priority_input;

    // clamp mode to range 0-3
    led_mode = std::max(0, std::min(3, led_mode));
    // clamp r/g/b to range 0-255
    r = std::max(0, std::min(255, r));
    g = std::max(0, std::min(255, g));
    b = std::max(0, std::min(255, b));
    // clamp priority to non-negative
    priority = std::max(0, priority);

    if (client_node) {
      client_node->send_request(led_mode, r, g, b, priority,
                                reset_priority_input != 0);
    }

    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

输出示例：

=== LED Strip Control Example ===
Select LED strip mode:
0 - Steady On
1 - Breathing (4s period, sinusoidal brightness)
2 - Blinking (1s period, 0.5s on, 0.5s off)
3 - Flow (2s period, lights turn on left to right)
Enter mode (0-3): 1

Set RGB color values (0-255):
Red component (R): 255
Green component (G): 0
Blue component (B): 0

Set priority (higher value = higher priority, default=6): 6
Reset priority after command? (1=yes, 0=no, default=0): 0

[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.29 获取当前麦克风

#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) {
          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.30 切换麦克风

#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) {
          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 - 内置麦