mirs_esp_pio
読み取り中…
検索中…
一致する文字列を見つけられません
odom_publisher.cpp
[詳解]
1#include "odom_publisher.hpp"
2
3#include <rclc/rclc.h>
4
5#include <cmath>
6#include <cstring>
7
8bool OdomPublisher::advertise(rcl_node_t *node) {
9 odom_msg_.header.frame_id.data = (char *)"odom";
10 odom_msg_.header.frame_id.size = strlen("odom");
11 odom_msg_.header.frame_id.capacity = odom_msg_.header.frame_id.size + 1;
12 odom_msg_.child_frame_id.data = (char *)"base_footprint";
13 odom_msg_.child_frame_id.size = strlen("base_footprint");
14 odom_msg_.child_frame_id.capacity = odom_msg_.child_frame_id.size + 1;
15
16 // pose / twist covariance: diagonal (yawのみ0.05。rough estimate)
17 constexpr double kCovDiag = 0.01;
18 constexpr double kCovYaw = 0.05;
19 auto set_diag6 = [](double *cov) {
20 cov[0] = kCovDiag;
21 cov[7] = kCovDiag;
22 cov[14] = kCovDiag;
23 cov[21] = kCovDiag;
24 cov[28] = kCovDiag;
25 cov[35] = kCovDiag;
26 };
27 set_diag6(odom_msg_.pose.covariance);
28 odom_msg_.pose.covariance[14] = kCovYaw;
29 set_diag6(odom_msg_.twist.covariance);
30
31 return rclc_publisher_init_default(&odom_pub_, node,
32 ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs, msg, Odometry),
33 "/odom") == RCL_RET_OK;
34}
35
36void OdomPublisher::publish(const SharedMotion &motion, const SharedSensor & /*sensor*/,
37 int32_t now_sec, uint32_t now_nsec) {
38 if (!divider_.tick()) {
39 return;
40 }
41 const double half_yaw = motion.odom_theta * 0.5;
42
43 odom_msg_.header.stamp.sec = now_sec;
44 odom_msg_.header.stamp.nanosec = now_nsec;
45 odom_msg_.pose.pose.position.x = motion.odom_x;
46 odom_msg_.pose.pose.position.y = motion.odom_y;
47 odom_msg_.pose.pose.orientation.z = std::sin(half_yaw);
48 odom_msg_.pose.pose.orientation.w = std::cos(half_yaw);
49 odom_msg_.twist.twist.linear.x = motion.lin_vel;
50 odom_msg_.twist.twist.angular.z = motion.ang_vel;
51
52 ignoreResult(rcl_publish(&odom_pub_, &odom_msg_, NULL));
53}
54
55void OdomPublisher::release(rcl_node_t *node) {
56 ignoreResult(rcl_publisher_fini(&odom_pub_, node));
57}
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生成+メッセージバッファ確保。
推定した機体の位置・向き・速度をOdometryとして配信する。
void ignoreResult(rcl_ret_t rc)
ros_task用PublisherプラグインIF。センサ側Registryと対称の枠組み。
運動系テレメトリ。
センサ系テレメトリ。