Zenn
🐢

ASP3 版 mROS 2 で turtlesim を JoyStick で操作する方法

2024/05/15に公開
Ubuntu
ROS2
stm32
toppers
mros2
tech

はじめに

前回までで ASP3 版 mROS 2 のビルドできる環境が整ったので、今回から開発方法を探っていきたいと思います。

まず簡単な所からということで、以前 高瀬先生がデモされていたこちらをやっていきます。

https://twitter.com/takasehideki/status/1505066116921524228

Q. え？サンプルあるんじゃないの？
A. 残念ながらこちらは Mbed 版であり ASP3 版は現時点では存在しません。

新規に ASP3 版で作成して動作している様子については以下の動画で確認可能です。

※今回は STM32CubeIDE ではなく STM32CubeMX を使用した例を紹介します。

使用する機器および、環境

事前準備

Windows11 と Ubuntu22.04.4 は LAN ケーブルで直接接続し WinSCP を用いたファイルのやりとりを行いますので、前回の記事で扱った通り IP アドレスを固定しておきます。

https://zenn.dev/himura/articles/5906b8f8c075dc

固定 IP アドレスは次の通り

  • Ubuntu PC enp1s0 と STMicro Nucleo-F767ZI の間
    • 192.168.11.2　⇒　STMicro Nucleo-F767ZI (mros2-asp3-f767zi)
    • 192.168.11.3　⇒　GMKtec N6005 NucBox7 Mini PC Ubuntu22.04.4
  • Ubuntu PC enp2s0 と Windows PC の間
    • 192.168.10.3　⇒　GMKtec N6005 NucBox7 Mini PC Ubuntu22.04.4
    • 192.168.10.4　⇒　Windows11 PC

STMicro Nucleo-F767ZI と接続する enp1s0 も IP アドレスを同じように設定すれば、ルーターなしでも通信可能になります。

ROS 2 の通信で使用する enp1s0 は、以下のように設定しておきます。
こちらもルーター不要です。

enp1s0 IP アドレス
enp1s0 IP アドレス

Terminator

Ubuntu では Terminal がありますが ROS 2 のように複数のログ表示が必要な場合は大量に立ち上げないといけません。

Terminator は一つの画面を分割して複数の画面を取りあつかう事ができるため、扱いやすくて便利です。

以下のコマンドを実行するとインストールが完了します。

Terminal
sudo apt install terminator -y

起動するには以下のコマンドを打ち込むか、アプリケーション一覧から選択します。

Terminal
terminator

アプリケーション一覧から起動
アプリケーション一覧から起動

外観の違いはこんな感じです。

Terminal(左) と Terminator(右) の外観
Terminal(左) と Terminator(右) の外観

画面の分割を行う場合、画面上で右クリックをすると一覧が出てくるので、そこから選びます。

  • 水平に分割：　Split Horizontally
  • 垂直に分割：　Split Vertically

右クリックで表示される一覧
右クリックで表示される一覧

このような分割も可能です。

水平分割 & 上部垂直分割
水平分割 & 上部垂直分割

STM32CubeMX

当初は Ubuntu で実行しようと考えていましたが、操作中にフリーズするという現象が解決しなかったため Windows11 で使用する方法を紹介します。

ダウンロードとインストール

STM32Cube初期化コード生成ツール
STM32Cube初期化コード生成ツール

こちらからダウンロードを行います。
https://www.st.com/ja/development-tools/stm32cubemx.html

アカウントを作成してダウンロードするか、ゲストとしてダウンロードするか選んでください。

ログインするか、ゲストとしてでダウンロード
ログインするか、ゲストとしてでダウンロード

※インストールは特殊な事をしないので、説明については割愛します。

マイコンの選択

STM32CubeMX を起動すると Home 画面が表示されます。今回は ACCESS TO MCU SELECTOR を選びます。
STM32CubeMX Home
STM32CubeMX Home

左側の Series で STM32F7 を選択、 Package で LQFP144 を選択することでマイコンを絞り込みます。

右側の一覧から STM32F767ZI を選択します。これが今回使用する NUCLEO-F767ZI に搭載されているマイコンです。

選択したら、右上の Start Project を押下して次へ進みます。
STM32CubeMX MCU/MPU Selector
STM32CubeMX MCU/MPU Selector

マイコンの周辺機能を編集可能な画面が表示されます。
STM32CubeMX Pinout&Configuration
STM32CubeMX Pinout&Configuration

A/D 変換の設定

A/D 変換を行うピンの設定をします。

今回は Arduino 用 Grove ベースシールド V2 の A0 コネクタに Grove ジョイスティックを接続して使用するので STM32F767ZI のどのピンに繋がるか調べます。

各ピンアサインについては、以下のページが参考になります。
https://os.mbed.com/platforms/ST-Nucleo-F767ZI/
https://jp.seeedstudio.com/Base-Shield-V2.html

ピンの接続状態を確認した結果、以下のような関係が解ります

  • A0-A1 = PA3(STM32F767ZI) ⇒ ジョイスティック X 軸
  • A0-A0 = PC0(STM32F767ZI) ⇒ ジョイスティック Y 軸
  • A0-VCC ⇒ 3.3V
  • A0-GND

マイコンのピンをクリックすると設定可能な機能が表示されます。

ピンの機能選択
ピンの機能選択

一覧から ADC1_IN と記載のある機能を PC0 と PA3 へ設定します。
※マウスのホイールで画面を拡大すると作業しやすいです。

ピンの設定
ピンの設定

左の一覧から Analog ⇒ ADC1 の順に選択すると、以下のような画面が表示されます。 IN3 と IN10 のみが選択されていることを確認してください。
ADC1 Mode
ADC1 Mode

ADC1 の設定を変更する箇所は以下の通りです。

  • Scan Conversion Mode ⇒ Enable
  • Discontinuuous Conversion Mode ⇒ Enable
  • Number of Conversion ⇒ 2
  • Rank1 Channel ⇒ Channel 3
  • Rank2 Channel ⇒ Channel 10

ADC1 Configuration
ADC1 Configuration

コードの生成

上部の一覧にあるタブから Project Manager を選択すると、生成するコードの保存先などを設定する画面に切り替わります。

設定する場所は以下の通りです。

  • Project Name: 任意
  • Project Location: 任意
  • Application Structure: Basic
  • Do not generate the main(): ☑
  • Toolchain / IDE: Makefile

設定を終えたら GENERATE CODE をクリックしてコードを生成します。

Project Manager
Project Manager

完了後、生成ファイルのあるフォルダを開くか聞かれるので Open Folder をクリックします。

以下の画像の通りのファイルが生成されていれば OK で、今回必要になるのは src フォルダ内の二つのソースコードだけです。

  • main.c
  • stm32f7xx_hal_msp.c

生成ファイル
生成ファイル

mturtle_teleop_joy

A/D 変換をして操作するプログラムを追加するだけなので、ASP3 版の mturtle_teleop を元に改修していきます。

ディレクトリ構成

mturtle_teleop から一式コピーして、ディレクトリの名前を mturtle_teleop_joy と変更します。

ディレクトリ構造は以下の通りです。
mturtle_teleop_joy ディレクトリ構成
mturtle_teleop_joy ディレクトリ構成

追加するのは STM32CubeMX ディレクトリと中に格納されているソースファイルです。

生成した C ファイルは Windows11 に存在しますので Ubuntu 側にデータを送信する必要があります。
※WinSCP で移動するのが楽ですが、もちろん USB メモリ等で移動しても構いません。

内容の変更などは Windows、Ubuntu どちらで作業しても OK です。

最終的にディレクトリ構成とソースコードが正しければ問題ありません。

STM32CubeMX 関連ファイル

STM32CubeMX で生成したファイルの多くはすでに Toppers/ASP3 で使用されているため、追加機能として使用する A/D 変換関連の処理を追記するだけで機能します。

必要になるのは新規に追加した A/D 変換関連の処理がある以下の二つのソースコードです。

  • main.c
  • stm32f7xx_hal_msp.c

stm32f7xx_hal_msp.c は変更を加える必要がないため、生成されたファイルをそのまま使用します。

stm32f7xx_hal_mx.c 作成

STM32CubeMX が作成する main.c から必要な関数だけを抜き取り、新たな C のソースファイルを作成します。

内容は、生成された main.c のソースコードから ADC1 の初期化と GPIO の初期化を移植しており、それらで必要になるソースコードも記載しています。

stm32f7xx_hal_mx.c
#include "app.h"
#include "stm32f7xx_nucleo_144.h"

ADC_HandleTypeDef hadc1;

/**
  * @brief ADC1 Initialization Function
  * @param None
  * @retval None
  */
static void MX_ADC1_Init(void)
{

  /* USER CODE BEGIN ADC1_Init 0 */

  /* USER CODE END ADC1_Init 0 */

  ADC_ChannelConfTypeDef sConfig = {0};

  /* USER CODE BEGIN ADC1_Init 1 */

  /* USER CODE END ADC1_Init 1 */
  /** Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion)
  */
  hadc1.Instance = ADC1;
  hadc1.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV2;
  hadc1.Init.Resolution = ADC_RESOLUTION_12B;
  hadc1.Init.ScanConvMode = ADC_SCAN_ENABLE;
  hadc1.Init.ContinuousConvMode = DISABLE;
  hadc1.Init.DiscontinuousConvMode = ENABLE;
  hadc1.Init.NbrOfDiscConversion = 1;
  hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
  hadc1.Init.ExternalTrigConv = ADC_SOFTWARE_START;
  hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT;
  hadc1.Init.NbrOfConversion = 2;
  hadc1.Init.DMAContinuousRequests = DISABLE;
  hadc1.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
  if (HAL_ADC_Init(&hadc1) != HAL_OK)
  {
    Error_Handler();
  }
  /** Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time.
  */
  sConfig.Channel = ADC_CHANNEL_3;
  sConfig.Rank = ADC_REGULAR_RANK_1;
  sConfig.SamplingTime = ADC_SAMPLETIME_3CYCLES;
  if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
  {
    Error_Handler();
  }
  /** Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time.
  */
  sConfig.Channel = ADC_CHANNEL_10;
  sConfig.Rank = ADC_REGULAR_RANK_2;
  if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN ADC1_Init 2 */

  /* USER CODE END ADC1_Init 2 */

}

/**
  * @brief GPIO Initialization Function
  * @param None
  * @retval None
  */
static void MX_GPIO_Init(void)
{

  /* GPIO Ports Clock Enable */
  __HAL_RCC_GPIOC_CLK_ENABLE();
  __HAL_RCC_GPIOA_CLK_ENABLE();

}

void MX_Init(void)
{
  MX_GPIO_Init();
  MX_ADC1_Init();
}

main.h 作成

生成された stm32f7xx_hal_msp.c は、インクルードファイルに main.h の記載があります。
stm32f7xx_hal_msp.c を無修正で使用するためにダミーのヘッダーファイルとして作成します。

中身はたった一行 stm32f7xx_nucleo_144.h を記載します。

main.h
#include "stm32f7xx_nucleo_144.h"

各ファイルの変更

公開されている mturtle_teleop は turtlesim を PC のキーボードで操作する処理になっています。

これをジョイスティックから取得した電圧を A/D 変換して turtlesim を操作できるように変更を加えます。

app.c

こちらのファイルがメインに処理を行うファイルになります。

main 関数が最初に呼ばれるため、最初に A/D 変換関連の初期化を行っています。

teleop_task 関数では A/D 変換した値を twist して ROS 2 へ送信する処理を行っています。

処理の最後に配置している osDelay は 100 ミリ秒に設定しているので、必要に応じて数値を変えてください。

app.cpp
#include "app.h"
#include "mros2.h"
#include "geometry_msgs/msg/vector3.hpp"
#include "geometry_msgs/msg/twist.hpp"

#include "stm32f7xx_nucleo_144.h"

#include "kernel_cfg.h"
#include "syssvc/serial.h"

#define COEFF_LIN 10.0
#define COEFF_ANG 10.0
#define CONSOLE_LIN 0.5
#define CONSOLE_ANG 1.0

ADC_HandleTypeDef hadc1;

int main(int argc, char * argv[])
{
  MX_Init();

  MROS2_INFO("mROS 2 application is started");
  MROS2_INFO("app name: mturtle_teleop");

  mros2::init(argc, argv);
  MROS2_DEBUG("mROS 2 initialization is completed");
  BSP_LED_Toggle(LED1);

  act_tsk(TELEOP_TASK);

  BSP_LED_Toggle(LED3);
}

geometry_msgs::msg::Twist set_twist_val(
  double linear_x, double linear_y, double linear_z,
  double angular_x, double angular_y, double angular_z)
{
  geometry_msgs::msg::Vector3 linear;
  geometry_msgs::msg::Vector3 angular;
  geometry_msgs::msg::Twist twist;

  linear.x = linear_x;
  linear.y = linear_y;
  linear.z = linear_z;
  angular.x = angular_x;
  angular.y = angular_y;
  angular.z = angular_z;
  twist.linear = linear;
  twist.angular = angular;

  return twist;
}

std::string double_to_string(double value)
{
  int intpart, fracpart;
  char str[12];
  intpart = (int)value;
  fracpart = (int)((value - intpart) * 10000);
  sprintf(str, "%d.%04d", intpart, fracpart);
  return str;
}

void teleop_task(void)
{
  unsigned short adc_init[2];
  unsigned short adc_data[2];

  mros2::Node node = mros2::Node::create_node("mturtle_teleop");
  mros2::Publisher pub = node.create_publisher<geometry_msgs::msg::Twist>("turtle1/cmd_vel", 10);  
  MROS2_INFO("ready to pub/sub message");

  geometry_msgs::msg::Vector3 linear;
  geometry_msgs::msg::Vector3 angular;
  geometry_msgs::msg::Twist twist;
  linear.y = 0;
  linear.z = 0;
  angular.x = 0;
  angular.y = 0;

  HAL_ADC_Start(&hadc1);
  HAL_ADC_PollForConversion(&hadc1, 100);
  adc_init[0] = HAL_ADC_GetValue(&hadc1);

  HAL_ADC_Start(&hadc1);
  HAL_ADC_PollForConversion(&hadc1, 100);
  adc_init[1] = HAL_ADC_GetValue(&hadc1);

  auto publish_count = 10;
  double speed = 0.5;
  double turn = 1.0;
  char c;
  while (1)
  {
    HAL_ADC_Start(&hadc1);
    HAL_ADC_PollForConversion(&hadc1, 100);
    adc_data[0] = HAL_ADC_GetValue(&hadc1);

    HAL_ADC_Start(&hadc1);
    HAL_ADC_PollForConversion(&hadc1, 100);
    adc_data[1] = HAL_ADC_GetValue(&hadc1);

    linear.x  = COEFF_LIN * (double)(adc_data[0] - adc_init[0]) / 4096.0;
    angular.z = COEFF_ANG * (double)(adc_data[1] - adc_init[1]) / 4096.0;
    
    MROS2_INFO("ADC:%4d, %4d linear.x: %s, angular.z: %s", \
      adc_data[0], adc_data[1], double_to_string(linear.x).c_str(), double_to_string(angular.z).c_str() );

    twist.linear = linear;
    twist.angular = angular;
    pub.publish(twist);
    osDelay(100);
  }
  mros2::spin();
}

void main_task(void)
{
  /*
   *  initialize serial port
   */
  serial_opn_por(TASK_PORTID);
  serial_ctl_por(TASK_PORTID, (IOCTL_CRLF | IOCTL_FCSND | IOCTL_FCRCV));

  main(0, NULL);
}

void
led_cyclic_handler(intptr_t exinf)
{
  BSP_LED_Toggle(LED2);
}

app.h

app.cpp から STM32CubeMX で生成したファイルの初期化を行いたいので MX_Init 関数を追加します。
それ以外の変更はありません。

+ void MX_Init(void);
app.h
/*
 * header file for the mros2 application
 */

#include <kernel.h>

/*
 * Priorities for TOPPERS tasks
 */

#define MAIN_PRIORITY	5

#define HIGH_PRIORITY	9
#define MID_PRIORITY	10
#define LOW_PRIORITY	11

/*
 * Constants that may depend on the target
 */

#ifndef TASK_PORTID
#define	TASK_PORTID		1
#endif /* TASK_PORTID */

#ifndef STACK_SIZE
#define	STACK_SIZE		4096
#endif /* STACK_SIZE */

/*
 * Declaration of prototyle functions
 */
#ifndef TOPPERS_MACRO_ONLY
#ifdef __cplusplus
extern "C" {
#endif
void main_task(void);
void teleop_task(void);
extern void	led_cyclic_handler(intptr_t exinf);

void MX_Init(void);
#ifdef __cplusplus
}
#endif
void Error_Handler();
#endif /* TOPPERS_MACRO_ONLY */

Makefile.inc

STM32CubeMX 関連や A/D 変換に必要になるファイル類を make の時に使用されるように追加しています。

以下の二つのファイルは別の場所でビルドされるため、ファイル自体の追加はしていません。

  • stm32f7xx_hal_adc.c
  • stm32f7xx_hal_gpio.c

しかし app.cpp のビルドには必要になるため、オブジェクトファイルを使用するように設定しています。

Makefile.inc
#
# Makefile definition for mros2 application
#

# name of main application
APPLNAME = app

# log level: 0(DEBUG), 1(INFO), 2(WARN), 3(ERROR), 4(FATAL), 5(NONE)
# NOTE: also check `initLogMask` in app.cdl for syslog setting
CDEFS := -DMROS2_LOG_MIN_SEVERITY=0

# information for directory tree
APPLDIR	:= $(APPDIR)
APPLDIR	+= $(APPDIR)/config
APPLDIR	+= $(APPDIR)/STM32CubeMX

# link option for C++ code
SRCLANG = c++
APPL_CXXLIBS += -specs=nano.specs

# compile switch
CDEFS += -DUSE_ASP3_FOR_STM

# additional files for configuration
APPL_COBJS := task_impl.o
APPL_COBJS += autosar_os_ext_user_config.o
APPL_COBJS += autosar_os_ext_asp3_user_config.o
APPL_COBJS += mros2_user_config.o
APPL_COBJS += stm32f7xx_hal_mx.o
APPL_COBJS += stm32f7xx_hal_msp.o
APPL_COBJS += stm32f7xx_hal_adc.o
APPL_COBJS += stm32f7xx_hal_gpio.o

mros2-asp3-f767zi

ビルド環境構築は済んでいるので、このままビルドすることが可能です。

もしもまだビルド環境が構築出来てない場合は、以下の記事をご参照ください。
https://zenn.dev/himura/articles/6f80c6c63d4116

mROS 2 app のビルドと書き込み

ワークスペースへ移動し、今回は新たに作成した mturtle_teleop_joy を make します。

cd ~/mros2-asp3-f767zi/workspace/
make clean
make app=mturtle_teleop_joy

エラーもなくビルドが完了すると asp.bin ファイルが生成されています。
以下のコマンドで確認できます。

ls asp*

ls asp*
bin ファイルの確認

書き込みをする際はSTMicro Nucleo-F767ZI と Mini-PC がUSB ケーブルで接続されているか確認してください。

次に以下のコマンドで STMicro Nucleo-F767ZI に直接書き込む事ができます。

cp asp.bin /media/$USER/NODE_F767ZI/

※$USER は各自が設定したログイン名（私の場合は himura）

コマンドで書き込みを行った際の様子
コマンドで書き込みを行った際の様子

mturtlesim のビルド

mturtlesim をビルドします。
以下のコマンドを打ち込みリポジトリのクローンを作成します。

cd ~/ros2_humble/src/ 
git clone -b mros2/humble-devel https://github.com/mROS-base/ros_tutorials

リポジトリのクローンを作成したらビルドを行います。

cd ~/ros2_humble/ && colcon build --packages-select mturtlesim

mturtle_teleop_joy の実行

STMicro Nucleo-F767ZI の mROS 2 と Mini-PC でビルドした mturtlesim を起動し、動作を確認します。

  1. 以下のコマンド打ち込み picocom を立ち上げます。
sudo picocom -b 115200 /dev/ttyACM0
  1. 次にボード上の黒いリセットスイッチを1度押下します。
    以下の画像のようにボードからのログが表示されれば OK です。

STMicro Nucleo-F767ZIのログ
STMicro Nucleo-F767ZI から送信されてきたログ

  1. mturtlesim を起動します。
cd ~/ros2_humble/
source install/local_setup.bash
ros2 run mturtlesim turtlesim_node

mturtlesim が起動すると LAN ケーブルを介して STMicro Nucleo-F767ZI と通信し 表示されている亀がジョイスティックで操作できるのが解ります。
ジョイスティックで操作される亀の様子
ジョイスティックで操作される亀の様子

おわりに

予想していたよりも説明が長くなってしまいました。

しかし、実際の内容はそんなに多くないので、やりやすいと思います。

STM32CubeMX を利用して機能を追加できることが分かったので、次は別の機能を試してみたいですね。

今回の記事も参考になりましたら幸いです。

Discussion