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.
1#include "aimdk_msgs/srv/get_mc_action.hpp"
2#include "aimdk_msgs/msg/common_request.hpp"
3#include "aimdk_msgs/msg/response_header.hpp"
4#include "rclcpp/rclcpp.hpp"
5#include <chrono>
6#include <memory>
7#include <signal.h>
8
9// Global variable used for signal handling
10std::shared_ptr<rclcpp::Node> g_node = nullptr;
11
12// Signal handler function
13void signal_handler(int signal) {
14 if (g_node) {
15 RCLCPP_INFO(g_node->get_logger(), "Received signal %d, shutting down...",
16 signal);
17 g_node.reset();
18 }
19 rclcpp::shutdown();
20 exit(signal);
21}
22
23class GetMcActionClient : public rclcpp::Node {
24public:
25 GetMcActionClient() : Node("get_mc_action_client") {
26
27 client_ = this->create_client<aimdk_msgs::srv::GetMcAction>(
28 "/aimdk_5Fmsgs/srv/GetMcAction"); // correct the service path
29 RCLCPP_INFO(this->get_logger(), "✅ GetMcAction client node created.");
30
31 while (!client_->wait_for_service(std::chrono::seconds(2))) {
32 RCLCPP_INFO(this->get_logger(), "⏳ Service unavailable, waiting...");
33 }
34 RCLCPP_INFO(this->get_logger(),
35 "🟢 Service available, ready to send request.");
36 }
37
38 void send_request() {
39 try {
40 auto request = std::make_shared<aimdk_msgs::srv::GetMcAction::Request>();
41 request->request = aimdk_msgs::msg::CommonRequest();
42
43 RCLCPP_INFO(this->get_logger(), "📨 Sending request to get robot mode");
44
45 // Set a service call timeout
46 const std::chrono::milliseconds timeout(250);
47 for (int i = 0; i < 8; i++) {
48 request->request.header.stamp = this->now();
49 auto future = client_->async_send_request(request);
50 auto retcode = rclcpp::spin_until_future_complete(shared_from_this(),
51 future, timeout);
52 if (retcode != rclcpp::FutureReturnCode::SUCCESS) {
53 // retry as remote peer is NOT handled well by ROS
54 RCLCPP_INFO(this->get_logger(), "trying ... [%d]", i);
55 continue;
56 }
57 // future.done
58 auto response = future.get();
59 RCLCPP_INFO(this->get_logger(), "✅ Robot mode get successfully.");
60 RCLCPP_INFO(this->get_logger(), "Mode name: %s",
61 response->info.action_desc.c_str());
62 RCLCPP_INFO(this->get_logger(), "Mode status: %d",
63 response->info.status.value);
64 return;
65 }
66 RCLCPP_ERROR(this->get_logger(), "❌ Service call failed or timed out.");
67 } catch (const std::exception &e) {
68 RCLCPP_ERROR(this->get_logger(), "Exception occurred: %s", e.what());
69 }
70 }
71
72private:
73 rclcpp::Client<aimdk_msgs::srv::GetMcAction>::SharedPtr client_;
74};
75
76int main(int argc, char *argv[]) {
77 try {
78 rclcpp::init(argc, argv);
79
80 // Set up signal handlers
81 signal(SIGINT, signal_handler);
82 signal(SIGTERM, signal_handler);
83
84 // Create node
85 g_node = std::make_shared<GetMcActionClient>();
86 auto client = std::dynamic_pointer_cast<GetMcActionClient>(g_node);
87 if (client) {
88 client->send_request();
89 }
90
91 // Clean up resources
92 g_node.reset();
93 rclcpp::shutdown();
94
95 return 0;
96 } catch (const std::exception &e) {
97 RCLCPP_ERROR(rclcpp::get_logger("main"),
98 "Program exited with exception: %s", e.what());
99 return 1;
100 }
101}
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/GetMcActionMessage:
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
1#include "aimdk_msgs/srv/set_mc_action.hpp"
2#include "aimdk_msgs/msg/common_response.hpp"
3#include "aimdk_msgs/msg/common_state.hpp"
4#include "aimdk_msgs/msg/mc_action.hpp"
5#include "aimdk_msgs/msg/mc_action_command.hpp"
6#include "aimdk_msgs/msg/request_header.hpp"
7#include "rclcpp/rclcpp.hpp"
8#include <chrono>
9#include <iomanip>
10#include <memory>
11#include <signal.h>
12#include <unordered_map>
13#include <vector>
14
15// Global variable used for signal handling
16std::shared_ptr<rclcpp::Node> g_node = nullptr;
17
18// Signal handler function
19void signal_handler(int signal) {
20 if (g_node) {
21 RCLCPP_INFO(g_node->get_logger(), "Received signal %d, shutting down...",
22 signal);
23 g_node.reset();
24 }
25 rclcpp::shutdown();
26 exit(signal);
27}
28
29class SetMcActionClient : public rclcpp::Node {
30public:
31 SetMcActionClient() : Node("set_mc_action_client") {
32
33 client_ = this->create_client<aimdk_msgs::srv::SetMcAction>(
34 "/aimdk_5Fmsgs/srv/SetMcAction");
35 RCLCPP_INFO(this->get_logger(), "✅ SetMcAction client node created.");
36
37 // Wait for the service to become available
38 while (!client_->wait_for_service(std::chrono::seconds(2))) {
39 RCLCPP_INFO(this->get_logger(), "⏳ Service unavailable, waiting...");
40 }
41 RCLCPP_INFO(this->get_logger(),
42 "🟢 Service available, ready to send request.");
43 }
44
45 bool send_request(std::string &action_name) {
46 try {
47 auto request = std::make_shared<aimdk_msgs::srv::SetMcAction::Request>();
48 request->header = aimdk_msgs::msg::RequestHeader();
49
50 // Set robot mode
51 aimdk_msgs::msg::McActionCommand command;
52 command.action_desc = action_name;
53 request->command = command;
54
55 RCLCPP_INFO(this->get_logger(), "📨 Sending request to set robot mode: %s",
56 action_name.c_str());
57
58 // Set Service Call Timeout
59 const std::chrono::milliseconds timeout(250);
60 for (int i = 0; i < 8; i++) {
61 request->header.stamp = this->now();
62 auto future = client_->async_send_request(request);
63 auto retcode = rclcpp::spin_until_future_complete(shared_from_this(),
64 future, timeout);
65 if (retcode != rclcpp::FutureReturnCode::SUCCESS) {
66 // retry as remote peer is NOT handled well by ROS
67 RCLCPP_INFO(this->get_logger(), "trying ... [%d]", i);
68 continue;
69 }
70 // future.done
71 auto response = future.get();
72 if (response->response.status.value ==
73 aimdk_msgs::msg::CommonState::SUCCESS) {
74 RCLCPP_INFO(this->get_logger(), "✅ Robot mode set successfully.");
75 return true;
76 } else {
77 RCLCPP_ERROR(this->get_logger(), "❌ Failed to set robot mode: %s",
78 response->response.message.c_str());
79 return false;
80 }
81 }
82 RCLCPP_ERROR(this->get_logger(), "❌ Service call failed or timed out.");
83 return false;
84 } catch (const std::exception &e) {
85 RCLCPP_ERROR(this->get_logger(), "Exception occurred: %s", e.what());
86 return false;
87 }
88 }
89
90private:
91 rclcpp::Client<aimdk_msgs::srv::SetMcAction>::SharedPtr client_;
92};
93
94static std::unordered_map<std::string, std::vector<std::string>> g_action_info =
95 {
96 {"PASSIVE_DEFAULT", {"PD", "joints with zero torque"}},
97 {"DAMPING_DEFAULT", {"DD", "joints in damping mode"}},
98 {"JOINT_DEFAULT", {"JD", "Position Control Stand (joints locked)"}},
99 {"STAND_DEFAULT", {"SD", "Stable Stand (auto-balance)"}},
100 {"LOCOMOTION_DEFAULT", {"LD", "locomotion mode (walk or run)"}},
101};
102
103int main(int argc, char *argv[]) {
104 try {
105 rclcpp::init(argc, argv);
106
107 // Set up signal handlers
108 signal(SIGINT, signal_handler);
109 signal(SIGTERM, signal_handler);
110
111 // Create node
112 g_node = std::make_shared<SetMcActionClient>();
113 auto client = std::dynamic_pointer_cast<SetMcActionClient>(g_node);
114
115 if (client) {
116 std::unordered_map<std::string, std::string> choices;
117 std::string motion;
118
119 // Prefer command-line argument; otherwise prompt user
120 if (argc > 1) {
121 motion = argv[1];
122 RCLCPP_INFO(g_node->get_logger(),
123 "Using abbr of motion mode from cmdline: %s", argv[1]);
124 } else {
125 std::cout << std::left << std::setw(4) << "abbr"
126 << " - " << std::setw(20) << "robot mode"
127 << " : "
128 << "description" << std::endl;
129 for (auto &it : g_action_info) {
130 std::cout << std::left << std::setw(4) << it.second[0] << " - "
131 << std::setw(20) << it.first << " : " << it.second[1]
132 << std::endl;
133 }
134 std::cout << "Enter abbr of motion mode:";
135 std::cin >> motion;
136 }
137 for (auto &it : g_action_info) {
138 choices[it.second[0]] = it.first;
139 }
140
141 auto m = choices.find(motion);
142 if (m != choices.end()) {
143 auto &action_name = m->second;
144 client->send_request(action_name);
145 } else {
146 RCLCPP_ERROR(g_node->get_logger(), "Invalid abbr of robot mode: %s",
147 motion.c_str());
148 }
149 }
150
151 // Clean up resources
152 g_node.reset();
153 rclcpp::shutdown();
154
155 return 0;
156 } catch (const std::exception &e) {
157 RCLCPP_ERROR(rclcpp::get_logger("main"),
158 "Program exited with exception: %s", e.what());
159 return 1;
160 }
161}
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_DEFAULTmodeMode switching may take several seconds to complete
Interface Reference
Service:
/aimdk_5Fmsgs/srv/SetMcActionMessage:
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
1#include "aimdk_msgs/msg/common_response.hpp"
2#include "aimdk_msgs/msg/common_state.hpp"
3#include "aimdk_msgs/msg/common_task_response.hpp"
4#include "aimdk_msgs/msg/mc_control_area.hpp"
5#include "aimdk_msgs/msg/mc_preset_motion.hpp"
6#include "aimdk_msgs/msg/request_header.hpp"
7#include "aimdk_msgs/srv/set_mc_preset_motion.hpp"
8#include "rclcpp/rclcpp.hpp"
9#include <chrono>
10#include <memory>
11#include <signal.h>
12
13std::shared_ptr<rclcpp::Node> g_node = nullptr;
14
15void signal_handler(int signal) {
16 if (g_node) {
17 RCLCPP_INFO(g_node->get_logger(), "Received signal %d, shutting down...",
18 signal);
19 g_node.reset();
20 }
21 rclcpp::shutdown();
22 exit(signal);
23}
24
25class PresetMotionClient : public rclcpp::Node {
26public:
27 PresetMotionClient() : Node("preset_motion_client") {
28 const std::chrono::seconds timeout(8);
29
30 client_ = this->create_client<aimdk_msgs::srv::SetMcPresetMotion>(
31 "/aimdk_5Fmsgs/srv/SetMcPresetMotion");
32
33 RCLCPP_INFO(this->get_logger(), "✅ SetMcPresetMotion client node created.");
34
35 // Wait for the service to become available
36 while (!client_->wait_for_service(std::chrono::seconds(2))) {
37 RCLCPP_INFO(this->get_logger(), "⏳ Service unavailable, waiting...");
38 }
39 RCLCPP_INFO(this->get_logger(),
40 "🟢 Service available, ready to send request.");
41 }
42
43 bool send_request(int area_id, int motion_id) {
44 try {
45 auto request =
46 std::make_shared<aimdk_msgs::srv::SetMcPresetMotion::Request>();
47 request->header = aimdk_msgs::msg::RequestHeader();
48
49 aimdk_msgs::msg::McPresetMotion motion;
50 aimdk_msgs::msg::McControlArea area;
51
52 motion.value = motion_id; // Preset motion ID
53 area.value = area_id; // Control area ID
54 request->motion = motion;
55 request->area = area;
56 request->interrupt = false; // Not interrupt current motion
57
58 RCLCPP_INFO(this->get_logger(),
59 "📨 Sending request to set preset motion: motion=%d, area=%d",
60 motion_id, area_id);
61
62 const std::chrono::milliseconds timeout(250);
63 for (int i = 0; i < 8; i++) {
64 request->header.stamp = this->now();
65 auto future = client_->async_send_request(request);
66 auto retcode = rclcpp::spin_until_future_complete(shared_from_this(),
67 future, timeout);
68 if (retcode != rclcpp::FutureReturnCode::SUCCESS) {
69 // retry as remote peer is NOT handled well by ROS
70 RCLCPP_INFO(this->get_logger(), "trying ... [%d]", i);
71 continue;
72 }
73 // future.done
74 auto response = future.get();
75 if (response->response.state.value ==
76 aimdk_msgs::msg::CommonState::SUCCESS) {
77 RCLCPP_INFO(this->get_logger(),
78 "✅ Preset motion set successfully: %lu",
79 response->response.task_id);
80 return true;
81 } else if (response->response.state.value ==
82 aimdk_msgs::msg::CommonState::RUNNING) {
83 RCLCPP_INFO(this->get_logger(), "⏳ Preset motion executing: %lu",
84 response->response.task_id);
85 return true;
86 } else {
87 RCLCPP_WARN(this->get_logger(), "❌ Failed to set preset motion: %lu",
88 response->response.task_id);
89 return false;
90 }
91 }
92 RCLCPP_ERROR(this->get_logger(), "❌ Service call failed or timed out.");
93 return false;
94 } catch (const std::exception &e) {
95 RCLCPP_ERROR(this->get_logger(), "Exception occurred: %s", e.what());
96 return false;
97 }
98 }
99
100private:
101 rclcpp::Client<aimdk_msgs::srv::SetMcPresetMotion>::SharedPtr client_;
102};
103
104int main(int argc, char *argv[]) {
105 try {
106 rclcpp::init(argc, argv);
107 signal(SIGINT, signal_handler);
108 signal(SIGTERM, signal_handler);
109
110 g_node = std::make_shared<PresetMotionClient>();
111 // Cast g_node (std::shared_ptr<rclcpp::Node>) to a derived
112 // PresetMotionClient pointer (std::shared_ptr<PresetMotionClient>)
113 auto client = std::dynamic_pointer_cast<PresetMotionClient>(g_node);
114
115 int area = 1;
116 int motion = 1003;
117 std::cout << "Enter arm area ID (1-left, 2-right): ";
118 std::cin >> area;
119 std::cout
120 << "Enter preset motion ID (1001-raise, 1002-wave, 1003-handshake, "
121 "1004-airkiss): ";
122 std::cin >> motion;
123 if (client) {
124 client->send_request(area, motion);
125 }
126
127 // Clean up resources
128 g_node.reset();
129 rclcpp::shutdown();
130
131 return 0;
132 } catch (const std::exception &e) {
133 RCLCPP_ERROR(rclcpp::get_logger("main"),
134 "Program exited with exception: %s", e.what());
135 return 1;
136 }
137}
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.
1#include "aimdk_msgs/msg/hand_command.hpp"
2#include "aimdk_msgs/msg/hand_command_array.hpp"
3#include "aimdk_msgs/msg/hand_type.hpp"
4#include "aimdk_msgs/msg/message_header.hpp"
5#include "rclcpp/rclcpp.hpp"
6#include <chrono>
7#include <vector>
8
9class HandControl : public rclcpp::Node {
10public:
11 HandControl()
12 : Node("hand_control"), position_pairs_({
13 {1.0, 1.0},
14 {0.0, 0.0},
15 {0.5, 0.5},
16 {0.2, 0.8},
17 {0.7, 0.3},
18 }),
19 current_index_(0) {
20 publisher_ = this->create_publisher<aimdk_msgs::msg::HandCommandArray>(
21 "/aima/hal/joint/hand/command", 10);
22
23 timer_ = this->create_wall_timer(
24 std::chrono::milliseconds(20), // 50Hz
25 std::bind(&HandControl::publish_hand_commands, this));
26
27 last_switch_time_ = now();
28 RCLCPP_INFO(this->get_logger(), "The hand control node has been started!");
29 }
30
31 void publish_hand_commands() {
32 // 1. Determine if it's time to switch parameters.
33 auto now_time = this->now();
34 if ((now_time - last_switch_time_).seconds() >= 2.0) {
35 current_index_ = (current_index_ + 1) % position_pairs_.size();
36 last_switch_time_ = now_time;
37 RCLCPP_INFO(this->get_logger(),
38 "Switched to the next parameter group, index=%zu (left=%.2f, "
39 "right=%.2f)",
40 current_index_, position_pairs_[current_index_].first,
41 position_pairs_[current_index_].second);
42 }
43
44 auto msg = std::make_unique<aimdk_msgs::msg::HandCommandArray>();
45 msg->header = aimdk_msgs::msg::MessageHeader();
46
47 float left_position = position_pairs_[current_index_].first;
48 float right_position = position_pairs_[current_index_].second;
49
50 aimdk_msgs::msg::HandCommand left_hands;
51 left_hands.name = "left_hand";
52 left_hands.position = left_position;
53 left_hands.velocity = 1.0;
54 left_hands.acceleration = 1.0;
55 left_hands.deceleration = 1.0;
56 left_hands.effort = 1.0;
57
58 aimdk_msgs::msg::HandCommand right_hands;
59 right_hands.name = "right_hand";
60 right_hands.position = right_position;
61 right_hands.velocity = 1.0;
62 right_hands.acceleration = 1.0;
63 right_hands.deceleration = 1.0;
64 right_hands.effort = 1.0;
65
66 msg->left_hands.push_back(left_hands);
67 msg->right_hands.push_back(right_hands);
68 msg->left_hand_type.value = 2;
69 msg->right_hand_type.value = 2;
70
71 publisher_->publish(std::move(msg));
72 }
73
74private:
75 rclcpp::Publisher<aimdk_msgs::msg::HandCommandArray>::SharedPtr publisher_;
76 rclcpp::TimerBase::SharedPtr timer_;
77
78 std::vector<std::pair<float, float>> position_pairs_;
79 size_t current_index_;
80
81 rclcpp::Time last_switch_time_;
82};
83
84int main(int argc, char *argv[]) {
85 rclcpp::init(argc, argv);
86 auto hand_control_node = std::make_shared<HandControl>();
87 rclcpp::spin(hand_control_node);
88 rclcpp::shutdown();
89 return 0;
90}
6.2.5 Dexterous Hand Control (Unavailable During Upgrade)
This example uses omnihand_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.
1#include "aimdk_msgs/msg/hand_command.hpp"
2#include "aimdk_msgs/msg/hand_command_array.hpp"
3#include "aimdk_msgs/msg/hand_type.hpp"
4#include "aimdk_msgs/msg/message_header.hpp"
5#include "rclcpp/rclcpp.hpp"
6#include <chrono>
7#include <vector>
8
9/**
10 * @brief Omnihand control node
11 */
12class OmnihandControl : public rclcpp::Node {
13public:
14 OmnihandControl()
15 : Node("omnihand_control"), position_pairs_({
16 {1.0, 1.0}, // Hands fully open
17 {0.0, 0.0}, // Hands fully closed
18 {0.5, 0.5}, // Hands half open
19 {0.3, 0.7}, // Left closed right open
20 {0.8, 0.2} // Left open right closed
21 }),
22 current_index_(0) {
23 publisher_ = this->create_publisher<aimdk_msgs::msg::HandCommandArray>(
24 "/aima/hal/joint/hand/command", 10);
25
26 timer_ = this->create_wall_timer(
27 std::chrono::milliseconds(20), // 50Hz
28 std::bind(&OmnihandControl::publish_hand_commands, this));
29
30 last_switch_time_ = now();
31 RCLCPP_INFO(this->get_logger(), "Omnihand control node has been started!");
32 }
33
34 /**
35 * @brief Publish hand commands
36 */
37 void publish_hand_commands() {
38 // 1. Switches parameter set every 5 seconds
39 auto now_time = this->now();
40 if ((now_time - last_switch_time_).seconds() >= 5.0) {
41 current_index_ = (current_index_ + 1) % position_pairs_.size();
42 last_switch_time_ = now_time;
43 RCLCPP_INFO(
44 this->get_logger(),
45 "Switched to parameter set %zu, left hand = %.2f, right hand = %.2f",
46 current_index_, position_pairs_[current_index_].first,
47 position_pairs_[current_index_].second);
48 }
49
50 auto msg = std::make_unique<aimdk_msgs::msg::HandCommandArray>();
51 msg->header = aimdk_msgs::msg::MessageHeader();
52
53 float left_position = position_pairs_[current_index_].first;
54 float right_position = position_pairs_[current_index_].second;
55
56 // Left hand 10 joints
57 for (int i = 0; i < 10; ++i) {
58 aimdk_msgs::msg::HandCommand left_hands;
59 left_hands.name = "left_hand_joint" + std::to_string(i);
60 left_hands.position = left_position;
61 left_hands.velocity = 1.0;
62 left_hands.acceleration = 1.0;
63 left_hands.deceleration = 1.0;
64 left_hands.effort = 1.0;
65 msg->left_hands.push_back(left_hands);
66 }
67 // Right hand 10 joints
68 for (int i = 0; i < 10; ++i) {
69 aimdk_msgs::msg::HandCommand right_hands;
70 right_hands.name = "right_hand_joint" + std::to_string(i);
71 right_hands.position = right_position;
72 right_hands.velocity = 1.0;
73 right_hands.acceleration = 1.0;
74 right_hands.deceleration = 1.0;
75 right_hands.effort = 1.0;
76 msg->right_hands.push_back(right_hands);
77 }
78
79 // Type 1: Dexterous hand mode
80 msg->left_hand_type.value = 1;
81 msg->right_hand_type.value = 1;
82
83 publisher_->publish(std::move(msg));
84 }
85
86private:
87 // Member variables
88 rclcpp::Publisher<aimdk_msgs::msg::HandCommandArray>::SharedPtr publisher_;
89 rclcpp::TimerBase::SharedPtr timer_;
90
91 std::vector<std::pair<float, float>> position_pairs_;
92 size_t current_index_;
93 rclcpp::Time last_switch_time_;
94};
95
96/**
97 * @brief Main function
98 */
99// Initializes ROS, creates node, and starts spinning
100int main(int argc, char *argv[]) {
101 rclcpp::init(argc, argv);
102 auto omnihand_control_node = std::make_shared<OmnihandControl>();
103 rclcpp::spin(omnihand_control_node);
104 rclcpp::shutdown();
105 return 0;
106}
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.
1#include "aimdk_msgs/srv/set_mc_input_source.hpp"
2#include "aimdk_msgs/msg/common_request.hpp"
3#include "aimdk_msgs/msg/common_response.hpp"
4#include "aimdk_msgs/msg/common_state.hpp"
5#include "aimdk_msgs/msg/common_task_response.hpp"
6#include "aimdk_msgs/msg/mc_input_action.hpp"
7#include "rclcpp/rclcpp.hpp"
8#include <chrono>
9#include <memory>
10#include <signal.h>
11
12std::shared_ptr<rclcpp::Node> g_node = nullptr;
13
14void signal_handler(int signal) {
15 if (g_node) {
16 RCLCPP_INFO(g_node->get_logger(), "Received signal %d, shutting down...",
17 signal);
18 g_node.reset();
19 }
20 rclcpp::shutdown();
21 exit(signal);
22}
23
24class McInputClient : public rclcpp::Node {
25public:
26 McInputClient() : Node("set_mc_input_source_client") {
27 client_ = this->create_client<aimdk_msgs::srv::SetMcInputSource>(
28 "/aimdk_5Fmsgs/srv/SetMcInputSource");
29
30 RCLCPP_INFO(this->get_logger(), "✅ SetMcInputSource client node created.");
31
32 // Wait for the service to become available
33 while (!client_->wait_for_service(std::chrono::seconds(2))) {
34 RCLCPP_INFO(this->get_logger(), "⏳ Service unavailable, waiting...");
35 }
36 RCLCPP_INFO(this->get_logger(),
37 "🟢 Service available, ready to send request.");
38 }
39
40 bool send_request() {
41 try {
42 auto request =
43 std::make_shared<aimdk_msgs::srv::SetMcInputSource::Request>();
44
45 // Set request data
46 request->action.value = 1001; // Add new input source
47 request->input_source.name = "node"; // Set message source
48 request->input_source.priority = 40; // Set priority
49 request->input_source.timeout = 1000; // Set timeout (ms)
50
51 RCLCPP_INFO(this->get_logger(), "📨 Sending input source request: (ID=%d)",
52 request->action.value);
53
54 auto timeout = std::chrono::milliseconds(250);
55 for (int i = 0; i < 8; i++) {
56 // Set header timestamp
57 request->request.header.stamp = this->now(); // use Node::now()
58 auto future = client_->async_send_request(request);
59 auto retcode = rclcpp::spin_until_future_complete(
60 this->shared_from_this(), future, timeout);
61 if (retcode != rclcpp::FutureReturnCode::SUCCESS) {
62 // retry as remote peer is NOT handled well by ROS
63 RCLCPP_INFO(this->get_logger(), "trying ... [%d]", i);
64 continue;
65 }
66 // future.done
67 auto response = future.get();
68 auto code = response->response.header.code;
69 if (code == 0) {
70 RCLCPP_INFO(this->get_logger(),
71 "✅ Input source set successfully. task_id=%lu",
72 response->response.task_id);
73 return true;
74 } else {
75 RCLCPP_ERROR(
76 this->get_logger(),
77 "❌ Input source set failed. ret_code=%ld, task_id=%lu "
78 "(duplicated ADD? or MODIFY/ENABLE/DISABLE for unknown source?)",
79 code, response->response.task_id);
80 return false;
81 }
82 }
83 RCLCPP_ERROR(this->get_logger(), "❌ Service call failed or timed out.");
84 return false;
85 } catch (const std::exception &e) {
86 RCLCPP_ERROR(this->get_logger(), "Exception occurred: %s", e.what());
87 return false;
88 }
89 }
90
91private:
92 rclcpp::Client<aimdk_msgs::srv::SetMcInputSource>::SharedPtr client_;
93};
94
95int main(int argc, char *argv[]) {
96 try {
97 rclcpp::init(argc, argv);
98 signal(SIGINT, signal_handler);
99 signal(SIGTERM, signal_handler);
100
101 g_node = std::make_shared<McInputClient>();
102 auto client = std::dynamic_pointer_cast<McInputClient>(g_node);
103
104 if (client) {
105 client->send_request();
106 }
107
108 g_node.reset();
109 rclcpp::shutdown();
110
111 return 0;
112 } catch (const std::exception &e) {
113 RCLCPP_ERROR(rclcpp::get_logger("main"),
114 "Program terminated with exception: %s", e.what());
115 return 1;
116 }
117}
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.
1#include "aimdk_msgs/srv/get_current_input_source.hpp"
2#include "aimdk_msgs/msg/common_request.hpp"
3#include "aimdk_msgs/msg/response_header.hpp"
4#include "rclcpp/rclcpp.hpp"
5#include <chrono>
6#include <memory>
7#include <signal.h>
8
9// Global node object
10std::shared_ptr<rclcpp::Node> g_node = nullptr;
11
12// Signal handler
13void signal_handler(int signal) {
14 if (g_node) {
15 RCLCPP_INFO(g_node->get_logger(), "Received signal %d, shutting down...",
16 signal);
17 g_node.reset();
18 }
19 rclcpp::shutdown();
20 exit(signal);
21}
22
23// Client Class
24class GetCurrentInputSourceClient : public rclcpp::Node {
25public:
26 GetCurrentInputSourceClient() : Node("get_current_input_source_client") {
27
28 client_ = this->create_client<aimdk_msgs::srv::GetCurrentInputSource>(
29 "/aimdk_5Fmsgs/srv/GetCurrentInputSource");
30
31 RCLCPP_INFO(this->get_logger(),
32 "✅ GetCurrentInputSource client node created.");
33
34 // Wait for the service to become available
35 while (!client_->wait_for_service(std::chrono::seconds(2))) {
36 RCLCPP_INFO(this->get_logger(), "⏳ Service unavailable, waiting...");
37 }
38 RCLCPP_INFO(this->get_logger(),
39 "🟢 Service available, ready to send request.");
40 }
41
42 void send_request() {
43 try {
44 auto request =
45 std::make_shared<aimdk_msgs::srv::GetCurrentInputSource::Request>();
46 request->request = aimdk_msgs::msg::CommonRequest();
47
48 RCLCPP_INFO(this->get_logger(),
49 "📨 Sending request to get current input source");
50
51 auto timeout = std::chrono::milliseconds(250);
52
53 for (int i = 0; i < 8; i++) {
54 request->request.header.stamp = this->now();
55 auto future = client_->async_send_request(request);
56 auto retcode = rclcpp::spin_until_future_complete(
57 this->shared_from_this(), future, timeout);
58 if (retcode != rclcpp::FutureReturnCode::SUCCESS) {
59 // retry as remote peer is NOT handled well by ROS
60 RCLCPP_INFO(this->get_logger(), "trying ... [%d]", i);
61 continue;
62 }
63 // future.done
64 auto response = future.get();
65 if (response->response.header.code == 0) {
66 RCLCPP_INFO(this->get_logger(),
67 "✅ Current input source get successfully:");
68 RCLCPP_INFO(this->get_logger(), "Name: %s",
69 response->input_source.name.c_str());
70 RCLCPP_INFO(this->get_logger(), "Priority: %d",
71 response->input_source.priority);
72 RCLCPP_INFO(this->get_logger(), "Timeout: %d",
73 response->input_source.timeout);
74 } else {
75 RCLCPP_WARN(this->get_logger(),
76 "❌ Current input source get failed, return code: %ld",
77 response->response.header.code);
78 }
79 return;
80 }
81 RCLCPP_ERROR(this->get_logger(), "❌ Service call failed or timed out.");
82 } catch (const std::exception &e) {
83 RCLCPP_ERROR(this->get_logger(), "Exception occurred: %s", e.what());
84 }
85 }
86
87private:
88 rclcpp::Client<aimdk_msgs::srv::GetCurrentInputSource>::SharedPtr client_;
89};
90
91int main(int argc, char *argv[]) {
92 try {
93 rclcpp::init(argc, argv);
94
95 signal(SIGINT, signal_handler);
96 signal(SIGTERM, signal_handler);
97
98 g_node = std::make_shared<GetCurrentInputSourceClient>();
99 auto client =
100 std::dynamic_pointer_cast<GetCurrentInputSourceClient>(g_node);
101
102 if (client) {
103 client->send_request();
104 }
105
106 g_node.reset();
107 rclcpp::shutdown();
108 return 0;
109 } catch (const std::exception &e) {
110 RCLCPP_ERROR(rclcpp::get_logger("main"),
111 "Program exited with exception: %s", e.what());
112 return 1;
113 }
114}
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.
进入稳定站立模式后执行本程序
1#include "aimdk_msgs/msg/mc_locomotion_velocity.hpp"
2#include "aimdk_msgs/msg/common_request.hpp"
3#include "aimdk_msgs/msg/common_response.hpp"
4#include "aimdk_msgs/msg/common_state.hpp"
5#include "aimdk_msgs/msg/common_task_response.hpp"
6#include "aimdk_msgs/msg/mc_input_action.hpp"
7#include "aimdk_msgs/msg/message_header.hpp"
8#include "aimdk_msgs/srv/set_mc_input_source.hpp"
9
10#include "rclcpp/rclcpp.hpp"
11#include <chrono>
12#include <memory>
13#include <signal.h>
14#include <thread>
15
16class DirectVelocityControl : public rclcpp::Node {
17public:
18 DirectVelocityControl() : Node("direct_velocity_control") {
19 // Create publisher
20 publisher_ = this->create_publisher<aimdk_msgs::msg::McLocomotionVelocity>(
21 "/aima/mc/locomotion/velocity", 10);
22 // Create service client
23 client_ = this->create_client<aimdk_msgs::srv::SetMcInputSource>(
24 "/aimdk_5Fmsgs/srv/SetMcInputSource");
25
26 // Maximum speed limits
27 max_forward_speed_ = 1.0; // m/s
28 max_lateral_speed_ = 1.0; // m/s
29 max_angular_speed_ = 1.0; // rad/s
30 // Minimum speed limits (0 is also OK)
31 min_forward_speed_ = 0.2; // m/s
32 min_lateral_speed_ = 0.2; // m/s
33 min_angular_speed_ = 0.1; // rad/s
34
35 RCLCPP_INFO(this->get_logger(), "Direct velocity control node started.");
36 }
37
38 void start_publish() {
39 if (timer_ != nullptr) {
40 return;
41 }
42 // Set timer to periodically publish velocity messages (50Hz)
43 timer_ = this->create_wall_timer(
44 std::chrono::milliseconds(20),
45 std::bind(&DirectVelocityControl::publish_velocity, this));
46 }
47
48 bool register_input_source() {
49 const std::chrono::seconds timeout(8);
50 auto start_time = std::chrono::steady_clock::now();
51 while (!client_->wait_for_service(std::chrono::seconds(2))) {
52 if (std::chrono::steady_clock::now() - start_time > timeout) {
53 RCLCPP_ERROR(this->get_logger(), "Waiting for service timed out");
54 return false;
55 }
56 RCLCPP_INFO(this->get_logger(), "Waiting for input source service...");
57 }
58
59 auto request =
60 std::make_shared<aimdk_msgs::srv::SetMcInputSource::Request>();
61 request->action.value = 1001;
62 request->input_source.name = "node";
63 request->input_source.priority = 40;
64 request->input_source.timeout = 1000;
65
66 auto timeout2 = std::chrono::milliseconds(250);
67
68 for (int i = 0; i < 8; i++) {
69 request->request.header.stamp = this->now();
70 auto future = client_->async_send_request(request);
71 auto retcode = rclcpp::spin_until_future_complete(
72 this->shared_from_this(), future, timeout2);
73 if (retcode != rclcpp::FutureReturnCode::SUCCESS) {
74 // retry as remote peer is NOT handled well by ROS
75 RCLCPP_INFO(this->get_logger(),
76 "trying to register input source... [%d]", i);
77 continue;
78 }
79 // future.done
80 auto response = future.get();
81 int state = response->response.state.value;
82 RCLCPP_INFO(this->get_logger(),
83 "Set input source succeeded: state=%d, task_id=%lu", state,
84 response->response.task_id);
85 return true;
86 }
87 RCLCPP_ERROR(this->get_logger(), "Service call failed or timed out");
88 return false;
89 }
90
91 void publish_velocity() {
92 auto msg = std::make_unique<aimdk_msgs::msg::McLocomotionVelocity>();
93 msg->header = aimdk_msgs::msg::MessageHeader();
94 msg->header.stamp = this->now();
95 msg->source = "node"; // Set message source
96 msg->forward_velocity = forward_velocity_;
97 msg->lateral_velocity = lateral_velocity_;
98 msg->angular_velocity = angular_velocity_;
99
100 publisher_->publish(std::move(msg));
101 RCLCPP_INFO(this->get_logger(),
102 "Published velocity: Forward %.2f m/s, Lateral %.2f m/s, "
103 "Angular %.2f rad/s",
104 forward_velocity_, lateral_velocity_, angular_velocity_);
105 }
106
107 void clear_velocity() {
108 forward_velocity_ = 0.0;
109 lateral_velocity_ = 0.0;
110 angular_velocity_ = 0.0;
111 }
112
113 bool set_forward(double forward) {
114 if (abs(forward) < 0.005) {
115 forward_velocity_ = 0.0;
116 return true;
117 } else if ((abs(forward) > max_forward_speed_) ||
118 (abs(forward) < min_forward_speed_)) {
119 RCLCPP_ERROR(this->get_logger(), "input value out of range, exiting");
120 return false;
121 } else {
122 forward_velocity_ = forward;
123 return true;
124 }
125 }
126
127 bool set_lateral(double lateral) {
128 if (abs(lateral) < 0.005) {
129 lateral_velocity_ = 0.0;
130 return true;
131 } else if ((abs(lateral) > max_lateral_speed_) ||
132 (abs(lateral) < min_lateral_speed_)) {
133 RCLCPP_ERROR(this->get_logger(), "input value out of range, exiting");
134 return false;
135 } else {
136 lateral_velocity_ = lateral;
137 return true;
138 }
139 }
140
141 bool set_angular(double angular) {
142 if (abs(angular) < 0.005) {
143 angular_velocity_ = 0.0;
144 return true;
145 } else if ((abs(angular) > max_angular_speed_) ||
146 (abs(angular) < min_angular_speed_)) {
147 RCLCPP_ERROR(this->get_logger(), "input value out of range, exiting");
148 return false;
149 } else {
150 angular_velocity_ = angular;
151 return true;
152 }
153 }
154
155private:
156 rclcpp::Publisher<aimdk_msgs::msg::McLocomotionVelocity>::SharedPtr
157 publisher_;
158 rclcpp::Client<aimdk_msgs::srv::SetMcInputSource>::SharedPtr client_;
159 rclcpp::TimerBase::SharedPtr timer_;
160
161 double forward_velocity_;
162 double lateral_velocity_;
163 double angular_velocity_;
164
165 double max_forward_speed_;
166 double max_lateral_speed_;
167 double max_angular_speed_;
168
169 double min_forward_speed_;
170 double min_lateral_speed_;
171 double min_angular_speed_;
172};
173
174// Signal Processing
175std::shared_ptr<DirectVelocityControl> global_node = nullptr;
176void signal_handler(int sig) {
177 if (global_node) {
178 global_node->clear_velocity();
179 RCLCPP_INFO(global_node->get_logger(),
180 "Received signal %d: clearing velocity and shutting down", sig);
181 }
182 rclcpp::shutdown();
183 exit(sig);
184}
185
186int main(int argc, char *argv[]) {
187 rclcpp::init(argc, argv);
188 signal(SIGINT, signal_handler);
189 signal(SIGTERM, signal_handler);
190
191 global_node = std::make_shared<DirectVelocityControl>();
192 auto node = global_node;
193
194 if (!node->register_input_source()) {
195 RCLCPP_ERROR(node->get_logger(),
196 "Input source registration failed, exiting");
197 global_node.reset();
198 rclcpp::shutdown();
199 return 1;
200 }
201
202 // get and check control values
203 // notice that mc has thresholds to start movement
204 double forward, lateral, angular;
205 std::cout << "Enter forward speed 0 or ±(0.2 ~ 1.0) m/s: ";
206 std::cin >> forward;
207 if (!node->set_forward(forward)) {
208 return 2;
209 }
210 std::cout << "Enter lateral speed 0 or ±(0.2 ~ 1.0) m/s: ";
211 std::cin >> lateral;
212 if (!node->set_lateral(lateral)) {
213 return 2;
214 }
215 std::cout << "Enter angular speed 0 or ±(0.1 ~ 1.0) rad/s: ";
216 std::cin >> angular;
217 if (!node->set_angular(angular)) {
218 return 2;
219 }
220
221 RCLCPP_INFO(node->get_logger(), "Setting velocity; moving for 5 seconds");
222
223 node->start_publish();
224
225 auto start_time = node->now();
226 while ((node->now() - start_time).seconds() < 5.0) {
227 rclcpp::spin_some(node);
228 std::this_thread::sleep_for(std::chrono::milliseconds(1));
229 }
230
231 node->clear_velocity();
232 RCLCPP_INFO(node->get_logger(), "5 seconds elapsed; robot stopped");
233
234 rclcpp::spin(node);
235 rclcpp::shutdown();
236 return 0;
237}
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:
Joint model definition
Trajectory interpolation using Ruckig
Multi-joint coordinated control
Real-time position, velocity, and acceleration control
Dependencies
ROS2
Ruckig library
aimdk_msgs package
Build Instructions
Place the code in the
srcdirectory of your ROS2 workspaceAdd 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
)
Add dependencies in package.xml:
<depend>rclcpp</depend>
<depend>aimdk_msgs</depend>
<depend>ruckig</depend>
Example Function Overview
Four controller nodes are created to control:
Legs × 2 (12 joints)
Waist × 1 (3 joints)
Arms × 2 (14 joints)
Head × 1 (2 joints)
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
Add new control logic:
Modify the
SetTargetPositionfunctionAdd new control callback functions
Adjust control frequency:
Modify the period of
control_timer_(currently 2 ms)
1#include <aimdk_msgs/msg/joint_command_array.hpp>
2#include <aimdk_msgs/msg/joint_state_array.hpp>
3#include <atomic>
4#include <cstdlib>
5#include <memory>
6#include <rclcpp/rclcpp.hpp>
7#include <ruckig/ruckig.hpp>
8#include <signal.h>
9#include <string>
10#include <unordered_map>
11#include <vector>
12
13/**
14 * @brief Global variables and signal handling
15 */
16// Global variables to control program state
17std::atomic<bool> g_running(true);
18std::atomic<bool> g_emergency_stop(false);
19
20// Signal handler function
21void signal_handler(int) {
22 g_running = false;
23 RCLCPP_INFO(rclcpp::get_logger("main"),
24 "Received termination signal, shutting down...");
25}
26
27/**
28 * @brief Robot model definition
29 */
30enum class JointArea {
31 HEAD, // Head joints
32 ARM, // Arm joints
33 WAIST, // Waist joints
34 LEG, // Leg joints
35};
36
37/**
38 * @brief Joint information structure
39 */
40struct JointInfo {
41 std::string name; // Joint name
42 double lower_limit; // Joint angle lower limit
43 double upper_limit; // Joint angle upper limit
44 double kp; // Position control gain
45 double kd; // Velocity control gain
46};
47
48/**
49 * @brief Robot model configuration
50 * Contains parameters for all joints, enabling or disabling specific joints as
51 * needed
52 */
53std::map<JointArea, std::vector<JointInfo>> robot_model = {
54 {JointArea::LEG,
55 {
56 // Left leg joint configuration
57 {"left_hip_pitch_joint", -2.4871, 2.4871, 40.0, 4.0}, // Left hip pitch
58 {"left_hip_roll_joint", -0.12217, 2.9059, 40.0, 4.0}, // Left hip roll
59 {"left_hip_yaw_joint", -1.6842, 3.4296, 30.0, 3.0}, // Left hip yaw
60 {"left_knee_joint", 0.026179, 2.1206, 80.0, 8.0}, // Left knee
61 {"left_ankle_pitch_joint", -0.80285, 0.45378, 40.0,
62 4.0}, // Left ankle pitch
63 {"left_ankle_roll_joint", -0.2618, 0.2618, 20.0,
64 2.0}, // Left ankle roll
65 // Right leg joint configuration
66 {"right_hip_pitch_joint", -2.4871, 2.4871, 40.0,
67 4.0}, // Right hip pitch
68 {"right_hip_roll_joint", -2.9059, 0.12217, 40.0,
69 4.0}, // Right hip roll
70 {"right_hip_yaw_joint", -3.4296, 1.6842, 30.0, 3.0}, // Right hip yaw
71 {"right_knee_joint", 0.026179, 2.1206, 80.0, 8.0}, // Right knee
72 {"right_ankle_pitch_joint", -0.80285, 0.45378, 40.0,
73 4.0}, // Right ankle pitch
74 {"right_ankle_roll_joint", -0.2618, 0.2618, 20.0,
75 2.0}, // Right ankle roll
76 }},
77
78 {JointArea::WAIST,
79 {
80 // Waist joint configuration
81 {"waist_yaw_joint", -3.4296, 2.3824, 20.0, 4.0}, // Waist yaw
82 {"waist_pitch_joint", -0.17453, 0.17453, 20.0, 4.0}, // Waist pitch
83 {"waist_roll_joint", -0.48869, 0.48869, 20.0, 4.0}, // Waist roll
84 }},
85 {JointArea::ARM,
86 {
87 // Left arm joint configuration
88 {"left_shoulder_pitch_joint", -2.5569, 2.5569, 20.0,
89 2.0}, // Left shoulder pitch
90 {"left_shoulder_roll_joint", -0.06108, 3.3598, 20.0,
91 2.0}, // Left shoulder roll
92 {"left_shoulder_yaw_joint", -2.5569, 2.5569, 20.0,
93 2.0}, // Left shoulder yaw
94 {"left_elbow_pitch_joint", -2.4435, 0.0, 20.0,
95 2.0}, // Left elbow pitch
96 {"left_elbow_roll_joint", -2.5569, 2.5569, 20.0,
97 2.0}, // Left elbow roll
98 {"left_wrist_pitch_joint", -0.5585, 0.5585, 20.0,
99 2.0}, // Left wrist pitch
100 {"left_wrist_yaw_joint", -1.5446, 1.5446, 20.0,
101 2.0}, // Left wrist yaw
102 // Right arm joint configuration
103 {"right_shoulder_pitch_joint", -2.5569, 2.5569, 20.0,
104 2.0}, // Right shoulder pitch
105 {"right_shoulder_roll_joint", -3.3598, 0.06108, 20.0,
106 2.0}, // Right shoulder roll
107 {"right_shoulder_yaw_joint", -2.5569, 2.5569, 20.0,
108 2.0}, // Right shoulder yaw
109 {"right_elbow_pitch_joint", 0.0, 2.4435, 20.0,
110 2.0}, // Right elbow pitch
111 {"right_elbow_roll_joint", -2.5569, 2.5569, 20.0,
112 2.0}, // Right elbow roll
113 {"right_wrist_pitch_joint", -0.5585, 0.5585, 20.0,
114 2.0}, // Right wrist pitch
115 {"right_wrist_yaw_joint", -1.5446, 1.5446, 20.0,
116 2.0}, // Right wrist yaw
117 }},
118 {JointArea::HEAD,
119 {
120 // Head joint configuration
121 {"head_pitch_joint", -0.61087, 0.61087, 20.0, 2.0}, // Head pitch
122 {"head_yaw_joint", -0.61087, 0.61087, 20.0, 2.0}, // Head yaw
123 }},
124};
125
126/**
127 * @brief Joint controller node class
128 * @tparam DOFs Degrees of freedom
129 * @tparam Area Joint area
130 */
131template <int DOFs, JointArea Area>
132class JointControllerNode : public rclcpp::Node {
133public:
134 /**
135 * @brief Constructor
136 * @param node_name Node name
137 * @param sub_topic Subscription topic name
138 * @param pub_topic Publication topic name
139 * @param qos QoS configuration
140 */
141 JointControllerNode(std::string node_name, std::string sub_topic,
142 std::string pub_topic,
143 rclcpp::QoS qos = rclcpp::SensorDataQoS())
144 : Node(node_name), ruckig(0.002) {
145 joint_info_ = robot_model[Area];
146 if (joint_info_.size() != DOFs) {
147 RCLCPP_ERROR(this->get_logger(), "Joint count mismatch.");
148 exit(1);
149 }
150
151 // Set motion constraints for Ruckig trajectory planner
152 for (int i = 0; i < DOFs; ++i) {
153 input.max_velocity[i] = 1.0; // Max velocity limit
154 input.max_acceleration[i] = 1.0; // Max acceleration limit
155 input.max_jerk[i] = 25.0; // Max jerk (change of acceleration) limit
156 }
157
158 // Create joint state subscriber
159 sub_ = this->create_subscription<aimdk_msgs::msg::JointStateArray>(
160 sub_topic, qos,
161 std::bind(&JointControllerNode::JointStateCallback, this,
162 std::placeholders::_1));
163
164 // Create joint command publisher
165 pub_ = this->create_publisher<aimdk_msgs::msg::JointCommandArray>(pub_topic,
166 qos);
167 }
168
169private:
170 // Ruckig trajectory planner variables
171 ruckig::Ruckig<DOFs> ruckig; // Trajectory planner instance
172 ruckig::InputParameter<DOFs> input; // Input parameters
173 ruckig::OutputParameter<DOFs> output; // Output parameters
174 bool ruckig_initialized_ = false; // Trajectory planner initialization flag
175 std::vector<JointInfo> joint_info_; // Joint information list
176
177 // ROS communication variables
178 rclcpp::Subscription<aimdk_msgs::msg::JointStateArray>::SharedPtr
179 sub_; // State subscriber
180 rclcpp::Publisher<aimdk_msgs::msg::JointCommandArray>::SharedPtr
181 pub_; // Command publisher
182
183 /**
184 * @brief Joint state callback function
185 * @param msg Joint state message
186 */
187 void
188 JointStateCallback(const aimdk_msgs::msg::JointStateArray::SharedPtr msg) {
189 // Initialize trajectory planner on first state reception
190 if (!ruckig_initialized_) {
191 for (int i = 0; i < DOFs; ++i) {
192 input.current_position[i] = msg->joints[i].position;
193 input.current_velocity[i] = msg->joints[i].velocity;
194 input.current_acceleration[i] = 0.0;
195 }
196 ruckig_initialized_ = true;
197 RCLCPP_INFO(this->get_logger(),
198 "Ruckig trajectory planner initialization complete");
199 }
200 }
201
202public:
203 /**
204 * @brief Set target joint position
205 * @param joint_name Joint name
206 * @param target_position Target position
207 * @return Whether the target position was successfully set
208 */
209 bool SetTargetPosition(std::string joint_name, double target_position) {
210 if (!ruckig_initialized_) {
211 RCLCPP_WARN(this->get_logger(),
212 "Ruckig trajectory planner not initialized");
213 return false;
214 }
215
216 // Find target joint and set its position
217 int target_joint = -1;
218 for (int i = 0; i < DOFs; ++i) {
219 if (joint_info_[i].name == joint_name) {
220 // Check if target position is within limits
221 if (target_position < joint_info_[i].lower_limit ||
222 target_position > joint_info_[i].upper_limit) {
223 RCLCPP_ERROR(
224 this->get_logger(),
225 "Target position %.3f exceeds limit for joint %s [%.3f, %.3f]",
226 target_position, joint_name.c_str(), joint_info_[i].lower_limit,
227 joint_info_[i].upper_limit);
228 return false;
229 }
230 input.target_position[i] = target_position;
231 input.target_velocity[i] = 0.0;
232 input.target_acceleration[i] = 0.0;
233 target_joint = i;
234 } else {
235 input.target_position[i] = input.current_position[i];
236 input.target_velocity[i] = 0.0;
237 input.target_acceleration[i] = 0.0;
238 }
239 }
240
241 if (target_joint == -1) {
242 RCLCPP_ERROR(this->get_logger(), "Joint %s not found",
243 joint_name.c_str());
244 return false;
245 }
246
247 // Perform trajectory planning and send command using Ruckig
248 const double tolerance = 1e-6;
249 while (g_running && rclcpp::ok() && !g_emergency_stop) {
250 auto result = ruckig.update(input, output);
251 if (result != ruckig::Result::Working &&
252 result != ruckig::Result::Finished) {
253 RCLCPP_WARN(this->get_logger(), "Trajectory planning failed");
254 break;
255 }
256
257 // Update current state
258 for (int i = 0; i < DOFs; ++i) {
259 input.current_position[i] = output.new_position[i];
260 input.current_velocity[i] = output.new_velocity[i];
261 input.current_acceleration[i] = output.new_acceleration[i];
262 }
263
264 // Check if target position is reached
265 if (std::abs(output.new_position[target_joint] - target_position) <
266 tolerance) {
267 RCLCPP_INFO(this->get_logger(), "Joint %s reached target position",
268 joint_name.c_str());
269 break;
270 }
271
272 // Create and send joint command
273 aimdk_msgs::msg::JointCommandArray cmd;
274 cmd.joints.resize(DOFs);
275 for (int i = 0; i < DOFs; ++i) {
276 auto &joint = joint_info_[i];
277 cmd.joints[i].name = joint.name;
278 cmd.joints[i].position = output.new_position[i];
279 cmd.joints[i].velocity = output.new_velocity[i];
280 cmd.joints[i].stiffness = joint.kp;
281 cmd.joints[i].damping = joint.kd;
282 }
283 pub_->publish(cmd);
284
285 // Short delay to avoid excessive CPU usage
286 std::this_thread::sleep_for(std::chrono::milliseconds(2));
287 }
288
289 return true;
290 }
291
292 /**
293 * @brief Safely stop all joints
294 */
295 void safe_stop() {
296 if (!ruckig_initialized_) {
297 RCLCPP_WARN(this->get_logger(), "Ruckig trajectory planner not "
298 "initialized, cannot perform safe stop");
299 return;
300 }
301
302 RCLCPP_INFO(this->get_logger(), "Performing safe stop...");
303
304 // Set all joint target positions to current positions
305 for (int i = 0; i < DOFs; ++i) {
306 input.target_position[i] = input.current_position[i];
307 input.target_velocity[i] = 0.0;
308 input.target_acceleration[i] = 0.0;
309 }
310
311 // Send final command to ensure joints stop
312 aimdk_msgs::msg::JointCommandArray cmd;
313 cmd.joints.resize(DOFs);
314 for (int i = 0; i < DOFs; ++i) {
315 auto &joint = joint_info_[i];
316 cmd.joints[i].name = joint.name;
317 cmd.joints[i].position = input.current_position[i];
318 cmd.joints[i].velocity = 0.0;
319 cmd.joints[i].stiffness = joint.kp;
320 cmd.joints[i].damping = joint.kd;
321 }
322 pub_->publish(cmd);
323
324 RCLCPP_INFO(this->get_logger(), "Safe stop complete");
325 }
326
327 /**
328 * @brief Emergency stop for all joints
329 */
330 void emergency_stop() {
331 g_emergency_stop = true;
332 safe_stop();
333 RCLCPP_ERROR(this->get_logger(), "Emergency stop triggered");
334 }
335};
336
337/**
338 * @brief Main function
339 */
340int main(int argc, char *argv[]) {
341 rclcpp::init(argc, argv);
342
343 // Set up signal handling
344 signal(SIGINT, signal_handler);
345 signal(SIGTERM, signal_handler);
346
347 try {
348 // Create leg controller node
349 auto leg_node = std::make_shared<JointControllerNode<12, JointArea::LEG>>(
350 "leg_node", "/aima/hal/joint/leg/state", "/aima/hal/joint/leg/command");
351
352 // Create timer node
353 rclcpp::Node::SharedPtr timer_node =
354 rclcpp::Node::make_shared("timer_node");
355 double position = 0.8;
356
357 // Create timer callback function
358 auto timer = timer_node->create_wall_timer(std::chrono::seconds(3), [&]() {
359 if (!g_running || g_emergency_stop)
360 return; // If the program is shutting down or emergency stopped, do not
361 // execute new actions
362 position = -position;
363 position = 1.3 + position;
364 if (!leg_node->SetTargetPosition("left_knee_joint", position)) {
365 RCLCPP_ERROR(rclcpp::get_logger("main"),
366 "Failed to set target position");
367 }
368 });
369
370 // Create executor
371 rclcpp::executors::MultiThreadedExecutor executor;
372 executor.add_node(leg_node);
373 executor.add_node(timer_node);
374
375 // Main loop
376 while (g_running && rclcpp::ok() && !g_emergency_stop) {
377 executor.spin_once(std::chrono::milliseconds(100));
378 }
379
380 // Safely stop all joints
381 RCLCPP_INFO(rclcpp::get_logger("main"), "Safely stopping all joints...");
382 leg_node->safe_stop();
383
384 // Wait a short time to ensure command transmission is complete
385 std::this_thread::sleep_for(std::chrono::milliseconds(100));
386
387 // Clean up resources
388 RCLCPP_INFO(rclcpp::get_logger("main"), "Cleaning up resources...");
389 leg_node.reset();
390 timer_node.reset();
391
392 } catch (const std::exception &e) {
393 RCLCPP_ERROR(rclcpp::get_logger("main"), "Exception occurred: %s",
394 e.what());
395 g_emergency_stop = true;
396 } catch (...) {
397 RCLCPP_ERROR(rclcpp::get_logger("main"), "Unknown exception occurred");
398 g_emergency_stop = true;
399 }
400
401 RCLCPP_INFO(rclcpp::get_logger("main"), "Program exited safely");
402 rclcpp::shutdown();
403 return 0;
404}
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
1#include "aimdk_msgs/msg/common_request.hpp"
2#include "aimdk_msgs/msg/common_response.hpp"
3#include "aimdk_msgs/msg/common_state.hpp"
4#include "aimdk_msgs/msg/common_task_response.hpp"
5#include "aimdk_msgs/msg/mc_input_action.hpp"
6#include "aimdk_msgs/msg/mc_locomotion_velocity.hpp"
7#include "aimdk_msgs/msg/message_header.hpp"
8#include "aimdk_msgs/srv/set_mc_input_source.hpp"
9
10#include <algorithm>
11#include <chrono>
12#include <csignal>
13#include <curses.h>
14#include <rclcpp/rclcpp.hpp>
15
16using aimdk_msgs::msg::McLocomotionVelocity;
17using std::placeholders::_1;
18
19class KeyboardVelocityController : public rclcpp::Node {
20public:
21 KeyboardVelocityController()
22 : Node("keyboard_velocity_controller"), forward_velocity_(0.0),
23 lateral_velocity_(0.0), angular_velocity_(0.0), step_(0.2),
24 angular_step_(0.1) {
25 pub_ = this->create_publisher<McLocomotionVelocity>(
26 "/aima/mc/locomotion/velocity", 10);
27 client_ = this->create_client<aimdk_msgs::srv::SetMcInputSource>(
28 "/aimdk_5Fmsgs/srv/SetMcInputSource");
29 // Register input source
30 if (!register_input_source()) {
31 RCLCPP_ERROR(this->get_logger(),
32 "Input source registration failed, exiting");
33 throw std::runtime_error("Input source registration failed");
34 }
35 // Initialize ncurses
36 initscr();
37 cbreak();
38 noecho();
39 keypad(stdscr, TRUE);
40 nodelay(stdscr, TRUE);
41
42 timer_ = this->create_wall_timer(
43 std::chrono::milliseconds(50),
44 std::bind(&KeyboardVelocityController::checkKeyAndPublish, this));
45
46 RCLCPP_INFO(this->get_logger(),
47 "Control started: W/S Forward/Backward | A/D Strafe Left/Right "
48 "| Q/E Turn Left/Right | Space Stop | ESC Exit");
49 }
50
51 ~KeyboardVelocityController() {
52 endwin(); // Restore terminal
53 }
54
55private:
56 rclcpp::Publisher<McLocomotionVelocity>::SharedPtr pub_;
57 rclcpp::Client<aimdk_msgs::srv::SetMcInputSource>::SharedPtr client_;
58 rclcpp::TimerBase::SharedPtr timer_;
59
60 float forward_velocity_, lateral_velocity_, angular_velocity_;
61 const float step_, angular_step_;
62
63 bool register_input_source() {
64 const std::chrono::seconds srv_timeout(8);
65 auto start_time = std::chrono::steady_clock::now();
66 while (!client_->wait_for_service(std::chrono::seconds(2))) {
67 if (std::chrono::steady_clock::now() - start_time > srv_timeout) {
68 RCLCPP_ERROR(this->get_logger(), "Waiting for service timed out");
69 return false;
70 }
71 RCLCPP_INFO(this->get_logger(), "Waiting for input source service...");
72 }
73
74 auto request =
75 std::make_shared<aimdk_msgs::srv::SetMcInputSource::Request>();
76 request->action.value = 1001;
77 request->input_source.name = "node";
78 request->input_source.priority = 40;
79 request->input_source.timeout = 1000;
80
81 auto timeout = std::chrono::milliseconds(250);
82
83 for (int i = 0; i < 8; i++) {
84 request->request.header.stamp = this->now();
85 auto future = client_->async_send_request(request);
86 auto retcode = rclcpp::spin_until_future_complete(
87 this->get_node_base_interface(), future, timeout);
88 if (retcode != rclcpp::FutureReturnCode::SUCCESS) {
89 // retry as remote peer is NOT handled well by ROS
90 RCLCPP_INFO(this->get_logger(),
91 "trying to register input source... [%d]", i);
92 continue;
93 }
94 // future.done
95 auto response = future.get();
96 int state = response->response.state.value;
97 RCLCPP_INFO(this->get_logger(),
98 "Set input source succeeded: state=%d, task_id=%lu", state,
99 response->response.task_id);
100 return true;
101 }
102 RCLCPP_ERROR(this->get_logger(), "Service call failed or timed out");
103 return false;
104 }
105
106 void checkKeyAndPublish() {
107 int ch = getch(); // non-blocking read
108
109 switch (ch) {
110 case ' ': // Space key
111 forward_velocity_ = 0.0;
112 lateral_velocity_ = 0.0;
113 angular_velocity_ = 0.0;
114 break;
115 case 'w':
116 forward_velocity_ = std::min(forward_velocity_ + step_, 1.0f);
117 break;
118 case 's':
119 forward_velocity_ = std::max(forward_velocity_ - step_, -1.0f);
120 break;
121 case 'a':
122 lateral_velocity_ = std::min(lateral_velocity_ + step_, 1.0f);
123 break;
124 case 'd':
125 lateral_velocity_ = std::max(lateral_velocity_ - step_, -1.0f);
126 break;
127 case 'q':
128 angular_velocity_ = std::min(angular_velocity_ + angular_step_, 1.0f);
129 break;
130 case 'e':
131 angular_velocity_ = std::max(angular_velocity_ - angular_step_, -1.0f);
132 break;
133 case 27: // ESC Key
134 RCLCPP_INFO(this->get_logger(), "Exiting control");
135 rclcpp::shutdown();
136 return;
137 }
138
139 auto msg = std::make_unique<McLocomotionVelocity>();
140 msg->header = aimdk_msgs::msg::MessageHeader();
141 msg->header.stamp = this->now();
142 msg->source = "node";
143 msg->forward_velocity = forward_velocity_;
144 msg->lateral_velocity = lateral_velocity_;
145 msg->angular_velocity = angular_velocity_;
146
147 float fwd = forward_velocity_;
148 float lat = lateral_velocity_;
149 float ang = angular_velocity_;
150
151 pub_->publish(std::move(msg));
152
153 // Screen Output
154 clear();
155 mvprintw(0, 0,
156 "W/S: Forward/Backward | A/D: Left/Right Strafe | Q/E: Turn "
157 "Left/Right | Space: Stop | ESC: Exit");
158 mvprintw(2, 0,
159 "Speed Status: Forward: %.2f m/s | Lateral: %.2f m/s | Angular: "
160 "%.2f rad/s",
161 fwd, lat, ang);
162 refresh();
163 }
164};
165
166int main(int argc, char *argv[]) {
167 rclcpp::init(argc, argv);
168 try {
169 auto node = std::make_shared<KeyboardVelocityController>();
170 rclcpp::spin(node);
171 } catch (const std::exception &e) {
172 RCLCPP_FATAL(rclcpp::get_logger("main"),
173 "Program exited with exception: %s", e.what());
174 }
175 rclcpp::shutdown();
176 return 0;
177}
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.
1#include <chrono>
2#include <cv_bridge/cv_bridge.h>
3#include <filesystem>
4#include <opencv2/opencv.hpp>
5#include <rclcpp/rclcpp.hpp>
6#include <sensor_msgs/msg/image.hpp>
7#include <string>
8
9class SaveOneRaw : public rclcpp::Node {
10public:
11 SaveOneRaw() : Node("save_one_image"), saved_(false) {
12 topic_ = declare_parameter<std::string>(
13 "image_topic", "/aima/hal/sensor/stereo_head_front_left/rgb_image");
14
15 std::filesystem::create_directories("images");
16
17 auto qos = rclcpp::SensorDataQoS(); // BestEffort/Volatile
18 sub_ = create_subscription<sensor_msgs::msg::Image>(
19 topic_, qos, std::bind(&SaveOneRaw::cb, this, std::placeholders::_1));
20
21 RCLCPP_INFO(get_logger(), "Subscribing (raw): %s", topic_.c_str());
22 }
23
24private:
25 void cb(const sensor_msgs::msg::Image::SharedPtr msg) {
26 if (saved_)
27 return;
28
29 try {
30 // Obtain the Mat without copying by not specifying encoding
31 cv_bridge::CvImageConstPtr cvp = cv_bridge::toCvShare(msg);
32 cv::Mat img = cvp->image;
33
34 // Convert to BGR for uniform saving
35 if (msg->encoding == "rgb8") {
36 cv::cvtColor(img, img, cv::COLOR_RGB2BGR);
37 } else if (msg->encoding == "mono8") {
38 cv::cvtColor(img, img, cv::COLOR_GRAY2BGR);
39 } // bgr8 Use this directly; add more branches as needed to support
40 // additional encodings.
41
42 auto now = std::chrono::system_clock::now();
43 auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(
44 now.time_since_epoch())
45 .count();
46 std::string path = "images/frame_" + std::to_string(ms) + ".png";
47
48 if (cv::imwrite(path, img)) {
49 RCLCPP_INFO(get_logger(), "Saved: %s (%dx%d)", path.c_str(), img.cols,
50 img.rows);
51 saved_ = true;
52 rclcpp::shutdown();
53 } else {
54 RCLCPP_ERROR(get_logger(), "cv::imwrite failed: %s", path.c_str());
55 }
56 } catch (const std::exception &e) {
57 RCLCPP_ERROR(get_logger(), "raw decode failed: %s", e.what());
58 // Do not set the saved flag; wait for the next frame
59 }
60 }
61
62 std::string topic_;
63 bool saved_;
64 rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_;
65};
66
67int main(int argc, char **argv) {
68 rclcpp::init(argc, argv);
69 rclcpp::spin(std::make_shared<SaveOneRaw>());
70 return 0;
71}
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)
1#include <deque>
2#include <iomanip>
3#include <memory>
4#include <rclcpp/rclcpp.hpp>
5#include <sensor_msgs/msg/camera_info.hpp>
6#include <sensor_msgs/msg/compressed_image.hpp>
7#include <sensor_msgs/msg/image.hpp>
8#include <sensor_msgs/msg/point_cloud2.hpp>
9#include <sstream>
10#include <string>
11#include <vector>
12
13// OpenCV headers for image/video writing
14#include <cv_bridge/cv_bridge.h>
15#include <opencv2/opencv.hpp>
16
17/**
18 * @brief Example of subscribing to multiple topics for the head depth camera
19 *
20 * You can select which topic type to subscribe to via the startup argument
21 * --ros-args -p topic_type:=<type>:
22 * - depth_pointcloud: Depth point cloud (sensor_msgs/PointCloud2)
23 * - depth_image: Depth image (sensor_msgs/Image)
24 * - rgb_image: RGB image (sensor_msgs/Image)
25 * - rgb_image_compressed: RGB compressed image (sensor_msgs/CompressedImage)
26 * - rgb_camera_info: Camera intrinsic parameters (sensor_msgs/CameraInfo)
27 * - depth_camera_info: Camera intrinsic parameters (sensor_msgs/CameraInfo)
28 *
29 * Examples:
30 * ros2 run examples echo_camera_rgbd --ros-args -p
31 * topic_type:=depth_pointcloud ros2 run examples echo_camera_rgbd --ros-args -p
32 * topic_type:=rgb_image ros2 run examples echo_camera_rgbd --ros-args -p
33 * topic_type:=rgb_camera_info
34 *
35 * topic_type defaults to "rgb_image"
36 *
37 * See individual callbacks for more detailed comments
38 */
39class CameraTopicEcho : public rclcpp::Node {
40public:
41 CameraTopicEcho() : Node("camera_topic_echo") {
42 // Select which topic type to subscribe to
43 topic_type_ = declare_parameter<std::string>("topic_type", "rgb_image");
44 dump_video_path_ = declare_parameter<std::string>("dump_video_path", "");
45
46 // Subscribed topics and their message layouts
47 // 1. /aima/hal/sensor/rgbd_head_front/depth_pointcloud
48 // - topic_type: depth_pointcloud
49 // - message type: sensor_msgs::msg::PointCloud2
50 // - frame_id: rgbd_head_front
51 // - child_frame_id: /
52 // - contents: depth point cloud
53 // 2. /aima/hal/sensor/rgbd_head_front/depth_image
54 // - topic_type: depth_image
55 // - message type: sensor_msgs::msg::Image
56 // - frame_id: rgbd_head_front
57 // - contents: depth image
58 // 3. /aima/hal/sensor/rgbd_head_front/rgb_image
59 // - topic_type: rgb_image
60 // - message type: sensor_msgs::msg::Image
61 // - frame_id: rgbd_head_front
62 // - contents: RGB image
63 // 4. /aima/hal/sensor/rgbd_head_front/rgb_image/compressed
64 // - topic_type: rgb_image_compressed
65 // - message type: sensor_msgs::msg::CompressedImage
66 // - frame_id: rgbd_head_front
67 // - contents: RGB compressed image
68 // 5. /aima/hal/sensor/rgbd_head_front/rgb_camera_info
69 // - topic_type: camera_info
70 // - message type: sensor_msgs::msg::CameraInfo
71 // - frame_id: rgbd_head_front
72 // - contents: RGB camera intrinsic parameters
73 // 6. /aima/hal/sensor/rgbd_head_front/depth_camera_info
74 // - topic_type: camera_info
75 // - message type: sensor_msgs::msg::CameraInfo
76 // - frame_id: rgbd_head_front
77 // - contents: RGB camera intrinsic parameters
78
79 auto qos = rclcpp::SensorDataQoS();
80
81 // Enable depth pointcloud subscription
82 if (topic_type_ == "depth_pointcloud") {
83 topic_name_ = "/aima/hal/sensor/rgbd_head_front/depth_pointcloud";
84 sub_pointcloud_ = create_subscription<sensor_msgs::msg::PointCloud2>(
85 topic_name_, qos,
86 std::bind(&CameraTopicEcho::cb_pointcloud, this,
87 std::placeholders::_1));
88 RCLCPP_INFO(get_logger(), "✅ Subscribing PointCloud2: %s",
89 topic_name_.c_str());
90
91 // Enable depth image subscription
92 } else if (topic_type_ == "depth_image") {
93 topic_name_ = "/aima/hal/sensor/rgbd_head_front/depth_image";
94 sub_image_ = create_subscription<sensor_msgs::msg::Image>(
95 topic_name_, qos,
96 std::bind(&CameraTopicEcho::cb_image, this, std::placeholders::_1));
97 RCLCPP_INFO(get_logger(), "✅ Subscribing Depth Image: %s",
98 topic_name_.c_str());
99
100 // Enable RGB image subscription
101 } else if (topic_type_ == "rgb_image") {
102 topic_name_ = "/aima/hal/sensor/rgbd_head_front/rgb_image";
103 sub_image_ = create_subscription<sensor_msgs::msg::Image>(
104 topic_name_, qos,
105 std::bind(&CameraTopicEcho::cb_image, this, std::placeholders::_1));
106 RCLCPP_INFO(get_logger(), "✅ Subscribing RGB Image: %s",
107 topic_name_.c_str());
108 if (!dump_video_path_.empty()) {
109 RCLCPP_INFO(get_logger(), "📝 Will dump received images to video: %s",
110 dump_video_path_.c_str());
111 }
112
113 // Enable RGB compressed image subscription
114 } else if (topic_type_ == "rgb_image_compressed") {
115 topic_name_ = "/aima/hal/sensor/rgbd_head_front/rgb_image/compressed";
116 sub_compressed_ = create_subscription<sensor_msgs::msg::CompressedImage>(
117 topic_name_, qos,
118 std::bind(&CameraTopicEcho::cb_compressed, this,
119 std::placeholders::_1));
120 RCLCPP_INFO(get_logger(), "✅ Subscribing CompressedImage: %s",
121 topic_name_.c_str());
122
123 // Enable rgb camera info subscription
124 } else if (topic_type_ == "rgb_camera_info") {
125 topic_name_ = "/aima/hal/sensor/rgbd_head_front/rgb_camera_info";
126 // RGB-D CameraInfo subscriptions is different with other cameras.
127 // The messages arrive in about 10Hz and SensorDataQoS is enough.
128 sub_camerainfo_ = create_subscription<sensor_msgs::msg::CameraInfo>(
129 topic_name_, qos,
130 std::bind(&CameraTopicEcho::cb_camerainfo, this,
131 std::placeholders::_1));
132 RCLCPP_INFO(get_logger(), "✅ Subscribing RGB CameraInfo: %s",
133 topic_name_.c_str());
134
135 // Enable depth camera info subscription
136 } else if (topic_type_ == "depth_camera_info") {
137 topic_name_ = "/aima/hal/sensor/rgbd_head_front/depth_camera_info";
138 // RGB-D CameraInfo subscriptions is different with other cameras.
139 // The messages arrive in about 10Hz and SensorDataQoS is enough.
140 sub_camerainfo_ = create_subscription<sensor_msgs::msg::CameraInfo>(
141 topic_name_, qos,
142 std::bind(&CameraTopicEcho::cb_camerainfo, this,
143 std::placeholders::_1));
144 RCLCPP_INFO(get_logger(), "✅ Subscribing Depth CameraInfo: %s",
145 topic_name_.c_str());
146
147 // Unknown topic_type error
148 } else {
149 RCLCPP_ERROR(get_logger(), "Unknown topic_type: %s", topic_type_.c_str());
150 throw std::runtime_error("Unknown topic_type");
151 }
152 }
153
154 ~CameraTopicEcho() override {
155 if (video_writer_.isOpened()) {
156 video_writer_.release();
157 RCLCPP_INFO(get_logger(), "Video file closed.");
158 }
159 }
160
161private:
162 // PointCloud2 callback
163 void cb_pointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
164 // Update arrivals (for FPS calculation)
165 update_arrivals();
166
167 // Print point cloud basic info
168 if (should_print()) {
169 std::ostringstream oss;
170 oss << "🌫️ PointCloud2 received\n"
171 << " • frame_id: " << msg->header.frame_id << "\n"
172 << " • stamp (sec): "
173 << rclcpp::Time(msg->header.stamp).seconds() << "\n"
174 << " • width x height: " << msg->width << " x " << msg->height
175 << "\n"
176 << " • point_step: " << msg->point_step << "\n"
177 << " • row_step: " << msg->row_step << "\n"
178 << " • fields: ";
179 for (const auto &f : msg->fields)
180 oss << f.name << "(" << (int)f.datatype << ") ";
181 oss << "\n • is_bigendian: " << msg->is_bigendian
182 << "\n • is_dense: " << msg->is_dense
183 << "\n • data size: " << msg->data.size()
184 << "\n • recv FPS (1s): " << get_fps();
185 RCLCPP_INFO(get_logger(), "%s", oss.str().c_str());
186 }
187 }
188
189 // Image callback (depth/RGB image)
190 void cb_image(const sensor_msgs::msg::Image::SharedPtr msg) {
191 update_arrivals();
192
193 if (should_print()) {
194 RCLCPP_INFO(get_logger(),
195 "📸 %s received\n"
196 " • frame_id: %s\n"
197 " • stamp (sec): %.6f\n"
198 " • encoding: %s\n"
199 " • size (WxH): %u x %u\n"
200 " • step (bytes/row):%u\n"
201 " • is_bigendian: %u\n"
202 " • recv FPS (1s): %.1f",
203 topic_type_.c_str(), msg->header.frame_id.c_str(),
204 rclcpp::Time(msg->header.stamp).seconds(),
205 msg->encoding.c_str(), msg->width, msg->height, msg->step,
206 msg->is_bigendian, get_fps());
207 }
208
209 // Video dump is supported only for RGB images
210 if (topic_type_ == "rgb_image" && !dump_video_path_.empty()) {
211 dump_image_to_video(msg);
212 }
213 }
214
215 // CompressedImage callback
216 void cb_compressed(const sensor_msgs::msg::CompressedImage::SharedPtr msg) {
217 update_arrivals();
218
219 if (should_print()) {
220 RCLCPP_INFO(get_logger(),
221 "🗜️ CompressedImage received\n"
222 " • frame_id: %s\n"
223 " • stamp (sec): %.6f\n"
224 " • format: %s\n"
225 " • data size: %zu\n"
226 " • recv FPS (1s): %.1f",
227 msg->header.frame_id.c_str(),
228 rclcpp::Time(msg->header.stamp).seconds(),
229 msg->format.c_str(), msg->data.size(), get_fps());
230 }
231 }
232
233 // CameraInfo callback (camera intrinsic parameters)
234 void cb_camerainfo(const sensor_msgs::msg::CameraInfo::SharedPtr msg) {
235 // CameraInfo is typically published once; print it once
236 std::ostringstream oss;
237 oss << "📷 " << topic_type_ << " received\n"
238 << " • frame_id: " << msg->header.frame_id << "\n"
239 << " • stamp (sec): " << rclcpp::Time(msg->header.stamp).seconds()
240 << "\n"
241 << " • width x height: " << msg->width << " x " << msg->height << "\n"
242 << " • distortion_model:" << msg->distortion_model << "\n"
243 << " • D: [";
244 for (size_t i = 0; i < msg->d.size(); ++i) {
245 oss << msg->d[i];
246 if (i + 1 < msg->d.size())
247 oss << ", ";
248 }
249 oss << "]\n • K: [";
250 for (int i = 0; i < 9; ++i) {
251 oss << msg->k[i];
252 if (i + 1 < 9)
253 oss << ", ";
254 }
255 oss << "]\n • R: [";
256 for (int i = 0; i < 9; ++i) {
257 oss << msg->r[i];
258 if (i + 1 < 9)
259 oss << ", ";
260 }
261 oss << "]\n • P: [";
262 for (int i = 0; i < 12; ++i) {
263 oss << msg->p[i];
264 if (i + 1 < 12)
265 oss << ", ";
266 }
267 oss << "]\n"
268 << " • binning_x: " << msg->binning_x << "\n"
269 << " • binning_y: " << msg->binning_y << "\n"
270 << " • roi: { x_offset: " << msg->roi.x_offset
271 << ", y_offset: " << msg->roi.y_offset
272 << ", height: " << msg->roi.height << ", width: " << msg->roi.width
273 << ", do_rectify: " << (msg->roi.do_rectify ? "true" : "false") << " }";
274 RCLCPP_INFO(get_logger(), "%s", oss.str().c_str());
275 }
276
277 // Track arrival timestamps to compute FPS
278 void update_arrivals() {
279 const rclcpp::Time now = this->get_clock()->now();
280 arrivals_.push_back(now);
281 while (!arrivals_.empty() && (now - arrivals_.front()).seconds() > 1.0) {
282 arrivals_.pop_front();
283 }
284 }
285 double get_fps() const { return static_cast<double>(arrivals_.size()); }
286
287 // Control printing frequency
288 bool should_print() {
289 const rclcpp::Time now = this->get_clock()->now();
290 if ((now - last_print_).seconds() >= 1.0) {
291 last_print_ = now;
292 return true;
293 }
294 return false;
295 }
296
297 // Dump received images to a video file (RGB images only)
298 void dump_image_to_video(const sensor_msgs::msg::Image::SharedPtr &msg) {
299 cv::Mat image;
300 try {
301 // Obtain the Mat without copying by not specifying encoding
302 cv_bridge::CvImageConstPtr cvp = cv_bridge::toCvShare(msg);
303 image = cvp->image;
304 // Convert to BGR for uniform saving
305 if (msg->encoding == "rgb8") {
306 cv::cvtColor(image, image, cv::COLOR_RGB2BGR);
307 } else {
308 RCLCPP_WARN(get_logger(), "image encoding not expected: %s",
309 msg->encoding.c_str());
310 return;
311 }
312 } catch (const std::exception &e) {
313 RCLCPP_WARN(get_logger(), "cv_bridge exception: %s", e.what());
314 return;
315 }
316
317 // Initialize VideoWriter
318 if (!video_writer_.isOpened()) {
319 int fourcc = cv::VideoWriter::fourcc('M', 'J', 'P', 'G');
320 double fps = std::max(1.0, get_fps());
321 bool ok = video_writer_.open(dump_video_path_, fourcc, fps,
322 cv::Size(image.cols, image.rows), true);
323 if (!ok) {
324 RCLCPP_ERROR(get_logger(), "Failed to open video file: %s",
325 dump_video_path_.c_str());
326 dump_video_path_.clear(); // stop trying
327 return;
328 }
329 RCLCPP_INFO(get_logger(), "VideoWriter started: %s, size=%dx%d, fps=%.1f",
330 dump_video_path_.c_str(), image.cols, image.rows, fps);
331 }
332 video_writer_.write(image);
333 }
334
335 // Member variables
336 std::string topic_type_;
337 std::string topic_name_;
338 std::string dump_video_path_;
339
340 // Subscriptions
341 rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_image_;
342 rclcpp::Subscription<sensor_msgs::msg::CompressedImage>::SharedPtr
343 sub_compressed_;
344 rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr sub_camerainfo_;
345 rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr
346 sub_pointcloud_;
347
348 // FPS statistics
349 rclcpp::Time last_print_{0, 0, RCL_ROS_TIME};
350 std::deque<rclcpp::Time> arrivals_;
351
352 // Video writer
353 cv::VideoWriter video_writer_;
354};
355
356int main(int argc, char **argv) {
357 rclcpp::init(argc, argv);
358 auto node = std::make_shared<CameraTopicEcho>();
359 rclcpp::spin(node);
360 rclcpp::shutdown();
361 return 0;
362}
Usage Instructions:
Subscribe to depth point cloud data:
ros2 run examples echo_camera_rgbd --ros-args -p topic_type:=depth_pointcloud
Subscribe to RGB image data:
ros2 run examples echo_camera_rgbd --ros-args -p topic_type:=rgb_image
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
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)
1#include <deque>
2#include <iomanip>
3#include <memory>
4#include <rclcpp/rclcpp.hpp>
5#include <sensor_msgs/msg/camera_info.hpp>
6#include <sensor_msgs/msg/compressed_image.hpp>
7#include <sensor_msgs/msg/image.hpp>
8#include <sstream>
9#include <string>
10#include <vector>
11
12// OpenCV headers for image/video writing
13#include <cv_bridge/cv_bridge.h>
14#include <opencv2/opencv.hpp>
15
16/**
17 * @brief Example of subscribing to multiple topics for the stereo head camera
18 *
19 * You can select which topic type to subscribe to via the startup argument
20 * --ros-args -p topic_type:=<type>:
21 * - left_rgb_image: left camera RGB image (sensor_msgs/Image)
22 * - left_rgb_image_compressed: left camera RGB compressed image
23 * (sensor_msgs/CompressedImage)
24 * - left_camera_info: left camera intrinsic parameters
25 * (sensor_msgs/CameraInfo)
26 * - right_rgb_image: right camera RGB image (sensor_msgs/Image)
27 * - right_rgb_image_compressed: right camera RGB compressed image
28 * (sensor_msgs/CompressedImage)
29 * - right_camera_info: right camera intrinsic parameters
30 * (sensor_msgs/CameraInfo)
31 *
32 * Examples:
33 * ros2 run examples echo_camera_stereo --ros-args -p
34 * topic_type:=left_rgb_image ros2 run examples echo_camera_stereo --ros-args -p
35 * topic_type:=right_rgb_image ros2 run examples echo_camera_stereo --ros-args
36 * -p topic_type:=left_camera_info
37 *
38 * topic_type defaults to "left_rgb_image"
39 *
40 * See individual callbacks for more detailed comments
41 */
42class StereoCameraTopicEcho : public rclcpp::Node {
43public:
44 StereoCameraTopicEcho() : Node("stereo_camera_topic_echo") {
45 // Select which topic type to subscribe to
46 topic_type_ =
47 declare_parameter<std::string>("topic_type", "left_rgb_image");
48 dump_video_path_ = declare_parameter<std::string>("dump_video_path", "");
49
50 // Subscribed topics and their message layouts
51 // 1. /aima/hal/sensor/stereo_head_front_left/rgb_image
52 // - topic_type: left_rgb_image
53 // - message type: sensor_msgs::msg::Image
54 // - frame_id: stereo_head_front
55 // - child_frame_id: /
56 // - contents: left camera raw image
57 // 2. /aima/hal/sensor/stereo_head_front_left/rgb_image/compressed
58 // - topic_type: left_rgb_image_compressed
59 // - message type: sensor_msgs::msg::CompressedImage
60 // - frame_id: stereo_head_front
61 // - contents: left camera compressed image
62 // 3. /aima/hal/sensor/stereo_head_front_left/camera_info
63 // - topic_type: left_camera_info
64 // - message type: sensor_msgs::msg::CameraInfo
65 // - frame_id: stereo_head_front
66 // - contents: left camera intrinsic parameters
67 // 4. /aima/hal/sensor/stereo_head_front_right/rgb_image
68 // - topic_type: right_rgb_image
69 // - message type: sensor_msgs::msg::Image
70 // - frame_id: stereo_head_front_right
71 // - child_frame_id: /
72 // - contents: right camera raw image
73 // 5. /aima/hal/sensor/stereo_head_front_right/rgb_image/compressed
74 // - topic_type: right_rgb_image_compressed
75 // - message type: sensor_msgs::msg::CompressedImage
76 // - frame_id: stereo_head_front_right
77 // - contents: right camera compressed image
78 // 6. /aima/hal/sensor/stereo_head_front_right/camera_info
79 // - topic_type: right_camera_info
80 // - message type: sensor_msgs::msg::CameraInfo
81 // - frame_id: stereo_head_front_right
82 // - contents: right camera intrinsic parameters
83
84 // Set QoS parameters - use SensorData QoS
85 auto qos = rclcpp::SensorDataQoS();
86
87 // Enable left camera RGB image subscription
88 if (topic_type_ == "left_rgb_image") {
89 topic_name_ = "/aima/hal/sensor/stereo_head_front_left/rgb_image";
90 sub_image_ = create_subscription<sensor_msgs::msg::Image>(
91 topic_name_, qos,
92 std::bind(&StereoCameraTopicEcho::cb_image, this,
93 std::placeholders::_1));
94 RCLCPP_INFO(get_logger(), "✅ Subscribing Left RGB Image: %s",
95 topic_name_.c_str());
96 if (!dump_video_path_.empty()) {
97 RCLCPP_INFO(get_logger(), "📝 Will dump received images to video: %s",
98 dump_video_path_.c_str());
99 }
100
101 // Enable left camera RGB compressed image subscription
102 } else if (topic_type_ == "left_rgb_image_compressed") {
103 topic_name_ =
104 "/aima/hal/sensor/stereo_head_front_left/rgb_image/compressed";
105 sub_compressed_ = create_subscription<sensor_msgs::msg::CompressedImage>(
106 topic_name_, qos,
107 std::bind(&StereoCameraTopicEcho::cb_compressed, this,
108 std::placeholders::_1));
109 RCLCPP_INFO(get_logger(), "✅ Subscribing Left CompressedImage: %s",
110 topic_name_.c_str());
111
112 // Enable left camera info subscription
113 } else if (topic_type_ == "left_camera_info") {
114
115 topic_name_ = "/aima/hal/sensor/stereo_head_front_left/camera_info";
116 // CameraInfo subscriptions must use reliable + transient_local
117 // QoS in order to receive latched/history messages (even if only one
118 // message was published). Here we use keep_last(1) + reliable
119 // + transient_local.
120 sub_camerainfo_ = create_subscription<sensor_msgs::msg::CameraInfo>(
121 topic_name_,
122 rclcpp::QoS(rclcpp::KeepLast(1)).reliable().transient_local(),
123 std::bind(&StereoCameraTopicEcho::cb_camerainfo, this,
124 std::placeholders::_1));
125 RCLCPP_INFO(get_logger(),
126 "✅ Subscribing Left CameraInfo (with transient_local): %s",
127 topic_name_.c_str());
128
129 // Enable right camera RGB image subscription
130 } else if (topic_type_ == "right_rgb_image") {
131 topic_name_ = "/aima/hal/sensor/stereo_head_front_right/rgb_image";
132 sub_image_ = create_subscription<sensor_msgs::msg::Image>(
133 topic_name_, qos,
134 std::bind(&StereoCameraTopicEcho::cb_image, this,
135 std::placeholders::_1));
136 RCLCPP_INFO(get_logger(), "✅ Subscribing Right RGB Image: %s",
137 topic_name_.c_str());
138 if (!dump_video_path_.empty()) {
139 RCLCPP_INFO(get_logger(), "📝 Will dump received images to video: %s",
140 dump_video_path_.c_str());
141 }
142
143 // Enable right camera RGB compressed image subscription
144 } else if (topic_type_ == "right_rgb_image_compressed") {
145 topic_name_ =
146 "/aima/hal/sensor/stereo_head_front_right/rgb_image/compressed";
147 sub_compressed_ = create_subscription<sensor_msgs::msg::CompressedImage>(
148 topic_name_, qos,
149 std::bind(&StereoCameraTopicEcho::cb_compressed, this,
150 std::placeholders::_1));
151 RCLCPP_INFO(get_logger(), "✅ Subscribing Right CompressedImage: %s",
152 topic_name_.c_str());
153
154 // Enable right camera info subscription
155 } else if (topic_type_ == "right_camera_info") {
156 topic_name_ = "/aima/hal/sensor/stereo_head_front_right/camera_info";
157 // CameraInfo subscriptions must use reliable + transient_local
158 // QoS in order to receive latched/history messages (even if only one
159 // message was published). Here we use keep_last(1) + reliable
160 // + transient_local.
161 sub_camerainfo_ = create_subscription<sensor_msgs::msg::CameraInfo>(
162 topic_name_,
163 rclcpp::QoS(rclcpp::KeepLast(1)).reliable().transient_local(),
164 std::bind(&StereoCameraTopicEcho::cb_camerainfo, this,
165 std::placeholders::_1));
166 RCLCPP_INFO(get_logger(),
167 "✅ Subscribing Right CameraInfo (with transient_local): %s",
168 topic_name_.c_str());
169
170 // Unknown topic_type error
171 } else {
172 RCLCPP_ERROR(get_logger(), "Unknown topic_type: %s", topic_type_.c_str());
173 throw std::runtime_error("Unknown topic_type");
174 }
175 }
176
177 ~StereoCameraTopicEcho() override {
178 if (video_writer_.isOpened()) {
179 video_writer_.release();
180 RCLCPP_INFO(get_logger(), "Video file closed.");
181 }
182 }
183
184private:
185 // Image callback (left/right RGB image)
186 void cb_image(const sensor_msgs::msg::Image::SharedPtr msg) {
187 update_arrivals();
188
189 if (should_print()) {
190 RCLCPP_INFO(get_logger(),
191 "📸 %s received\n"
192 " • frame_id: %s\n"
193 " • stamp (sec): %.6f\n"
194 " • encoding: %s\n"
195 " • size (WxH): %u x %u\n"
196 " • step (bytes/row):%u\n"
197 " • is_bigendian: %u\n"
198 " • recv FPS (1s): %.1f",
199 topic_type_.c_str(), msg->header.frame_id.c_str(),
200 rclcpp::Time(msg->header.stamp).seconds(),
201 msg->encoding.c_str(), msg->width, msg->height, msg->step,
202 msg->is_bigendian, get_fps());
203 }
204
205 // Video dump is supported only for RGB images
206 if ((topic_type_ == "left_rgb_image" || topic_type_ == "right_rgb_image") &&
207 !dump_video_path_.empty()) {
208 dump_image_to_video(msg);
209 }
210 }
211
212 // CompressedImage callback (left/right RGB compressed image)
213 void cb_compressed(const sensor_msgs::msg::CompressedImage::SharedPtr msg) {
214 update_arrivals();
215
216 if (should_print()) {
217 RCLCPP_INFO(get_logger(),
218 "🗜️ %s received\n"
219 " • frame_id: %s\n"
220 " • stamp (sec): %.6f\n"
221 " • format: %s\n"
222 " • data size: %zu\n"
223 " • recv FPS (1s): %.1f",
224 topic_type_.c_str(), msg->header.frame_id.c_str(),
225 rclcpp::Time(msg->header.stamp).seconds(),
226 msg->format.c_str(), msg->data.size(), get_fps());
227 }
228 }
229
230 // CameraInfo callback (left/right camera intrinsic parameters)
231 void cb_camerainfo(const sensor_msgs::msg::CameraInfo::SharedPtr msg) {
232 // CameraInfo is typically published once; print it once
233 std::ostringstream oss;
234 oss << "📷 " << topic_type_ << " received\n"
235 << " • frame_id: " << msg->header.frame_id << "\n"
236 << " • stamp (sec): " << rclcpp::Time(msg->header.stamp).seconds()
237 << "\n"
238 << " • width x height: " << msg->width << " x " << msg->height << "\n"
239 << " • distortion_model:" << msg->distortion_model << "\n"
240 << " • D: [";
241 for (size_t i = 0; i < msg->d.size(); ++i) {
242 oss << msg->d[i];
243 if (i + 1 < msg->d.size())
244 oss << ", ";
245 }
246 oss << "]\n • K: [";
247 for (int i = 0; i < 9; ++i) {
248 oss << msg->k[i];
249 if (i + 1 < 9)
250 oss << ", ";
251 }
252 oss << "]\n • R: [";
253 for (int i = 0; i < 9; ++i) {
254 oss << msg->r[i];
255 if (i + 1 < 9)
256 oss << ", ";
257 }
258 oss << "]\n • P: [";
259 for (int i = 0; i < 12; ++i) {
260 oss << msg->p[i];
261 if (i + 1 < 12)
262 oss << ", ";
263 }
264 oss << "]\n"
265 << " • binning_x: " << msg->binning_x << "\n"
266 << " • binning_y: " << msg->binning_y << "\n"
267 << " • roi: { x_offset: " << msg->roi.x_offset
268 << ", y_offset: " << msg->roi.y_offset
269 << ", height: " << msg->roi.height << ", width: " << msg->roi.width
270 << ", do_rectify: " << (msg->roi.do_rectify ? "true" : "false") << " }";
271 RCLCPP_INFO(get_logger(), "%s", oss.str().c_str());
272 }
273
274 // Track arrival timestamps to compute FPS
275 void update_arrivals() {
276 const rclcpp::Time now = this->get_clock()->now();
277 arrivals_.push_back(now);
278 while (!arrivals_.empty() && (now - arrivals_.front()).seconds() > 1.0) {
279 arrivals_.pop_front();
280 }
281 }
282 double get_fps() const { return static_cast<double>(arrivals_.size()); }
283
284 // Control printing frequency
285 bool should_print() {
286 const rclcpp::Time now = this->get_clock()->now();
287 if ((now - last_print_).seconds() >= 1.0) {
288 last_print_ = now;
289 return true;
290 }
291 return false;
292 }
293
294 // Dump received images to a video file (RGB images only)
295 void dump_image_to_video(const sensor_msgs::msg::Image::SharedPtr &msg) {
296 cv::Mat image;
297 try {
298 // Obtain the Mat without copying by not specifying encoding
299 cv_bridge::CvImageConstPtr cvp = cv_bridge::toCvShare(msg);
300 image = cvp->image;
301 // Convert to BGR for uniform saving
302 if (msg->encoding == "rgb8") {
303 cv::cvtColor(image, image, cv::COLOR_RGB2BGR);
304 } else {
305 RCLCPP_WARN(get_logger(), "image encoding not expected: %s",
306 msg->encoding.c_str());
307 return;
308 }
309 } catch (const std::exception &e) {
310 RCLCPP_WARN(get_logger(), "cv_bridge exception: %s", e.what());
311 return;
312 }
313
314 // Initialize VideoWriter
315 if (!video_writer_.isOpened()) {
316 int fourcc = cv::VideoWriter::fourcc('M', 'J', 'P', 'G');
317 double fps = std::max(1.0, get_fps());
318 bool ok = video_writer_.open(dump_video_path_, fourcc, fps,
319 cv::Size(image.cols, image.rows), true);
320 if (!ok) {
321 RCLCPP_ERROR(get_logger(), "Failed to open video file: %s",
322 dump_video_path_.c_str());
323 dump_video_path_.clear(); // stop trying
324 return;
325 }
326 RCLCPP_INFO(get_logger(), "VideoWriter started: %s, size=%dx%d, fps=%.1f",
327 dump_video_path_.c_str(), image.cols, image.rows, fps);
328 }
329 video_writer_.write(image);
330 }
331
332 // Member variables
333 std::string topic_type_;
334 std::string topic_name_;
335 std::string dump_video_path_;
336
337 // Subscriptions
338 rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_image_;
339 rclcpp::Subscription<sensor_msgs::msg::CompressedImage>::SharedPtr
340 sub_compressed_;
341 rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr sub_camerainfo_;
342
343 // FPS statistics
344 rclcpp::Time last_print_{0, 0, RCL_ROS_TIME};
345 std::deque<rclcpp::Time> arrivals_;
346
347 // Video writer
348 cv::VideoWriter video_writer_;
349};
350
351int main(int argc, char **argv) {
352 rclcpp::init(argc, argv);
353 auto node = std::make_shared<StereoCameraTopicEcho>();
354 rclcpp::spin(node);
355 rclcpp::shutdown();
356 return 0;
357}
Usage Instructions:
Subscribe to left camera RGB image:
ros2 run examples echo_camera_stereo --ros-args -p topic_type:=left_rgb_image
Subscribe to right camera RGB image:
ros2 run examples echo_camera_stereo --ros-args -p topic_type:=right_rgb_image
Subscribe to left camera intrinsic parameters:
ros2 run examples echo_camera_stereo --ros-args -p topic_type:=left_camera_info
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, compressed images, and camera intrinsic parameters.
Features:
Supports rear head camera data subscription
Real-time FPS statistics and data display
Supports RGB video recording
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)
1#include <deque>
2#include <iomanip>
3#include <memory>
4#include <rclcpp/rclcpp.hpp>
5#include <sensor_msgs/msg/camera_info.hpp>
6#include <sensor_msgs/msg/compressed_image.hpp>
7#include <sensor_msgs/msg/image.hpp>
8#include <sstream>
9#include <string>
10#include <vector>
11
12// OpenCV headers for image/video writing
13#include <cv_bridge/cv_bridge.h>
14#include <opencv2/opencv.hpp>
15
16/**
17 * @brief Example of subscribing to multiple topics for the rear head monocular
18 * camera
19 *
20 * You can select which topic type to subscribe to via the startup argument
21 * --ros-args -p topic_type:=<type>:
22 * - rgb_image: RGB image (sensor_msgs/Image)
23 * - rgb_image_compressed: RGB compressed image (sensor_msgs/CompressedImage)
24 * - camera_info: Camera intrinsic parameters (sensor_msgs/CameraInfo)
25 *
26 * Examples:
27 * ros2 run examples echo_camera_head_rear --ros-args -p topic_type:=rgb_image
28 * ros2 run examples echo_camera_head_rear --ros-args -p
29 * topic_type:=rgb_image_compressed ros2 run examples echo_camera_head_rear
30 * --ros-args -p topic_type:=camera_info
31 *
32 * topic_type defaults to "rgb_image"
33 *
34 * See individual callbacks for more detailed comments
35 */
36class HeadRearCameraTopicEcho : public rclcpp::Node {
37public:
38 HeadRearCameraTopicEcho() : Node("head_rear_camera_topic_echo") {
39 // Select which topic type to subscribe to
40 topic_type_ = declare_parameter<std::string>("topic_type", "rgb_image");
41 dump_video_path_ = declare_parameter<std::string>("dump_video_path", "");
42
43 // Subscribed topics and their message layouts
44 // 1. /aima/hal/sensor/rgb_head_rear/rgb_image
45 // - topic_type: rgb_image
46 // - message type: sensor_msgs::msg::Image
47 // - frame_id: rgb_head_rear
48 // - child_frame_id: /
49 // - contents: raw image data
50 // 2. /aima/hal/sensor/rgb_head_rear/rgb_image/compressed
51 // - topic_type: rgb_image_compressed
52 // - message type: sensor_msgs::msg::CompressedImage
53 // - frame_id: rgb_head_rear
54 // - contents: compressed image data
55 // 3. /aima/hal/sensor/rgb_head_rear/camera_info
56 // - topic_type: camera_info
57 // - message type: sensor_msgs::msg::CameraInfo
58 // - frame_id: rgb_head_rear
59 // - contents: camera intrinsic parameters
60
61 // Set QoS parameters - use SensorData QoS
62 auto qos = rclcpp::SensorDataQoS();
63
64 // Enable RGB image subscription
65 if (topic_type_ == "rgb_image") {
66 topic_name_ = "/aima/hal/sensor/rgb_head_rear/rgb_image";
67 sub_image_ = create_subscription<sensor_msgs::msg::Image>(
68 topic_name_, qos,
69 std::bind(&HeadRearCameraTopicEcho::cb_image, this,
70 std::placeholders::_1));
71 RCLCPP_INFO(get_logger(), "✅ Subscribing RGB Image: %s",
72 topic_name_.c_str());
73 if (!dump_video_path_.empty()) {
74 RCLCPP_INFO(get_logger(), "📝 Will dump received images to video: %s",
75 dump_video_path_.c_str());
76 }
77
78 // Enable RGB compressed image subscription
79 } else if (topic_type_ == "rgb_image_compressed") {
80 topic_name_ = "/aima/hal/sensor/rgb_head_rear/rgb_image/compressed";
81 sub_compressed_ = create_subscription<sensor_msgs::msg::CompressedImage>(
82 topic_name_, qos,
83 std::bind(&HeadRearCameraTopicEcho::cb_compressed, this,
84 std::placeholders::_1));
85 RCLCPP_INFO(get_logger(), "✅ Subscribing CompressedImage: %s",
86 topic_name_.c_str());
87
88 // Enable camera info subscription
89 } else if (topic_type_ == "camera_info") {
90 topic_name_ = "/aima/hal/sensor/rgb_head_rear/camera_info";
91 // CameraInfo subscriptions must use reliable + transient_local
92 // QoS in order to receive latched/history messages (even if only one
93 // message was published). Here we use keep_last(1) + reliable
94 // + transient_local.
95 sub_camerainfo_ = create_subscription<sensor_msgs::msg::CameraInfo>(
96 topic_name_,
97 rclcpp::QoS(rclcpp::KeepLast(1)).reliable().transient_local(),
98 std::bind(&HeadRearCameraTopicEcho::cb_camerainfo, this,
99 std::placeholders::_1));
100 RCLCPP_INFO(get_logger(),
101 "✅ Subscribing CameraInfo (with transient_local): %s",
102 topic_name_.c_str());
103
104 // Unknown topic_type error
105 } else {
106 RCLCPP_ERROR(get_logger(), "Unknown topic_type: %s", topic_type_.c_str());
107 throw std::runtime_error("Unknown topic_type");
108 }
109 }
110
111 ~HeadRearCameraTopicEcho() override {
112 if (video_writer_.isOpened()) {
113 video_writer_.release();
114 RCLCPP_INFO(get_logger(), "Video file closed.");
115 }
116 }
117
118private:
119 // Image callback (RGB image)
120 void cb_image(const sensor_msgs::msg::Image::SharedPtr msg) {
121 update_arrivals();
122
123 if (should_print()) {
124 RCLCPP_INFO(get_logger(),
125 "📸 %s received\n"
126 " • frame_id: %s\n"
127 " • stamp (sec): %.6f\n"
128 " • encoding: %s\n"
129 " • size (WxH): %u x %u\n"
130 " • step (bytes/row):%u\n"
131 " • is_bigendian: %u\n"
132 " • recv FPS (1s): %.1f",
133 topic_type_.c_str(), msg->header.frame_id.c_str(),
134 rclcpp::Time(msg->header.stamp).seconds(),
135 msg->encoding.c_str(), msg->width, msg->height, msg->step,
136 msg->is_bigendian, get_fps());
137 }
138
139 // Video dump is supported only for RGB images
140 if (topic_type_ == "rgb_image" && !dump_video_path_.empty()) {
141 dump_image_to_video(msg);
142 }
143 }
144
145 // CompressedImage callback (RGB compressed image)
146 void cb_compressed(const sensor_msgs::msg::CompressedImage::SharedPtr msg) {
147 update_arrivals();
148
149 if (should_print()) {
150 RCLCPP_INFO(get_logger(),
151 "🗜️ %s received\n"
152 " • frame_id: %s\n"
153 " • stamp (sec): %.6f\n"
154 " • format: %s\n"
155 " • data size: %zu\n"
156 " • recv FPS (1s): %.1f",
157 topic_type_.c_str(), msg->header.frame_id.c_str(),
158 rclcpp::Time(msg->header.stamp).seconds(),
159 msg->format.c_str(), msg->data.size(), get_fps());
160 }
161 }
162
163 // CameraInfo callback (camera intrinsic parameters)
164 void cb_camerainfo(const sensor_msgs::msg::CameraInfo::SharedPtr msg) {
165 // CameraInfo is typically published once; print it once
166 std::ostringstream oss;
167 oss << "📷 " << topic_type_ << " received\n"
168 << " • frame_id: " << msg->header.frame_id << "\n"
169 << " • stamp (sec): " << rclcpp::Time(msg->header.stamp).seconds()
170 << "\n"
171 << " • width x height: " << msg->width << " x " << msg->height << "\n"
172 << " • distortion_model:" << msg->distortion_model << "\n"
173 << " • D: [";
174 for (size_t i = 0; i < msg->d.size(); ++i) {
175 oss << msg->d[i];
176 if (i + 1 < msg->d.size())
177 oss << ", ";
178 }
179 oss << "]\n • K: [";
180 for (int i = 0; i < 9; ++i) {
181 oss << msg->k[i];
182 if (i + 1 < 9)
183 oss << ", ";
184 }
185 oss << "]\n • R: [";
186 for (int i = 0; i < 9; ++i) {
187 oss << msg->r[i];
188 if (i + 1 < 9)
189 oss << ", ";
190 }
191 oss << "]\n • P: [";
192 for (int i = 0; i < 12; ++i) {
193 oss << msg->p[i];
194 if (i + 1 < 12)
195 oss << ", ";
196 }
197 oss << "]\n"
198 << " • binning_x: " << msg->binning_x << "\n"
199 << " • binning_y: " << msg->binning_y << "\n"
200 << " • roi: { x_offset: " << msg->roi.x_offset
201 << ", y_offset: " << msg->roi.y_offset
202 << ", height: " << msg->roi.height << ", width: " << msg->roi.width
203 << ", do_rectify: " << (msg->roi.do_rectify ? "true" : "false") << " }";
204 RCLCPP_INFO(get_logger(), "%s", oss.str().c_str());
205 }
206
207 // Track arrival timestamps to compute FPS
208 void update_arrivals() {
209 const rclcpp::Time now = this->get_clock()->now();
210 arrivals_.push_back(now);
211 while (!arrivals_.empty() && (now - arrivals_.front()).seconds() > 1.0) {
212 arrivals_.pop_front();
213 }
214 }
215 double get_fps() const { return static_cast<double>(arrivals_.size()); }
216
217 // Control printing frequency
218 bool should_print() {
219 const rclcpp::Time now = this->get_clock()->now();
220 if ((now - last_print_).seconds() >= 1.0) {
221 last_print_ = now;
222 return true;
223 }
224 return false;
225 }
226
227 // Dump received images to a video file (RGB images only)
228 void dump_image_to_video(const sensor_msgs::msg::Image::SharedPtr &msg) {
229 cv::Mat image;
230 try {
231 // Obtain the Mat without copying by not specifying encoding
232 cv_bridge::CvImageConstPtr cvp = cv_bridge::toCvShare(msg);
233 image = cvp->image;
234 // Convert to BGR for uniform saving
235 if (msg->encoding == "rgb8") {
236 cv::cvtColor(image, image, cv::COLOR_RGB2BGR);
237 } else {
238 RCLCPP_WARN(get_logger(), "image encoding not expected: %s",
239 msg->encoding.c_str());
240 return;
241 }
242 } catch (const std::exception &e) {
243 RCLCPP_WARN(get_logger(), "cv_bridge exception: %s", e.what());
244 return;
245 }
246
247 // Initialize VideoWriter
248 if (!video_writer_.isOpened()) {
249 int fourcc = cv::VideoWriter::fourcc('M', 'J', 'P', 'G');
250 double fps = std::max(1.0, get_fps());
251 bool ok = video_writer_.open(dump_video_path_, fourcc, fps,
252 cv::Size(image.cols, image.rows), true);
253 if (!ok) {
254 RCLCPP_ERROR(get_logger(), "Failed to open video file: %s",
255 dump_video_path_.c_str());
256 dump_video_path_.clear(); // stop trying
257 return;
258 }
259 RCLCPP_INFO(get_logger(), "VideoWriter started: %s, size=%dx%d, fps=%.1f",
260 dump_video_path_.c_str(), image.cols, image.rows, fps);
261 }
262 video_writer_.write(image);
263 }
264
265 // Member variables
266 std::string topic_type_;
267 std::string topic_name_;
268 std::string dump_video_path_;
269
270 // Subscriptions
271 rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_image_;
272 rclcpp::Subscription<sensor_msgs::msg::CompressedImage>::SharedPtr
273 sub_compressed_;
274 rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr sub_camerainfo_;
275
276 // FPS statistics
277 rclcpp::Time last_print_{0, 0, RCL_ROS_TIME};
278 std::deque<rclcpp::Time> arrivals_;
279
280 // Video writer
281 cv::VideoWriter video_writer_;
282};
283
284int main(int argc, char **argv) {
285 rclcpp::init(argc, argv);
286 auto node = std::make_shared<HeadRearCameraTopicEcho>();
287 rclcpp::spin(node);
288 rclcpp::shutdown();
289 return 0;
290}
Usage Instructions:
Subscribe to RGB image data:
ros2 run examples echo_camera_head_rear --ros-args -p topic_type:=rgb_image
Subscribe to compressed image data:
ros2 run examples echo_camera_head_rear --ros-args -p topic_type:=rgb_image_compressed
Subscribe to camera intrinsic parameters:
ros2 run examples echo_camera_head_rear --ros-args -p topic_type:=camera_info
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
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 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
1#include <deque>
2#include <iomanip>
3#include <memory>
4#include <rclcpp/rclcpp.hpp>
5#include <sensor_msgs/msg/imu.hpp>
6#include <sensor_msgs/msg/point_cloud2.hpp>
7#include <sstream>
8#include <string>
9#include <vector>
10
11/**
12 * @brief Example for subscribing to chest LIDAR data
13 *
14 * Supports subscribing to the following topics:
15 * 1. /aima/hal/sensor/lidar_chest_front/lidar_pointcloud
16 * - Data type: sensor_msgs::msg::PointCloud2
17 * - frame_id: lidar_chest_front
18 * - child_frame_id: /
19 * - Content: LIDAR point cloud data
20 * 2. /aima/hal/sensor/lidar_chest_front/imu
21 * - Data type: sensor_msgs::msg::Imu
22 * - frame_id: lidar_imu_chest_front
23 * - Content: LIDAR IMU data
24 *
25 * You can select the topic type to subscribe to using the launch parameter
26 * --ros-args -p topic_type:=<type>:
27 * - pointcloud: Subscribe to LIDAR point cloud
28 * - imu: Subscribe to LIDAR IMU
29 * The default topic_type is pointcloud
30 */
31class LidarChestEcho : public rclcpp::Node {
32public:
33 LidarChestEcho() : Node("lidar_chest_echo") {
34 topic_type_ = declare_parameter<std::string>("topic_type", "pointcloud");
35
36 auto qos = rclcpp::SensorDataQoS();
37
38 if (topic_type_ == "pointcloud") {
39 topic_name_ = "/aima/hal/sensor/lidar_chest_front/lidar_pointcloud";
40 sub_pointcloud_ = create_subscription<sensor_msgs::msg::PointCloud2>(
41 topic_name_, qos,
42 std::bind(&LidarChestEcho::cb_pointcloud, this,
43 std::placeholders::_1));
44 RCLCPP_INFO(get_logger(), "✅ Subscribing LIDAR PointCloud2: %s",
45 topic_name_.c_str());
46 } else if (topic_type_ == "imu") {
47 topic_name_ = "/aima/hal/sensor/lidar_chest_front/imu";
48 sub_imu_ = create_subscription<sensor_msgs::msg::Imu>(
49 topic_name_, qos,
50 std::bind(&LidarChestEcho::cb_imu, this, std::placeholders::_1));
51 RCLCPP_INFO(get_logger(), "✅ Subscribing LIDAR IMU: %s",
52 topic_name_.c_str());
53 } else {
54 RCLCPP_ERROR(get_logger(), "Unknown topic_type: %s", topic_type_.c_str());
55 throw std::runtime_error("Unknown topic_type");
56 }
57 }
58
59private:
60 // PointCloud2 callback
61 void cb_pointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
62 update_arrivals();
63
64 if (should_print()) {
65 std::ostringstream oss;
66 oss << "🟢 LIDAR PointCloud2 received\n"
67 << " • frame_id: " << msg->header.frame_id << "\n"
68 << " • stamp (sec): "
69 << rclcpp::Time(msg->header.stamp).seconds() << "\n"
70 << " • width x height: " << msg->width << " x " << msg->height
71 << "\n"
72 << " • point_step: " << msg->point_step << "\n"
73 << " • row_step: " << msg->row_step << "\n"
74 << " • fields: ";
75 for (const auto &f : msg->fields)
76 oss << f.name << "(" << (int)f.datatype << ") ";
77 oss << "\n • is_bigendian: " << msg->is_bigendian
78 << "\n • is_dense: " << msg->is_dense
79 << "\n • data size: " << msg->data.size()
80 << "\n • recv FPS (1s): " << get_fps();
81 RCLCPP_INFO(get_logger(), "%s", oss.str().c_str());
82 }
83 }
84
85 // IMU callback
86 void cb_imu(const sensor_msgs::msg::Imu::SharedPtr msg) {
87 update_arrivals();
88
89 if (should_print()) {
90 std::ostringstream oss;
91 oss << "🟢 LIDAR IMU received\n"
92 << " • frame_id: " << msg->header.frame_id << "\n"
93 << " • stamp (sec): "
94 << rclcpp::Time(msg->header.stamp).seconds() << "\n"
95 << " • orientation: [" << msg->orientation.x << ", "
96 << msg->orientation.y << ", " << msg->orientation.z << ", "
97 << msg->orientation.w << "]\n"
98 << " • angular_velocity:[" << msg->angular_velocity.x << ", "
99 << msg->angular_velocity.y << ", " << msg->angular_velocity.z << "]\n"
100 << " • linear_accel: [" << msg->linear_acceleration.x << ", "
101 << msg->linear_acceleration.y << ", " << msg->linear_acceleration.z
102 << "]\n"
103 << " • recv FPS (1s): " << get_fps();
104 RCLCPP_INFO(get_logger(), "%s", oss.str().c_str());
105 }
106 }
107
108 // Update FPS statistics
109 void update_arrivals() {
110 const rclcpp::Time now = this->get_clock()->now();
111 arrivals_.push_back(now);
112 while (!arrivals_.empty() && (now - arrivals_.front()).seconds() > 1.0) {
113 arrivals_.pop_front();
114 }
115 }
116 double get_fps() const { return static_cast<double>(arrivals_.size()); }
117
118 // Control print frequency
119 bool should_print() {
120 const rclcpp::Time now = this->get_clock()->now();
121 if ((now - last_print_).seconds() >= 1.0) {
122 last_print_ = now;
123 return true;
124 }
125 return false;
126 }
127
128 // Member variables
129 std::string topic_type_;
130 std::string topic_name_;
131
132 rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr
133 sub_pointcloud_;
134 rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr sub_imu_;
135
136 rclcpp::Time last_print_{0, 0, RCL_ROS_TIME};
137 std::deque<rclcpp::Time> arrivals_;
138};
139
140int main(int argc, char **argv) {
141 rclcpp::init(argc, argv);
142 auto node = std::make_shared<LidarChestEcho>();
143 rclcpp::spin(node);
144 rclcpp::shutdown();
145 return 0;
146}
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.14 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.
1#include "aimdk_msgs/srv/play_video.hpp"
2#include "aimdk_msgs/msg/common_request.hpp"
3#include "rclcpp/rclcpp.hpp"
4#include <chrono>
5#include <memory>
6#include <signal.h>
7#include <string>
8
9// Global variable used for signal handling
10std::shared_ptr<rclcpp::Node> g_node = nullptr;
11
12// Signal handler function
13void signal_handler(int signal) {
14 if (g_node) {
15 RCLCPP_INFO(g_node->get_logger(), "Received signal %d, shutting down...",
16 signal);
17 g_node.reset();
18 }
19 rclcpp::shutdown();
20 exit(signal);
21}
22
23class PlayVideoClient : public rclcpp::Node {
24public:
25 PlayVideoClient() : Node("play_video_client") {
26 client_ = this->create_client<aimdk_msgs::srv::PlayVideo>(
27 "/face_ui_proxy/play_video");
28 RCLCPP_INFO(this->get_logger(), "✅ PlayVideo client node started.");
29
30 // Wait for the service to become available
31 while (!client_->wait_for_service(std::chrono::seconds(2))) {
32 RCLCPP_INFO(this->get_logger(), "⏳ Service unavailable, waiting...");
33 }
34 RCLCPP_INFO(this->get_logger(),
35 "🟢 Service available, ready to send request.");
36 }
37
38 bool send_request(const std::string &video_path, uint8_t mode,
39 int32_t priority) {
40 try {
41 auto request = std::make_shared<aimdk_msgs::srv::PlayVideo::Request>();
42
43 request->video_path = video_path;
44 request->mode = mode;
45 request->priority = priority;
46
47 RCLCPP_INFO(this->get_logger(),
48 "📨 Sending request to play video: mode=%hhu video=%s", mode,
49 video_path.c_str());
50
51 const std::chrono::milliseconds timeout(250);
52 for (int i = 0; i < 8; i++) {
53 request->header.header.stamp = this->now();
54 auto future = client_->async_send_request(request);
55 auto retcode = rclcpp::spin_until_future_complete(shared_from_this(),
56 future, timeout);
57 if (retcode != rclcpp::FutureReturnCode::SUCCESS) {
58 // retry as remote peer is NOT handled well by ROS
59 RCLCPP_INFO(this->get_logger(), "trying ... [%d]", i);
60 continue;
61 }
62 // future.done
63 auto response = future.get();
64 if (response->success) {
65 RCLCPP_INFO(this->get_logger(),
66 "✅ Request to play video recorded successfully: %s",
67 response->message.c_str());
68 return true;
69 } else {
70 RCLCPP_ERROR(this->get_logger(),
71 "❌ Failed to record play-video request: %s",
72 response->message.c_str());
73 return false;
74 }
75 }
76 RCLCPP_ERROR(this->get_logger(), "❌ Service call failed or timed out.");
77 return false;
78 } catch (const std::exception &e) {
79 RCLCPP_ERROR(this->get_logger(), "Exception occurred: %s", e.what());
80 return false;
81 }
82 }
83
84private:
85 rclcpp::Client<aimdk_msgs::srv::PlayVideo>::SharedPtr client_;
86};
87
88int main(int argc, char **argv) {
89 try {
90 rclcpp::init(argc, argv);
91
92 // Set up signal handlers
93 signal(SIGINT, signal_handler);
94 signal(SIGTERM, signal_handler);
95
96 std::string video_path =
97 "/agibot/data/home/agi/zhiyuan.mp4"; // Default video path; modify as
98 // needed
99 int32_t priority = 5;
100 int mode = 2; // Loop playback
101 std::cout << "Enter video play mode (1: once, 2: loop): ";
102 std::cin >> mode;
103 if (mode < 1 || mode > 2) {
104 RCLCPP_ERROR(rclcpp::get_logger("main"), "Invalid play mode: %d", mode);
105 rclcpp::shutdown();
106 return 1;
107 }
108
109 g_node = std::make_shared<PlayVideoClient>();
110 auto client = std::dynamic_pointer_cast<PlayVideoClient>(g_node);
111
112 if (client) {
113 client->send_request(video_path, mode, priority);
114 }
115
116 // Clean up resources
117 g_node.reset();
118 rclcpp::shutdown();
119
120 return 0;
121 } catch (const std::exception &e) {
122 RCLCPP_ERROR(rclcpp::get_logger("main"),
123 "Program exited with exception: %s", e.what());
124 return 1;
125 }
126}
6.2.15 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)
1#include <aimdk_msgs/msg/tts_priority_level.hpp>
2#include <aimdk_msgs/srv/play_media_file.hpp>
3#include <iostream>
4#include <rclcpp/rclcpp.hpp>
5#include <string>
6
7using PlayMediaFile = aimdk_msgs::srv::PlayMediaFile;
8
9int main(int argc, char **argv) {
10 rclcpp::init(argc, argv);
11 auto node = rclcpp::Node::make_shared("play_media_file_client_min");
12
13 // 1) Service name
14 const std::string service_name = "/aimdk_5Fmsgs/srv/PlayMediaFile";
15 auto client = node->create_client<PlayMediaFile>(service_name);
16
17 // 2) Input file path (prompt user if not provided as argument)
18 std::string default_file =
19 "/agibot/data/var/interaction/tts_cache/normal/demo.wav";
20 std::string file_name;
21
22 if (argc > 1) {
23 file_name = argv[1];
24 } else {
25 std::cout << "Enter the media file path to play (default: " << default_file
26 << "): ";
27 std::getline(std::cin, file_name);
28 if (file_name.empty()) {
29 file_name = default_file;
30 }
31 }
32
33 // 3) Build the request
34 auto req = std::make_shared<PlayMediaFile::Request>();
35 // CommonRequest request -> RequestHeader header -> builtin_interfaces/Time
36 // stamp
37 req->header.header.stamp = node->now();
38
39 // PlayMediaFileRequest required fields
40 req->media_file_req.file_name = file_name;
41 req->media_file_req.domain = "demo_client"; // Required: identifies the caller
42 req->media_file_req.trace_id = "demo"; // Optional: request identifier
43 req->media_file_req.is_interrupted =
44 true; // Whether to interrupt same-priority playback
45 req->media_file_req.priority_weight = 0; // Optional: 0~99
46 // Priority level: default INTERACTION_L6
47 req->media_file_req.priority_level.value = 6;
48
49 // 4) Wait for service and call
50 RCLCPP_INFO(node->get_logger(), "Waiting for service: %s",
51 service_name.c_str());
52 if (!client->wait_for_service(std::chrono::seconds(5))) {
53 RCLCPP_ERROR(node->get_logger(), "Service unavailable: %s",
54 service_name.c_str());
55 rclcpp::shutdown();
56 return 1;
57 }
58
59 auto future = client->async_send_request(req);
60 auto rc = rclcpp::spin_until_future_complete(node, future,
61 std::chrono::seconds(10));
62
63 if (rc == rclcpp::FutureReturnCode::INTERRUPTED) {
64 RCLCPP_WARN(node->get_logger(), "Interrupted while waiting");
65 rclcpp::shutdown();
66 return 1;
67 }
68
69 if (rc != rclcpp::FutureReturnCode::SUCCESS) {
70 RCLCPP_ERROR(node->get_logger(), "Call timed out or did not complete");
71 rclcpp::shutdown();
72 return 1;
73 }
74
75 // 5) Handle response (success is in tts_resp)
76 try {
77 const auto resp = future.get();
78 bool success = resp->tts_resp.is_success;
79
80 if (success) {
81 RCLCPP_INFO(node->get_logger(), "✅ Media file play request succeeded: %s",
82 file_name.c_str());
83 } else {
84 RCLCPP_ERROR(node->get_logger(), "❌ Media file play request failed: %s",
85 file_name.c_str());
86 }
87 } catch (const std::exception &e) {
88 RCLCPP_ERROR(node->get_logger(), "Call exception: %s", e.what());
89 }
90
91 rclcpp::shutdown();
92 return 0;
93}
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.16 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
1#include <aimdk_msgs/msg/tts_priority_level.hpp>
2#include <aimdk_msgs/srv/play_tts.hpp>
3#include <iostream>
4#include <rclcpp/rclcpp.hpp>
5#include <string>
6
7using PlayTTS = aimdk_msgs::srv::PlayTts;
8
9int main(int argc, char **argv) {
10 rclcpp::init(argc, argv);
11 auto node = rclcpp::Node::make_shared("play_tts_client_min");
12
13 const std::string service_name = "/aimdk_5Fmsgs/srv/PlayTts";
14 auto client = node->create_client<PlayTTS>(service_name);
15
16 // Get text to speak
17 std::string tts_text;
18 if (argc > 1) {
19 tts_text = argv[1];
20 } else {
21 std::cout << "Enter text to speak: ";
22 std::getline(std::cin, tts_text);
23 if (tts_text.empty()) {
24 tts_text = "Hello, I am Lingxi X2.";
25 }
26 }
27
28 auto req = std::make_shared<PlayTTS::Request>();
29 req->header.header.stamp = node->now();
30 req->tts_req.text = tts_text;
31 req->tts_req.domain = "demo_client"; // Required: identifies the caller
32 req->tts_req.trace_id =
33 "demo"; // Optional: request identifier for the TTS request
34 req->tts_req.is_interrupted =
35 true; // Required: whether to interrupt same-priority playback
36 req->tts_req.priority_weight = 0;
37 req->tts_req.priority_level.value = 6;
38
39 if (!client->wait_for_service(
40 std::chrono::duration_cast<std::chrono::seconds>(
41 std::chrono::seconds(5)))) {
42 RCLCPP_ERROR(node->get_logger(), "Service unavailable: %s",
43 service_name.c_str());
44 rclcpp::shutdown();
45 return 1;
46 }
47
48 auto future = client->async_send_request(req);
49 if (rclcpp::spin_until_future_complete(
50 node, future,
51 std::chrono::duration_cast<std::chrono::seconds>(
52 std::chrono::seconds(10))) != rclcpp::FutureReturnCode::SUCCESS) {
53 RCLCPP_ERROR(node->get_logger(), "Call timed out");
54 rclcpp::shutdown();
55 return 1;
56 }
57
58 const auto resp = future.get();
59 if (resp->tts_resp.is_success) {
60 RCLCPP_INFO(node->get_logger(), "✅ TTS play request succeeded");
61 } else {
62 RCLCPP_ERROR(node->get_logger(), "❌ TTS play request failed");
63 }
64
65 rclcpp::shutdown();
66 return 0;
67}
Usage Instructions
# Use command-line arguments to speak text (recommended)
ros2 run examples play_tts "Hello, I am the Lingxi 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/PlayTtsMessage:
aimdk_msgs/srv/PlayTts
6.2.17 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 speech1: Speech start2: Speech processing3: Speech end
1#include <aimdk_msgs/msg/audio_vad_state_type.hpp>
2#include <aimdk_msgs/msg/processed_audio_output.hpp>
3#include <chrono>
4#include <ctime>
5#include <filesystem>
6#include <fstream>
7#include <iomanip>
8#include <rclcpp/rclcpp.hpp>
9#include <sstream>
10#include <string>
11#include <unordered_map>
12#include <vector>
13
14namespace fs = std::filesystem;
15
16class AudioSubscriber : public rclcpp::Node {
17public:
18 AudioSubscriber() : rclcpp::Node("audio_subscriber") {
19 // Audio buffers, stored separately by stream_id
20 // stream_id -> buffer
21 audio_buffers_ = {};
22 recording_state_ = {};
23
24 audio_output_dir_ = "audio_recordings";
25 fs::create_directories(audio_output_dir_);
26
27 // Note: deep queue to avoid missing data in a burst at start of VAD.
28 auto qos = rclcpp::QoS(
29 rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_sensor_data));
30 qos.keep_last(500).best_effort();
31
32 subscription_ =
33 this->create_subscription<aimdk_msgs::msg::ProcessedAudioOutput>(
34 "/agent/process_audio_output", qos,
35 std::bind(&AudioSubscriber::audio_callback, this,
36 std::placeholders::_1));
37
38 RCLCPP_INFO(this->get_logger(),
39 "Starting to subscribe to denoised audio data...");
40 }
41
42private:
43 void
44 audio_callback(const aimdk_msgs::msg::ProcessedAudioOutput::SharedPtr msg) {
45 try {
46 uint32_t stream_id = msg->stream_id;
47 uint8_t vad_state = msg->audio_vad_state.value;
48 const std::vector<uint8_t> &audio_data = msg->audio_data;
49
50 static const std::unordered_map<uint8_t, std::string> vad_state_names = {
51 {0, "No Speech"},
52 {1, "Speech Start"},
53 {2, "Speech Processing"},
54 {3, "Speech End"}};
55 static const std::unordered_map<uint32_t, std::string> stream_names = {
56 {1, "Internal Microphone"}, {2, "External Microphone"}};
57
58 RCLCPP_INFO(this->get_logger(),
59 "Audio data received: stream_id=%u, vad_state=%u(%s), "
60 "audio_size=%zu bytes",
61 stream_id, vad_state,
62 vad_state_names.count(vad_state)
63 ? vad_state_names.at(vad_state).c_str()
64 : "Unknown State",
65 audio_data.size());
66
67 handle_vad_state(stream_id, vad_state, audio_data);
68 } catch (const std::exception &e) {
69 RCLCPP_ERROR(this->get_logger(), "Error processing audio message: %s",
70 e.what());
71 }
72 }
73
74 void handle_vad_state(uint32_t stream_id, uint8_t vad_state,
75 const std::vector<uint8_t> &audio_data) {
76 // Initialize the buffer for this stream_id (if it does not exist)
77 if (audio_buffers_.count(stream_id) == 0) {
78 audio_buffers_[stream_id] = std::vector<uint8_t>();
79 recording_state_[stream_id] = false;
80 }
81
82 static const std::unordered_map<uint8_t, std::string> vad_state_names = {
83 {0, "No Speech"},
84 {1, "Speech Start"},
85 {2, "Speech Processing"},
86 {3, "Speech End"}};
87 static const std::unordered_map<uint32_t, std::string> stream_names = {
88 {1, "Internal Microphone"}, {2, "External Microphone"}};
89
90 RCLCPP_INFO(this->get_logger(), "[%s] VAD Atate: %s Audio Data: %zu bytes",
91 stream_names.count(stream_id)
92 ? stream_names.at(stream_id).c_str()
93 : ("Unknown Stream " + std::to_string(stream_id)).c_str(),
94 vad_state_names.count(vad_state)
95 ? vad_state_names.at(vad_state).c_str()
96 : ("Unknown State" + std::to_string(vad_state)).c_str(),
97 audio_data.size());
98
99 // AUDIO_VAD_STATE_BEGIN
100 if (vad_state == 1) {
101 RCLCPP_INFO(this->get_logger(), "🎤 Speech detected - Start");
102 if (recording_state_[stream_id] == false) {
103 audio_buffers_[stream_id].clear();
104 recording_state_[stream_id] = true;
105 }
106 if (!audio_data.empty()) {
107 audio_buffers_[stream_id].insert(audio_buffers_[stream_id].end(),
108 audio_data.begin(), audio_data.end());
109 }
110
111 // AUDIO_VAD_STATE_PROCESSING
112 } else if (vad_state == 2) {
113 RCLCPP_INFO(this->get_logger(), "🔄 Speech Processing...");
114 if (recording_state_[stream_id] && !audio_data.empty()) {
115 audio_buffers_[stream_id].insert(audio_buffers_[stream_id].end(),
116 audio_data.begin(), audio_data.end());
117 }
118
119 // AUDIO_VAD_STATE_END
120 } else if (vad_state == 3) {
121 RCLCPP_INFO(this->get_logger(), "✅ Speech End");
122 if (recording_state_[stream_id] && !audio_data.empty()) {
123 audio_buffers_[stream_id].insert(audio_buffers_[stream_id].end(),
124 audio_data.begin(), audio_data.end());
125 }
126 if (recording_state_[stream_id] && !audio_buffers_[stream_id].empty()) {
127 save_audio_segment(audio_buffers_[stream_id], stream_id);
128 }
129 recording_state_[stream_id] = false;
130
131 // AUDIO_VAD_STATE_NONE
132 } else if (vad_state == 0) {
133 if (recording_state_[stream_id]) {
134 RCLCPP_INFO(this->get_logger(), "⏹️ Recording state reset");
135 recording_state_[stream_id] = false;
136 }
137 }
138
139 // Output the current buffer status.
140 size_t buffer_size = audio_buffers_[stream_id].size();
141 bool recording = recording_state_[stream_id];
142 RCLCPP_DEBUG(this->get_logger(),
143 "[Stream %u] Buffer size: %zu bytes, Recording state: %s",
144 stream_id, buffer_size, recording ? "true" : "false");
145 }
146
147 void save_audio_segment(const std::vector<uint8_t> &audio_data,
148 uint32_t stream_id) {
149 if (audio_data.empty())
150 return;
151
152 // Get the current timestamp.
153 auto now = std::chrono::system_clock::now();
154 std::time_t t = std::chrono::system_clock::to_time_t(now);
155 auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(
156 now.time_since_epoch()) %
157 1000;
158
159 std::ostringstream oss;
160 oss << std::put_time(std::localtime(&t), "%Y%m%d_%H%M%S") << "_"
161 << std::setw(3) << std::setfill('0') << ms.count();
162
163 // Create a subdirectory by stream_id.
164 fs::path stream_dir =
165 fs::path(audio_output_dir_) / ("stream_" + std::to_string(stream_id));
166 fs::create_directories(stream_dir);
167
168 static const std::unordered_map<uint32_t, std::string> stream_names = {
169 {1, "internal_mic"}, {2, "external_mic"}};
170 std::string stream_name = stream_names.count(stream_id)
171 ? stream_names.at(stream_id)
172 : ("stream_" + std::to_string(stream_id));
173 std::string filename = stream_name + "_" + oss.str() + ".pcm";
174 fs::path filepath = stream_dir / filename;
175
176 try {
177 std::ofstream ofs(filepath, std::ios::binary);
178 ofs.write(reinterpret_cast<const char *>(audio_data.data()),
179 audio_data.size());
180 ofs.close();
181 RCLCPP_INFO(this->get_logger(),
182 "Audio segment saved: %s (size: %zu bytes)", filepath.c_str(),
183 audio_data.size());
184
185 // Record audio file duration (assuming 16kHz, 16-bit, mono)
186 int sample_rate = 16000;
187 int bits_per_sample = 16;
188 int channels = 1;
189 int bytes_per_sample = bits_per_sample / 8;
190 size_t total_samples = audio_data.size() / (bytes_per_sample * channels);
191 double duration_seconds =
192 static_cast<double>(total_samples) / sample_rate;
193
194 RCLCPP_INFO(this->get_logger(),
195 "Audio duration: %.2f seconds (%zu samples)",
196 duration_seconds, total_samples);
197 } catch (const std::exception &e) {
198 RCLCPP_ERROR(this->get_logger(), "Failed to save audio file: %s",
199 e.what());
200 }
201 }
202
203 // Member variables
204 std::unordered_map<uint32_t, std::vector<uint8_t>> audio_buffers_;
205 std::unordered_map<uint32_t, bool> recording_state_;
206 std::string audio_output_dir_;
207 rclcpp::Subscription<aimdk_msgs::msg::ProcessedAudioOutput>::SharedPtr
208 subscription_;
209};
210
211int main(int argc, char **argv) {
212 rclcpp::init(argc, argv);
213 auto node = std::make_shared<AudioSubscriber>();
214 RCLCPP_INFO(node->get_logger(),
215 "Listening for denoised audio data, press Ctrl+C to exit...");
216 rclcpp::spin(node);
217 rclcpp::shutdown();
218 return 0;
219}
Usage Instructions:
After running the node, an
audio_recordingsdirectory will be created automaticallyAudio files are stored by stream_id:
stream_1/: Internal microphone audiostream_2/: External microphone audio
File naming format:
{stream_name}_{timestamp}.pcmAudio format: 16 kHz, 16-bit, mono PCM
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
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.18 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.
1#include "aimdk_msgs/srv/play_emoji.hpp"
2#include "aimdk_msgs/msg/common_request.hpp"
3#include "rclcpp/rclcpp.hpp"
4#include <chrono>
5#include <memory>
6#include <signal.h>
7#include <string>
8
9// Global variable used for signal handling
10std::shared_ptr<rclcpp::Node> g_node = nullptr;
11
12// Signal handler function
13void signal_handler(int signal) {
14 if (g_node) {
15 RCLCPP_INFO(g_node->get_logger(), "Received signal %d, shutting down...",
16 signal);
17 g_node.reset();
18 }
19 rclcpp::shutdown();
20 exit(signal);
21}
22
23class PlayEmojiClient : public rclcpp::Node {
24public:
25 PlayEmojiClient() : Node("play_emoji_client") {
26 client_ = this->create_client<aimdk_msgs::srv::PlayEmoji>(
27 "/face_ui_proxy/play_emoji");
28 RCLCPP_INFO(this->get_logger(), "✅ PlayEmoji client node started.");
29
30 // Wait for the service to become available
31 while (!client_->wait_for_service(std::chrono::seconds(2))) {
32 RCLCPP_INFO(this->get_logger(), "⏳ Service unavailable, waiting...");
33 }
34 RCLCPP_INFO(this->get_logger(),
35 "🟢 Service available, ready to send request.");
36 }
37
38 bool send_request(uint8_t emoji, uint8_t mode, int32_t priority) {
39 try {
40 auto request = std::make_shared<aimdk_msgs::srv::PlayEmoji::Request>();
41
42 request->emotion_id = emoji;
43 request->mode = mode;
44 request->priority = priority;
45
46 RCLCPP_INFO(
47 this->get_logger(),
48 "📨 Sending request to play emoji: id=%hhu, mode=%hhu, priority=%d",
49 emoji, mode, priority);
50
51 const std::chrono::milliseconds timeout(250);
52 for (int i = 0; i < 8; i++) {
53 request->header.header.stamp = this->now();
54 auto future = client_->async_send_request(request);
55 auto retcode = rclcpp::spin_until_future_complete(shared_from_this(),
56 future, timeout);
57 if (retcode != rclcpp::FutureReturnCode::SUCCESS) {
58 // retry as remote peer is NOT handled well by ROS
59 RCLCPP_INFO(this->get_logger(), "trying ... [%d]", i);
60 continue;
61 }
62 // future.done
63 auto response = future.get();
64 if (response->success) {
65 RCLCPP_INFO(this->get_logger(),
66 "✅ Request to play emoji recorded successfully: %s",
67 response->message.c_str());
68 return true;
69 } else {
70 RCLCPP_ERROR(this->get_logger(),
71 "❌ Failed to record play-emoji request: %s",
72 response->message.c_str());
73 return false;
74 }
75 }
76 RCLCPP_ERROR(this->get_logger(), "❌ Service call failed or timed out.");
77 return false;
78 } catch (const std::exception &e) {
79 RCLCPP_ERROR(this->get_logger(), "Exception occurred: %s", e.what());
80 return false;
81 }
82 }
83
84private:
85 rclcpp::Client<aimdk_msgs::srv::PlayEmoji>::SharedPtr client_;
86};
87
88int main(int argc, char **argv) {
89 try {
90 rclcpp::init(argc, argv);
91
92 // Set up signal handlers
93 signal(SIGINT, signal_handler);
94 signal(SIGTERM, signal_handler);
95
96 int32_t priority = 10;
97
98 int emotion = 1; // Expression type, 1 means Blink
99 std::cout
100 << "Enter expression ID: 1-Blink, 60-Bored, 70-Abnormal, 80-Sleeping, "
101 "90-Happy, 190-Very Angry, 200-Adoration"
102 << std::endl;
103 std::cin >> emotion;
104
105 int mode = 1; // Playback mode, 1 means play once, 2 means loop
106 std::cout << "Enter play mode (1: once, 2: loop): ";
107 std::cin >> mode;
108 if (mode < 1 || mode > 2) {
109 RCLCPP_ERROR(rclcpp::get_logger("main"), "Invalid play mode: %d", mode);
110 rclcpp::shutdown();
111 return 1;
112 }
113
114 g_node = std::make_shared<PlayEmojiClient>();
115 auto client = std::dynamic_pointer_cast<PlayEmojiClient>(g_node);
116
117 if (client) {
118 client->send_request(emotion, mode, priority);
119 }
120
121 // Clean up resources
122 g_node.reset();
123 rclcpp::shutdown();
124
125 return 0;
126 } catch (const std::exception &e) {
127 RCLCPP_ERROR(rclcpp::get_logger("main"),
128 "Program exited with exception: %s", e.what());
129 return 1;
130 }
131}
6.2.19 LED Strip Control
Function Description: Demonstrates how to control the robot’s LED strip, supporting multiple display modes and customizable colors.
Core Code:
1#include <aimdk_msgs/msg/common_request.hpp>
2#include <aimdk_msgs/srv/led_strip_command.hpp>
3#include <chrono>
4#include <memory>
5#include <rclcpp/rclcpp.hpp>
6#include <signal.h>
7#include <string>
8
9std::shared_ptr<rclcpp::Node> g_node = nullptr;
10
11void signal_handler(int signal) {
12 if (g_node) {
13 RCLCPP_INFO(g_node->get_logger(), "Received signal %d, shutting down...",
14 signal);
15 g_node.reset();
16 }
17 rclcpp::shutdown();
18 exit(signal);
19}
20
21class PlayLightsClient : public rclcpp::Node {
22public:
23 PlayLightsClient() : Node("play_lights_client") {
24 client_ = this->create_client<aimdk_msgs::srv::LedStripCommand>(
25 "/aimdk_5Fmsgs/srv/LedStripCommand");
26 RCLCPP_INFO(this->get_logger(), "✅ PlayLights client node started.");
27
28 // Wait for the service to become available
29 while (!client_->wait_for_service(std::chrono::seconds(2))) {
30 RCLCPP_INFO(this->get_logger(), "⏳ Service unavailable, waiting...");
31 }
32 RCLCPP_INFO(this->get_logger(),
33 "🟢 Service available, ready to send request.");
34 }
35
36 bool send_request(uint8_t led_mode, uint8_t r, uint8_t g, uint8_t b) {
37 try {
38 auto request =
39 std::make_shared<aimdk_msgs::srv::LedStripCommand::Request>();
40
41 request->led_strip_mode = led_mode;
42 request->r = r;
43 request->g = g;
44 request->b = b;
45
46 RCLCPP_INFO(this->get_logger(),
47 "📨 Sending request to control led strip: mode=%hhu, "
48 "RGB=(%hhu, %hhu, %hhu)",
49 led_mode, r, g, b);
50
51 // LED strip is slow to response (up to ~5s)
52 const std::chrono::milliseconds timeout(5000);
53 for (int i = 0; i < 4; i++) {
54 request->request.header.stamp = this->now();
55 auto future = client_->async_send_request(request);
56 auto retcode = rclcpp::spin_until_future_complete(
57 this->shared_from_this(), future, timeout);
58
59 if (retcode != rclcpp::FutureReturnCode::SUCCESS) {
60 // retry as remote peer is NOT handled well by ROS
61 RCLCPP_INFO(this->get_logger(), "trying ... [%d]", i);
62 continue;
63 }
64 // future.done
65 auto response = future.get();
66 if (response->status_code == 0) {
67 RCLCPP_INFO(this->get_logger(),
68 "✅ LED strip command sent successfully.");
69 return true;
70 } else {
71 RCLCPP_ERROR(this->get_logger(),
72 "❌ LED strip command failed with status: %d",
73 response->status_code);
74 return false;
75 }
76 }
77 RCLCPP_ERROR(this->get_logger(), "❌ Service call failed or timed out.");
78 return false;
79 } catch (const std::exception &e) {
80 RCLCPP_ERROR(this->get_logger(), "Exception occurred: %s", e.what());
81 return false;
82 }
83 }
84
85private:
86 rclcpp::Client<aimdk_msgs::srv::LedStripCommand>::SharedPtr client_;
87};
88
89int main(int argc, char **argv) {
90 try {
91 rclcpp::init(argc, argv);
92 signal(SIGINT, signal_handler);
93 signal(SIGTERM, signal_handler);
94
95 g_node = std::make_shared<PlayLightsClient>();
96 auto client_node = std::dynamic_pointer_cast<PlayLightsClient>(g_node);
97
98 int led_mode = 0; // LED Strip Mode
99 int r = 255, g = 0, b = 0; // RGB values
100
101 std::cout << "=== LED Strip Control Example ===" << std::endl;
102 std::cout << "Select LED strip mode:" << std::endl;
103 std::cout << "0 - Steady On" << std::endl;
104 std::cout << "1 - Breathing (4s period, sinusoidal brightness)"
105 << std::endl;
106 std::cout << "2 - Blinking (1s period, 0.5s on, 0.5s off)" << std::endl;
107 std::cout << "3 - Flow (2s period, lights turn on left to right)"
108 << std::endl;
109 std::cout << "Enter mode (0-3): ";
110 std::cin >> led_mode;
111
112 std::cout << "\nSet RGB color values (0-255):" << std::endl;
113 std::cout << "Red component (R): ";
114 std::cin >> r;
115 std::cout << "Green component (G): ";
116 std::cin >> g;
117 std::cout << "Blue component (B): ";
118 std::cin >> b;
119
120 // clamp mode to range 0-3
121 led_mode = std::max(0, std::min(3, led_mode));
122 // clamp r/g/b to range 0-255
123 r = std::max(0, std::min(255, r));
124 g = std::max(0, std::min(255, g));
125 b = std::max(0, std::min(255, b));
126
127 if (client_node) {
128 client_node->send_request(led_mode, r, g, b);
129 }
130
131 g_node.reset();
132 rclcpp::shutdown();
133
134 return 0;
135 } catch (const std::exception &e) {
136 RCLCPP_ERROR(rclcpp::get_logger("main"),
137 "Program terminated with exception: %s", e.what());
138 return 1;
139 }
140}
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