mirs_esp_pio
読み取り中…
検索中…
一致する文字列を見つけられません
ros_task.cpp
[詳解]
1#include "ros_task.hpp"
2#include <micro_ros_platformio.h>
3#include <Arduino.h>
4#include <freertos/FreeRTOS.h>
5#include <freertos/task.h>
6#include <rcl/rcl.h>
7#include <rclc/rclc.h>
8#include <rclc/executor.h>
9#include <rmw_microros/rmw_microros.h>
10#include <geometry_msgs/msg/twist.h>
11#include <std_msgs/msg/empty.h>
12#include <mirs_msgs/msg/basic_param.h>
13#include <cmath>
14
19#include "hardware_config.hpp"
20#include "SystemContext.hpp"
21#include "control_task.hpp"
22
23// ============================================================
24// 通信タスク設定
25// ============================================================
26#define ROS_DOMAIN_ID 90
27
28// /params受付範囲(範囲外・NaNは車体を不安定化させるため棄却する)
29#define PARAM_RADIUS_MAX 0.5 // [m]
30#define PARAM_BASE_MAX 2.0 // [m]
31#define PARAM_GAIN_ABS_MAX 1000.0
32
33// ============================================================
34// micro-ROS オブジェクト
35// ============================================================
36static rcl_allocator_t allocator;
37static rclc_support_t support;
38static rcl_node_t node;
39static rclc_executor_t executor;
40static rcl_timer_t timer;
41
42static rcl_subscription_t cmd_vel_sub, param_sub, reset_odom_sub;
43
44static geometry_msgs__msg__Twist cmd_vel_msg;
45static mirs_msgs__msg__BasicParam param_msg;
46static std_msgs__msg__Empty reset_odom_msg;
47
48// ---- 合成ルート:発行トピックの追加・削除はこの登録リスト1行 ----
49// データ不在時の無発行は各プラグインが担当するため、#if分離はしない。
58
59// /params最終受信時刻。parameter_publisherは500ms周期のため、
60// agent到達可なのに3s途絶えたら陳腐セッションとみなして再setupする。
61static uint32_t last_param_ms = 0;
62static bool params_seen = false;
63
64// micro-ROS コールバック
65static void cmd_vel_callback(const void *msgin) {
66 const auto *msg = (const geometry_msgs__msg__Twist *)msgin;
67 // 制御タスクへmailbox投入する(直接触らない。ウォッチドッグは制御側で判定)
69 cmd.stamp_ms = millis();
70
71 // 非有限値をPID/モーター出力へ流さない。不正指令はmailboxをゼロ指令で
72 // 上書きし、ROSタスクと制御タスク間の遅延を最小にして停止させる。
73 if (!std::isfinite(msg->linear.x) || !std::isfinite(msg->angular.z)) {
74 cmd.linear_x = 0.0f;
75 cmd.angular_z = 0.0f;
76 (void)g_sys.pushRosCmd(cmd);
77 return;
78 }
79
80 // ROSメッセージはdouble、mailboxはfloat。変換前に機体上限へ制限し、
81 // finiteだがfloat範囲外の値が変換後にInfになるのを防ぐ。
82 const double max_linear = static_cast<double>(MAX_LINEAR_SPEED);
83 const double max_angular = static_cast<double>(MAX_ANGULAR_SPEED);
84 const double linear = msg->linear.x;
85 const double angular = msg->angular.z;
86 cmd.linear_x = static_cast<float>(linear > max_linear ? max_linear
87 : (linear < -max_linear ? -max_linear : linear));
88 cmd.angular_z = static_cast<float>(angular > max_angular ? max_angular
89 : (angular < -max_angular ? -max_angular : angular));
90 (void)g_sys.pushRosCmd(cmd);
91}
92
93static bool paramsInRange(const mirs_msgs__msg__BasicParam *p) {
94 if (!std::isfinite(p->wheel_radius) || p->wheel_radius <= 0.0 ||
95 p->wheel_radius > PARAM_RADIUS_MAX) {
96 return false;
97 }
98 if (!std::isfinite(p->wheel_base) || p->wheel_base <= 0.0 ||
99 p->wheel_base > PARAM_BASE_MAX) {
100 return false;
101 }
102 const double gains[6] = {p->rkp, p->rki, p->rkd, p->lkp, p->lki, p->lkd};
103 for (double g : gains) {
104 if (!std::isfinite(g) || std::fabs(g) > PARAM_GAIN_ABS_MAX) {
105 return false;
106 }
107 }
108 return true;
109}
110
111static void param_callback(const void *msgin) {
112 const auto *p = (const mirs_msgs__msg__BasicParam *)msgin;
113 last_param_ms = millis();
114 params_seen = true;
115 if (!paramsInRange(p)) {
116 Serial.println("[ros] /params rejected (out of range or NaN)");
117 return;
118 }
119 // SystemContextへ書込む。制御タスクが毎周期適用する
120 SharedParams sp;
121 sp.wheel_radius = p->wheel_radius;
122 sp.wheel_base = p->wheel_base;
123 sp.rkp = p->rkp;
124 sp.rki = p->rki;
125 sp.rkd = p->rkd;
126 sp.lkp = p->lkp;
127 sp.lki = p->lki;
128 sp.lkd = p->lkd;
129 g_sys.setParams(sp);
130}
131
132// 原点リセット要求(停止中に呼ぶこと)。制御周期内でodom x/y/thetaをゼロ化する。
133static void reset_odom_callback(const void * /*msgin*/) {
135}
136
137static void timer_callback(rcl_timer_t * /*timer*/, int64_t /*last_call_time*/) {
138 const SharedMotion motion = g_sys.getMotion();
139 const SharedSensor sensor = g_sys.getSensor();
140
141 const int64_t now_ns = (int64_t)rmw_uros_epoch_nanos();
142 const int32_t now_sec = (int32_t)(now_ns / 1000000000LL);
143 const uint32_t now_nsec = (uint32_t)(now_ns % 1000000000LL);
144
145 for (IPublisherPlugin *pub : publishers) {
146 pub->publish(motion, sensor, now_sec, now_nsec);
147 }
148}
149
150// micro-ROS セットアップ。成功時true、失敗時はrosTask側でteardown→リトライする
151#define ROS_CHECK(fn) \
152 do { \
153 rcl_ret_t _rc = (fn); \
154 if (_rc != RCL_RET_OK) { \
155 Serial.printf("[ros] setup failed: %s:%d rc=%d\n", __func__, __LINE__, (int)_rc); \
156 return false; \
157 } \
158 } while (0)
159
160static bool ros_setup() {
161 allocator = rcl_get_default_allocator();
162
163 // ROS_DOMAIN_ID 設定 (Jazzy 以降)
164 rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
165 ROS_CHECK(rcl_init_options_init(&init_options, allocator));
166 rcl_ret_t domain_rc = rcl_init_options_set_domain_id(&init_options, ROS_DOMAIN_ID);
167 if (domain_rc != RCL_RET_OK) {
168 Serial.printf("[ros] domain_id failed rc=%d\n", (int)domain_rc);
169 ignoreResult(rcl_init_options_fini(&init_options));
170 return false;
171 }
172 rcl_ret_t support_rc = rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator);
173 ignoreResult(rcl_init_options_fini(&init_options));
174 if (support_rc != RCL_RET_OK) {
175 Serial.printf("[ros] support init failed rc=%d (agent不在の可能性)\n", (int)support_rc);
176 return false;
177 }
178
179 rcl_node_options_t node_ops = rcl_node_get_default_options();
180 ROS_CHECK(rclc_node_init_with_options(&node, "ESP32_node", "", &support, &node_ops));
181
182 // パブリッシャー(登録リスト順。失敗時は確保分を戻して全体失敗)
183 for (IPublisherPlugin *pub : publishers) {
184 if (!pub->advertise(&node)) {
185 Serial.printf("[ros] advertise failed: %s\n", pub->name());
186 for (IPublisherPlugin *rel : publishers) {
187 rel->release(&node);
188 }
189 return false;
190 }
191 }
192
193 // サブスクライバー
194 ROS_CHECK(rclc_subscription_init_default(&cmd_vel_sub, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist), "/cmd_vel"));
195 ROS_CHECK(rclc_subscription_init_default(&param_sub, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(mirs_msgs, msg, BasicParam), "/params"));
196 ROS_CHECK(rclc_subscription_init_default(&reset_odom_sub, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Empty), "/reset_odometry"));
197
198 // タイマー
199 ROS_CHECK(rclc_timer_init_default2(&timer, &support,
200 RCL_MS_TO_NS(TIMER_INTERVAL_MS), timer_callback, true));
201
202 // エグゼキューター: subscriber×3 + timer×1 = 4 ハンドル
203 ROS_CHECK(rclc_executor_init(&executor, &support.context, 4, &allocator));
204 ROS_CHECK(rclc_executor_add_subscription(&executor, &cmd_vel_sub, &cmd_vel_msg, &cmd_vel_callback, ON_NEW_DATA));
205 ROS_CHECK(rclc_executor_add_subscription(&executor, &param_sub, &param_msg, &param_callback, ON_NEW_DATA));
206 ROS_CHECK(rclc_executor_add_subscription(&executor, &reset_odom_sub, &reset_odom_msg, &reset_odom_callback, ON_NEW_DATA));
207 ROS_CHECK(rclc_executor_add_timer(&executor, &timer));
208 return true;
209}
210
211// 部分初期化済みリソースの後始末。未初期化へのfiniはエラーを無視する
212static void ros_teardown() {
213 ignoreResult(rclc_executor_fini(&executor));
214 ignoreResult(rcl_timer_fini(&timer));
215 ignoreResult(rcl_subscription_fini(&cmd_vel_sub, &node));
216 ignoreResult(rcl_subscription_fini(&param_sub, &node));
217 ignoreResult(rcl_subscription_fini(&reset_odom_sub, &node));
218 for (IPublisherPlugin *pub : publishers) {
219 pub->release(&node);
220 }
221 ignoreResult(rcl_node_fini(&node));
222 ignoreResult(rclc_support_fini(&support));
223}
224
225void rosTask(void *arg) {
226 (void)arg;
227 Serial.begin(115200);
228 Serial.printf("[fw] mirs_esp_pio %s\n", FW_VERSION);
229 set_microros_serial_transports(Serial);
230
231 // agent不在でもcontrol/sensorは継続。rosタスクのみここで待機・リトライする
232 // (advertise失敗時もteardown→リトライする。control/sensorは継続する)
233 for (;;) {
234 // --- agent待ち (他タスクは継続中) ---
235 while (rmw_uros_ping_agent(500, 2) != RMW_RET_OK) {
236 vTaskDelay(pdMS_TO_TICKS(2000));
237 }
238 if (!ros_setup()) {
239 ros_teardown();
240 vTaskDelay(pdMS_TO_TICKS(2000));
241 continue;
242 }
243 // 時刻同期はsupport初期化後(=setup後)に行う。初期化前のsyncは
244 // 効かず、未同期stampでのpublishはEKFを不可逆汚染するため出さない。
245 bool synced = false;
246 for (int i = 0; i < 5 && !synced; ++i) {
247 synced = (rmw_uros_sync_session(1000) == RMW_RET_OK);
248 }
249 if (!synced) {
250 Serial.println("[ros] time sync failed, retrying setup");
251 ros_teardown();
252 vTaskDelay(pdMS_TO_TICKS(2000));
253 continue;
254 }
255 // 新しいROSセッションでは新しい/params受信から途絶監視を始める。
256 // 前セッションの受信状態を残すと、再接続直後に即teardownし得る。
257 params_seen = false;
258 last_param_ms = millis();
259 // --- 接続中ループ。切断検出で抜けて再接続へ ---
260 bool time_synced = true; // setup直後に同期済み。再同期の遷移検出用。
261 uint32_t last_ping_ms = millis();
262 uint8_t ping_fail = 0;
263 bool session_broken = false;
264 while (!session_broken) {
265 // micro-ROS通信処理(ノンブロッキング)
266 rclc_executor_spin_some(&executor, RCL_MS_TO_NS(1));
267 // agent再起動後の陳腐セッション対策:pingは通るが購読が届かない状態を
268 // 500ms周期の/params途絶(3s)で検出して作り直す。一度も受信前は判定しない。
269 if (params_seen && (millis() - last_param_ms > 3000)) {
270 Serial.println("[ros] params stalled, re-syncing session");
271 session_broken = true;
272 break;
273 }
274 if ((millis() - last_ping_ms) >= 2000) {
275 last_ping_ms = millis();
276 if (rmw_uros_ping_agent(100, 1) != RMW_RET_OK) {
277 if (++ping_fail >= 3) {
278 Serial.println("[ros] agent lost, reconnecting");
279 session_broken = true;
280 }
281 } else {
282 ping_fail = 0;
283 // 接続後の再同期(水晶ドリフト対策)。成功遷移時のみログする。
284 // NOTE: 未同期publishは行わない方針(setup直後に同期済みのため)。
285 if (rmw_uros_sync_session(100) == RMW_RET_OK && !time_synced) {
286 time_synced = true;
287 Serial.println("[ros] time synced");
288 }
289 }
290 }
291 vTaskDelay(pdMS_TO_TICKS(1));
292 }
293 ros_teardown();
294 vTaskDelay(pdMS_TO_TICKS(1000));
295 }
296}
SystemContext g_sys
制御・通信・センサのタスク間で、指令と計測値を排他制御して受け渡す。
IMU発行(/imu/data_raw・/imu/mag。15ms周期の4回に1回=約17Hz)。
オドメトリ発行(/odom。15ms周期の4回に1回=約17Hz。frame: odom -> base_footprint)。
void setParams(const SharedParams &p)
bool pushRosCmd(const RosVelocityCmd &cmd)
SharedSensor getSensor()
SharedMotion getMotion()
低頻度テレメトリ一括発行(/encoder・/vel・/vlt・/rc_debug)。
void requestOdometryReset()
指令の受付、操作元の切替、速度推定、モータ出力を周期的に実行する。
ESP32のピン、RC信号の範囲、速度上限と周期を定義する。
#define FW_VERSION
#define MAX_LINEAR_SPEED
#define MAX_ANGULAR_SPEED
#define TIMER_INTERVAL_MS
共有されたIMUの計測値をROSメッセージに詰め、配信する。
推定した機体の位置・向き・速度をOdometryとして配信する。
ROS配信処理の初期化と周期処理に使う共通の窓口を定義する。
void ignoreResult(rcl_ret_t rc)
ros_task用PublisherプラグインIF。センサ側Registryと対称の枠組み。
static OdomPublisher odom_pub_plugin
static void ros_teardown()
#define PARAM_GAIN_ABS_MAX
static bool params_seen
static rcl_subscription_t reset_odom_sub
static void timer_callback(rcl_timer_t *, int64_t)
#define ROS_DOMAIN_ID
static uint32_t last_param_ms
static void param_callback(const void *msgin)
static rcl_subscription_t param_sub
static void reset_odom_callback(const void *)
static std_msgs__msg__Empty reset_odom_msg
static rclc_executor_t executor
static mirs_msgs__msg__BasicParam param_msg
static geometry_msgs__msg__Twist cmd_vel_msg
static rcl_subscription_t cmd_vel_sub
static void cmd_vel_callback(const void *msgin)
static ImuPublisher imu_pub_plugin
static rcl_node_t node
static bool paramsInRange(const mirs_msgs__msg__BasicParam *p)
void rosTask(void *arg)
static rcl_allocator_t allocator
static rclc_support_t support
static IPublisherPlugin * publishers[]
#define ROS_CHECK(fn)
static TelemetryPublisher telemetry_pub_plugin
#define PARAM_BASE_MAX
#define PARAM_RADIUS_MAX
static bool ros_setup()
static rcl_timer_t timer
micro-ROSの接続と再接続、指令の受信、計測値の配信を処理する。
ROS速度指令メールボックス要素。
運動系テレメトリ。
車体・PID共有パラメータ。
センサ系テレメトリ。
車輪・RC信号・電圧などの計測値を確認用のROSトピックへ配信する。