mBotでROSを試す

最近、自動運転等にも利用されているというROS(Robot Operating System)に興味を持ちました。とりあえず「ROSではじめるロボットプログラミング」（工学者）を読んでなんとなく理解しましたので、リアルロボットでちょこっと試してみました。

使用したロボットはmBot(Makeblock)というArduinoベースの教育用STEMロボットです。１年ほど前に購入して30分くらい動かしてそのまま放置していたもので、その再利用です。

ROSはUbuntu16.04をインストールしたRaspberryPi3に載せ、Arduino(mBot)との間で"rosserial"という仕組みを使ってシリアル通信します。RaspiはパソコンからSSH接続できるようにしておきます。ROS側からのコマンドでロボットの前進・後進、ロボットに付属の照度センサの入力で一時停止させるような動作を実現させます。

・mBot(Arduino)側
Arduinoに書き込むプログラムは具体的にモーター駆動やセンサー読み取りを行なうデバイスドライバのような位置づけかと思います。
mBotには簡単にモーターやセンサーを扱えるライブラリがあるのですが、これを使うとArduinoのRAMの容量が足りなくなってしまいましたので、ここでは普通のanalogReadやanalogWriteを用いています。またROSには並進・回転速度をまとめて扱えるTwistというメッセージがありますが、今回はこの中の並進速度(linear.x)のみを使っています。

#include <ros.h>
#include <ros/time.h>
#include <std_msgs/UInt16.h>
#include <geometry_msgs/Twist.h>
#include <rosserial_arduino/Adc.h>

uint16_t motorSpeed = 0;

void motorCallBack(const geometry_msgs::Twist& vel_msg)
{
  motorSpeed = vel_msg.linear.x * 100;
  if(vel_msg.linear.x > 0){
    digitalWrite(4, HIGH);
    digitalWrite(7, LOW);
  }else{
    digitalWrite(4, LOW);
    digitalWrite(7, HIGH);
  }
}

ros::NodeHandle  nh;
geometry_msgs::Twist vel_msg;    //receiving motor speed(pwm)
std_msgs::UInt16 light_msg;      //sending light sensor value
ros::Publisher pub_light( "/light", &light_msg);
ros::Subscriber <geometry_msgs::Twist> sub("/cmd_vel", &motorCallBack);

void setup()
{
  nh.initNode();
  nh.subscribe(sub);
  nh.advertise(pub_light);
  
  pinMode(4,OUTPUT);    //R_motor direction
  pinMode(5,OUTPUT);    //R_motor pwm
  pinMode(6,OUTPUT);    //L_motor pwm
  pinMode(7,OUTPUT);    //L_motor direction
  pinMode(A6,INPUT);    //Light_Sensor input
  digitalWrite(4, LOW);
  digitalWrite(7, LOW);
  analogWrite(5, motorSpeed);
  analogWrite(6, motorSpeed);
}

long light_time;

void loop()
{  
  //publish the adc value every 50 milliseconds
  //since it takes that long for the sensor to stablize
  if ( millis() >= light_time ){
    light_msg.data = analogRead(6);	//read from ADC6
    pub_light.publish(&light_msg);
    light_time =  millis() + 50;
  }
  analogWrite(5, motorSpeed);
  analogWrite(6, motorSpeed);
  nh.spinOnce();
}

次はROS側のpythonスクリプトです。
・vel_publisher.py
whileループの中でキーボード入力があれば、ロボット速度をpublishします。一方subscriberはcallback関数の中で照度センサの値を読み込み、照度が低ければ（手でセンサを押さえた）速度ゼロを、照度が高ければ（普通の明るさ）現状速度をpublishします。

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from std_msgs.msg import UInt16

preVel = 0.0

pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

def callback(message):
    vel = Twist()
    if message.data < 900:
        vel.linear.x = 0.0
        pub.publish(vel)
    else:
        vel.linear.x = preVel
        pub.publish(vel)
def control():
    global prevVel
    rospy.init_node('vel_publisher')
    rospy.Subscriber('/light', UInt16, callback)

    while not rospy.is_shutdown():
        vel = Twist()
        direction = raw_input('f:forward, b:backward > ')
        if 'f' in direction:
            prevVel = 1.0
            vel.linear.x = 1.0
        if 'b' in direction:
            prevVel = -1.0
            vel.linear.x = -1.0
        if 'q' in direction:
            prevVel = 0.0
            vel.linear.x = 0.0
        pub.publish(vel)
if __name__ == '__main__':
    try:
        control()
    except rospy.ROSInterruptException:
        pass

・動作確認
mBotからRaspiに電源供給できなかったので、外部からRaspiにUSB電源接続しています。

$roscore

$rosrun rosserial_python serial_node.py _port:=/dev/ttyUSB0

$chmod 755 vel_publisher.py
$./vel_publisher.py

MENU

Categories