Table of Contents
ROS is free and open source set of libraries that helps you build robotic applications. Robots are complex systems that involve hardware and software components; in such an environment, a good communication infrastructure is essential. So far, we have worked with simple models that can be simulated in the same computer we run the algorithms to control them. However, in real-world robotic systems, with multiple sensors and actuators, the software and the hardware must communicate in a distributed fashion to accomplish the tasks they were designed to perform.
ROS(Robot Operating System): The Infrastructure of Modern Robotics I
Also Read: Simple agents, complex behaviours III: Oscillators [Robotics Tutorial]
A naive approach would be to use low-level communication exploiting the TCP stack. With our simple differential drive robot, we would need to send messages to the wheels and receive information from the sensors to close the sensory-motor loop. In this respect, a client-server architecture seems appropriate for the task: Appropriate but not simple; brace yourself for a TCP detour.
A Simple Communication System for the Braitenberg Vehicle
Wanting to implement the communication schema from scratch seems like a bad idea, and it most certainly is when, as we will see, there are better alternatives. But let's give it a try. We would need some basic knowledge about sockets, threads and TCP connections that we do not have time to review; however, the example is pretty much self-contained and intuitive.
First, we reuse our Vehicle class from previous articles with the method "wheelVelocities" added so that we can change the velocities asynchronously; this vehicle will be our robot.
In our communication schema, the robot will host the server and accept connections from clients that intend to send control commands. The server is encapsulated in the RoboServer class. In the constructor, we initialize a socket that will listen on a given port in the local computer.
Note that we input an observer object as a parameter; here, we use the Observer programming pattern to notify or perform callbacks to the robot when a client command is received. This is an essential concept that we will repeatedly see in future. A method called "dispatch" takes care of incoming connections in two steps. First, it listens and waits for any client. Second, once a client is connected, the connection is handled by the "serve" function:-
The connection handling receives and decodes the command notifying the observer appropriately. The process requires the definition of a message format that should be agreed upon between the client and the server; in this case, the only command available has the format "LEFT_VELOCITY;RIGHT_VELOCITY". Once the message is decoded and sent to the observer, the connection is closed, and the server resumes listening for more clients.
Note that this procedure blocks the current thread until a new client is connected. This forbids any other action to be taken and so is undesirable; hence we need to create a new thread to run the server in parallel with the rest of the logic:-
The final component of our communication system is a Controller whose responsibility is to relay the commands from the clients to the robot while managing the rest of the simulator. It contains instances of the robot and the server and implements the method notify from the observer interface so that it can accept callbacks. Whenever that method is called, the wheel speed is updated.
The method "run" starts the simulation by updating and drawing the robot in a loop. While there are no client commands, the robot will move at the given velocities. Once a command is received, the process continues with the new velocities.
Here is the complete server.py code:-
import socket
import threading
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Circle
import time
# Generic vehicle
class Vehicle(object):
def __init__(self, x0, y0, theta0):
self.phi_R = 0.0
self.phi_L = 0.0
self.pstep = 0.01
self.ostep = 1.0
self.x, self.y, self.theta = x0, y0, theta0
self.scale = 0.05
self.r = 1.0*self.scale # Wheel radius
self.D = 2.0*self.scale # Diameter of the robot
# Position of the sensors in the body frame of reference
self.p_sensors = np.array([[self.r*np.cos(np.pi/4.0), self.r*np.sin(np.pi/4.0), 1],
[self.r*np.cos(-np.pi/4.0), self.r*np.sin(-np.pi/4.0), 1]]).T
def getBodyTransform( self ):
# Affine transform from the body to the work FoR
return np.array([[np.cos(self.theta), -np.sin(self.theta), self.x],
[np.sin(self.theta), np.cos(self.theta), self.y],
[0.0, 0.0, 1.0]])
def wrap( self, x, y ):
if x < 0:
x = 2.0
elif x > 2.0:
x = 0.0
if y < 0:
y = 2.0
elif y > 2.0:
y = 0
return x,y
def wheelVelocities( self, l, r ):
self.phi_R = r
self.phi_L = l
def updatePosition(self):
# Then compute forward kinematics
vl = (self.r/self.D)*(self.phi_R + self.phi_L)
omega = (self.r/self.D)*(self.phi_R - self.phi_L)
# Update the next statefrom the previous one
self.theta += self.ostep*omega
self.x += self.pstep*vl*np.cos(self.theta)
self.y += self.pstep*vl*np.sin(self.theta)
self.x, self.y = self.wrap(self.x, self.y)
return self.x, self.y, self.theta
def draw(self, ax):
T = self.getBodyTransform()
n_sensors = np.dot(T, self.p_sensors)
left_wheel = np.dot(T, np.array([0, self.D/2.0, 1]).T)
right_wheel = np.dot(T, np.array([0, -self.D/2.0, 1]).T)
# drawing body
body = Circle((self.x, self.y), self.D/2.0, fill = False, color = [0, 0, 0] )
# Drawing sensors
s1 = Circle((n_sensors[0, 0], n_sensors[1, 0]), self.scale*0.1, color = 'red' )
s2 = Circle((n_sensors[0, 1], n_sensors[1, 1]), self.scale*0.1, color = 'red' )
w1 = Circle((left_wheel[0], left_wheel[1]), self.scale*0.2, color = 'black' )
w2 = Circle((right_wheel[0], right_wheel[1]), self.scale*0.2, color = 'black' )
ax.add_patch(body)
ax.add_patch(s1)
ax.add_patch(s2)
ax.add_patch(w1)
ax.add_patch(w2)
class RoboServer:
def __init__( self, observer ):
self.observer = observer
self.host = '127.0.0.1'
self.port = 64788
self.close = False
self.roboSocket = socket.socket( socket.AF_INET, socket.SOCK_STREAM )
def serve( self , cl):
while True:
data = cl.recv(1024)
if data:
r = data.decode().split(';')
l,r = float(r[0]), float(r[1])
cl.send('Command accepted'.encode())
self.observer.notify(l, r)
else:
break
def dispatch( self ):
self.roboSocket.bind((self.host, self.port))
self.roboSocket.listen()
print('Robot listening in port ', str(self.port))
while not self.close:
cl, addr = self.roboSocket.accept()
print('Client connected')
self.serve(cl)
self.roboSocket.close()
def closeConnection(self):
self.close = True
def run( self ):
x = threading.Thread(target=self.dispatch)
x.start()
class RoboController:
def __init__(self):
self.vehicle = Vehicle(0.5, 0.5, 0)
self.server = RoboServer( self )
self.server.run()
def notify( self, l, r ):
print('Notifying the controller')
self.vehicle.wheelVelocities( l, r )
def run( self ):
T = 1000
Path = np.zeros((2, T))
fig, ax = plt.subplots()
plt.show(block = False)
for i in range(T):
ax.cla()
x, y, theta = self.vehicle.updatePosition()
for j in range(i):
ax.plot(Path[0,j], Path[1,j], 'k.', markersize = 5)
ax.plot(Path[0,0], Path[1,0], 'r*', markersize = 6)
self.vehicle.draw( ax )
Path[:,i] = np.array([x, y]).T
plt.axis([0, 2, 0, 2])
plt.xlabel('x')
plt.ylabel('y')
fig.canvas.draw()
fig.canvas.flush_events()
time.sleep(0.1)
self.server.close()
controller = RoboController()
controller.run()
A class diagram of the server-side system is shown below:-
The last component of our system is the client. We keep the implementation simple on purpose to avoid unnecessary noise, but the reader is encouraged to experiment with the code to get more behaviours:-
The results are not particularly interesting; the robot only moves when told to do so. However, that is the tip of the iceberg; we have managed to create a functional communication system that displays several interesting features:-
- It is asynchronous: the commands are received at any part of the simulation step, thanks to the callbacks.
- It underlines the need for messages and message formats for effective communication.
- It highlights the necessity of managing connections with clients in an efficient way.
The Need for an Appropriate Middleware
There are evident drawbacks to the low-level approach to communication just provided that render it unfeasible for real-world implementations:-
- The number of sensors and actuators in different robots exponentially increases the client-server design's complexity.
- Designing custom messages for each application hampers interoperability with other systems or platforms.
- Point-to-point architectures are inefficient in real-time scenarios in which different modules run in separate processes or even computers.
Consequently, the last decade saw the proliferation of robot-oriented middleware that added an abstraction layer to the traditional communication stack used in other applications. These systems were expanded with domain-specific knowledge to guarantee seamless hardware and software interaction.
An effective and fully distributed approach uses the concepts of publishers and subscribers to pass messages between the different components of the system. The principle is straightforward. Instead of direct communication between elements, each publishes messages on various topics, which are sort of message boards. Other components subscribe to those topics and receive notifications whenever a new message arrives.
In this case, there is no server or clients, only nodes that publish and subscribe in a device-agnostic way. Above figure shows our little example framed in terms of this new paradigm. The champion of such a scheme is the Robot Operating System, ROS.
Introducing ROS
ROS is not an operating system but a middleware that provides communication capabilities and tools in a dedicated ecosystem for robotic applications. Before learning the ins and outs, we can readily use it to simplify our example by mastering the main concepts behind the communication infrastructure:-
ROS is fully distributed, meaning each component is a node in a communication graph with no privileged actors. In ROS1, the oldest but still most widely used version, a "Master" process needs to be run in one of the nodes. Its role is to manage the topics, which are the message boards publishers and subscribers use to share information. A publisher posts messages on a given topic, while subscribers get notified when this happens.
Controlling the Braitenberg Vehicle with ROS
The previous concepts are enough to dramatically change our previous example making it more efficient and elegant. The first step is installing the latest ROS1 (Noetic) version and running the "roscore":-
roscore
Then we modify our controller to make it a subscriber of the topic "wheelvel". Two remarks are in order. First, every program that intends to be a node in the ROS graph needs to be initialized with a name:-
Second, a subscriber initialization must include a callback function and a message type. In this case, we keep the same message format as before; however, in the second part of this article, we will see standardized messages used for different control aspects. The callback function is equivalent to the one we defined previously, and the rest of the code is essentially the same, with the difference that we got rid of the server logic altogether.
Here is the complete subscriber.py code:-
import rospy
from std_msgs.msg import String
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Circle
import time
# Generic vehicle
class Vehicle(object):
def __init__(self, x0, y0, theta0):
self.phi_R = 0.0
self.phi_L = 0.0
self.pstep = 0.1
self.ostep = 0.5
self.x, self.y, self.theta = x0, y0, theta0
self.scale = 0.05
self.r = 1.0*self.scale # Wheel radius
self.D = 2.0*self.scale # Diameter of the robot
# Position of the sensors in the body frame of reference
self.p_sensors = np.array([[self.r*np.cos(np.pi/4.0), self.r*np.sin(np.pi/4.0), 1],
[self.r*np.cos(-np.pi/4.0), self.r*np.sin(-np.pi/4.0), 1]]).T
def getBodyTransform( self ):
# Affine transform from the body to the work FoR
return np.array([[np.cos(self.theta), -np.sin(self.theta), self.x],
[np.sin(self.theta), np.cos(self.theta), self.y],
[0.0, 0.0, 1.0]])
def wrap( self, x, y ):
if x < 0:
x = 2.0
elif x > 2.0:
x = 0.0
if y < 0:
y = 2.0
elif y > 2.0:
y = 0
return x,y
def wheelVelocities( self, l, r ):
self.phi_R = r
self.phi_L = l
def updatePosition(self):
# Then compute forward kinematics
vl = (self.r/self.D)*(self.phi_R + self.phi_L)
omega = (self.r/self.D)*(self.phi_R - self.phi_L)
# Update the next statefrom the previous one
self.theta += self.ostep*omega
self.x += self.pstep*vl*np.cos(self.theta)
self.y += self.pstep*vl*np.sin(self.theta)
self.x, self.y = self.wrap(self.x, self.y)
return self.x, self.y, self.theta
def draw(self, ax):
T = self.getBodyTransform()
n_sensors = np.dot(T, self.p_sensors)
left_wheel = np.dot(T, np.array([0, self.D/2.0, 1]).T)
right_wheel = np.dot(T, np.array([0, -self.D/2.0, 1]).T)
# drawing body
body = Circle((self.x, self.y), self.D/2.0, fill = False, color = [0, 0, 0] )
# Drawing sensors
s1 = Circle((n_sensors[0, 0], n_sensors[1, 0]), self.scale*0.1, color = 'red' )
s2 = Circle((n_sensors[0, 1], n_sensors[1, 1]), self.scale*0.1, color = 'red' )
w1 = Circle((left_wheel[0], left_wheel[1]), self.scale*0.2, color = 'black' )
w2 = Circle((right_wheel[0], right_wheel[1]), self.scale*0.2, color = 'black' )
ax.add_patch(body)
ax.add_patch(s1)
ax.add_patch(s2)
ax.add_patch(w1)
ax.add_patch(w2)
class RoboSubscriber:
def __init__(self):
rospy.init_node('braitenberg', anonymous=True)
self.vehicle = Vehicle(0.5, 0.5, 0)
rospy.Subscriber("wheelvel", String, self.notify)
def notify( self, data ):
r = data.data.split(';')
l,r = float(r[0]), float(r[1])
print('New message arrived')
self.vehicle.wheelVelocities( l, r )
def run( self ):
T = 1000
Path = np.zeros((2, T))
fig, ax = plt.subplots()
plt.show(block = False)
for i in range(T):
ax.cla()
x, y, theta = self.vehicle.updatePosition()
for j in range(i):
ax.plot(Path[0,j], Path[1,j], 'k.', markersize = 5)
ax.plot(Path[0,0], Path[1,0], 'r*', markersize = 6)
self.vehicle.draw( ax )
Path[:,i] = np.array([x, y]).T
plt.axis([0, 2, 0, 2])
plt.xlabel('x')
plt.ylabel('y')
fig.canvas.draw()
fig.canvas.flush_events()
if rospy.is_shutdown():
break
time.sleep(0.1)
controller = RoboSubscriber()
controller.run()
The "client" instead is turned into a publisher. Here we initialize a node and a publisher object onto the same topic and format as before. Notice that the roles are flexible. Our publisher can also subscribe to other topics (i.e. to receive sensor information):-
Here is the complete publisher.py code:-
import rospy
import numpy as np
from std_msgs.msg import String
class RoboPublisher:
def __init__(self):
rospy.init_node('controller', anonymous=True)
self.pub = rospy.Publisher('wheelvel', String, queue_size=10)
def run( self ):
rate = rospy.Rate(1)
l = 0.0
r = 0.0
while not rospy.is_shutdown():
if np.random.rand() > 0.6:
l = np.random.rand()
r = np.random.rand()
msg = str(l) + ";" + str(r)
self.pub.publish(msg)
rate.sleep()
p = RoboPublisher()
p.run()
For illustration purposes, we keep the main loop simple: we publish a random velocity change every now and then. Finally, we run the publisher and subscriber separately and enjoy the little Brownian dance:-
Conclusion
We have introduced one of the main tools of modern robotics in a way that hopefully highlights its usefulness in everyday applications. In the follow-up, we will work with a full-on simulator of an actual robot in order to learn more about the capabilities of this widely used middleware.
References
- Elkady, A., & Sobh, T. (2012). Robotics middleware: A comprehensive literature survey and attribute-based bibliography. Journal of Robotics, 2012.
- https://www.ros.org/