SC-635 Advanced Topics in Mobile Robotics

Experiment Module : Control of Differential Drive in ROS

January 24, 2020

Systems and Control Engineering

Indian Institute of Technology Bombay

1 / 22
Overview

1. Gazebo : getting started

2. Turtlebot Operation

2 / 22
Recapitulation

I Topics
I Nodes
I Messages
I Launch File
I Passing arguments to nodes from launch file

3 / 22
Gazebo simulation environment

Features:
I Separate open source project that aims at
making robot simulation easy
I 3D simulation
I Rigid body dynamics
I Open Dynamics, Bullet, Dart Engine
I Well integrated with ROS
find more information at

http://gazebosim.org/

4 / 22
Snapshot

5 / 22
Running Gazebo

I Need the package gazebo ros

I Installed as a part of ros-kinetic-desktop-full
I Execute following command to bring up the simulator gui
$ rosrun gazebo ros gazebo - -verbose
I ...

6 / 22
Running Gazebo

I Need the package gazebo ros

I Installed as a part of ros-kinetic-desktop-full
I Execute following command to bring up the simulator gui
$ rosrun gazebo ros gazebo - -verbose
I ...
I Mostly, the above command won’t work the first few times

6 / 22
Snapshot

7 / 22
Running Gazebo

I Need the package gazebo ros

I Installed as a part of the full desktop installation
I Execute following command to bring gazebo into action
$ rosrun gazebo ros gazebo –verbose
I Mostly, the above command won’t work in the first go
I Stop the process and Run again! untill ..

8 / 22
Snapshot

9 / 22
Running the turtlebot

I Example gazebo environment with turtlebot

$ roslaunch turtlebot gazebo turtlebot world.launch

10 / 22
Snapshot

11 / 22
Zoomed In

12 / 22
Turtlebot topics
Launching the turtlebot world simulation environment brings up
varius nodes. Use command $ rostopic list to see.

Some of the nodes are

I ...
I /camera/rgb/camera info
I /camera/rgb/image raw
I ...
I /cmd vel mux/input/navi
I /cmd vel mux/input/teleop
I ...
I /odom
I /scan

13 / 22
Controlling turtlebot
Turtlebot can be controlled by publishing a geometry msgs/Twist
message to either of these topics:
I /cmd vel mux/input/navi
I /cmd vel mux/input/teleop

From commandline : $ rostopic pub

I rostopic pub -r 10 /cmd vel mux/input/navi
geometry msgs/Twist “linear:
x: 0.3
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.2”

14 / 22
Controlling turtlebot from script
Need to publish geometry msgs/Twist messages.
1 #! / u s r / b i n / env p y t h o n
2 import r o s p y
3 from g e o m e t r y m s g s . msg import T w i s t
4
5 def c o n t r o l l o o p ( ) :
6 pub = r o s p y . P u b l i s h e r ( ’ / c m d v e l m u x / i n p u t / n a v i ’ ,
Twist , q u e u e s i z e =10)
7 rospy . init node ( ’ t u r t l e b o t c o n t r o l l e r ’ )
8 r a t e = r o s p y . Rate ( 1 )
9 v e l o c i t y m s g = Twist ( )
10 velocity msg . l i n e a r . x = 0.2
11 velocity msg . angular . z = 0.2
12 w h i l e not r o s p y . i s s h u t d o w n ( ) :
13 pub . p u b l i s h ( v e l o c i t y m s g )
14 rate . sleep ()
15
16 i f name == ’ m a i n ’ :
17 try :
18 control loop ()
19 except r o s p y . R O S I n t e r r u p t E x c e p t i o n :
20 pass 15 / 22
Getting position of turtlebot
The /odom topic provides nav msgs/Odometry message
A single instance of the Odometry message looks like :
1 header :
2 s e q : 326038
3 stamp :
4 s e c s : 3260
5 n s e c s : 480000000
6 f r a m e i d : ”odom”
7 child frame id : ”base footprint”
8 pose :
9 pose :
10 position :
11 x : 0.206763016474
12 y : −0.00754505523484
13 z : 0.0
14 orientation :
15 x : 0.0
16 y : −0.0
17 z : 0.0549024539071
18 w : −0.998491722827
19 covariance : [ . . . ]
20 twist :
21 twist :
22 linear :
23 x : −6.6322219357 e−06
24 y : 0.0
25 z : 0.0
26 angular :
27 x : 0.0
28 y : 0.0
29 z : −5.59266047396 e−05
30 covariance : [ . . . ]
16 / 22
Quaternion to Euler angle

The orientation is encoded in quaternion format in the Odometry

message.

Need to convert the quaternion into euler angles for computing

control law.

The following snippet does the conversion:

1 from t f . t r a n s f o r m a t i o n s import e u l e r f r o m q u a t e r n i o n
2
3 d e f q u a t 2 e u l e r ( x , y , z , w) :
4 quat = [ x , y , z ,w]
5 return e u l e r f r o m q u a t e r n i o n ( quat )

17 / 22
A node to log the position data
1 #! / u s r / b i n / env p y t h o n
2 import r o s p y
3 from n a v m s g s . msg import Odometry
4 from q u a t 2 e u l e r import q u a t 2 e u l e r
5
6 def c a l l b a c k ( data ) :
7 x = data . pose . pose . o r i e n t a t i o n . x ;
8 y = data . pose . pose . o r i e n t a t i o n . y ;
9 z = data . pose . pose . o r i e n t a t i o n . z ;
10 w = data . pose . pose . o r i e n t a t i o n .w;
11 pose = [ data . pose . pose . p o s i t i o n . x , data . pose . pose .
p o s i t i o n . y , q u a t 2 e u l e r ( x , y , z , w) [ 2 ] ]
12 p r i n t ( ” Robot p o s e i s : { : 0 . 3 f } { : 0 . 3 f } { : 0 . 2 f } ” .
format ( p o s e [ 0 ] , p o s e [ 1 ] , p o s e [ 2 ] ) )
13
14 d e f l o g g e r ( ) :
15 r o s p y . i n i t n o d e ( ’ Log odom ’ )
16 r o s p y . S u b s c r i b e r ( ’ /odom ’ , Odometry , c a l l b a c k )
17 rospy . spin ()
18
19 i f name == ’ m a i n ’ :
20 logger () 18 / 22
Project download

The project to log odometry data and control the turtlebot is

available as a zip file at:

http://bit.ly/2tdpControllerBasic

19 / 22
Assignment
A) Write a python script to generate waypoints (x,y) and plot them:

x = A cos(at)
y = A sin(bt)

with A = 3, B = 3, a=1, b=2

B) Write a node that uses the waypoint generation code and use
waypoints as target/goal point and the robots current location (from
/odom) as start point. Calculate following error terms

Epos : Euclidean distance

Eθ : Difference between current heading and desired heading

C) Extend the node to apply proportional control taught in class to drive

the robot along those waypoints. Plot the robot trajectory (superimposed
onto waypoints)
Guidelines :
Comment your code. Keep separation between part A, B and C (use different functions with meaningful names
OR use blank lines to create a visual separation)

20 / 22
Template code

A basic template for testing your code is available at

http://bit.ly/2tdpTemplateA2

21 / 22
Thank you

22 / 22