mirs_esp_pio
読み取り中…
検索中…
一致する文字列を見つけられません
telemetry_publisher.cpp
[詳解]
2
3#include <rclc/rclc.h>
4
5#include <cstdlib>
6
7bool TelemetryPublisher::allocBuffers() {
8 enc_msg_.data.capacity = 2;
9 enc_msg_.data.size = 2;
10 enc_msg_.data.data = (int32_t *)malloc(2 * sizeof(int32_t));
11 vel_msg_.data.capacity = 2;
12 vel_msg_.data.size = 2;
13 vel_msg_.data.data = (double *)malloc(2 * sizeof(double));
14 vlt_msg_.data.capacity = 2;
15 vlt_msg_.data.size = 2;
16 vlt_msg_.data.data = (double *)malloc(2 * sizeof(double));
17 rc_debug_msg_.data.capacity = 6;
18 rc_debug_msg_.data.size = 6;
19 rc_debug_msg_.data.data = (double *)malloc(6 * sizeof(double));
20
21 if (enc_msg_.data.data == nullptr || vel_msg_.data.data == nullptr ||
22 vlt_msg_.data.data == nullptr || rc_debug_msg_.data.data == nullptr) {
23 freeBuffers();
24 return false;
25 }
26 enc_msg_.data.data[0] = enc_msg_.data.data[1] = 0;
27 vel_msg_.data.data[0] = vel_msg_.data.data[1] = 0.0;
28 vlt_msg_.data.data[0] = vlt_msg_.data.data[1] = 0.0;
29 for (int i = 0; i < 6; i++) rc_debug_msg_.data.data[i] = 0.0;
30 return true;
31}
32
33void TelemetryPublisher::freeBuffers() {
34 free(enc_msg_.data.data);
35 free(vel_msg_.data.data);
36 free(vlt_msg_.data.data);
37 free(rc_debug_msg_.data.data);
38 enc_msg_.data.data = nullptr;
39 vel_msg_.data.data = nullptr;
40 vlt_msg_.data.data = nullptr;
41 rc_debug_msg_.data.data = nullptr;
42}
43
44bool TelemetryPublisher::advertise(rcl_node_t *node) {
45 if (!allocBuffers()) {
46 return false;
47 }
48 if (rclc_publisher_init_default(&enc_pub_, node,
49 ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32MultiArray),
50 "/encoder") != RCL_RET_OK ||
51 rclc_publisher_init_default(&vel_pub_, node,
52 ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float64MultiArray),
53 "/vel") != RCL_RET_OK ||
54 rclc_publisher_init_default(&vlt_pub_, node,
55 ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float64MultiArray),
56 "/vlt") != RCL_RET_OK ||
57 rclc_publisher_init_default(&rc_debug_pub_, node,
58 ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float64MultiArray),
59 "/rc_debug") != RCL_RET_OK) {
60 release(node);
61 return false;
62 }
63 return true;
64}
65
66void TelemetryPublisher::publish(const SharedMotion &motion, const SharedSensor &sensor,
67 int32_t /*now_sec*/, uint32_t /*now_nsec*/) {
68 enc_msg_.data.data[0] = motion.count_l;
69 enc_msg_.data.data[1] = motion.count_r;
70 vel_msg_.data.data[0] = motion.vel_l;
71 vel_msg_.data.data[1] = motion.vel_r;
72
73 if (!divider_.tick()) {
74 return;
75 }
76 vlt_msg_.data.data[0] = sensor.voltage_1;
77 vlt_msg_.data.data[1] = sensor.voltage_2;
78
79 // RC デバッグ情報: [0:ChLeft_us, 1:ChMode_us, 2:ChRight_us, 3:l_vel_cmd, 4:r_vel_cmd, 5:Mode(0:Manual,1:ROS2)]
80 rc_debug_msg_.data.data[0] = (double)motion.rc_pulse[0];
81 rc_debug_msg_.data.data[1] = (double)motion.rc_pulse[1];
82 rc_debug_msg_.data.data[2] = (double)motion.rc_pulse[2];
83 rc_debug_msg_.data.data[3] = motion.vel_cmd_l;
84 rc_debug_msg_.data.data[4] = motion.vel_cmd_r;
85 rc_debug_msg_.data.data[5] = (double)motion.ctrl_mode;
86
87 ignoreResult(rcl_publish(&enc_pub_, &enc_msg_, NULL));
88 ignoreResult(rcl_publish(&vel_pub_, &vel_msg_, NULL));
89 ignoreResult(rcl_publish(&vlt_pub_, &vlt_msg_, NULL));
90 ignoreResult(rcl_publish(&rc_debug_pub_, &rc_debug_msg_, NULL));
91}
92
93void TelemetryPublisher::release(rcl_node_t *node) {
94 ignoreResult(rcl_publisher_fini(&enc_pub_, node));
95 ignoreResult(rcl_publisher_fini(&vel_pub_, node));
96 ignoreResult(rcl_publisher_fini(&vlt_pub_, node));
97 ignoreResult(rcl_publisher_fini(&rc_debug_pub_, node));
98 freeBuffers();
99}
bool advertise(rcl_node_t *node) override
publisher生成+メッセージバッファ確保。
void release(rcl_node_t *node) override
publisher破棄+バッファ解放。未advertiseへの呼出しも安全にすること。
void publish(const SharedMotion &motion, const SharedSensor &sensor, int32_t now_sec, uint32_t now_nsec) override
周期発行。データ不在時は無発行にすること。
void ignoreResult(rcl_ret_t rc)
ros_task用PublisherプラグインIF。センサ側Registryと対称の枠組み。
運動系テレメトリ。
センサ系テレメトリ。
車輪・RC信号・電圧などの計測値を確認用のROSトピックへ配信する。