mirs_esp_pio
Toggle main menu visibility
読み取り中…
検索中…
一致する文字列を見つけられません
odom_publisher.cpp
[詳解]
1
#include "
odom_publisher.hpp
"
2
3
#include <rclc/rclc.h>
4
5
#include <cmath>
6
#include <cstring>
7
8
bool
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
36
void
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
55
void
OdomPublisher::release
(rcl_node_t *node) {
56
ignoreResult
(rcl_publisher_fini(&odom_pub_, node));
57
}
OdomPublisher::release
void release(rcl_node_t *node) override
publisher破棄+バッファ解放。未advertiseへの呼出しも安全にすること。
定義
odom_publisher.cpp:55
OdomPublisher::publish
void publish(const SharedMotion &motion, const SharedSensor &sensor, int32_t now_sec, uint32_t now_nsec) override
周期発行。データ不在時は無発行にすること。
定義
odom_publisher.cpp:36
OdomPublisher::advertise
bool advertise(rcl_node_t *node) override
publisher生成+メッセージバッファ確保。
定義
odom_publisher.cpp:8
odom_publisher.hpp
推定した機体の位置・向き・速度をOdometryとして配信する。
ignoreResult
void ignoreResult(rcl_ret_t rc)
ros_task用PublisherプラグインIF。センサ側Registryと対称の枠組み。
定義
publisher_plugin.hpp:13
SharedMotion
運動系テレメトリ。
定義
SystemContext.hpp:43
SharedMotion::odom_y
float odom_y
定義
SystemContext.hpp:53
SharedMotion::odom_x
float odom_x
定義
SystemContext.hpp:52
SharedMotion::ang_vel
float ang_vel
定義
SystemContext.hpp:56
SharedMotion::odom_theta
float odom_theta
定義
SystemContext.hpp:54
SharedMotion::lin_vel
float lin_vel
定義
SystemContext.hpp:55
SharedSensor
センサ系テレメトリ。
定義
SystemContext.hpp:61
src
publishers
odom_publisher.cpp
構築:
1.18.0