OGIMOノート

6歳の娘と、4歳の息子(脳性麻痺)を持った父親エンジニアの備忘録。自作の電子工作おもちゃ/リハビリ器具の製作記録、勉強記録を残していきます

【ROS】ラズパイで始めるROS BOT入門④ ~WaveShare社Alphabotの移動制御対応~

はじめに

ようやくラズパイ環境を構築できたので、いよいよ実物ロボットを使った自律移動を学んでいく。しかし、ここで問題になるのは、どのロボットを購入するかである。
安いと言われているTurtlebot3 Burgerで約7万円、RT社のマイクロマウスでも約5万円。小遣い制の父親エンジニアにはなかなか決裁が取れない様な金額である。

別途、mBot(1.3万円)は持っているのだが、これはモーターエンコーダなしのため、SLAM環境構築には厳しいと考える。
モーターエンコーダ付きの既存二輪走行ロボットでなんか安いのないかなぁー、とAmazonをさまよっていると……1万円以下で発見!!!


AlphaBot - Robotics

WaveShare社のAlphabotなる二輪走行ロボット。サイト情報からは、モーターエンコーダ搭載とあり、お値段はラズパイなしで当時(2017/11)で6500円!
Webサイトで調べるも全然レビュー例がないのではげしく不安だが、「この値段なら失敗してもまだなんとかなる!」と思い購入!

Alphabot組み立て

とりあえず無事に到着したので、組み立ててみる。
メーカーサイトにWikiがあり、組み立て説明書も以下のサイトにある。

AlphaBot - Waveshare Wiki
とりあえず手順に従って組み立てていく。
f:id:motokiinfinity8:20180507022501j:plainf:id:motokiinfinity8:20180508020809j:plain


一旦組み立て完了後の写真。思ってたより大きかった。Raspberry pi3 と Arduino Unoはボディ間にはさむ想定。(ただし、Arduinoはケースを外し、底はテープで絶縁する必要あり)

f:id:motokiinfinity8:20180508021104j:plain

各デバイス(モーター/エンコーダ/ライントレーサー/赤外線センサー)をRaspberry / Arduino のどちらと接続するかをジャンパーで切り替えれる様だ(左側の黄色ジャンパ) これは便利。また、RaspberryとArduino間はUARTで繋がってる様だ。これもスイッチでON/OFFできる。

f:id:motokiinfinity8:20180508021825j:plain

タイヤのエンコーダ。ギアボックス付きモーターにエンコーダ(黒い穴空き円盤)があり、フォトダイオードでパルス検出するタイプ。オシロでデジタル信号を確認したところ、キレイには取れていた!

不満点

便利に感じる面もあるが不満点も多々あり。
・底の補助輪(金属)の回転摩擦が大きく、移動するとガラガラ音が鳴る
・タイヤの付きが甘く外れやすい
・前後の重量バランスがシビアで、後ろ重心になるとタイヤが空回りしてしまう
・ラズパイのUSB電源とHDMIコネクタが内部モーター手前のため、ロボット状態でのケーブル抜き差し不可。
・バッテリーは見た目乾電池だが、乾電池ではなくリチウム電池18650を購入する必要あり。
私はamazonにて、以下のメーカー品を購入したのだが、安全最優先で保護回路付きにしたため、保護回路分電池が長く、無理やり押し込まないと入らない


結果、値段相当ではある印象。
まぁ、これを使いこなしてこそエンジニア!という事で、もう少し頑張ってみたいと思う。

ROSでのcmd_vel対応 (Arduinoマイコンでの左右モーター制御)

まずは走行ロボット制御の基礎である前後左右移動を実装する。操作はPS4コントローラのアナログ(左)スティックで行う。
ブロック図は以下。

f:id:motokiinfinity8:20180511014736j:plain

ラズパイ側では、PS4コントローラのjoyノードから通知されるjoy情報から速度情報(geometry::twist型/cmd_vel)を生成して、arduinoマイコンに通知する。arduinoマイコンはcmd_vel情報から、左右のモーターに必要なPWM値を算出して信号出力するイメージだ。

モーターの必要トルクについては不勉強なため、カット&トライアルを実施。モーターはPWMで制御。現状ではバッテリー80%で80/256くらいの回転数で動き出せる様だ。

Arduinoとのrosserial通信

個人的にROSの非常にありがたい機能であるroserial。これはシリアル通信を使ったROSメッセージを送る汎用プロトコル になり、これがらあれば非力な非OSのマイコンでもclientとしてのROS動作を実現できると非常に使いやすい!

rosserial 導入
sudo apt-get install ros-kinetic-rosserial-arduino ros-kinetic-rosserial
cd <sketchbook>/libraries
rm -rf ros_lib
rosrun rosserial_arduino make_libraries.py .

arduinoWindowsで書きこみたい場合は、作成したros_libをWindows内のarduino/library に置けばオッケー!(ros.hでインクルードエラーが置きなければ良い)

UART使用に向けて

Ubuntu MATE@Raspberry PiではdefaultでUARTが使えないため、以下の設定を実施してUARTを使用可能にする必要がある
①/boot/config.txtの以下の設定を有効にする
 core_freq=250
② /boot/cmdline.txtを以下に変更する
 dwc_otg.lpm_enable=0 console=tty1 root=/dev/mmcblk0p2 rootfstype=ext4 elevator=deadline rootwait quiet splash
③ raspi-configを実行
 「3:Interface Option」→「P5: Serial」→「login shel accessible: No」→「enable:Yes」
④再起動

作成コード

①raspberry側 alphabot_manual.launch

<?xml version="1.0"?>
<launch>
    <!-- Start joycon -->
    <node pkg="joy" name="joycon" type="joy_node"/>

    <!-- Start alphabot_manual node -->
    <node pkg="alphabot" name="AlphaBotManual" type="AlphaBotManual"  output="screen"/>

    <!-- Set to your serial port of Arduino -->
    <arg name="serial_port" default="/dev/ttyS0"/>
    <arg name="baud_rate" default="57600"/>
    <node pkg="rosserial_python" type="serial_node.py" name="serial_node" output="screen">
        <param name="port" type="string" value="$(arg serial_port)"/>
        <param name="baud" type="int" value="$(arg baud_rate)"/>
    </node>
</launch>



②raspberry側 alphabot_manual.cpp

#include <ros/ros.h>
#include <ros/wall_timer.h>
#include <math.h>
#include <sensor_msgs/Joy.h>
#include <geometry_msgs/Twist.h>

typedef enum {
	JOY_AXES_MANUAL_VERTICAL     = 1,   // Up or Down     (PS4:左スティック)      [joy_msg.axes]
	JOY_AXES_MANUAL_HORIZONTAL   = 0,   // Right or Left  (PS4:左スティック)      [joy_msg.axes]
} JOY_AXES;

typedef enum {
	JOY_BUTTON_MANUALBOOST        = 0,   // Set Manual Mode  (PS4:□ボタン)  [joy_msg.buttons]
} JOY_BUTTON;

#define UPDATE_TS 0.05

class AlphabotManual{
public:
	AlphabotManual();
	~AlphabotManual();
	void timerCallback(const ros::TimerEvent&);
	void JoyConCallback(const sensor_msgs::Joy &joy_msg);
private:
	ros::Timer timer;
	ros::NodeHandle nh;
	ros::Subscriber joy_sub;
	ros::Publisher twist_pub;
	geometry_msgs::Twist cmd_vel,p_cmd_vel;
};

AlphabotManual::AlphabotManual(){
	timer        = nh.createTimer(ros::Duration(UPDATE_TS), &AlphabotManual::timerCallback, this);
	joy_sub      = nh.subscribe("joy", 10, &AlphabotManual::JoyConCallback, this);
	twist_pub    = nh.advertise<geometry_msgs::Twist>("alphabot/cmd_vel", 10);
}

AlphabotManual::~AlphabotManual(){
}

/***************************************************************************
* PS4コントローラによる操作
***************************************************************************/
void AlphabotManual::JoyConCallback(const sensor_msgs::Joy &joy_msg){ 
	int boost = (joy_msg.buttons[JOY_BUTTON_MANUALBOOST] == 1) ? 3 : 1;
	cmd_vel.linear.x = joy_msg.axes[JOY_AXES_MANUAL_VERTICAL]*0.25*boost;
	cmd_vel.angular.z = joy_msg.axes[JOY_AXES_MANUAL_HORIZONTAL]*0.5*boost;
   	return;
}

void AlphabotManual::timerCallback(const ros::TimerEvent&){
	// cmd_vel更新
	twist_pub.publish(cmd_vel);
	if(p_cmd_vel.linear.x != cmd_vel.linear.x || p_cmd_vel.angular.z != cmd_vel.angular.z ){
		ROS_INFO("[Manual mode]linear.x: %0.2f, angular.z: %0.2f",cmd_vel.linear.x, cmd_vel.angular.z);
		p_cmd_vel = cmd_vel;
	}	
	return;
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "AlphaBotManual");
	AlphabotManual client;
	ros::spin();
	return 0;
}



arduino側 alphabot_cmdvel.ino

#include <ros.h>
#include <geometry_msgs/Twist.h>
//駆動に必要な最低PCMパルス数
#define L_PULSE_START 80 //駆動に必要な最低PCMパルス数
#define R_PULSE_START 80 //駆動に必要な最低PCMパルス数
#define PULSE_1MPS 80 //1.0m/sに必要なPCMパルス数

ros::NodeHandle nh;
int cmdvel_cnt=0;
int l_motor=0, r_motor=0;
std_msgs::Int32 count_msg;
void MotorCmdCallback(const geometry_msgs::Twist& msg){
cmdvel_cnt = 0;

// 進行方向設定
if(msg.linear.x > 0.0){
  l_motor = (int)(L_PULSE_START + PULSE_1MPS * msg.linear.x);
  r_motor = (int)(R_PULSE_START + PULSE_1MPS * msg.linear.x);
}else if(msg.linear.x < 0.0){
  l_motor = (int)(-1*L_PULSE_START + PULSE_1MPS * msg.linear.x);
  r_motor = (int)(-1*R_PULSE_START + PULSE_1MPS * msg.linear.x);
}else{
  l_motor = 0;
  r_motor = 0;
}
// 角度方向設定
if(msg.angular.z > 0.0){
  if(msg.linear.x == 0.0){
    l_motor = (int)(-1*L_PULSE_START - PULSE_1MPS * msg.angular.z * 0.75 /2);
    r_motor = (int)( R_PULSE_START + PULSE_1MPS * msg.angular.z * 0.75 /2); 
  }else if(msg.linear.x > 0.0){
    r_motor += PULSE_1MPS * msg.angular.z;
    l_motor -= PULSE_1MPS * msg.angular.z*0.5;
  }else{
    r_motor -= PULSE_1MPS * msg.angular.z;
    l_motor += PULSE_1MPS * msg.angular.z*0.5;
   }
 }else if(msg.angular.z < 0.0){
    if(msg.linear.x == 0.0){
      l_motor = (int)( L_PULSE_START + PULSE_1MPS * msg.angular.z * -0.75 /2);
      r_motor = (int)( -1 * R_PULSE_START - PULSE_1MPS * msg.angular.z * -0.75 /2); 
    }else if(msg.linear.x > 0.0){
      l_motor += PULSE_1MPS * msg.angular.z * -1;
      r_motor -= PULSE_1MPS * msg.angular.z*0.5 * -1;
  } else{
      l_motor -= PULSE_1MPS * msg.angular.z * -1;
      r_motor += PULSE_1MPS * msg.angular.z*0.5 * -1;
  }
}
  //限度設定
  if(l_motor > 255) l_motor = 255;
  if(l_motor < -255) l_motor = -255;
  if(r_motor > 255) r_motor = 255;
  if(r_motor < -255) r_motor = -255;
return;
}
bool MotorRun(int LS,int RS){ 
  if(LS >= 0 && LS <= 255){
    analogWrite(5, LS); 
    digitalWrite(14, HIGH); 
    digitalWrite(15, LOW);
  }
  if(LS < 0 && LS >= -255){
    analogWrite(5, abs(LS)); 
    digitalWrite(14, LOW); 
    digitalWrite(15, HIGH);
  } 
  if(RS >= 0 && RS <= 255){
    analogWrite(6, RS); 
    digitalWrite(17, HIGH); 
    digitalWrite(16, LOW);
  }
  if(RS < 0 && RS >= -255){
    analogWrite(6, RS); 
    digitalWrite(17, LOW); 
    digitalWrite(16, HIGH);
  }
  if  (RS > 255 || RS < -255 || LS > 255 || LS < -255){
    return false;
  } 
  return true;
}

bool MotorBrake(){ 
  digitalWrite(5,LOW);
  digitalWrite(6,LOW);
}
ros::Subscriber<geometry_msgs::Twist> sub_cmdvel("alphabot/cmd_vel", MotorCmdCallback);
void setup(){
  nh.initNode();
  nh.subscribe(sub_cmdvel);
}

void loop(){
//cmd_vel設定の反映
  if(cmdvel_cnt <= 20){ //cmd_vel命令が 1000ms(50*20)以内の場合
    MotorRun(l_motor, r_motor); 
    cmdvel_cnt++; 
  }else{
    MotorBrake(); 
  }
nh.spinOnce();
delay(50);
}



これで無事にPS4コントローラから二輪走行ロボットを動かすことが出来た。左右のトルクばらつきなどでまっすく進まないケースがあるため、そこについては微調整が必要になりそうだ。次回はOdometryによる自己位置推定のベース作成に行きたいと思う