Back to [[ros:main-ros|ROS main page]]


Here we will see how to create the ROS driver for an existing non ROS robot.

====== Hardware ======
The DART V2 robot is an educational robot, built at ENSTA Bretagne to teach mobile robotics. The DARTV2 robot is a 4X4 wheeled robot with differential drive.
{{ :ros:dartv2_img_7309.jpg?400 |}}
The actuators are :
  - 2 left motors : front and rear left wheels with the same command
  - 2 right motors : same as above for right wheels
  - the 2 digits seven segment display : if not changed show the ID (number) of the robot
The motors are driven by a T-REX power board, connected in I2C.
The sensors are :
  - Pololu IMU 9V5 inertial motion unit, i2C bus
  - Home made encoders (odometers) on rear wheels, i2C bus
  - T-REX encoders (odometers) on front wheels, i2C bus
  - 4 cardinal (front, left, right and rear) ultrasonic sensors, I2C bus  
  - 2 diagonal (front-left and front-right) ultrasonic sensors , I2C bus

====== Software : ROS drivers ======

This first version the DARTV2 ROS drivers is written in Python 2.7. We will see after how to to the same drivers in C++ and in Python 3 for ROS2.

===== ROS drivers Python 2.7 for kinetic and melodic =====

ROS kinetic is running on the robot and ROS melodic on the host computer. 

==== Connect the Robot to the ROS master on the host computer ====

The workspace is created on the robot , to get the link between the robot and the host computer we define
the host computer as the master. The master executes the **roscore** command. The IP address of the master  is for example 172.20.22.34 and the master runs ROS melodic (Ubuntu 18.04) :

<code bash>
export ROS_IP=172.20.22.34
export ROS_MASTER_URI=http://172.20.22.34:11311
source /opt/ros/melodic/setup.bash
roscore
</code>

The robot runs ROS kinetic and the link to the master is defined as follows :
<code bash>
export ROS_IP=172.20.25.102
export ROS_HOSTNAME=DART02
export ROS_MASTER_URI=http://172.20.22.34:11311
source /opt/ros/kinetic/setup.bash
</code>

==== Create ROS workspace and package ====

On the robot, create a folder to implement the ROS nodes with the drivers code. Then create a ROS package called **dartv2_drivers**for the drivers : 
<code bash>
cd $HOME
mkdir -p ros/dart_drivers_py27_ws/src
cd ros/dart_drivers_py27_ws/src
catkin_create_pkg dartv2_drivers std_msgs message_generation  rospy
cd ..
catkin_make
</code>

Add folders for Python scripts, launch files and custom messages :
<code bash>
cd $HOME/ros/dart_drivers_py27_ws/src/dartv2_drivers
mkdir bin
mkdir launch
mkdir msg
</code>

The python code of the node will be in the **src** folder. 
The **bin** folder contains the python executable code of the nodes. The file names of the nodes codes are generally not ended by .py, and must be executable.

To check that all is working fine, we create a simple python node. We are using recommendations in [[http://wiki.ros.org/rospy_tutorials/Tutorials/Makefile|this ROS makefile guide]] to create the folder tree structure. The node is called **hello** and is located in **bin** folder. It uses a function defined in **hello.py** located in **src** folder.

<code bash>
cd $HOME/ros/dart_drivers_py27_ws/src/dartv2_drivers
cd bin
cat << EOF > hello
#! /usr/bin/env python
import dartv2_drivers.hello as drt
if __name__ == '__main__':
    drt.say('my friend!')
EOF
chmod +x hello
cd ../src
mkdir dartv2_drivers
cd dartv2_drivers
touch __init__.py
cat << EOF > hello.py
def say(name):
    print('Hello ' + name)
EOF
</code>

A Python setup file (setup.py) must be added in the package folder (dart_drivers_py27_ws)  :
<code bash>
cd $HOME/ros/dart_drivers_py27_ws/src/dartv2_drivers
cat << EOF > setup.py
## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
# fetch values from package.xml
setup_args = generate_distutils_setup(
     packages=['dartv2_drivers'],
     package_dir={'': 'src'},
)
setup(**setup_args)
EOF
</code>

The line **# catkin_python_setup()** in **CMakeLists.txt** must be uncommented to execute **setup.py** during **catkin_make**. This change can be made with sed. Once it's done the workspace is updated :
<code bash>
cd $HOME/ros/dart_drivers_py27_ws/src/dartv2_drivers
sed -i 's/# catkin_python_setup()/catkin_python_setup()/g' CMakeLists.txt
cd ../..
catkin_make
source devel/setup.bash
</code>

Finally, the test program should execute well and it can be removed 
<code bash>
rosrun dartv2_drivers hello
rm $HOME/ros/dart_drivers_py27_ws/src/dartv2_drivers/bin/hello
rm $HOME/ros/dart_drivers_py27_ws/src/dartv2_drivers/src/dartv2_drivers/hello.py*
</code> 
==== First driver - Odometers ====

We will use a custom message format for the odometers. A micro-controller gives the value of the 2 rear wheel odometers with a time stamp. The micro-controller is also giving the battery voltage.
The front odometers are given by the T-REX motor power board. All these data will be placed in custom messages. So we need to setup the node so that custom message can be recognized and made available to ROS.
The custom messages are defined is the **msg** folder :

<code bash>
cd $HOME/ros/dart_drivers_py27_ws/src/dartv2_drivers/msg
cat << EOF > Odometers.msg
int32 odom_front_left
int32 odom_front_right
int32 odom_rear_left
int32 odom_rear_right
float32 time_stamp_rear
EOF
cat << EOF > Battery.msg
float32 v
EOF
</code>

To use the new messages, the package has to be recompiled and then the messages can be accessed (with **rosmsg** for example) :
<code bash>
cd $HOME/ros/dart_drivers_py27_ws
catkin_make
source devel/setup.bash 
rosmsg show dartv2_drivers/Battery 
rosmsg show dartv2_drivers/Odometers 
</code>

**CMakeLists.txt** (in  dartv2_drivers  folder) must be modified to be able to import new messages in Python code. In **CMakeLists.txt**, remove comments in **add_message_files** section and add the .msg file names, also remove comments in **generate_messages** section.
In **CMakeLists.txt**, the default **add_message_files** section should look like this :
<file>
## Generate messages in the 'msg' folder
# add_message_files(
#   FILES
#   Message1.msg
#   Message2.msg
# )
</file>
To add our custom messages we change it to :
<file>
## Generate messages in the 'msg' folder
add_message_files(
  FILES
  Odometers.msg
  Battery.msg
)
</file>
After removing comments in **generate_messages** section of **CMakeLists.txt**, you should have this :
<file>
## Generate added messages and services with any dependencies listed here
generate_messages(
  DEPENDENCIES
  std_msgs
)
</file>
<code bash>
cd $HOME/ros/dart_drivers_py27_ws
catkin_make
source devel/setup.bash
</code>

Once the custom messages are defined and recognized, a dummy publisher can be written and tested
<code bash>
cd $HOME/ros/dart_drivers_py27_ws/src/dartv2_drivers/bin
cat << EOF > Odometers
#!/usr/bin/env python
# license removed for brevity
import rospy
from dartv2_drivers.msg import Odometers, Battery
import dartv2_drivers.drivers.encoders as odo_rear_drv
import time
def talker():
    global odo_rear
    pub = rospy.Publisher('dartv2_odometers_pub', Odometers)
    rospy.init_node('dartv2_odometers', anonymous=True)
    r = rospy.Rate(1) #1hz
    msg = Odometers()
    t0 = time.time()
    while not rospy.is_shutdown():
        orl,orr,odt = odo_rear.read_encoders_both(dt=True)
        msg.odom_rear_left = orl
        msg.odom_rear_right = orr
        rospy.loginfo(msg)
        pub.publish(msg)
        r.sleep()
if __name__ == '__main__':
    try:
        odo_rear = odo_rear_drv.EncodersIO()
        talker()
    except rospy.ROSInterruptException: pass
EOF
chmod +x Odometers
rosrun dartv2_drivers Odometers
</code>

==== Motors ====

The motors are actuators. Instead of a "talking" node we need to define a "listening" node. In ROS motor command is generally achieved thorough the classical topic **/cmd_vel**. Here, we will use a more simple topic based on a custom  message that directly controls the PWM of the left and right sides :
<code bash>
cd $HOME/ros/dart_drivers_py27_ws/src/dartv2_drivers/msg
cat << EOF > Motors.msg
int16 pwm_l
int16 pwm_r
EOF
</code>

==== Launch ====

<code bash>
roslaunch dartv2_drivers dartv2_drivers.launch
</code>

in another terminal send PWM commands to wheel motors

<code bash>
rostopic pub -1  dartv2_motors dartv2_drivers/Motors "{pwm_l: -90, pwm_r: 90}"
rostopic pub -1  dartv2_motors dartv2_drivers/Motors "{pwm_l: 0, pwm_r: 0}"

</code>

==== Sonars ====

DART V2 uses two types of sonars :
  * 4 cardinal sonars (front,left,back and right) managed by a dedicated micro-controller via I2C bus
  * 2 front diagonal sonars (front-left and front-right) directly connected on I2C

The 4 cardinal sonars are used in mode 2 (sync) that gives a new measurement every 200 ms, associated with the time stamp of the measurement. All 4 sonars are fired simultaneously. The 2 diagonal sonars are acquired simultaneously every 200 ms.  
The python code **Sonars** publishes the results on a custom message defined in **Sonars.msg** :

<code bash>
cd $HOME/ros/dart_drivers_py27_ws/src/dartv2_drivers/msg
cat << EOF > Sonars.msg
float32 front
float32 left
float32 back
float32 right
float32 front_left
float32 front_right
EOF
</code>

The **add_message_files** section of **CMakeList.txt** must be updated to add **Sonars.msg**.

==== IMU ====


IMU is a mini IMU 9 from Pololu. The raw data message is mode of the 3 coordinates of the magnetic filed, the acceleration along the 3 axis and rotation speed around the 3 axis. A custom message, **ImuRaw.msg** is created :
<code bash>
cd $HOME/ros/dart_drivers_py27_ws/src/dartv2_drivers/msg
cat << EOF > ImuRaw.msg
float32 magx
float32 magy
float32 magz
float32 accelx
float32 accely
float32 accelz
float32 gyrox
float32 gyroy
float32 gyroz
EOF
</code>

The **add_message_files** section of **CMakeList.txt** must be updated to add **ImuRaw.msg**.