RaspberryPi pico wでmicro-ROSを動かす(準備編)

2024年1月15日 20:06

背景

micro-ROSのビルドはROS環境が必要で、事実上Linuxでの作業になる。
しかし、ライブラリのビルドのビルドさえできてしまえば、Windows環境でも開発が可能になると思い、ライブラリのビルドだけをDocker開発コンテナにやらせる方法を考えた。

環境

WIndows11 + VScode + PlatformIO + Docker

手順

1. PlatformIOにてRaspberryPi pico向けのプロジェクトを新規作成

2. platformio.iniをpico-w向けに編集

3. Docker開発コンテナを登録

micro-ROSライブラリはROS2上でのコンパイルが必要なので、無理くりDocker上でコンパイルする

Docker開発コンテナに関しては以下のサイトを参考にした

F1キーでコマンドパレットを開き、「add dev container」と入力する

ROSを選択

セットアップに時間がかかるようなのでひたすら待つ

4. micro-ROSライブラリ追加

5.開発コンテナーを閉じる

ライブラリのコンパイルさえできてしまえば、あとはローカルの環境で開発可能

6.ローカルでプロジェクトを開く

いったんプロジェクトは閉じてしまうので、今度はローカルにて同じプロジェクトを開く

7.サンプルを参考にmicro-ROSの実装を行う(シリアル版)

main.cpp

#include <Arduino.h>

#include <micro_ros_platformio.h>

#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <std_msgs/msg/u_int16.h>

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


#define msMAIN_LOOP_PERIOD (10U)

rcl_allocator_t allocator;
rcl_init_options_t init_options;
rclc_support_t support;

rcl_publisher_t publisher;
rclc_executor_t executor;

std_msgs__msg__UInt16 msg;
rcl_timer_t timer;

rcl_node_t node;

uint16_t cur_pos; // 現在点
uint16_t blink_timer; // LED点滅用

// Error handle loop
void error_loop(uint32_t error) {
  Serial.print(__FILE__);
  Serial.print(":");
  Serial.print(__LINE__, DEC);
  Serial.print(", temp_rc = ");
  Serial.println(error, HEX);
  while(1) {
    delay(100);
  }
}


void timer_callback(rcl_timer_t * timer, int64_t last_call_time) {
  RCLC_UNUSED(last_call_time);
  if (timer != NULL) {
    msg.data = cur_pos;
    RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
    cur_pos += 0x10;
  }
}


void setup() {
  Serial.begin(115200);             // USB-Serial用
  set_microros_serial_transports(Serial); 

  pinMode(LED_BUILTIN, OUTPUT);     // LED端子
  digitalWrite(LED_BUILTIN, HIGH);
  blink_timer = 0;

  allocator = rcl_get_default_allocator();
  init_options = rcl_get_zero_initialized_init_options();
  RCCHECK(rcl_init_options_init(&init_options, allocator));
  RCCHECK(rcl_init_options_set_domain_id(&init_options, 0));
  //create init_options
  // エージェント起動を待つ
  rcl_ret_t temp_rc; 
  do{
//    temp_rc = rclc_support_init(&support, 0, NULL, &allocator);
    temp_rc = rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator);
    Serial.print("rclc_support_init(): ");
    Serial.println(temp_rc, HEX);
  }while(temp_rc != RCL_RET_OK);

  // create node
  RCCHECK(rclc_node_init_default(&node, "micro_ros_servo_node", "", &support));

  // create publisher
  RCCHECK(rclc_publisher_init_default(
    &publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, UInt16),
    "micro_ros_servo_node_publisher"));

  // create timer,
  const unsigned int timer_timeout = 1000;
  RCCHECK(rclc_timer_init_default(
    &timer,
    &support,
    RCL_MS_TO_NS(timer_timeout),
    timer_callback));

  // create executor
  RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
  RCCHECK(rclc_executor_add_timer(&executor, &timer));

  msg.data = 0;

}

void loop() {
  // put your main code here, to run repeatedly:
  delay(100);
  RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));

  if(blink_timer < 500){
    digitalWrite(LED_BUILTIN, LOW);
  }else{
    digitalWrite(LED_BUILTIN, HIGH);
  }

  blink_timer += 100;
  if(blink_timer >= 1000){
    blink_timer = 0;
  }
}