mirs_esp_pio
Toggle main menu visibility
読み取り中…
検索中…
一致する文字列を見つけられません
telemetry_publisher.cpp
[詳解]
1
#include "
telemetry_publisher.hpp
"
2
3
#include <rclc/rclc.h>
4
5
#include <cstdlib>
6
7
bool
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
33
void
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
44
bool
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
66
void
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
93
void
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
}
TelemetryPublisher::advertise
bool advertise(rcl_node_t *node) override
publisher生成+メッセージバッファ確保。
定義
telemetry_publisher.cpp:44
TelemetryPublisher::release
void release(rcl_node_t *node) override
publisher破棄+バッファ解放。未advertiseへの呼出しも安全にすること。
定義
telemetry_publisher.cpp:93
TelemetryPublisher::publish
void publish(const SharedMotion &motion, const SharedSensor &sensor, int32_t now_sec, uint32_t now_nsec) override
周期発行。データ不在時は無発行にすること。
定義
telemetry_publisher.cpp:66
ignoreResult
void ignoreResult(rcl_ret_t rc)
ros_task用PublisherプラグインIF。センサ側Registryと対称の枠組み。
定義
publisher_plugin.hpp:13
SharedMotion
運動系テレメトリ。
定義
SystemContext.hpp:43
SharedMotion::vel_l
float vel_l
定義
SystemContext.hpp:46
SharedMotion::vel_cmd_l
float vel_cmd_l
定義
SystemContext.hpp:48
SharedMotion::rc_pulse
uint16_t rc_pulse[3]
定義
SystemContext.hpp:50
SharedMotion::vel_cmd_r
float vel_cmd_r
定義
SystemContext.hpp:49
SharedMotion::ctrl_mode
uint8_t ctrl_mode
定義
SystemContext.hpp:51
SharedMotion::count_r
int32_t count_r
定義
SystemContext.hpp:45
SharedMotion::vel_r
float vel_r
定義
SystemContext.hpp:47
SharedMotion::count_l
int32_t count_l
定義
SystemContext.hpp:44
SharedSensor
センサ系テレメトリ。
定義
SystemContext.hpp:61
SharedSensor::voltage_1
float voltage_1
定義
SystemContext.hpp:62
SharedSensor::voltage_2
float voltage_2
定義
SystemContext.hpp:63
telemetry_publisher.hpp
車輪・RC信号・電圧などの計測値を確認用のROSトピックへ配信する。
src
publishers
telemetry_publisher.cpp
構築:
1.18.0