はじめに

ROS2で動くロボットを製作しました。理由としては、SLAMやNavigationなどの強力なツールをすぐに利用できる点に大きな魅力を感じたからです。そのため、これらの機能を最大限に活用できるように設計を進めました。

ROS2を活用したロボット開発のメリット

ROS2には、SLAMやNavigationなど複雑な機能を比較的簡単に導入できる豊富なパッケージがそろっています。具体的には、自己位置推定による地図生成や、経路計画による自律走行の実装などが組み込みやすい構造になっています。これにより、単純な試作から高度な制御までスケールアップが容易になるため、開発の効率を高めることができます。

作ったもの

こんなロボットを作ってみました。

構成部品

• Raspberry Pi 4B(メインコンピュータ)・・・Ubuntu22.04にROS2 humbleをインストール
• Raspberry Pi ポータブルディスプレイ(顔表示器)
• DDSM210(走行用ブラシレスモータ)
• makita 18Vバッテリ(メインバッテリ)
• DCDC降圧基板18V-5V(3A品)
• SPRESENSE メインボードw/3DToF(MM-TOF10-IS)・・・PointCloud2データとしてpublish
• SPRESENSE メインボードw/6軸IMU(BMI270)・・・IMUデータとしてpublish

IMU

SPRESENSEのIMUから加速度、角速度をtopicでpublishし、ROS2のMadgwickフィルタライブラリで姿勢情報を追加してimu/dataとしてpublishしRVizで可視化しています。

IMUをROS2トピックでpublishする手順

事前準備

RaspberryPiでUSBポートに固定名称を割り当てます。この設定によりUSBデバイスの接続ポートが変動しても一意のデバイス名でアクセス可能になります。
udevルールファイルを編集してのポート名称を追加します。

sudo vi /etc/udev/rules.d/99-usb-serial.rules

追加する情報

SUBSYSTEM=="tty", KERNELS=="1-1.2", SYMLINK+="ttyUSB_port2", MODE="0666"
SUBSYSTEM=="tty", KERNELS=="1-1.3", SYMLINK+="ttyUSB_port3", MODE="0666"
SUBSYSTEM=="tty", KERNELS=="1-1.4", SYMLINK+="ttyUSB_port4", MODE="0666"
SUBSYSTEM=="tty", KERNELS=="1-1.1", SYMLINK+="ttyUSB_port1", MODE="0666"

ルールの適用

sudo udevadm trigger

スケッチを書き込む

6軸センサadd onボードを組みつけたSPRESENSEにSPRESENSE用スケッチ(for IMU) を書き込みます。

ROS2 micro-ROS-agentを起動する

Raspberry Piで新しいターミナルを開き以下のコマンドを実行します。ポート名称は差し込むポートに合わせて設定してください。(太字部)
これでSPRESENSEから送られたIMUの生データをROS2のトピックとして受け取れるようになります。

sudo docker run -it --rm -v /dev:/dev --privileged --net=host microros/micro-ros-agent:humble serial --dev /dev/ttyUSB_port1

IMU加速度,角速度から姿勢を求める

Madgwickフィルタで姿勢を求めます。SPRESENSEで実装することもできますが、ROS2の便利なライブラリを使って簡単に実現します。

MadgWickフィルタライブラリをインストール

以下のコマンドでインストールします。

sudo apt install ros-humble-imu-filter-madgwick

フィルタ実行

以下のコマンドで実行します。これで姿勢情報付きの/imu/data_rawがtopicでpublishされます。

ros2 run imu_filter_madgwick imu_filter_madgwick_node --ros-args -r /imu/data_raw:=/imu_spresense -p use_mag:=false -p world_frame:=enu -p publish_tf:=false

3DToF

SPRESENSEの3DToFで計測した距離をPointCloud2形式でtopicとしてpublishし、RVizで可視化しています。

スケッチを書き込む

3DToFをPointCloud2形式でpublishするスケッチ
#include <micro_ros_arduino.h>
#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <sensor_msgs/msg/point_cloud2.h>
#include <sensor_msgs/msg/point_field.h>
#include <SPI.h>

#define COLSIZE 8
#define ROWSIZE 4
#define PIXELS (COLSIZE * ROWSIZE)
#define POINT_STEP 12  // x, y, z (各4バイト)

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

rcl_publisher_t publisher;
sensor_msgs__msg__PointCloud2 msg;
rclc_executor_t executor;
rcl_allocator_t allocator;
rclc_support_t support;
rcl_node_t node;

struct StSpiData {
  char bMagic;
  char bSeq1;
  char bRsv1;
  char bRsv2;
  uint32_t iRange1;
  uint16_t wLight1;
  uint32_t iRange[PIXELS];
  uint16_t wLight[PIXELS];
  char bRsv3[53];
  char bSeq2;
};
// エラーハンドリング関数の修正
void error_handler(const char* error_msg) {
  //Serial.print("Error: ");
  //Serial.println(error_msg);
  // エラーカウンターをインクリメントするなどの処理を追加
}
void error_loop0() {
  while (1) {
    digitalWrite(LED_BUILTIN0, !digitalRead(LED_BUILTIN0));
    delay(100);
    if (rmw_uros_ping_agent(100, 1)) {
      // 再接続成功
      return;
    }
  }
}
void error_loop1() {
  while (1) {
    digitalWrite(LED_BUILTIN1, !digitalRead(LED_BUILTIN1));
    //Serial.println("error___");
    delay(100);
  }
}
void error_loop2() {
  while (1) {
    digitalWrite(LED_BUILTIN2, !digitalRead(LED_BUILTIN2));
   // Serial.println("error___");
    delay(100);
  }
}
void error_loop3() {
  while (1) {
    digitalWrite(LED_BUILTIN3, !digitalRead(LED_BUILTIN3));
   // Serial.println("error___");
    delay(100);
  }
}
void error_loop03() {
  while (1) {
    digitalWrite(LED_BUILTIN3, !digitalRead(LED_BUILTIN3));
   // Serial.println("error___");
    delay(1000);
  }
}

static inline char spigetb() {
  return SPI5.transfer(0) & 0xff;
}

static uint16_t spiget16() {
  uint16_t i;
  i = spigetb() << 8;
  i |= spigetb();
  return i;
}

static uint32_t spiget32() {
  uint32_t i;
  i = spigetb() << 24;
  i |= spigetb() << 16;
  i |= spigetb() << 8;
  i |= spigetb();
  return i;
}

static void spiskip(int len) {
  while (len > 0) {
    spigetb();
    len--;
  }
}

static void spicommandinner(int cmd, int val) {
  SPI5.transfer(0xeb);
  SPI5.transfer(cmd);
  SPI5.transfer(1);
  SPI5.transfer(val);
  SPI5.transfer(0xed);
}

static void spicommand(int cmd, int val) {
  spicommandinner(cmd, val);
  spiskip(256 - 5);
}

void recvproc() {
  struct StSpiData GetData;
  static int oldseq = -1;
  float angle_x, angle_y;
  float x, y, z;
  
  for (int i = 0; i < 4; i++) {
    GetData.bMagic = spigetb();
    GetData.bSeq1 = spigetb();
    spiskip(2);
    GetData.iRange1 = spiget32();
    GetData.wLight1 = spiget16();
    /*3Dの距離*/
    for (int j = 0; j < PIXELS; j++) {
      GetData.iRange[j] = spiget32();
    }
    /*3Dの光量*/
    for (int j = 0; j < PIXELS; j++) {
      GetData.wLight[j] = spiget16();
    }
    spiskip(256 - 11 - 6 * PIXELS);
    GetData.bSeq2 = spigetb();

    if (GetData.bMagic != 0xe9) {
      continue;     
    }
   if (GetData.bSeq1 != GetData.bSeq2){
      continue;
	}
    if (oldseq < 0)
      ;
    else if (((oldseq - GetData.bSeq1) & 0x80) == 0){
      continue;
	}
    oldseq = GetData.bSeq1;

    for (int row = 0; row < ROWSIZE; row++) {
      for (int col = 0; col < COLSIZE; col++) {
        int idx = row * COLSIZE + col;
        float distance = (float)GetData.iRange[idx] / 1000000.0;  // メートル単位に変換

        angle_x = (col - (COLSIZE - 1) / 2.0f) * (30.0f / COLSIZE) * (M_PI / 180.0f);
        angle_y = (row - (ROWSIZE - 1) / 2.0f) * (15.0f / ROWSIZE) * (M_PI / 180.0f);

        z = distance * cos(angle_y) * cos(angle_x);
        x = distance * cos(angle_y) * sin(angle_x);
        y = distance * sin(angle_y);

        memcpy(&msg.data.data[idx * POINT_STEP], &x, 4);
        memcpy(&msg.data.data[idx * POINT_STEP + 4], &y, 4);
        memcpy(&msg.data.data[idx * POINT_STEP + 8], &z, 4);
      }
    }
    msg.header.stamp.sec = millis() / 1000;
    msg.header.stamp.nanosec = (millis() % 1000) * 1000000;
    RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
    delay(100);
  }


}

void setup() {
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, HIGH);

  set_microros_transports();

  allocator = rcl_get_default_allocator();

  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
  RCCHECK(rclc_node_init_default(&node, "tof_publisher_node", "", &support));

  const rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data;
  RCCHECK(rclc_publisher_init_default(
    &publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, PointCloud2),
    "point_cloud"));

  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));

  msg.header.frame_id.data = (char*)"tof_sensor_link";
  msg.header.frame_id.size = strlen("tof_sensor_link");
  msg.header.frame_id.capacity = strlen("tof_sensor_link");

  msg.height = ROWSIZE;
  msg.width = COLSIZE;
  msg.is_dense = true;
  msg.point_step = POINT_STEP;
  msg.row_step = POINT_STEP * COLSIZE;

  msg.fields.size = 3;
  msg.fields.capacity = 3;
  msg.fields.data = (sensor_msgs__msg__PointField*)malloc(3 * sizeof(sensor_msgs__msg__PointField));

  msg.fields.data[0].name.data = (char*)"x";
  msg.fields.data[0].offset = 0;
  msg.fields.data[0].datatype = 7;  // FLOAT32
  msg.fields.data[0].count = 1;

  msg.fields.data[1].name.data = (char*)"y";
  msg.fields.data[1].offset = 4;
  msg.fields.data[1].datatype = 7;
  msg.fields.data[1].count = 1;

  msg.fields.data[2].name.data = (char*)"z";
  msg.fields.data[2].offset = 8;
  msg.fields.data[2].datatype = 7;
  msg.fields.data[2].count = 1;

  msg.data.capacity = PIXELS * POINT_STEP;
  msg.data.size = PIXELS * POINT_STEP;
  msg.data.data = (uint8_t*)malloc(PIXELS * POINT_STEP);

  if (msg.fields.data == NULL || msg.data.data == NULL) {
    error_handler("Failed to allocate memory");
    error_loop1();
  }
  pinMode(LED_BUILTIN1, OUTPUT);
  digitalWrite(LED_BUILTIN1, HIGH);

  SPI5.begin();
  SPI5.beginTransaction(SPISettings(8000000, MSBFIRST, SPI_MODE3));

  pinMode(LED_BUILTIN2, OUTPUT);
  digitalWrite(LED_BUILTIN2, HIGH);

  spiskip(256);
  delay(500);
  spicommand(0, 0xff);  // sync mode.
  delay(500);
  spiskip(256);
  spiskip(spigetb());  // sync.
  spicommand(0, 0);    // normal mode.
  delay(500);
//  spicommand(0x12, 1);  // long range
  spicommand(0x12, 0); // short range  delay(500);
  spiskip(256);
  spicommand(0x10, 0);  // 10frames/s
  delay(500);
  spiskip(256);

  pinMode(LED_BUILTIN3, OUTPUT);
  digitalWrite(LED_BUILTIN3, HIGH);
  //Serial.println("set up ok");
}

void loop() {
   delay(100);
  recvproc();
}

ROS2 micro-ros-agentを起動する

SPRESENSEをRaspberryPiのUSBポートに差し込み起動します。

sudo docker run -it --rm -v /dev:/dev --privileged --net=host microros/micro-ros-agent:humble serial --dev /dev/ttyUSB_port2

モータ制御(DDSM210)

ハードウェア

UARTで動くモータなのでUSBポートから駆動します。
USB-TTL変換モジュールを使用して接続します。

ソフトウェア

モータ制御とオドメトリ算出を行うプログラム

モータ制御用プログラム
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float32
from std_srvs.srv import Trigger
from geometry_msgs.msg import Twist, Pose2D
from nav_msgs.msg import Odometry
import serial
import time
import math

def calculate_crc8(data):
    crc = 0x00
    for byte in data:
        crc ^= byte
        for _ in range(8):
            if (crc & 0x01) != 0:
                crc = (crc >> 1) ^ 0x8C
            else:
                crc >>= 1
    return crc & 0xFF

def create_velocity_command(speed_rpm):
    if not -210 <= speed_rpm <= 210:
        raise ValueError("Speed must be between -210 and 210 rpm.")
    
    speed_value = int(speed_rpm * 10)
    high_byte = (speed_value >> 8) & 0xFF
    low_byte = speed_value & 0xFF
    
    command = [0x01, 0x64, high_byte, low_byte, 0x00, 0x00, 0x00, 0x00, 0x00]
    crc = calculate_crc8(command)
    command.append(crc)
    
    return bytes(command)

def create_rotation_count_command():
    command = [0x01, 0x74, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00]
    crc = calculate_crc8(command)
    command.append(crc)
    return bytes(command)

def parse_rotation_count_response(response):
    if len(response) < 10:  # 応答データの長さを確認
        raise ValueError("Invalid response length")
    
    # 回転数は4バイトで表現される（32ビット符号付き整数）
    rotation_count = int.from_bytes(response[2:6], byteorder='little', signed=True)
    return rotation_count

def parse_velocity_response(response):
    if len(response) < 10:  # 応答データの長さを確認
        raise ValueError("Invalid response length")
    
    # 速度は2バイトで表現される（16ビット符号付き整数）
    velocity = int.from_bytes(response[2:4], byteorder='little', signed=True)
    
    # 電流は2バイトで表現される（16ビット符号付き整数）
    current = int.from_bytes(response[4:6], byteorder='little', signed=True)
    
    return velocity, current

class DualMotorControlNode(Node):
    def __init__(self):
        super().__init__('dual_motor_control_node')
        self.target_rpm1 = 0.0
        self.target_rpm2 = 0.0
        self.current_rpm1 = 0.0 
        self.current_rpm2 = 0.0
        self.ser1 = None
        self.ser2 = None
        self.port1 = self.declare_parameter('port1', '/dev/ttyUSB_port3').value
        self.port2 = self.declare_parameter('port2', '/dev/ttyUSB_port4').value
        self.baudrate = self.declare_parameter('baudrate', 115200).value
        
        self.cmd_vel_subscription = self.create_subscription(
            Twist, 'cmd_vel', self.cmd_vel_callback, 10)
        
        self.publisher1 = self.create_publisher(Float32, 'motor1_speed', 10)
        self.publisher2 = self.create_publisher(Float32, 'motor2_speed', 10)
        self.odom_publisher = self.create_publisher(Odometry, 'odom', 10)
        
        # distance1 と distance2 をパブリッシュするためのパブリッシャーを追加
        self.distance1_publisher = self.create_publisher(Float32, 'distance1', 10)
        self.distance2_publisher = self.create_publisher(Float32, 'distance2', 10)
        
        # デバッグ用のパブリッシャーを追加
        self.rotation_count1_publisher = self.create_publisher(Float32, 'rotation_count1', 10)
        self.delta_rotation1_publisher = self.create_publisher(Float32, 'delta_rotation1', 10)
        self.wheel_radius_publisher = self.create_publisher(Float32, 'wheel_radius', 10)
        self.encoder_resolution_publisher = self.create_publisher(Float32, 'encoder_resolution', 10)
        
        self.connect_serial()
        
        self.wheel_separation = 0.130
        self.wheel_radius = 0.036
        self.max_rpm = 210.0
        self.acceleration = 100.0  # RPM/s
        self.current_rpm1 = 0.0
        self.current_rpm2 = 0.0

        self.recovery_service = self.create_service(Trigger, 'motor_recovery', self.recovery_callback)
        self.create_timer(5.0, self.check_and_recover_connection)
        self.create_timer(0.1, self.smooth_acceleration)  # 10Hzで加速度制御
        self.create_timer(0.1, self.publish_odometry)  # 10Hzでオドメトリをパブリッシュ

        self.pose = Pose2D()
        self.pose.x = 0.0
        self.pose.y = 0.0
        self.pose.theta = 0.0

        self.last_rotation_count1 = 0
        self.last_rotation_count2 = 0
        self.last_time = self.get_clock().now()

    def connect_serial(self):
        max_retries = 5
        retry_delay = 2.0

        for _ in range(max_retries):
            try:
                self.ser1 = serial.Serial(self.port1, self.baudrate, timeout=1)
                self.ser2 = serial.Serial(self.port2, self.baudrate, timeout=1)
                self.ser1.reset_input_buffer()  # バッファをクリア
                self.ser2.reset_input_buffer()  # バッファをクリア
                self.get_logger().info(f"Connected to {self.port1} and {self.port2}")
                return
            except serial.SerialException as e:
                self.get_logger().error(f"Serial error: {e}")
                time.sleep(retry_delay)

        self.get_logger().error("Failed to connect to serial ports after multiple attempts")

    def cmd_vel_callback(self, msg):
        linear_x = msg.linear.x
        angular_z = msg.angular.z

        right_velocity = (linear_x + angular_z * self.wheel_separation / 2.0) / self.wheel_radius
        left_velocity = (linear_x - angular_z * self.wheel_separation / 2.0) / self.wheel_radius

        self.target_rpm1 = left_velocity * 60.0 / (2.0 * 3.14159)
        self.target_rpm2 = -right_velocity * 60.0 / (2.0 * 3.14159)

        self.target_rpm1 = max(min(self.target_rpm1, self.max_rpm), -self.max_rpm)
        self.target_rpm2 = max(min(self.target_rpm2, self.max_rpm), -self.max_rpm)

    def smooth_acceleration(self):
        self.current_rpm1 = self.accelerate_towards(self.current_rpm1, self.target_rpm1)
        self.current_rpm2 = self.accelerate_towards(self.current_rpm2, self.target_rpm2)

        self.control_motor(self.ser1, self.current_rpm1, "Motor 1")
        self.control_motor(self.ser2, self.current_rpm2, "Motor 2")

    def accelerate_towards(self, current, target):
        if current < target:
            return min(current + self.acceleration * 0.1, target)
        elif current > target:
            return max(current - self.acceleration * 0.1, target)
        return current

    def control_motor(self, ser, speed, motor_name):
        if ser is None or not ser.is_open:
            self.get_logger().warn(f"Serial port for {motor_name} is not open")
            return
        
        try:
            # 速度コマンドを送信
            command = create_velocity_command(speed)
            ser.write(command)
            self.get_logger().info(f"{motor_name} Velocity Command sent: {command.hex()}")

            # モーターからのフィードバックを読み取る
            response = ser.read(10)  # 応答は10バイト
            self.get_logger().info(f"Raw response: {response.hex()}")  # 応答データをログ出力

            # 応答データの先頭バイトを確認
            if len(response) == 10 and response[0] == 0x01 and response[1] == 0x64:
                # 0164: 速度ループの応答
                velocity, current = parse_velocity_response(response)
                self.get_logger().info(f"{motor_name} Velocity: {velocity}, Current: {current}")
            else:
                self.get_logger().warn(f"Invalid response for {motor_name} velocity command")

        except ValueError as e:
            self.get_logger().error(f"{motor_name}: {str(e)}")
        except serial.SerialException as e:
            self.get_logger().error(f"{motor_name} Serial error: {str(e)}")
            self.attempt_recovery(ser, motor_name)

    def attempt_recovery(self, ser, motor_name):
        try:
            ser.close()
            time.sleep(1)
            ser.open()
            self.get_logger().info(f"Successfully recovered {motor_name} connection")
        except serial.SerialException as e:
            self.get_logger().error(f"Failed to recover {motor_name} connection: {str(e)}")

    def recovery_callback(self, request, response):
        self.get_logger().info("Attempting motor recovery")
        self.connect_serial()
        response.success = True
        response.message = "Recovery attempt completed"
        return response

    def check_and_recover_connection(self):
        if self.ser1 is None or not self.ser1.is_open:
            self.get_logger().warn("Motor 1 connection lost, attempting recovery")
            self.attempt_recovery(self.ser1, "Motor 1")
        if self.ser2 is None or not self.ser2.is_open:
            self.get_logger().warn("Motor 2 connection lost, attempting recovery")
            self.attempt_recovery(self.ser2, "Motor 2")

    def get_rotation_count(self, ser):
        try:
            command = create_rotation_count_command()
            ser.write(command)
            
            # 応答データを読み取る
            response = ser.read(10)  # 応答は10バイト
            self.get_logger().info(f"Raw response: {response.hex()}")  # 応答データをログ出力
            
            # 応答データの長さと先頭バイトを確認
            if len(response) == 10 and response[0] == 0x01 and response[1] == 0x74:
                # 0174: 回転数取得コマンドの応答
                rotation_count = parse_rotation_count_response(response)
                self.get_logger().info(f"Parsed rotation count: {rotation_count}")  # 解析後の回転数をログ出力
                return rotation_count
            else:
                # 0174 で始まらない応答データは無視
                self.get_logger().warn("Invalid response for rotation count command")
                return None
        except serial.SerialException as e:
            self.get_logger().error(f"Serial error: {str(e)}")
            return None

    def publish_odometry(self):
        current_time = self.get_clock().now()
        dt = (current_time - self.last_time).nanoseconds / 1e9
        self.last_time = current_time

        # モーターの回転数を取得
        rotation_count1 = self.get_rotation_count(self.ser1)
        rotation_count2 = self.get_rotation_count(self.ser2)

        if rotation_count1 is None or rotation_count2 is None:
            return

        # 回転数の差分を計算
        delta_rotation1 = rotation_count1 - self.last_rotation_count1
        delta_rotation2 = rotation_count2 - self.last_rotation_count2

        # 回転数を更新
        self.last_rotation_count1 = rotation_count1
        self.last_rotation_count2 = rotation_count2

        # 車輪の移動距離を計算
        distance1 = delta_rotation1 * (2 * math.pi * self.wheel_radius) / 4096  # 4096はエンコーダーの分解能
        distance2 = delta_rotation2 * (2 * math.pi * self.wheel_radius) / 4096

        # distance1 と distance2 をパブリッシュ
        distance1_msg = Float32()
        distance1_msg.data = distance1
        self.distance1_publisher.publish(distance1_msg)

        distance2_msg = Float32()
        distance2_msg.data = distance2
        self.distance2_publisher.publish(distance2_msg)

        # ロボットの速度と角速度を計算
        linear_velocity = (distance1 + distance2) / (2.0 * dt)
        angular_velocity = (distance2 - distance1) / (self.wheel_separation * dt)

        # オドメトリの更新
        self.pose.x += linear_velocity * math.cos(self.pose.theta) * dt
        self.pose.y += linear_velocity * math.sin(self.pose.theta) * dt
        self.pose.theta += angular_velocity * dt

        # Odometryメッセージを作成
        odom_msg = Odometry()
        odom_msg.header.stamp = current_time.to_msg()
        odom_msg.header.frame_id = "odom"
        odom_msg.child_frame_id = "base_link"
        odom_msg.pose.pose.position.x = self.pose.x
        odom_msg.pose.pose.position.y = self.pose.y
        odom_msg.pose.pose.orientation.z = math.sin(self.pose.theta / 2.0)
        odom_msg.pose.pose.orientation.w = math.cos(self.pose.theta / 2.0)
        odom_msg.twist.twist.linear.x = linear_velocity
        odom_msg.twist.twist.angular.z = angular_velocity

        # オドメトリをパブリッシュ
        self.odom_publisher.publish(odom_msg)

def main(args=None):
    rclpy.init(args=args)
    node = DualMotorControlNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        if node.ser1 is not None and node.ser1.is_open:
            node.ser1.close()
        if node.ser2 is not None and node.ser2.is_open:
            node.ser2.close()
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

おわりに

センサーデータの可視化とオドメトリの取得に成功：自己位置推定への第一歩

ROS 2の強力な可視化ツールであるRViz2を活用し、各種センサーからのデータを効果的に可視化することに成功しました。これにより、ロボットの状態やセンサーの出力を直感的に理解し、デバッグや開発プロセスを大幅に効率化できるようになりました。
同時に、モーターの制御システムを実装し、エンコーダーからのフィードバックを利用してオドメトリデータの取得にも成功しました。このオドメトリ情報は、ロボットの移動距離や方向の変化を推定する上で非常に重要な役割を果たします。
これらの成果は、自己位置推定システムの構築に向けた重要な基盤となります。センサーデータの可視化とオドメトリの正確な取得は、SLAMやナビゲーションなどの高度な自律移動技術の実現に不可欠な要素です。
今後は、取得したセンサーデータとオドメトリ情報を統合し、確率的手法や機械学習アルゴリズムを用いて、より精密な自己位置推定システムの開発に取り組む予定です。この過程で、環境マッピングや障害物回避など、自律移動ロボットの実用化に向けたさらなる機能の実装も視野に入れています。
ROS 2の豊富なツールセットと、今回得られた技術的知見を活かし、信頼性の高い自己位置推定システムの構築を目指して、開発を進めていきます。