Micro-ROS で がいためIMU publish

噂の「がいためMulti IMU」を購入したので早速ROS 2に流してみようと思います！
Arduino + Micro-ROS で動かすので誰でもかんたんにできます。

ハマりポイント

1. frame_id設定
   rosidl_runtime_c__String__assignでStringを代入
2. timestamp同期
   Micro-ROSにIssueがあったのでそれを参考にsetup時にPCと時間合わせ(オフセット保存)をし、loop内で時間計算
3. 単位、正負
   単位はsensor_msgs/msg/Imuと同じなので符号だけ変えました

Install

1. Spresense SDK、Arduino導入 (本記事では省略します)
2. Micro-ROS設定
   以下のリポジトリのzipをダウンロードし、スケッチー＞ライブラリをインクルードー＞zipで改造版micro_ros_arduinoを導入します。

1. Agent設定
   以下コマンドでAgentを設定できます。

source /opt/ros/$ROS_DISTRO/setup.bash
mkdir uros_ws && cd uros_ws
git clone -b $ROS_DISTRO https://github.com/micro-ROS/micro_ros_setup.git src/micro_ros_setup
rosdep update && rosdep install --from-paths src --ignore-src -y
colcon build
source install/setup.bash
ros2 run micro_ros_setup create_agent_ws.sh
ros2 run micro_ros_setup build_agent.sh
source install/setup.bash

コード

#include <micro_ros_arduino.h>

#include <stdio.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <nuttx/sensors/cxd5602pwbimu.h>
#include <arch/board/cxd56_cxd5602pwbimu.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <rosidl_runtime_c/string_functions.h>
#include <rmw_microros/rmw_microros.h>

#include <sensor_msgs/msg/imu.h>

static rcl_allocator_t allocator;
static rclc_support_t support;
static rcl_node_t node;
static rcl_publisher_t publisher;
sensor_msgs__msg__Imu msg;

#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}


#define CXD5602PWBIMU_DEVPATH      "/dev/imu0"
#define MAX_NFIFO (4)

static cxd5602pwbimu_data_t g_data[MAX_NFIFO];
int devfd;
int ret;

static int start_sensing(int fd, int rate, int adrange, int gdrange,
                         int nfifos)
{
  cxd5602pwbimu_range_t range;

  ioctl(fd, SNIOC_SSAMPRATE, rate);
  range.accel = adrange;
  range.gyro = gdrange;
  ioctl(fd, SNIOC_SDRANGE, (unsigned long)(uintptr_t)&range);
  ioctl(fd, SNIOC_SFIFOTHRESH, nfifos);
  ioctl(fd, SNIOC_ENABLE, 1);

  return 0;
}

static int drop_50msdata(int fd, int samprate)
{
  int cnt = samprate / 20; /* data size of 50ms */

  cnt = ((cnt + MAX_NFIFO - 1) / MAX_NFIFO) * MAX_NFIFO;
  if (cnt == 0) cnt = MAX_NFIFO;

  while (cnt)
    {
      read(fd, g_data, sizeof(g_data[0]) * MAX_NFIFO);
      cnt -= MAX_NFIFO;
    }

  return 0;
}

void error_loop() {
  while(1) {
    digitalWrite(LED0, !digitalRead(LED0));
    delay(100);
  }
}


void setup() {

  board_cxd5602pwbimu_initialize(5);

  devfd = open(CXD5602PWBIMU_DEVPATH, O_RDONLY);

  start_sensing(devfd, 960, 16, 4000, MAX_NFIFO);
  drop_50msdata(devfd, 960);
  // dump_data(devfd);


  set_microros_transports();
  delay(2000);

  allocator = rcl_get_default_allocator();

  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

  // node名
  RCCHECK(rclc_node_init_default(&node, "gaitame_imu_node", "", &support));

  RCCHECK(rclc_publisher_init_default(
    &publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Imu),
    "/imu"  // topic名
  ));

  String frame_id = "imu";  // tf名
  rosidl_runtime_c__String__init(&msg.header.frame_id);
  rosidl_runtime_c__String__assign(&msg.header.frame_id, frame_id.c_str());

  rmw_uros_sync_session(1000);

  digitalWrite(LED0, HIGH);
}

void loop() {
  ret = read(devfd, g_data, sizeof(g_data[0]) * MAX_NFIFO);
  if(ret == sizeof(g_data[0]) * MAX_NFIFO) {
    for(int i=0; i<MAX_NFIFO; i++) {
      msg.linear_acceleration.x = -g_data[i].ax;
      msg.linear_acceleration.y = -g_data[i].ay;
      msg.linear_acceleration.z = -g_data[i].az;
      msg.angular_velocity.x = -g_data[i].gx;
      msg.angular_velocity.y = -g_data[i].gy;
      msg.angular_velocity.z =  g_data[i].gz;
      int64_t time_ns = rmw_uros_epoch_nanos();
      msg.header.stamp.sec = (int32_t)(time_ns / 1000000000);
      msg.header.stamp.nanosec = (int32_t)(time_ns % 1000000000);
      RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
    }
  }
}

実行

ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyUSB0 

rviz2で確認したい場合は

sudo apt install ros-humble-imu-tools

(赤矢印が加速度)

参考

神記事

スケッチ例

単位確認

タイムスタンプ同期