6.2 C++ Interface Usage Example

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

Build & Run Instructions

• Enter the top-level directory of the extracted SDK and execute the following commands

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

📝 Code Notes

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

Caution

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

6.2.1 Get Robot Mode

Retrieve the robot’s current operating mode by calling the GetMcAction service, including the description, and status information.

Motion Mode Definitions

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Usage Instructions

ros2 run examples get_mc_action

Output Example

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

Interface Reference

• Service: /aimdk_5Fmsgs/srv/GetMcAction

• Message: aimdk_msgs/srv/GetMcAction

6.2.2 Set Robot Mode

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

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

Usage Instructions

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

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

Output Example

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

Notes

• Ensure the robot’s feet are on the ground before switching to the STAND_DEFAULT mode

• Mode switching may take several seconds to complete

Interface Reference

• Service: /aimdk_5Fmsgs/srv/SetMcAction

• Message: aimdk_msgs/srv/SetMcAction

6.2.3 Set Robot Motion

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

Available parameters can be found in the Preset Motion Table

#include "aimdk_msgs/msg/common_response.hpp"
#include "aimdk_msgs/msg/common_state.hpp"
#include "aimdk_msgs/msg/common_task_response.hpp"
#include "aimdk_msgs/msg/mc_control_area.hpp"
#include "aimdk_msgs/msg/mc_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 Gripper Control

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

Attention

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

#include "aimdk_msgs/msg/hand_command.hpp"
#include "aimdk_msgs/msg/hand_command_array.hpp"
#include "aimdk_msgs/msg/hand_type.hpp"
#include "aimdk_msgs/msg/message_header.hpp"
#include "rclcpp/rclcpp.hpp"
#include <chrono>
#include <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 Dexterous Hand Control

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

Attention

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

#include <aimdk_msgs/msg/hand_command_array.hpp>
#include <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 Register Secondary Development Input Source

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

#include "aimdk_msgs/srv/set_mc_input_source.hpp"
#include "aimdk_msgs/msg/common_request.hpp"
#include "aimdk_msgs/msg/common_response.hpp"
#include "aimdk_msgs/msg/common_state.hpp"
#include "aimdk_msgs/msg/common_task_response.hpp"
#include "aimdk_msgs/msg/mc_input_action.hpp"
#include "rclcpp/rclcpp.hpp"
#include <chrono>
#include <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 Get Current Input Source

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

#include "aimdk_msgs/srv/get_current_input_source.hpp"
#include "aimdk_msgs/msg/common_request.hpp"
#include "aimdk_msgs/msg/response_header.hpp"
#include "rclcpp/rclcpp.hpp"
#include <chrono>
#include <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;
  }
}

Usage Instructions

# Get current input source information
ros2 run examples get_current_input_source

Output Example

[INFO] [get_current_input_source_client]: Current input source: node
[INFO] [get_current_input_source_client]: Priority: 40
[INFO] [get_current_input_source_client]: Timeout: 1000

Notes

• Ensure the GetCurrentInputSource service is running properly

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

• A status code of 0 indicates a successful query

6.2.8 Robot Locomotion Control

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

Start the node after switching to Stable Standing Mode:

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

#include "rclcpp/rclcpp.hpp"
#include <chrono>
#include <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 Joint Motor Control

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

Attention

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

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

Robot Joint Control Example

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

1. Joint model definition

2. Trajectory interpolation using Ruckig

3. Multi-joint coordinated control

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

Dependencies

• ROS2

• Ruckig library

• aimdk_msgs package

Build Instructions

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

2. Add the following to your CMakeLists.txt:

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

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

3. Add dependencies in package.xml:

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

Example Function Overview

1. Four controller nodes are created to control:

   • Legs × 2 (12 joints)

   • Waist × 1 (3 joints)

   • Arms × 2 (14 joints)

   • Head × 1 (2 joints)

2. Demonstrated features:

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

   • Generate smooth motion trajectories using the Ruckig library

   • Publish joint control commands in real time

Customization

2. Add new control logic:

   • Modify the SetTargetPosition function

   • Add new control callback functions

3. Adjust control frequency:

   • Modify the period of control_timer_ (currently 2 ms)

#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 Keyboard Control of the Robot

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

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

Caution

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

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

  sudo apt install libncurses-dev

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

#include <algorithm>
#include <chrono>
#include <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

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

#include <chrono>
#include <cv_bridge/cv_bridge.h>
#include <filesystem>
#include <opencv2/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 Camera Streaming Example Collection

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

Depth Camera Data Subscription

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

Features:

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

• Real-time FPS statistics and data display

• Supports RGB video recording

• Configurable topic type selection

Supported Data Types:

• depth_pointcloud: Depth point cloud data (sensor_msgs/PointCloud2)

• depth_image: Depth image (sensor_msgs/Image)

• rgb_image: RGB image (sensor_msgs/Image)

• rgb_image_compressed: Compressed RGB image (sensor_msgs/CompressedImage)

• camera_info: Camera intrinsic parameters (sensor_msgs/CameraInfo)

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

Usage Instructions:

1. Subscribe to depth point cloud data:

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

2. Subscribe to RGB image data:

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

3. Subscribe to camera intrinsic parameters:

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

4. Record RGB video:

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

Stereo Camera Data Subscription

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

Features:

• Supports independent subscription for left and right cameras

• Real-time FPS statistics and data display

• Supports RGB video recording

• Configurable camera selection (left/right)

Supported Data Types:

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

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

• left_camera_info: Left camera intrinsic parameters (sensor_msgs/CameraInfo)

• right_rgb_image: Right camera RGB image (sensor_msgs/Image)

• right_rgb_image_compressed: Right camera compressed RGB image (sensor_msgs/CompressedImage)

• right_camera_info: Right camera intrinsic parameters (sensor_msgs/CameraInfo)

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

Usage Instructions:

1. Subscribe to left camera RGB image:

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

2. Subscribe to right camera RGB image:

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

3. Subscribe to left camera intrinsic parameters:

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

4. Record left camera video:

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

Rear Head Monocular Camera Data Subscription

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

Features:

• Supports rear head camera data subscription

• Real-time FPS statistics and data display

• Supports RGB video recording with/without obstructed area masked

• Configurable topic type selection

Supported Data Types:

• rgb_image: RGB image (sensor_msgs/Image)

• rgb_image_compressed: Compressed RGB image (sensor_msgs/CompressedImage)

• camera_info: Camera intrinsic parameters (sensor_msgs/CameraInfo)

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

Usage Instructions:

1. Subscribe to RGB image data:

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

2. Subscribe to compressed image data:

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

3. Subscribe to camera intrinsic parameters:

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

4. Record video:

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

5. Record video and mask obstructed area:

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

Application Scenarios:

• Face recognition and tracking

• Object detection and recognition

• Visual SLAM

• Image processing and computer vision algorithm development

• Robot visual navigation

6.2.13 Head touch sensor data subscription

This example uses echo_head_touch_sensor, which subscribes to the /aima/hal/sensor/touch_head topic to receive the robot’s touch sensor data on the head.

Features:

• The event data would change from “IDLE” to “TOUCH” when robot’s head touched

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

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

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

Usage Instructions:

ros2 ros2 run examples echo_head_touch_sensor

Example Output:

[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 LiDAR Data Subscription

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

Features:

• Supports LiDAR point cloud subscription

• Supports LiDAR IMU data subscription

• Real-time FPS statistics and data display

• Configurable topic type selection

• Detailed output of data field information

Supported Data Types:

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

• Imu: LiDAR IMU data (sensor_msgs/Imu)

Technical Implementation:

• Uses SensorDataQoS configuration (BEST_EFFORT + VOLATILE)

• Supports parsing and displaying point cloud field information

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

• Provides detailed debugging log output

Application Scenarios:

• LiDAR data acquisition and analysis

• Point cloud data processing and visualization

• Robot navigation and localization

• SLAM algorithm development

• Environmental perception and mapping

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

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

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

Usage Instructions:

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

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

Example Output:

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

6.2.15 Play Video

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

Attention

⚠️ Warning! The Interaction Computing Unit (PC3) is independent from the Development Computing Unit (PC2) where secondary development programs run. Audio and video files must be stored on the Interaction Computing Unit (IP: 10.0.1.42).
Audio and video files (and all parent directories up to root) must be readable by all users(new subdirectory under /var/tmp/ is recommended)

Function Description By calling the PlayVideo service, the robot can play a video file located at a specified path on its screen. Ensure the video file has been uploaded to the Interaction Computing Unit, otherwise playback will fail.

#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 Media File Playback

This example uses play_media, enabling playback of specified media files (such as audio files). It supports audio formats including WAV and MP3.

Features:

• Supports multiple audio formats (WAV, MP3, etc.)

• Supports priority control, allowing playback priority configuration

• Supports interruption mechanism, allowing ongoing playback to be interrupted

• Supports custom file paths and playback parameters

• Provides complete error handling and playback status feedback

Technical Implementation:

• Uses the PlayMediaFile service to play media files

• Supports priority level settings (0–99)

• Supports interrupt control (via the is_interrupted parameter)

• Provides detailed playback status feedback

Application Scenarios:

• Audio playback and media control

• Voice prompts and sound effects playback

• Multimedia application development

• Robot interaction audio feedback

Attention

⚠️ Warning! The Interaction Computing Unit (PC3) is independent from the Development Computing Unit (PC2) where secondary development programs run. Audio and video files must be stored on the Interaction Computing Unit (IP: 10.0.1.42).
Audio and video files (and all parent directories up to root) must be readable by all users(new subdirectory under /var/tmp/ is recommended)

#include <aimdk_msgs/msg/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;
}

Usage Instructions:

# Play default audio file
ros2 run examples play_media

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

# Play TTS cached audio file
ros2 run examples play_media /agibot/data/var/interaction/tts_cache/normal/demo.wav

Example Output:

[INFO] [play_media_file_client_min]: ✅ Media file playback request succeeded

Notes:

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

• Supported file formats: WAV, MP3, etc.

• Priority settings affect playback queue order

• Interruption mechanism can stop the currently playing audio

6.2.17 TTS (Text-to-Speech)

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

Features:

• Supports command-line arguments and interactive input

• Includes full service availability checks and error handling

• Supports priority control and interruption mechanism

• Provides detailed playback status feedback

Core Code

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

Usage Instructions

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

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

Output Example

[INFO] [play_tts_client_min]: ✅ TTS request succeeded

Notes

• Ensure the TTS service is running properly

• Supports both Chinese and English text-to-speech

• Priority settings affect playback queue order

• Interruption mechanism can stop the currently playing speech

Interface Reference

• Service: /aimdk_5Fmsgs/srv/PlayTts

• Message: aimdk_msgs/srv/PlayTts

6.2.18 Microphone Audio Reception

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

Features:

• Supports receiving multiple audio streams simultaneously (internal mic stream_id=1, external mic stream_id=2)

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

• Automatically saves complete speech segments as PCM files

• Stores files organized by timestamp and audio stream ID

• Supports audio duration calculation and statistical output

VAD State Description:

• 0: No speech

• 1: Speech start

• 2: Speech processing

• 3: Speech end

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

Usage Instructions:

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

2. Audio files are stored by stream_id:

   • stream_1/: Internal microphone audio

   • stream_2/: External microphone audio

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

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

5. You can play the saved PCM file using the following command:

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

6. You should say wake words to make VAD ready to capture voice

Example Output:

[INFO] Start subscribing to denoised audio data...
[INFO] Received audio data: stream_id=2, vad_state=1 (Speech Start), audio_size=320 bytes
[INFO] [External Mic] VAD State: Speech Start, Audio: 320 bytes
[INFO] 🎤 Speech detected
[INFO] Received audio data: stream_id=2, vad_state=2 (In Speech), audio_size=320 bytes
[INFO] [External Mic] VAD State: In Speech, Audio: 320 bytes
[INFO] 🔄 Processing speech...
[INFO] Received audio data: stream_id=2, vad_state=3 (Speech End), audio_size=320 bytes
[INFO] [External Mic] VAD State: Speech End, Audio: 320 bytes
[INFO] ✅ Speech ended
[INFO] Audio segment saved: audio_recordings/stream_2/external_mic_20250909_133649_738.pcm (Size: 960 bytes)
[INFO] Audio duration: 0.06 seconds (480 samples)

Example: Playing a PCM audio file (using aplay on Linux)

Assuming you have recorded and saved the audio file external_mic_20250909_151117_223.pcm, you can play it using the following command:

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

Parameter explanation:

• -r 16000 # Sampling rate 16 kHz

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

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

Note: If you recorded audio from the internal microphone, the path should be audio_recordings/stream_1/internal_mic_xxx.pcm

6.2.19 Emoji Control

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

#include "aimdk_msgs/srv/play_emoji.hpp"
#include "aimdk_msgs/msg/common_request.hpp"
#include "rclcpp/rclcpp.hpp"
#include <chrono>
#include <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 Strip Control

Function Description: Demonstrates how to control the robot’s LED strip, supporting multiple display modes and customizable colors.

Core Code:

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

Usage Instructions:

# Build
colcon build --packages-select examples

# Run
ros2 run examples play_lights

Output Example:

=== LED Strip Control Example ===
Please select LED mode:
0 - Constant ON
1 - Breathing Mode (4s cycle, sinusoidal brightness)
2 - Blinking Mode (1s cycle, 0.5s ON / 0.5s OFF)
3 - Flowing Mode (2s cycle, lights on from left to right)
Enter mode (0–3): 1

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

Sending LED control command...
Mode: 1, Color: RGB(255, 0, 0)
✅ LED strip command sent successfully

Technical Features:

• Supports 4 LED display modes

• Customizable RGB colors

• Asynchronous service calls

• Input parameter validation

• User-friendly interaction interface