2#include <micro_ros_platformio.h>
4#include <freertos/FreeRTOS.h>
5#include <freertos/task.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>
26#define ROS_DOMAIN_ID 90
29#define PARAM_RADIUS_MAX 0.5
30#define PARAM_BASE_MAX 2.0
31#define PARAM_GAIN_ABS_MAX 1000.0
66 const auto *msg = (
const geometry_msgs__msg__Twist *)msgin;
73 if (!std::isfinite(msg->linear.x) || !std::isfinite(msg->angular.z)) {
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));
94 if (!std::isfinite(p->wheel_radius) || p->wheel_radius <= 0.0 ||
98 if (!std::isfinite(p->wheel_base) || p->wheel_base <= 0.0 ||
102 const double gains[6] = {p->rkp, p->rki, p->rkd, p->lkp, p->lki, p->lkd};
103 for (
double g : gains) {
112 const auto *p = (
const mirs_msgs__msg__BasicParam *)msgin;
116 Serial.println(
"[ros] /params rejected (out of range or NaN)");
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);
146 pub->publish(motion, sensor, now_sec, now_nsec);
151#define ROS_CHECK(fn) \
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); \
164 rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
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);
172 rcl_ret_t support_rc = rclc_support_init_with_options(&
support, 0, NULL, &init_options, &
allocator);
174 if (support_rc != RCL_RET_OK) {
175 Serial.printf(
"[ros] support init failed rc=%d (agent不在の可能性)\n", (
int)support_rc);
179 rcl_node_options_t node_ops = rcl_node_get_default_options();
184 if (!pub->advertise(&
node)) {
185 Serial.printf(
"[ros] advertise failed: %s\n", pub->name());
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"));
227 Serial.begin(115200);
228 Serial.printf(
"[fw] mirs_esp_pio %s\n",
FW_VERSION);
229 set_microros_serial_transports(Serial);
235 while (rmw_uros_ping_agent(500, 2) != RMW_RET_OK) {
236 vTaskDelay(pdMS_TO_TICKS(2000));
240 vTaskDelay(pdMS_TO_TICKS(2000));
246 for (
int i = 0; i < 5 && !synced; ++i) {
247 synced = (rmw_uros_sync_session(1000) == RMW_RET_OK);
250 Serial.println(
"[ros] time sync failed, retrying setup");
252 vTaskDelay(pdMS_TO_TICKS(2000));
260 bool time_synced =
true;
261 uint32_t last_ping_ms = millis();
262 uint8_t ping_fail = 0;
263 bool session_broken =
false;
264 while (!session_broken) {
266 rclc_executor_spin_some(&
executor, RCL_MS_TO_NS(1));
270 Serial.println(
"[ros] params stalled, re-syncing session");
271 session_broken =
true;
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;
285 if (rmw_uros_sync_session(100) == RMW_RET_OK && !time_synced) {
287 Serial.println(
"[ros] time synced");
291 vTaskDelay(pdMS_TO_TICKS(1));
294 vTaskDelay(pdMS_TO_TICKS(1000));
制御・通信・センサのタスク間で、指令と計測値を排他制御して受け渡す。
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)
低頻度テレメトリ一括発行(/encoder・/vel・/vlt・/rc_debug)。
void requestOdometryReset()
指令の受付、操作元の切替、速度推定、モータ出力を周期的に実行する。
ESP32のピン、RC信号の範囲、速度上限と周期を定義する。
#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 rcl_subscription_t reset_odom_sub
static void timer_callback(rcl_timer_t *, int64_t)
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 bool paramsInRange(const mirs_msgs__msg__BasicParam *p)
static rcl_allocator_t allocator
static rclc_support_t support
static IPublisherPlugin * publishers[]
static TelemetryPublisher telemetry_pub_plugin
micro-ROSの接続と再接続、指令の受信、計測値の配信を処理する。
車輪・RC信号・電圧などの計測値を確認用のROSトピックへ配信する。