mirs_esp_pio
読み取り中…
検索中…
一致する文字列を見つけられません
imu_publisher.cpp
[詳解]
1#include "imu_publisher.hpp"
2
3#include <rclc/rclc.h>
4
5#include <cstring>
6
7bool 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
45void 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
81void ImuPublisher::release(rcl_node_t *node) {
82 ignoreResult(rcl_publisher_fini(&imu_pub_, node));
83 ignoreResult(rcl_publisher_fini(&mag_pub_, node));
84}
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
周期発行。データ不在時は無発行にすること。
bool advertise(rcl_node_t *node) override
publisher生成+メッセージバッファ確保。
共有されたIMUの計測値をROSメッセージに詰め、配信する。
void ignoreResult(rcl_ret_t rc)
ros_task用PublisherプラグインIF。センサ側Registryと対称の枠組み。
運動系テレメトリ。
センサ系テレメトリ。