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

通过调用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.2 设置机器人模式

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

      // 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.3 设置机器人动作

该示例中用到了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.4 夹爪控制

该示例中用到了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.5 灵巧手控制

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

    // Create a timer to publish once per second
    int target_finger = 0;
    int step_ = 1;
    bool increasing_ = true;
  }

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.6 注册二开输入源

对于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.7 获取当前输入源

该示例中用到了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.8 机器人走跑控制

该示例中用到了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.9 关节电机控制

本示例展示了如何使用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.10 键盘控制机器人

本示例实现了通过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.11 拍照

该示例中用到了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.12 相机推流示例集

该示例集提供了多种相机数据订阅和处理功能，支持深度相机、双目相机和单目相机的数据流订阅。
这些相机数据订阅 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_pointcloud: 深度点云数据 (sensor_msgs/PointCloud2)

• 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 <sensor_msgs/msg/point_cloud2.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_pointcloud: Depth point cloud (sensor_msgs/PointCloud2)
 *   - 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:=depth_pointcloud 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_pointcloud
    //    - topic_type: depth_pointcloud
    //    - message type: sensor_msgs::msg::PointCloud2
    //    - frame_id: rgbd_head_front
    //    - child_frame_id: /
    //    - contents: depth point cloud
    // 2. /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
    // 3. /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
    // 4. /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
    // 5. /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
    // 6. /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 pointcloud subscription
    if (topic_type_ == "depth_pointcloud") {
      topic_name_ = "/aima/hal/sensor/rgbd_head_front/depth_pointcloud";
      sub_pointcloud_ = create_subscription<sensor_msgs::msg::PointCloud2>(
          topic_name_, qos,
          std::bind(&CameraTopicEcho::cb_pointcloud, this,
                    std::placeholders::_1));
      RCLCPP_INFO(get_logger(), "✅ Subscribing PointCloud2: %s",
                  topic_name_.c_str());

      // Enable depth image subscription
    } else 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:
  // PointCloud2 callback
  void cb_pointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
    // Update arrivals (for FPS calculation)
    update_arrivals();

    // Print point cloud basic info
    if (should_print()) {
      std::ostringstream oss;
      oss << "🌫️ 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());
    }
  }

  // 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', '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_;
  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr
      sub_pointcloud_;

  // 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. 订阅深度点云数据：

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

2. 订阅RGB图像数据：

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

3. 订阅相机内参：

   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
   

4. 录制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.13 头部触摸传感器数据订阅

该示例中用到了 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 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.14 激光雷达数据订阅

该示例中用到了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.15 播放视频

该示例中用到了play_video，在运行节点程序前，需要先将视频上传到机器人的**交互计算单元(PC3)**上（用户可在其上创建一个用来存储视频的目录如/var/tmp/videos/），然后将节点程序中的video_path改为需要播放视频的路径。

注意

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

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

#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.16 媒体文件播放

该示例中用到了play_media，通过该节点可实现播放指定的媒体文件（如音频文件），支持WAV、MP3等格式的音频文件播放。

功能特点：

• 支持多种音频格式播放（WAV、MP3等）

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

• 支持打断机制，可中断当前播放

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

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

技术实现：

• 使用PlayMediaFile服务进行媒体文件播放

• 支持优先级级别设置（0-99）

• 支持中断控制（is_interrupted参数）

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

应用场景：

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

• 语音提示和音效播放

• 多媒体应用开发

• 机器人交互音频反馈

注意

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

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

using PlayMediaFile = aimdk_msgs::srv::PlayMediaFile;

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

  // 1) Service name
  const std::string service_name = "/aimdk_5Fmsgs/srv/PlayMediaFile";
  auto client = node->create_client<PlayMediaFile>(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
              << "): ";
    std::getline(std::cin, file_name);
    if (file_name.empty()) {
      file_name = default_file;
    }
  }

  // 3) Build the request
  auto req = std::make_shared<PlayMediaFile::Request>();
  // CommonRequest request -> RequestHeader header -> builtin_interfaces/Time
  // stamp
  req->header.header.stamp = node->now();

  // PlayMediaFileRequest required fields
  req->media_file_req.file_name = file_name;
  req->media_file_req.domain = "demo_client"; // Required: identifies the caller
  req->media_file_req.trace_id = "demo";      // Optional: request identifier
  req->media_file_req.is_interrupted =
      true; // Whether to interrupt same-priority playback
  req->media_file_req.priority_weight = 0; // Optional: 0~99
  // Priority level: default INTERACTION_L6
  req->media_file_req.priority_level.value = 6;

  // 4) Wait for service and call
  RCLCPP_INFO(node->get_logger(), "Waiting for service: %s",
              service_name.c_str());
  if (!client->wait_for_service(std::chrono::seconds(5))) {
    RCLCPP_ERROR(node->get_logger(), "Service unavailable: %s",
                 service_name.c_str());
    rclcpp::shutdown();
    return 1;
  }

  auto future = client->async_send_request(req);
  auto rc = rclcpp::spin_until_future_complete(node, future,
                                               std::chrono::seconds(10));

  if (rc == rclcpp::FutureReturnCode::INTERRUPTED) {
    RCLCPP_WARN(node->get_logger(), "Interrupted while waiting");
    rclcpp::shutdown();
    return 1;
  }

  if (rc != rclcpp::FutureReturnCode::SUCCESS) {
    RCLCPP_ERROR(node->get_logger(), "Call timed out or did not complete");
    rclcpp::shutdown();
    return 1;
  }

  // 5) Handle response (success is in tts_resp)
  try {
    const auto resp = future.get();
    bool success = resp->tts_resp.is_success;

    if (success) {
      RCLCPP_INFO(node->get_logger(), "✅ Media file play request succeeded: %s",
                  file_name.c_str());
    } else {
      RCLCPP_ERROR(node->get_logger(), "❌ Media 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_media

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

# 播放TTS缓存文件
ros2 run examples play_media /agibot/data/var/interaction/tts_cache/normal/demo.wav

输出示例：

[INFO] [play_media_file_client_min]: ✅ 媒体文件播放请求成功

注意事项：

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

• 支持的文件格式：WAV、MP3等

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

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

6.2.17 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: ";
    std::getline(std::cin, tts_text);
    if (tts_text.empty()) {
      tts_text = "Hello, I am AgiBot X2.";
    }
  }

  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)))) {
    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) {
    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.18 麦克风数据接收

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

功能特点：

• 支持多音频流同时接收（内置麦克风 stream_id=1，外置麦克风 stream_id=2）

• 基于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_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.19 表情控制

该示例中用到了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.20 LED灯带控制

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

核心代码：

#include <aimdk_msgs/msg/common_request.hpp>
#include <aimdk_msgs/srv/led_strip_command.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::LedStripCommand>(
        "/aimdk_5Fmsgs/srv/LedStripCommand");
    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) {
    try {
      auto request =
          std::make_shared<aimdk_msgs::srv::LedStripCommand::Request>();

      request->led_strip_mode = led_mode;
      request->r = r;
      request->g = g;
      request->b = b;

      RCLCPP_INFO(this->get_logger(),
                  "📨 Sending request to control led strip: mode=%hhu, "
                  "RGB=(%hhu, %hhu, %hhu)",
                  led_mode, r, g, b);

      // 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::LedStripCommand>::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

    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;

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

    if (client_node) {
      client_node->send_request(led_mode, r, g, b);
    }

    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灯带控制示例 ===
请选择灯带模式：
0 - 常亮模式
1 - 呼吸模式 (4s周期，亮度正弦变化)
2 - 闪烁模式 (1s周期，0.5s亮，0.5s灭)
3 - 流水模式 (2s周期，从左到右依次点亮)
请输入模式 (0-3): 1

请设置RGB颜色值 (0-255)：
红色分量 (R): 255
绿色分量 (G): 0
蓝色分量 (B): 0

发送LED控制命令...
模式: 1, 颜色: RGB(255, 0, 0)
✅ LED strip command sent successfully

技术特点：

• 支持4种LED显示模式

• RGB颜色自定义

• 异步服务调用

• 输入参数验证

• 友好的用户交互界面