ROS Archives - Page 9 of 9

Using OpenAI with ROS

OpenAI has released the Gym, a toolkit for developing and comparing reinforcement learning (RL) algorithms. That toolkit is a huge opportunity for speeding up the progress in the creation of better reinforcement algorithms, since it provides an easy way of comparing them, on the same conditions, independently of where the algorithm is executed.

The toolkit is mainly aimed at the creation of RL algorithms for a general abstract agent. Here, we are interested in applying it to the control of robots (of course!). Specifically, we are interested in ROS based robots. That is why, in this post we describe how to apply the OpenAI Gym to the control of a drone that runs with ROS

Let’s see an example of training.

The drone training example

In this example, we are going to train a ROS based drone to be able to go to a location of the space moving as low as possible (may be to avoid being detected), but avoiding obstacles in its way.

For developing the algorithm we are going to use the ROS Development Studio (RDS). That is an environment that allows to program with ROS and its simulations with a web browser, without having to install anything on the computer. So we have all the required packages for ROS and OpenAI Gym and Gazebo simulations already installed. You can follow the rest of the post you have two options:

Either go to the ROS Development Studio and create a free account.
Either you install everything in your computer. If you need to install those packages, have a look here for ROS installation, here for OpenAI installation, and here to download the Gazebo simulation (for Indigo + Gazebo 7).

What follows are the instructions for developing the training program with RDS, but as you can see, the steps are the same for your local installation.

Get the prepared code for drone training

We have prepared the training code already for you, so you don’t have to build everything from scratch. The goal of this post is to show you how this
code works, and how you would modify it for your own case (different robot, or different task).

In order to get the code, just open the RDS (http://rds.theconstructsim.com) and create a new project. You can call it openai_with_ros_example. Then open the project by clicking on the Open Project button.

Once you have the environment open, go to the Tools menu and open a Linux Shell. Inside the shell go to the catkin_ws/src directory. This is the place where ROS code must be put in the RDS in order to build, test, debug and execute it against robot simulations. Once there, clone the following git repo which contains the code to train the drone with OpenAI:

git clone https://bitbucket.org/theconstructcore/drone_training.git

Now, you should have a ROS package named drone_training inside the catkin_ws. Let’s test it right now!

Testing what you have installed

First thing would be to test what you got so we can see what we are trying to understand. For this, follow the next steps:

Launch the Parrot drone simulation. On RDS, you can find it as Parrot AR.Drone at the menu Simulations. After launching, you show see a window like this.

Parrot Drone with ROS simulation

Let’s launch our package so it will start training the Parrot drone. For that, type the following in the previous shell:

> roslaunch drone_training main.launch

You should see the drone start moving doing some strange things. It actually looks like the drone is drunk! That makes perfect sense.

What is happening is that the robot is learning. It is exploring its space of actions and practicing what it will sense based on the actions that it takes. That is exactly how the reinforcement learning problem works. Basically, the robot is performing the classical RL loop o the figure:

How a reinforcement learning problem works (image from StackOverflow)

The agent (the drone plus the learning algorithm), decides to take an action from the pool of available actions (for example, move forward), and executes it in the environment (the drone moves forward). The result of that action, makes the agent closer or not to its target (to fly to a given location). If the robot is closer, it gets a good reward. If it is further away, it gets a bad reward.
In any case, the agent perceives the current state of itself and the environment (where it is located now), and then feeds reward, previous state, new state and action taken to the learning algorithm (to learn the results based on its actions). Then the process repeats again for the number of steps the robot is allowed to experiment. When the number of steps is done, the final reward is obtained and the robot starts again from the initial position, now with an improved algorithm. The whole process is repeated again and again for a given number of episodes (usually high).

Now, let’s see how all that works together in the code. Let’s see its structure:

**The drone_training package for OpenAI training with ROS**

This package is just an example of how you can interface OpenAI with ROS robots. There are other ways of doing it, and in future posts we will explore them.

The package contains the following directories:

launch directory. It contains the main.launch file that we used to launch the whole training thing.
config directory. It contains a configuration file qlearn_params.yaml with the desired parameters for the training. Usually, you need to tune the parameters by trying several times, so it is a good practice to keep them structured on a file or files for easier modification/review.
training_results directory. It will contain the results of our training for later analysis.
utils directory. Contains a Python file plot_results.py that we will use to plot the training results.
src directory. The crux of the matter. It contains the code that makes possible the training of the drone. Let’s have a deeper look at this one.

The src directory

The launch file will launch the start_training.py file. That is the file that orchestrates the training. Let’s see what it does step by step:

Initializes the ROS node

rospy.init_node('drone_gym', anonymous=True)

Of course, first thing is to declare that code as a node of ROS.

Creates the Gym environment

env = gym.make('QuadcopterLiveShow-v0')

That is the main class that OpenAI provides. Every experiment of OpenAI must be defined within an environment. By organizing like that, different developers can test different algorithms always comparing against the same environment. Hence we can compare if an algorithm is better than another always on the same conditions.

The environment defines the actions available to the agent, how to compute the reward based on its actions and results, and how to obtain the state of the world of the agent.

Every environment in OpenAI must define the following things:

A function _reset that sets the training environment to its initial state.
A function _step that makes the changes in the environment based on the last action taken, and then observes what the new state of the environment is. Based on those two things, it generates a reward.
A function _seed used for initializing the random number generator.
A function render function used to show on screen what is happening in the environment.
What is the task that the agent has to solve in this environment.
The number of possible actions in the environment. In our case, we are allowing the robot to take 5 actions.
A way to compute the reward obtained by the agent. Those are the points provided to the agent on each step, based on how good or how bad has it done in the environment to solve the task at hands.
A way to determine if the task at hands has been solved.

We are going to see how to do the code of the environment below.

Loading the parameters of the algorithm from the ROS param server

  Alpha = rospy.get_param("/alpha")
  Epsilon = rospy.get_param("/epsilon")
  Gamma = rospy.get_param("/gamma")
  epsilon_discount = rospy.get_param("/epsilon_discount")
  nepisodes = rospy.get_param("/nepisodes")
  nsteps = rospy.get_param("/nsteps")

Those are the parameters that our learning algorithm needs. That section will change based on the parameters your algorithm needs.

Initialization of the learning algorithm

We create an instance of the learning algorithm we are going to use.

qlearn = qlearn.QLearn(actions=range(env.action_space.n), alpha=Alpha, gamma=Gamma, epsilon=Epsilon)

In this case we are using a Qlearning reinforcement learning algorithm. But you can use any other of the available (including deep learning) or encode your own.

That is the key part that we want to test. How good is this algorithm for solving the task at hands.

Implementation of the training loop

The training loop is the one that repeats the learning cycle explained above. That is where the learning code is executed. It basically consists of two main loops:

First we have a loop with the number of episodes that the robot will be tested. Each episode means the number of times that we will allow the robot to try to solve the task.
Second, we have the loop with the number of steps. For each episode, we allow the robot to take a given number of actions, number of steps, number of loops into the reinforcement cycle. If the robot consumes all the steps, we consider it has not solved the task and hence, a new episode must start.

The number of episodes loop

It starts with the code:

for x in range(nepisodes):

Remember that the number of episodes is a parameter from the config file. The loop basically does is to reset the environment (initialize the robot) so a new trial can start from the original position. It also gets the initial state observation required by the learning algorithm to generate the first action.

observation = env.reset()

The number of steps loop

It starts with the code:

for i in range(nsteps):

and basically what it does is:

Make the learning algorithm choose an action based on the current state

action = qlearn.chooseAction(state)

Execute the action in the environment

observation, reward, done, info = env.step(action)

Get the new state after the action

nextState = ''.join(map(str, observation))

Learn from that result

qlearn.learn(state, action, reward, nextState)

And that is all. That simple. The loops will repeat based on the parameters values, and once they finish, the log files will generate in the training_results directory.

About the learning algorithm

In this example, we are using the Qlearn reinforcement learning algorithm. That is a classical algorithm of reinforcement learning. You can find here a description of it.

The code for the Qlearn algorithm is provided in the qlearn.py file. It has been taken from Victor Mayoral’s git, and you can find the original code here (thanks Victor for such a good work!).

You could change this algorithm for another one that you may have developed and that is going to be the next hit in artificial intelligence. Just create the code (like the qlearn.py) with the same inputs and outputs, and then substitute the call in the start_training.py file. That is the greatness of the OpenAI framework: that you can just plug your algorithm, and do not change anything of the rest, and the whole learning system will still work. By doing this, you can compare your algorithm with the others under the exact same conditions.

Additionally, we have included in the repo another classic reinforcement learning algorithm called Sarsa (sarsa.py).

Here your first homework!

Change the learning algorithm inside the start_training.py file by the Sarsa algorithm, and watch if there is any difference in learning speed or improved behavior.

The Gym environment

As I said, the environment defines the actions available to the agent, how to compute the reward based on its actions and results, and how to obtain the state of the world of the agent, after that actions have been performed.

OpenAI provides an standarized way of creating an environment. Basically, you must create an environment class which must inherit from gym.Env. That inheritance, entitles you to implement within that class the functions _seed, _reset and _step (explained above).

In our case, we have created a class named QuadCopterEnv. You can find the code in the myquadcopter_env.py file.

The code starts by registering the class into the pool of available environments of OpenAI. You register a new environment with the following code:

reg = register(
 id='QuadcopterLiveShow-v0',
 entry_point='myquadcopter_env:QuadCopterEnv',
 timestep_limit=100,
 )

Then the class starts initializing the topics it needs to connect to, gets the configuration parameters from the ROS param server, and connects to the Gazebo simulation.

 def __init__(self):
 
   self.vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
   self.takeoff_pub = rospy.Publisher('/drone/takeoff', EmptyTopicMsg, queue_size=0)
 
   self.speed_value = rospy.get_param("/speed_value")
   self.desired_pose = Pose()
   self.desired_pose.position.z = rospy.get_param("/desired_pose/z")
   self.desired_pose.position.x = rospy.get_param("/desired_pose/x")
   self.desired_pose.position.y = rospy.get_param("/desired_pose/y")
   self.running_step = rospy.get_param("/running_step")
   self.max_incl = rospy.get_param("/max_incl")
   self.max_altitude = rospy.get_param("/max_altitude")

   self.gazebo = GazeboConnection()
 
   self.action_space = spaces.Discrete(5) #Forward,Left,Right,Up,Down
   self.reward_range = (-np.inf, np.inf)

   self._seed()

Now it is time for the definition of each of the mandatory functions for an environment.

In the function _seed, we initialize the random seed, required to generate random numbers. Those are used by the learning algorithm when generating random actions
In the function _reset we initialize the whole environment to a known initial state, so all the episodes can start with the same conditions. Very simple code. Just resets the simulation, and clears the topics. The only special thing is that at the end of the function we pause the simulation, because we do not want the robot be running while we are doing other computational tasks. Otherwise, we would not be able to guarantee the initial conditions for all the episodes, since it would largely depend on the execution time of other algorithms in the training computer.
The main function is the _step function. This is the one that is called during the loops of training. This function receives as a parameter the action selected by the learning algorithm. Remember that that parameter is just the number of the action select, not the actual action. The learning algorithm doesn’t know which actions we have for this task. It just knows the number of actions available and it picks one of them based on its current learning status. So what we receive here is just the number of the action selected by the learning algorithm.
- The first thing we do is to convert that number into the actual action command for the robot. That is what this code does, it converts the number into the movement action that we will send to the ROS robot:

 vel_cmd = Twist()
 if action == 0: #FORWARD
 vel_cmd.linear.x = self.speed_value
 vel_cmd.angular.z = 0.0
 elif action == 1: #LEFT
 vel_cmd.linear.x = 0.05
 vel_cmd.angular.z = self.speed_value
 elif action == 2: #RIGHT
 vel_cmd.linear.x = 0.05
 vel_cmd.angular.z = -self.speed_value
 elif action == 3: #Up
 vel_cmd.linear.z = self.speed_value
 vel_cmd.angular.z = 0.0
 elif action == 4: #Down
 vel_cmd.linear.z = -self.speed_value
 vel_cmd.angular.z = 0.0

Next step is to send the action to the robot. For that, we need to unpause the simulator, send the command, wait for some time for the execution of the command, take an observation of the state of the environment after the execution, and pause again the simulator.

 self.gazebo.unpauseSim()
 self.vel_pub.publish(vel_cmd)
 time.sleep(self.running_step)
 data_pose, data_imu = self.take_observation()
 self.gazebo.pauseSim()

Then, we process the current state of the robot/environment to calculate the reward. For the reward, we are taking into account how close to the desired position the drone is, but also other factors like the inclination of the drone, or it height. Additionally, we promote moving forward against turning.

 reward,done = self.process_data(data_pose, data_imu)
 if action == 0:
   reward += 100
 elif action == 1 or action == 2:
   reward -= 50
 elif action == 3:
   reward -= 150
 else:
   reward -= 50

Finally, we return the current state, the reward obtained and a flag indicating if this episode must be considered done (either because the drone achieved the goal, or either because it went against the conditions of height or inclination).

 state = [data_pose.position.x]
 return state, reward, done, {}

Basically, that’s it. The code above calls additional functions that you can check by looking into the QuadCopterEnv class. Those are the functions that do the dirty job of calculating the actual values, but we don’t need to review them here, because they are out of the scope of this subject.

One function that we may need to cover, though, is the function that computes the reward. Its code is the following:

def process_data(self, data_position, data_imu):
 done = False
 euler = tf.transformations.euler_from_quaternion([data_imu.orientation.x,
                                                   data_imu.orientation.y,
                                                   data_imu.orientation.z,
                                                   data_imu.orientation.w])
 roll = euler[0]
 pitch = euler[1]
 yaw = euler[2]

 pitch_bad = not(-self.max_incl < pitch < self.max_incl)
 roll_bad = not(-self.max_incl < roll < self.max_incl)
 altitude_bad = data_position.position.z > self.max_altitude

 if altitude_bad or pitch_bad or roll_bad:
   rospy.loginfo ("(Drone flight status is wrong) >>> ("+str(altitude_bad)+","+str(pitch_bad)+","+str(roll_bad)+")")
   done = True
   reward = -200
 else:
   reward = self.improved_distance_reward(data_position)

 return reward,done

That code, basically does two things:

First, detects if the robot has surpassed the previously defined operation limits of height and inclination. If that is the case, it considers the episode done
Computes the reward based on the distance to the goal

How to configure the test

You can find a yaml file in the config directory containing the different parameters required to configure the learning task. I have divided the parameters in two types:

Parameters related to the learning algorithm being used: those are the parameters that my algorithm needs. In this case, are specifically for a Qlearn algorithm. You would define here the ones your algorithm needs, and then read them in the start_training.py file
Parameters related to the environment: those are parameters that affect the way the reward is obtained, and hence, they affect the environment. Those include the goal position or the conditions for which an episode can be considered aborted due to unstable drone conditions (too much altitude or too much inclination).

How to plot the results of the training

Plotting results is very important because you can visually identify if your system is learning or not, and how fast. If you are able to early identify that your system is not learning properly, then you can modify the parameters (or even the conditions of the experiment), so you can retry fast.

In order to plot the results, I have provided a Python script in the utils directory that does the job. I did not create the code myself, I took it from somewhere else, but I cannot remember from where (if you are the author and want to have the credit just contact me). To launch that code just type on the utils directory:

> python plot_results.py

The script will take the results generated in the training_results directory and generate a plot with all the rewards obtained for each episode. In order to watch the plot, you must open the Graphic Tools window (Tools->Graphic Tools). You must see something like this:

For this post, I have run the code provided to you (as the version of 8 Feb2018) for 500 episodes, and the results are not very good, as you can see in the following figure

In that figure, you can see that there is no progress in the reward, episode after episode. Furthermore, the variations in the reward values look completely random. This means that the algorithm is actually not learning at all about the problem trying to solve. What can be the reasons for that? Well, I can figure out a few. The goal for the engineer is to devise ways to modify the learning situation so the learning can actually be accomplished. Some possible reasons why is not learning:

The reward function is not properly created. If the reward is too complex, the system may not be able to capture small baby steps of improvement.
The state provided to the learning algorithm is continuous, and Qlearning is not well suited for that. As you can see in the code, we are returning the current robot position in the x axis as the state of the environment. That is a continuous value. I would suggest to discretize the state into different zones (let’s say 10 zones). Each zone meaning getting closer and closer to the goal point.
The parameters of the learning algorithm are not correct.
The experiment is not completely sound per se. Take into account that the goal position of the robot is fixed, and that the obstacles cannot be detected by the robot (he can only infer their position after crashing many times against it in several episodes).

What is clear is that the structure of the training environment is correct (by structure, I mean the organization of the whole learning system). That is a good point, since it allow us to start looking for ways to improve the learning from within a learning structure that already works.

How to improve the learning

This example is massively improvable (as the plot of the results are showing ;-). Here a list of suggestions where you can improve the system to make it learn quickly and better solutions:

Take the observations/state in (x,y, z) instead of only x. Also, discretize the state space.
Make the robot detect obstacles with sonar sensor and use it to avoid obstacles.
Make the robot go to random points, not only to a fixed point

That is your homework!

Apply any of those improvements, and send me your plots of the improved reward evolution, and videos showing how the drone has learnt to do the task. We will publish them in our social channels giving you credit about it.

ROS Developers Live Show about this example OpenAI with ROS

We recently did a live class showing how all the explained above works in real time with many people attending at the same time and doing the exercises with me. It may clarify you all the content above. Have a look here:

We do a ROS Developers Live Show every Wednesday at 18:00 CET. You may want to subscribe to our Youtube channel in order to stay notified of our future Live Show.

Additionally, we have created an online course where to learn all the material above and other things about OpenAI for robotics. It is online with all the simulations integrated and requires just a web browser (you can do the exercises with ROS even from Windows!). You can find it here: OpenAI Gym for Robotics 101 (additionally, in case you like it, you can use the discount coupon 2AACEE38 for a 10% discount).

Conclusion

OpenAI is a very good framework for training robots to do things using the latest techniques in artificial intelligence. Also, as you have seen, it is not difficult to integrate with ROS based robots. This makes the tandem OpenAI+ROS a killer combination for robot development!

If you still have doubts, write your questions below the post and we will try to answer them all. Happy robot training!

The Need For Robotics Standards

Last week I was talking to one lead engineer of a Singapore company which is building a benchmarking system for robot solutions. Having seen my presentation at ROSCON2016 about robot benchmarking, he asked me how would I benchmark solutions that are non-ROS compatible. I said that I wouldn’t. I would not dedicate time to benchmark solutions that are not ROS based. Instead, I suggested, I would use the time to polish the ROS-based benchmarking and suggest the vendors to adopt that middleware in their products.

Benchmarks are necessary and they need standards

Benchmarks are necessary in order to improve any field. By having a benchmark, different solutions to a single problem can be compared and hence a direction for improvement can be traced. Up to the present, robotics lacks such benchmarking system.

I strongly believe that in order to create a benchmark for robotics we need a standard at the level of programming.

By having a standard at the level of programming, manufacturers can build their own hardware solutions at will, as far as they are programmable with the programming standard. That is the approach taken by devices that can be plugged into a computer. Manufacturers create the product on their own terms, and then provide a Windows driver that allows any computer in the world (that runs on the Windows standard) to communicate with the product. Once this communication computer-to-product is made, you can create (Windows) programs that compare the same type of devices (from different manufacturers) for performance, quality, noise, whatever your benchmark is trying to compare.

You see? Different types of devices, different types of hardware. But all of them can be compared through the same benchmarking system that relies on the Windows standard.

Software development for robots also needs standards

The need for standards is not only required for comparing solutions but also to speed robotics development. By having a robotics standard, developers can concentrate on building solutions that do not have to be re-implemented whenever the robot hardware changes. Actually, given the middleware structure, developers can disassociate so much from the hardware that they can almost rely 100% of its time in the software realm while developing for robotics (something that software developers like very much, to be away from the hardware. Actually, this is one of the reasons why so few good software developers exists in robotics (most of us are a mix of hardware and software… now you understand the current state of AI for robotics ;-)).

We need the same type of standard for robotics. We need a kind of operating system that allows us to compare different robotics solutions. We need the Windows of the PCs, the Android of the phones, the CAN of the buses…

A few standard proposals and a winner

But you already know that. I’m not the first one to state this. Actually, there have been many people already that tried to create such standard. Some examples include Player, ROS, YARP, OROCOS, Urbi, MIRA or JdE Robot, to name a few.

Personally, I actually don’t care which the standard is. It could be ROS, it could be YARP or it could be any other that still has not been created. The only thing I really care is that we adopt a standard as soon as possible.

And it looks like the developers have decided. Robotics developers prefer ROS as their common middleware to program robots with.

No other middleware for robotics has had so large adoption. Some data about it:

	ROS	YARP	OROCOS
Number of Google pages:	243.000	37.000	42.000
Number of citations to the paper describing the middleware:	3.638	463	563
Alexa ranking:	14.118	1.505.000	668.293

Note 1: Only showing the current big three players

Note 2: Very simple comparison. Difficult to compare in other terms since data is not available

Note 3: Data measured in August 2017. May vary at the time you are reading this. Links provided on the numbers themselves, so you can check by yourself.

So it is not only the feeling that we, roboticists, have. Also the numbers indicate that ROS is becoming that standard for robotics programming.

Why ROS?

The question is then, why has ROS emerged on top of all the other possible contestants. None of them is worst than ROS in terms of features. Actually you can find some feature in all the other middlewares that outperform ROS. If that is so, why or how has ROS achieved the status of becoming the standard ?

A simple answer from my point of view: excellent learning tutorials and debugging tools.

Here there is a video where Leila Takayama, early developer of ROS, explains when she realized that the key for having ROS used worldwide would be to provide tools that simplify the reuse of ROS code. None of the other projects have such a set of clear and structured tutorials. Even less, those other middlewares provide debugging tools into their packages. Lacking those two essential points are preventing new people to use their middlewares (even if I understand the developers of OROCOS and YARP for not providing it… who wants to write tutorials or build debugging tools… nobody! 😉 )

Additionally, it is not only about Tutorials and Debugging-Tools. ROS creators also managed to create a good system of managing packages. The result of that is that developers worldwide could use others packages in a (relatively) easy way. This created an explosion in ROS packages available, providing off-the-shelf almost anything for your brand new ROSified robot.

Now, the rate at which contributions to the ROS ecosystem are made is so big that makes ROS almost unstoppable in terms of growing.

What about companies?

At the beginning, ROS was mostly used by students at Universities. However, as ROS becomes more mature and the number of packages increases, companies are realizing that adopting ROS is also good for them because they will be able to use the code developed by others. On top of that, it will be easier for them to hire new engineers already knowing the middleware (otherwise they would need to teach the newcomers their own middleware).

Based of that principle, many companies have jumped into the ROS train, developing from scratch their products to be ROS compatible. Examples include Robotnik, Fetch Robotics, Savioke, Pal Robotics, Yujin Robots, The Construct, Rapyuta Robotics, Erle Robotics, Shadow Robot or Clearpath, to name a few of the sponsors of the next ROSCON 😉 . Creating their ROS-compatible products, they decreased their development time in several orders of magnitude.

To bring things further, two Spanish companies have revolutioned the standarization of robotics products towards the ROS middleware: in one side, Robotnik has created the ROS Components shop. A shop where anyone can buy ROS compatible devices, starting from mobile bases to sensors or actuators. On the other side, Erle Robotics (now Acutronic Robotics) is in the process of developing Hardware ROS. The H-ROS is a standardized software and hardware infrastructure to easily create reusable and reconfigurable robot hardware parts. ROS is taking the hardware standarization too! But this time is made by companies, not research! That must mean something…

Finally, it looks like industrial robots manufacturers have understood the value that a standard can provide to their business. Even if they do not build their industrial robots ROS enabled from scratch, they are adopting the ROS Industrial flavour of ROS, which allows them to ROSify their industrial robots and re-use all the software created for manipulators in the ROS ecosystem.

But are all companies getting into the ROS bus? Not all of them!

Some companies like Jibo, Aldebaran or Google still do not rely on ROS for their robots programming. Some of them rely on their own middleware created previously to the existence of ROS (that is the case of Aldebaran). Some others, though, are creating their own middleware from scratch. Their reasons: do not believe ROS is good, they have already created a middleware, or do not want to develop their products depending on the middleware of others. Those companies have very fair reasons to go their way. However, will that make them competitive? (if we have to judge from previous history, mobiles, VCRs, the answer may be no).

So is ROS the standard for programming robots?

That is a question still too soon to be answered. It looks like it is becoming the standard, but many things can change. It is unlikely that another middleware takes the current number one middleware title from ROS. But it may happen something that wipes ROS out of the map (may be Google will release its middleware to the public (like they did with Android) and take the sector by storm?).

Still, ROS has its problems, like a total lack of security or the instability of some important packages. Even if the OSRF group are working hard to build a better ROS system (for instance ROS2 is in beta phase with many root improvements), some hard work is still required for some basic things (like the ROS controllers for real robots).

Given those numbers, at The Construct we believe that ROS IS THE STANDARD (that is why we are devoted to creating the best ROS learning tutorials of the world). Actually, it was thanks to this standardization that two Barcelona students were able to create an autonomous robot product for coffee shops in only three months from zero knowledge of robotics (see Barista robot).

This is the future that is coming, and it is good. On that future, thanks to standards, almost anyone will be able to build, design and program their own robotics product, in a similar way as PC stores are building computers today.

So my advice, as I said to the Singapore engineer, is to bet for ROS. Right now, it is the best option for a robotics standard.

ROS Q&A | ROS Package Testing (Part 1)

Hello Developer!

In the last years, unit testing has become more popular in a lot of programming languages. It’s a very useful practice to any kind of program or project. The problem is “How do I start testing?”. If you have already a project that has not being tested from it’s beginning, it becomes hard to start, but not impossible!

Unit testing requires some practices from the developer. If you want to follow the TDD (Test Driven Development), you have to create your tests before coding the logic of your functions and methods. Unit testing is a way to test small parts of a project, in order to help you integrating all of them, or even understand why the whole project is not working the way you expected.

In this post, you’ll see a simple example of unit testing using C++, for a very simple ROS Package. The logic itself is not meaningful in a robotic application, but the configuration of the class and the unit testing is valuable for using in other projects. So, let’s start!

(more…)

ROS Q&A | Treating quaternions for 2D navigation

Are you having problems to make your robot navigate because of the kind of data you have to treat? It’s very common to get stuck when we have to work with quaternions, instead of RPY angles. Quaternions are used in robotics to represent the rotation, in 3 axis, of a rigid body in respect with a coordinate frame. But sometimes it’s too much for what we want to do.

In this post, we are going to see how to get only the necessary data from a quaternion for a 2D application. It is very helpful, since robotics algorithms works with quaternions, but user’s interface, RPY angles.

Let’s start with a robot, to make it clear. In this post, we are going to use the Turtlebot simulation, available in ROS Development Studio (RDS).

Turtlebot Simulation

We have provided by the robot sensors a /odom topic, which publishes messages of the nav_msgs/Odometry type. See below a single message example:

Odometry message

Now, unless you have a path described using quaternions, we need to convert this quaternion to RPY. To do the following, we will use the TF library. In the image below, you can see the program in charge of this task:

#include <tf/tf.h> #include <nav_msgs/Odometry.h> #include <geometry_msgs/Pose2D.h>

ros::Publisher pub_pose_;

void odometryCallback_(const nav_msgs::Odometry::ConstPtr msg) {
geometry_msgs::Pose2D pose2d;
pose2d.x = msg->pose.pose.position.x;
pose2d.y = msg->pose.pose.position.y;

tf::Quaternion q(
msg->pose.pose.orientation.x,
msg->pose.pose.orientation.y,
msg->pose.pose.orientation.z,
msg->pose.pose.orientation.w);
tf::Matrix3x3 m(q);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);

pose2d.theta = yaw;
pub_pose_.publish(pose2d);
}

int main(int argc, char **argv)
{
ros::init(argc, argv, “conversion_node”);

ros::NodeHandle nh_;

ros::Subscriber sub_odom_ = nh_.subscribe(“odom”, 1, odometryCallback_);
pub_pose_ = nh_.advertise<geometry_msgs::Pose2D>(“pose2d”, 1);

ros::spin();

return 0;
}

First, we are creating a node and subscribing the /odom topic. In the callback of this subscriber, we are getting the information that matters for us, which are the X, Y and Yaw data. To have the Yaw angle, we are using the quaternion data and converting to RPY (We need a Matrix3x3 object in the middle process). Since our robot has only three degrees of freedom (linear in X and Y, angular in Z), we are not considering Roll and Pitch, the Pose2D message type is enough for us. This is a simplification for a ground robot navigating in a planar scenario.

Go to point program for ground robots using ActionLib

ROS Development Studio Public Simulation List

Navigation is one of the challenging tasks in robotics. It’s not a simple task. To reach a goal or follow a trajectory, the robot must know the environment using and localize itself through sensors and a map.

But when we have a robot that has this information already, it’s possible to start navigating, defining points to follow in order to reach a goal. That’s the point we are starting from in this post. Using a very popular ground robot, Turtlebot 2, we are going to perform a navigation task, or part of it.

In order to program the robot, RDS (ROS Development Studio) is going to be used. Before starting, make sure you are logged in using your credentials and able to see the public simulation list (as below) and run the Kobuki gazebo simulation.

RDS Public Simulation List

At this point, you should have the simulation running (image below). On the left side of the screen, you have a Jupyter Notebook with the basics and some instructions about the robot. On the center, the simulation. On the right, an IDE and a terminal. It’s possible to set each one in full screen mode or reorganize the screen, as you wish. Feel free to explore it!

So, you have already noticed the simulation is running, the robot is ready and you might have send some velocity commands already. Now let’s start showing our Action Server that sends the velocity commands in order to the robot achieve a goal. First thing, clone this repository [https://marcoarruda@bitbucket.org/TheConstruct/theconstruct_navigation.git] to your workspace (yes, your cloud workspace, using RDS!). Done? Let’s explore it for a while. Open the file src/theconstruct_navigation/go_to_point2d/src/go_to_point2d_server.cpp. You must have something like this:

So, let’s start from the main function of the file. You can see there we are creating a node (ROS::init()) and a GoToPoint2DAction object. That’s the name of the class created at the beginning of the file. Once this variable is created, all methods and behaviors of the class will be working.

Now, taking a look inside the class we can see that there are some methods and attributes. The attributes are used only inside the class. The interface between the object and our ROS node are the methods, which are public.

When the object is instantiated, it registers some mandatory callbacks for the ActionLib library (goalCB, the one who receives the goal or points that we want to send the robot and preembCB, that allows us to interrupt the task). It’s also getting some parameters from the launch file. And finally, creating a publisher for the velocity topic and subscribing the odometry topic, which is used to localize the robot.

Let’s compile it! Using the terminal, enter into the directory catkin_ws (cd catkin_ws) and compile the workspace (catkin_make). It may take some minutes, because we are generating the message files. The action message is defined at the folder theconstruct_navigation/action/GoToPoint2D.action. You can explore there and see what it expects and delivers.

Finally, let’s run the action server. Use the launch file to set the parameters:
roslaunch go_to_point2d go_to_point2d_server.launch. Did the robot move? No? Great! The server is waiting for the messages, so it must not send any command until we create an action client and send the requests. First, let’s take a look in the launch file:

Notice that we have some parameters to define the limits of the robot operation. The first 3 sets the maximum and minimum linear velocity and a gain that is used to set the robot speed in a straight line, since it depends on the distance between the robot and the goal point.

The next 3 parameters set the same as the previous parameters, but for the angular velocity.

Finally the last 3 parameters are used to establish a tolerance for the robot. Well, the robot’s odometry and yaw measurement are not perfect, so we need to consider we’ll have some errors. The error cannot be too small, otherwise the robot will never get to its goal. If the error is too big, the robot will stop very far from the goal (it depends on the robot perception).

Now that we have the basic idea of how this package works, let’s use it! In order to create an action client and send a goal to the server, we are going to use the jupyter notebook and create a node in python. You can use the following code and see the robot going to the points:

Restart the notebook kernel before running it, because we have compiled a new package. Execute the cells, one by one, in the order and you’ll see the robot going to the point!
If you have any doubts about how to do it, please leave a comment. You can also check this video, where all the steps described in this post are done:
[ROS Q&A] How to test ROS algorithms using ROS Development Studio

Related ROS Answers Forum question: actionlib status update problem

Next Entries »