Make a ROS package to communicate with Arduino to control motors

Arduino Code

Arduino has to receive messages from raspberry pi and decode it. Move to the directory where you want to work with Arduino. Then, download the code.

$ cd /path/to/your/workplace
$ wget https://raw.githubusercontent.com/mktk1117/six_wheel_robot/master/Arduino/motor_driver/motor_driver.ino

or you can copy and paste from here.

Then, write this code to the Arduino.

ROS Code

Then, Raspberry Pi should receive the control command from /cmd_vel and translate it to the rotation signal of each motor and send it to Arduino. If you are not familiar with cmd_vel please see Understanding ROS Topics

Please move to catkin_ws/src and download and make the code.

pi@raspberry$ cd ~/catkin_ws/src
pi@raspberry$ svn export https://github.com/mktk1117/six_wheel_robot/trunk/ROS/motor_driver
pi@raspberry$ cd ~/catkin_ws
pi@raspberry$ catkin_make

The python script would be like this

#!/usr/bin/env python
# license removed for brevity
import rospy
from std_msgs.msg import String
from geometry_msgs.msg import Twist
import serial


class driver:
    def __init__(self):
        # init ros
        rospy.init_node('car_driver', anonymous=True)
        rospy.Subscriber('/cmd_vel', Twist, self.get_cmd_vel)
        self.ser = serial.Serial('/dev/ttyUSB0', 115200)
        self.get_arduino_message()

    # get cmd_vel message, and get linear velocity and angular velocity
    def get_cmd_vel(self, data):
        x = data.linear.x
        angular = data.angular.z
        self.send_cmd_to_arduino(x, angular)

    # translate x, and angular velocity to PWM signal of each wheels, and send to arduino
    def send_cmd_to_arduino(self, x, angular):
        # calculate right and left wheels' signal
        right = int((x + angular) * 50)
        left = int((x - angular) * 50)
        # format for arduino
        message = "{},{},{},{},{},{}*".format(right, left, right, left, right, left)
        print message
        # send by serial 
        self.ser.write(message)

    # receive serial text from arduino and publish it to '/arduino' message
    def get_arduino_message(self):
        pub = rospy.Publisher('arduino', String, queue_size=10)
        r = rospy.Rate(10)
        while not rospy.is_shutdown():
            message = self.ser.readline()
            pub.publish(message)
            r.sleep()

if __name__ == '__main__':
    try:
        d = driver()
    except rospy.ROSInterruptException: 
        pass

This script receives messages from cmd_vel and translates them to the rotation signal of each motor. Then, it sends to Arduino. At the same time, it receives the messages from Arduino and publishes to /arduino.

Run the program

Connect Raspberry Pi and Arduino like Communication between Arduino and Raspberry Pi

And don't forget to prepare Communication between Raspberry Pi and PC

1. roscore

First, run the roscore at Raspberry Pi

pi@raspberry$ roscore

2. Run motor_driver node

Second, login with another terminal with ssh and run the motor_driver node.

pi@raspberry$ rosrun motor_driver motor_driver.py

3. Check from your PC

If Raspberry Pi is successfully connected with PC, rostopic can be seen from your PC

$ rostopic list

It should be like this,

/arduino
/cmd_vel
/rosout
/rosout_agg

Let's check the arduino message. Do command $ rostopic echo /arduino It should be like this

data: 0,0,0,0,0,0

---
data: 0,0,0,0,0,0

---
data: 0,0,0,0,0,0

---
data: 0,0,0,0,0,0

---
data: 0,0,0,0,0,0

---
data: 0,0,0,0,0,0

---
data: 0,0,0,0,0,0

---
data: 0,0,0,0,0,0

4. Control from your PC

Then, let's control the six-wheel car from your PC. At your PC, run

$rosrun teleop_twist_keyboard teleop_twist_keyboard.py

If it is not installed in your PC, install it with

$sudo apt-get install ros-indigo-teleop-twist-keyboard

Control

Look the instruction on screen

Reading from the keyboard  and Publishing to Twist!
---------------------------
Moving around:
   u    i    o
   j    k    l
   m    ,    .

q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
anything else : stop

CTRL-C to quit