ROS対応のインホイールモーターを作る その2

ROS対応のインホイールモーターを作る その2

滝沢ロボティクス開発担当田尻です。
岩手県立大学のアドベントカレンダー2021の記事として書きました。
現在弊社で使用しているロボットtakiroboG1はArduPilotをメインとしておりますが、ROSやArduinoで使いたいという要望も多くあり、takiroboG1 for ROS/Arduino を開発しております。
それに伴い、ROS対応のインホイールモーター制御のソフトウェアを開発しました。といっても、これ自体はオープンソースのVESCという多機能モータードライバ用のライブラリを流用して作ったものになりますので普通に公開しちゃいます。もちろん、もっと直した方が良い部分はたくさんありますので使用される方は適宜調整いただけたらと思います。（本ソースの使用における一切の責任を負いかねます）

今回はgeometry/Twist形式のトピックを送るとモータが動き、オドメトリのTFのブロードキャスタとnav_msgs/Odometry形式のトピックが排出されるArduinoプログラムを作ります。前回の記事 https://takizawa-robotics.com/2021/07/09/vesc-ros/

前準備

使うボード：Arduino Mega
ROS：Jetson Xavier Nx（事前に入れておきましょう）
MD：VESC 2枚 https://ja.aliexpress.com/item/4000438827676.html?spm=a2g0o.productlist.0.0.1cb64a0aHUzlHe&algo_pvid=272f91fa-8df7-43ef-a5ab-825d0a938179&algo_exp_id=272f91fa-8df7-43ef-a5ab-825d0a938179-8&pdp_ext_f=%7B%22sku_id%22%3A%2210000001807138829%22%7D
aliexpressですけど実績のある店ですので、即日または翌日発送最短1週間で届きます。

筆者はJetsonにArduino環境を入れて使っております。

まずArduinoに2つのライブラリを入れます。
一つはVESCのライブラリです。https://github.com/SolidGeek/VescUart/
zip形式のままインクルードしちゃってください。

もう一つはros_libライブラリです。ros_serialを使うために入れます。
ただこのライブラリ、入れ方が2種類あるんですけどArduinoのライブラリマネージャから検索して入れる方法はコンパイルがうまく通らないので(とくにtf周り)、この記事の方法でライブラリを生成してください。http://blog.robotakao.jp/blog-entry-442.html
また設定サイズは下記の通りにしてください。

・・・
int MAX_SUBSCRIBERS = 5,
int MAX_PUBLISHERS = 5,
int INPUT_SIZE = 512,
int OUTPUT_SIZE = 1024>
・・・ 

実際につかってみた結果がこちら。
rvizで見るとオドメトリのTFが出ているのがわかります。

こちらがソースになります。
今回VESCはSerial2とSerial3に接続しています。ご自身の環境に合わせて適宜変更してください。

#include <ros.h>
#include <ros/time.h>
#include <std_msgs/Empty.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>
#include <sensor_msgs/JointState.h>
#include <VescUart.h>

/*定数*/
#define BASE_WIDTH  (0.52) //(m)トレッド幅
#define WHEEL_SIZE  (0.2) //(m)8インチタイヤ
#define MOTOR_POLES  (15.0) //極数
#define MOTOR_STEPS  (4.0) //4°
#define REFRESH_RATE  (5)//ms

/*フレーム名*/
const char *odom_frame = "/g1_odom";
const char *base_frame = "/g1_base_footprint";

/*ROSのインスタンス*/
ros::NodeHandle nh;
ros::Time current_time, last_time;

/*インホイールモータのインスタンス*/
VescUart LEFT_MOTOR;
VescUart RIGHT_MOTOR;

/*ROSメッセージの型*/
nav_msgs::Odometry odom_msg;
tf::TransformBroadcaster odom_broadcaster;
geometry_msgs::TransformStamped odom_tf;

/*グローバル変数*/
volatile uint8_t flag_firsttime = 1;

/*プロトタイプ宣言*/
void callBackMotorControl(const geometry_msgs::Twist& twist);
void callBackResetFlag(const std_msgs::Empty& flag);
void motorDriverSetSpeed(const float vel_left, const float vel_right);
void getOdometry(void);
bool getEMG(void);

/*ROS Publisherのインスタンス*/
/*トピック名:"g1/odometry"*/
ros::Publisher odomPublisher("g1/odometry", &odom_msg);

/*ROS Subscriberのインスタンス*/
/*トピック名:"g1/cmd_vel"*/
ros::Subscriber<geometry_msgs::Twist> twistSubscriber("g1/cmd_vel", &callBackMotorControl);
ros::Subscriber<std_msgs::Empty> resetSubscriber("g1/odom_reset", &callBackResetFlag);

/////////////
/*初期化設定*/
////////////
void setup()
{
  /** Setup UART port */
  Serial2.begin(115200);//Left Motor Driver
  Serial3.begin(115200);//Right Motor Driver

  while (!Serial2);
  while (!Serial3);

  /* Setup GPIO for Emargerncy Button*/
  pinMode(2,INPUT_PULLUP);

  /** Define which ports to use as UART */
  LEFT_MOTOR.setSerialPort(&Serial2);
  RIGHT_MOTOR.setSerialPort(&Serial3);

  /*ROSの初期化*/
  nh.getHardware()->setBaud(115200);
  nh.initNode();
  nh.subscribe(twistSubscriber);
  nh.subscribe(resetSubscriber);
  odom_broadcaster.init(nh);
  nh.advertise(odomPublisher);
}

/////////
/*メイン*/
/////////
void loop()
{
  long current_time_ms = 0;
  static long last_time_ms = 0;
  current_time_ms = millis();
  getOdometry();
  getEMG();

  if((current_time_ms - last_time_ms) > REFRESH_RATE)
  {
    /*トピックとtfを送る*/
    odomPublisher.publish(&odom_msg);
    odom_broadcaster.sendTransform(odom_tf);
    nh.spinOnce();
    last_time_ms = current_time_ms; 
  }

  if(0 > (current_time_ms - last_time_ms)){
    last_time_ms = current_time_ms;
  }

  
}

///////////////////////////////////////////////////////////////////////////
/*callback(トピックが飛んできた段階でモータに指令を出す)*/
///////////////////////////////////////////////////////////////////////////
void callBackMotorControl(const geometry_msgs::Twist& twist)
{
  /*Twist 情報の取得*/
  const float linear_x = twist.linear.x;//前後の移動方向
  const float angle_z = twist.angular.z;//z軸を中心としたときの回転、つまり旋回

  /*モーター速度の計算*/
  float vel_left_speed =  (angle_z * BASE_WIDTH - 2 * linear_x) / (-2.0);
  float vel_right_speed = (angle_z * BASE_WIDTH + 2 * linear_x) / 2.0;

  /*左右のモーターのスピードをセットする*/
  motorDriverSetSpeed(vel_left_speed, vel_right_speed);
  
  /*受信とモーターのスピートをセット出来たら成功*/
  char buf1[100] = "";
  char buf2[100] = "";
  char log_msg[100] ="";
  /*dtostrf(vel_left_speed,6,2,buf1);
  dtostrf(vel_right_speed,6,2,buf2);
  sprintf(log_msg, "set twist:( %s, %s )", buf1, buf2);
  nh.loginfo(log_msg);*/
}

///////////////////////////////////////
/*速度からrpmに変換しモーターを回転させる*/
///////////////////////////////////////
void motorDriverSetSpeed(float vel_left, float vel_right)
{
  /*0.2m = Wheel diameter, 15 = motor poles*/
  static float vel_left_rpm_old = 0;
  static float vel_right_rpm_old = 0;

  float vel_left_rpm = vel_left / (WHEEL_SIZE * PI) * 60.0 * MOTOR_POLES;
  float vel_right_rpm = vel_right / (WHEEL_SIZE * PI) * 60.0 * MOTOR_POLES;

  /*brake*/
  if(vel_left_rpm_old > 0){
    if(0 > vel_left_rpm){
      vel_left_rpm = 0;
      vel_right_rpm = 0;
    }
  }
  else if(0 > vel_left_rpm_old){
    if(vel_left_rpm > 0){
      vel_left_rpm = 0;
      vel_right_rpm = 0;
    }
  }

   if(vel_right_rpm_old > 0){
    if(0 > vel_right_rpm){
      vel_right_rpm = 0;
      vel_left_rpm = 0;
    }
  }
  else if(0 > vel_right_rpm_old){
    if(vel_right_rpm > 0){
      vel_right_rpm = 0;
      vel_left_rpm = 0;
    }
  }

  /*モーターの回転数をセットする*/
  LEFT_MOTOR.setRPM((float)vel_left_rpm);
  RIGHT_MOTOR.setRPM((float)vel_right_rpm);

  vel_right_rpm_old = vel_right_rpm;
  vel_left_rpm_old = vel_left_rpm;
}

////////////////////
/*初期位置のリセット*/
///////////////////
void callBackResetFlag(const std_msgs::Empty& flag)
{
  flag_firsttime = 1;
  nh.loginfo("Reset odometry!");
}

//////////////////////////////////////
/*オドメトリ情報を取得してトピックに入れる*/
//////////////////////////////////////
void getOdometry(void)
{
  /*ローカル変数　一時的に位置情報と速度情報を保存しておく*/
  static long LEFT_MOTOR_tacho_old = 0;
  static long RIGHT_MOTOR_tacho_old = 0;
  double LEFT_MOTOR_odom = 0;
  double RIGHT_MOTOR_odom = 0;
  double LEFT_MOTOR_velocity = 0;
  double RIGHT_MOTOR_velocity = 0;
  double distance = 0;
  double theta = 0;
  static double distance_old = 0;
  static double theta_old = 0;
  double d_x, d_y, d_t;
  static double pos_x = 0;
  static double pos_y = 0;
  static long current_time_ms = 0;
  static long last_time_ms = 0;
  geometry_msgs::Quaternion odom_quat;

  char buf[4][100] = {"","","",""};
  char log_msg[100] ="";

  //LEFT_MOTOR.getVescValues();RIGHT_MOTOR.getVescValues();
  if (LEFT_MOTOR.getVescValues() && RIGHT_MOTOR.getVescValues() ) 
  {
    /* getValues data struct
      float tempFET;
      float tempMotor;
      float avgMotorCurrent;
      float avgInputCurrent;
      float dutyCycleNow;
      long rpm;
      float inpVoltage;
      float ampHours;
      float ampHoursCharged;
      long tachometer;
      long tachometerAbs;
    */

    ///////////////////////
    /*スタート位置のリセット*/
    //////////////////////
    if ( flag_firsttime )
    {
      flag_firsttime = 0;

      LEFT_MOTOR_tacho_old = LEFT_MOTOR.data.tachometer;
      RIGHT_MOTOR_tacho_old = RIGHT_MOTOR.data.tachometer;
      distance_old = 0;
      theta_old = 0;
      pos_x = 0;
      pos_y = 0;
      last_time = nh.now();
      last_time_ms = millis();
      return;
    }

    /*現在の時間を保存する*/
    current_time = nh.now();
    current_time_ms = millis();
    //d_t = current_time.toSec() - last_time.toSec();
    d_t = (float)(current_time_ms - last_time_ms) / 1000.0;
    
    ///////////////////////////////////
    /*距離と角度の計算*/
    /*R = Rモーター回転角度×車輪直径×π÷360
      　  L = Lモーター回転角度×車輪直径×π÷360
      　  d = (L+R)÷2
      　  θ= atan((R-L)÷W)
      このモーターは4°=360/90毎に回転量が取れる*/
    ///////////////////////////////////
    LEFT_MOTOR_odom = ((float)(LEFT_MOTOR.data.tachometer  -  LEFT_MOTOR_tacho_old) * MOTOR_STEPS) * WHEEL_SIZE * PI / 360.0;
    RIGHT_MOTOR_odom = ((float)(RIGHT_MOTOR.data.tachometer - RIGHT_MOTOR_tacho_old) * MOTOR_STEPS) * WHEEL_SIZE * PI / 360.0;
    distance = (LEFT_MOTOR_odom + RIGHT_MOTOR_odom) / 2.0;
    theta = (RIGHT_MOTOR_odom - LEFT_MOTOR_odom) / BASE_WIDTH;

    dtostrf(distance,6,2,buf[0]);
    dtostrf(theta*180/PI,6,2,buf[1]);
    
    //////////////////////////////
    /*直交座標の位置の計算
      dx = velocity*cosθ
      dy = velocity*sinθ
      xt+1=xt+x˙Δtcos(θt+θ˙Δt/2)
      yt+1=yt+y˙Δtsin(θt+θ˙Δt/2)*/
    /////////////////////////////
    d_x = (distance - distance_old) * cos(theta);
    d_y = (distance - distance_old) * sin(theta);
    pos_x += d_x;
    pos_y += d_y;

    dtostrf(pos_x,6,2,buf[2]);
    dtostrf(pos_y,6,2,buf[3]);

    /*クオータニオンに変換する*/
    odom_quat = tf::createQuaternionFromYaw(theta);
    
    ///////////
    /*TFの排出*/
    ///////////
    odom_tf.header.stamp = current_time;
    odom_tf.header.frame_id = odom_frame;
    odom_tf.child_frame_id = base_frame;

    /*位置変換情報を入れる*/
    odom_tf.transform.translation.x = pos_x;
    odom_tf.transform.translation.y = pos_y;
    odom_tf.transform.translation.z = 0.0;
    odom_tf.transform.rotation = odom_quat;

    ////////////////////////
    /*Odometryトピックの排出*/
    ////////////////////////
    odom_msg.header.stamp = current_time;
    odom_msg.header.frame_id = odom_frame;
    odom_msg.child_frame_id = base_frame;

    /*位置情報を入れる*/
    odom_msg.pose.pose.position.x = pos_x;
    odom_msg.pose.pose.position.y = pos_y;
    odom_msg.pose.pose.position.z = 0.0;
    odom_msg.pose.pose.orientation = odom_quat;

    /*速度情報を入れる
      velocity = (left_velocity + right_velosity) / 2
      ω = (right_velocity - left_velocity) / トレッド幅*/
    LEFT_MOTOR_velocity =  (LEFT_MOTOR.data.rpm / MOTOR_POLES)  * WHEEL_SIZE * PI / 60.0;//m/s
    RIGHT_MOTOR_velocity = (RIGHT_MOTOR.data.rpm / MOTOR_POLES) * WHEEL_SIZE * PI / 60.0;//m/s

    odom_msg.twist.twist.linear.x = (LEFT_MOTOR_velocity + RIGHT_MOTOR_velocity) / 2.0;
    odom_msg.twist.twist.linear.y = 0;
    odom_msg.twist.twist.linear.z = 0;
    odom_msg.twist.twist.angular.z = 0;
    odom_msg.twist.twist.angular.y = 0;
    odom_msg.twist.twist.angular.z = (RIGHT_MOTOR_velocity - LEFT_MOTOR_velocity) / BASE_WIDTH;

    /*現在の時間を保存しておくことによって速度情報を計算する*/
    last_time = current_time;
    last_time_ms = current_time_ms;
    distance_old = distance;
    theta_old = theta;
    
    /*sprintf(log_msg, "dist:%s , theta:%s, pos_x:%s, pos_y:%s )", buf[0], buf[1], buf[2], buf[3]);
    nh.loginfo(log_msg);*/
  }
}

bool getEMG(){
  static bool EMG = false;
  if(digitalRead(2) == HIGH)
  {
    if(EMG == false)
    {
      motorDriverSetSpeed(0,0);
      EMG = true;
    }
  }
  else
  {
    EMG = false;
  }
  return EMG;
}

VESC自体の設定

事前にVESCを設定しておく必要があります。
VESCToolを開いて、(無い人は過去記事参照)
通信方式をUART, ボーレート115200に変更してください。
その他パラメータとしてタイムアウト時間150ms, タイムアウト時ブレーキ10A が目安です。
PIDゲイン調整やモーターやホールセンサのキャリブレーションは各自で行ってください。

動作

ROS側からros_serialのnodeを立ち上げてください。ボーレートは115200です。
rvizを開いて座標系の基準を設定するとTFが表示されるようになります。
モーターを動かす際は、joyコンを繋いでjoy_nodeやteleop_twist_joy等のROSパッケージを入れて動かしてみてください。
私はLogicoolのF310を使用しています。

思ったよりは、おどめとりの精度が出ます。
せっかくなのでgmappingでmappingした後、amclとnavigationで自動走行させてみました。
いい感じです。

以上です。

田尻　隼人