mirs_esp_pio
Toggle main menu visibility
読み取り中…
検索中…
一致する文字列を見つけられません
imu_publisher.cpp
[詳解]
1
#include "
imu_publisher.hpp
"
2
3
#include <rclc/rclc.h>
4
5
#include <cstring>
6
7
bool
ImuPublisher::advertise
(rcl_node_t *node) {
8
imu_msg_.header.frame_id.data = (
char
*)
"imu_link"
;
9
imu_msg_.header.frame_id.size = strlen(
"imu_link"
);
10
imu_msg_.header.frame_id.capacity = imu_msg_.header.frame_id.size + 1;
11
12
// orientationは機上で推定しないため未知扱い(covariance[0]=-1)。
13
// 四元数は無効値(0,0,0,0)を避けて単位四元子で初期化する(他consumer対策)
14
imu_msg_.orientation.w = 1.0;
15
// covariance diagonals are rough estimates (unit: sensor spec dependent)
16
constexpr
double
kCovDiag = 0.01;
17
auto
set_diag3 = [](
double
*cov) {
18
cov[0] = kCovDiag;
19
cov[4] = kCovDiag;
20
cov[8] = kCovDiag;
21
};
22
imu_msg_.orientation_covariance[0] = -1.0;
23
set_diag3(imu_msg_.angular_velocity_covariance);
24
set_diag3(imu_msg_.linear_acceleration_covariance);
25
26
mag_msg_.header.frame_id.data = (
char
*)
"imu_link"
;
27
mag_msg_.header.frame_id.size = strlen(
"imu_link"
);
28
mag_msg_.header.frame_id.capacity = mag_msg_.header.frame_id.size + 1;
29
30
// magnetic_field_covariance: diagonal (rough estimate)
31
set_diag3(mag_msg_.magnetic_field_covariance);
32
33
if
(rclc_publisher_init_default(&imu_pub_, node,
34
ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Imu),
35
"/imu/data_raw"
) != RCL_RET_OK ||
36
rclc_publisher_init_default(&mag_pub_, node,
37
ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, MagneticField),
38
"/imu/mag"
) != RCL_RET_OK) {
39
release
(node);
40
return
false
;
41
}
42
return
true
;
43
}
44
45
void
ImuPublisher::publish
(
const
SharedMotion
&
/*motion*/
,
const
SharedSensor
&sensor,
46
int32_t now_sec, uint32_t now_nsec) {
47
// IMU (BMX055) は15ms周期の4回に1回(約17Hz)パブリッシュする
48
if
(!divider_.tick()) {
49
return
;
50
}
51
if
(!sensor.
imu_ok
&& !sensor.
mag_ok
) {
52
return
;
53
}
54
if
(sensor.
imu_ok
) {
55
imu_msg_.header.stamp.sec = now_sec;
56
imu_msg_.header.stamp.nanosec = now_nsec;
57
58
imu_msg_.linear_acceleration.x = sensor.
ax
;
59
imu_msg_.linear_acceleration.y = sensor.
ay
;
60
imu_msg_.linear_acceleration.z = sensor.
az
;
61
62
imu_msg_.angular_velocity.x = sensor.
gx
;
63
imu_msg_.angular_velocity.y = sensor.
gy
;
64
imu_msg_.angular_velocity.z = sensor.
gz
;
65
66
ignoreResult
(rcl_publish(&imu_pub_, &imu_msg_, NULL));
67
}
68
69
if
(sensor.
mag_ok
) {
70
mag_msg_.header.stamp.sec = now_sec;
71
mag_msg_.header.stamp.nanosec = now_nsec;
72
73
mag_msg_.magnetic_field.x = sensor.
mx
* 1e-6f;
// uT -> Tesla
74
mag_msg_.magnetic_field.y = sensor.
my
* 1e-6f;
75
mag_msg_.magnetic_field.z = sensor.
mz
* 1e-6f;
76
77
ignoreResult
(rcl_publish(&mag_pub_, &mag_msg_, NULL));
78
}
79
}
80
81
void
ImuPublisher::release
(rcl_node_t *node) {
82
ignoreResult
(rcl_publisher_fini(&imu_pub_, node));
83
ignoreResult
(rcl_publisher_fini(&mag_pub_, node));
84
}
ImuPublisher::release
void release(rcl_node_t *node) override
publisher破棄+バッファ解放。未advertiseへの呼出しも安全にすること。
定義
imu_publisher.cpp:81
ImuPublisher::publish
void publish(const SharedMotion &motion, const SharedSensor &sensor, int32_t now_sec, uint32_t now_nsec) override
周期発行。データ不在時は無発行にすること。
定義
imu_publisher.cpp:45
ImuPublisher::advertise
bool advertise(rcl_node_t *node) override
publisher生成+メッセージバッファ確保。
定義
imu_publisher.cpp:7
imu_publisher.hpp
共有されたIMUの計測値をROSメッセージに詰め、配信する。
ignoreResult
void ignoreResult(rcl_ret_t rc)
ros_task用PublisherプラグインIF。センサ側Registryと対称の枠組み。
定義
publisher_plugin.hpp:13
SharedMotion
運動系テレメトリ。
定義
SystemContext.hpp:43
SharedSensor
センサ系テレメトリ。
定義
SystemContext.hpp:61
SharedSensor::gz
float gz
定義
SystemContext.hpp:67
SharedSensor::imu_ok
bool imu_ok
定義
SystemContext.hpp:64
SharedSensor::ax
float ax
定義
SystemContext.hpp:66
SharedSensor::az
float az
定義
SystemContext.hpp:66
SharedSensor::my
float my
定義
SystemContext.hpp:68
SharedSensor::mx
float mx
定義
SystemContext.hpp:68
SharedSensor::mag_ok
bool mag_ok
定義
SystemContext.hpp:65
SharedSensor::mz
float mz
定義
SystemContext.hpp:68
SharedSensor::gx
float gx
定義
SystemContext.hpp:67
SharedSensor::gy
float gy
定義
SystemContext.hpp:67
SharedSensor::ay
float ay
定義
SystemContext.hpp:66
src
publishers
imu_publisher.cpp
構築:
1.18.0