Mastering ROS for robotics programming Design, build, and simulate complex robots using Robot Operating System [second ed.] 9781788478953, 1788478959

Table of contents :
Title Page
Copyright and Credits
www.PacktPub.com
Contributors
Table of Contents
Preface
Chapter 1: Introduction to ROS
Why should we learn ROS?
Why we prefer ROS for robots
Why some do not prefer ROS for robots
Understanding the ROS filesystem level
ROS packages
ROS metapackages
ROS messages
The ROS services
Understanding the ROS computation graph level
ROS nodes
ROS messages
ROS topics
ROS services
ROS bags
The ROS Master
Using the ROS parameter
ROS community level
What are the prerequisites for starting with ROS?
Running the ROS Master and the ROS parameter server
Checking the roscore command output
Questions
Summary
Chapter 2: Getting Started with ROS Programming
Creating a ROS package
Working with ROS topics
Creating ROS nodes
Building the nodes
Adding custom msg and srv files
Working with ROS services
Working with ROS actionlib
Creating the ROS action server
Creating the ROS action client
Building the ROS action server and client
Creating launch files
Applications of topics, services, and actionlib
Maintaining the ROS package
Releasing your ROS package
Preparing the ROS package for the release
Releasing our package
Creating a Wiki page for your ROS package
Questions
Summary
Chapter 3: Working with 3D Robot Modeling in ROS
ROS packages for robot modeling
Understanding robot modeling using URDF
Creating the ROS package for the robot description
Creating our first URDF model
Explaining the URDF file
Visualizing the 3D robot model in RViz
Interacting with pan-and-tilt joints
Adding physical and collision properties to a URDF model
Understanding robot modeling using xacro
Using properties
Using the math expression
Using macros
Converting xacro to URDF
Creating the robot description for a seven DOF robot manipulator
Arm specification
Type of joints
Explaining the xacro model of the seven DOF arm
Using constants
Using macros
Including other xacro files
Using meshes in the link
Working with the robot gripper
Viewing the seven DOF arm in RViz
Understanding joint state publisher
Understanding robot state publisher
Creating a robot model for the differential drive mobile robot
Questions
Summary
Chapter 4: Simulating Robots Using ROS and Gazebo
Simulating the robotic arm using Gazebo and ROS
Creating the robotic arm simulation model for Gazebo
Adding colors and textures to the Gazebo robot model
Adding transmission tags to actuate the model
Adding the gazebo_ros_control plugin
Adding a 3D vision sensor to Gazebo
Simulating the robotic arm with Xtion Pro
Visualizing the 3D sensor data
Moving robot joints using ROS controllers in Gazebo
Understanding the ros_control packages
Different types of ROS controllers and hardware interfaces
How the ROS controller interacts with Gazebo
Interfacing joint state controllers and joint position controllers to the arm
Launching the ROS controllers with Gazebo
Moving the robot joints
Simulating a differential wheeled robot in Gazebo
Adding the laser scanner to Gazebo
Moving the mobile robot in Gazebo
Adding joint state publishers in the launch file
Adding the ROS teleop node
Questions
Summary
Chapter 5: Simulating Robots Using ROS and V-REP
Setting up V-REP with ROS
Understanding the vrep_plugin
Interacting with V-REP using ROS services
Interacting with V-REP using ROS topics
Simulating the robotic arm using V-REP and ROS
Adding the ROS interface to V-REP joint controllers
Simulating a differential wheeled robot in V-REP
Adding a laser sensor to V-REP
Adding a 3D vision sensor to V-REP
Questions
Summary
Chapter 6: Using the ROS MoveIt! and Navigation Stack
Installing MoveIt!
MoveIt! architecture
The move_group node
Motion planning using MoveIt!
Motion planning request adapters
MoveIt! planning scene
MoveIt! kinematics handling
MoveIt! collision checking
Generating MoveIt! configuration package using the Setup Assistant tool
Step 1 – Launching the Setup Assistant tool
Step 2 – Generating the Self-Collision matrix
Step 3 – Adding virtual joints
Step 4 – Adding planning groups
Step 5 – Adding the robot poses
Step 6 – Setting up the robot end effector
Step 7 – Adding passive joints
Step 8 – Author information
Step 9 – Generating configuration files
Motion planning of robot in RViz using MoveIt! configuration package
Using the RViz Motion Planning plugin
Interfacing the MoveIt! configuration package to Gazebo
Step 1 – Writing the controller configuration file for MoveIt!
Step 2 – Creating the controller launch files
Step 3 – Creating the controller configuration file for Gazebo
Step 4 – Creating the launch file for Gazebo trajectory controllers
Step 5 – Debugging the Gazebo- MoveIt! interface
Understanding the ROS Navigation stack
ROS Navigation hardware requirements
Working with Navigation packages
Understanding the move_base node
Working of Navigation stack
Localizing on the map
Sending a goal and path planning
Collision recovery behavior
Sending the command velocity
Installing the ROS Navigation stack
Building a map using SLAM
Creating a launch file for gmapping
Running SLAM on the differential drive robot
Implementing autonomous navigation using amcl and a static map
Creating an amcl launch file
Questions
Summary
Chapter 7: Working with pluginlib, Nodelets, and Gazebo Plugins
Understanding pluginlib
Creating plugins for the calculator application using pluginlib
Working with the pluginlib_calculator package
Step 1 - Creating the calculator_base header file
Step 2 - Creating the calculator_plugins header file
Step 3 - Exporting plugins using the calculator_plugins.cpp
Step 4 - Implementing  the plugin loader using the calculator_loader.cpp
Step 5 - Creating the plugin description file: calculator_plugins.xml
Step 6 - Registering the plugin with the ROS package system
Step 7 - Editing the CMakeLists.txt file
Step 8 - Querying the list of plugins in a package
Step 9 - Running the plugin loader
Understanding ROS nodelets
Creating a nodelet
Step 1 - Creating a package for a nodelet
Step 2 - Creating the hello_world.cpp nodelet
Step 3 - Explanation of hello_world.cpp
Step 4 - Creating the plugin description file
Step 5 - Adding the export tag in package.xml
Step 6 - Editing CMakeLists.txt
Step 7 - Building and running nodelets
Step 8 - Creating launch files for nodelets
Understanding the Gazebo plugins
Creating a basic world plugin
Questions
Summary
Chapter 8: Writing ROS Controllers and Visualization Plugins
Understanding ros_control packages
The controller_interface package
Initializating the controller
Starting the ROS controller
Updating the ROS controller
Stopping the controller
The controller_manager
Writing a basic joint controller in ROS
Step 1 - Creating the controller package
Step 2 – Creating the controller header file
Step 3 – Creating the controller source file
Step 4 – Explaining the controller source file
Step 5 – Creating the plugin description file
Step 6 – Updating package.xml
Step 7 – Updating CMakeLists.txt
Step 8 – Building the controller
Step 9 – Writing the controller configuration file
Step 10 – Writing the launch file for the controller
Step 11 – Running the controller along with the seven dof arm in Gazebo
Understanding the ROS visualization tool (RViz) and its plugins
Displays panel
RViz toolbar
Views
Time panel
Dockable panels
Writing an RViz plugin for teleoperation
Methodology of building the RViz plugin
Step 1 – Creating the RViz plugin package
Step 2 – Creating the RViz plugin header file
Step 3 – Creating the RViz plugin definition
Step 4 – Creating the plugin description file
Step 5 – Adding the export tags in package.xml
Step 6 – Editing CMakeLists.txt
Step 7 – Building and loading plugins
Questions
Summary
Chapter 9: Interfacing I/O Boards, Sensors, and Actuators to ROS
Understanding the Arduino-ROS interface
What is the Arduino-ROS interface?
Understanding the rosserial package in ROS
Installing rosserial packages on Ubuntu 16.04
Understanding ROS node APIs in Arduino
ROS - Arduino Publisher and Subscriber example
Arduino-ROS, example - blink LED and push button
Arduino-ROS, example - Accelerometer ADXL 335
Arduino-ROS, example - ultrasonic distance sensor
Equations to find distance using the ultrasonic range sensor
Arduino-ROS example - Odometry Publisher
Interfacing non-Arduino boards to ROS
Setting ROS on Odroid-XU4 and Raspberry Pi 2
How to install an OS image to Odroid-XU4 and Raspberry Pi 2
Installation in Windows
Installation in Linux
Connecting to Odroid-XU4 and Raspberry Pi 2 from a PC
Configuring an Ethernet hotspot for Odroid-XU4 and Raspberry Pi 2
Installing Wiring Pi on Odroid-XU4
Installing Wiring Pi on Raspberry Pi 2
Blinking LED using ROS on Raspberry Pi 2
Push button + blink LED using ROS on Raspberry Pi 2
Running examples in Raspberry Pi 2
Interfacing DYNAMIXEL actuators to ROS
Questions
Summary
Chapter 10: Programming Vision Sensors Using ROS, Open CV, and PCL
Understanding ROS – OpenCV interfacing packages
Understanding ROS - PCL interfacing packages
Installing ROS perception
Interfacing USB webcams in ROS
Working with ROS camera calibration
Converting images between ROS and OpenCV using cv_bridge
Image processing using ROS and OpenCV
Step 1 – Creating a ROS package for the experiment
Step 2 – Creating source files
Step 3 – Explanation of the code
Publishing and subscribing images using image_transport
Converting OpenCV to ROS images using cv_bridge
Finding edges on the image
Visualizing raw and edge-detected-images
Step 4 – Editing the CMakeLists.txt file
Step 5 – Building and running an example
Interfacing Kinect and Asus Xtion Pro in ROS
Interfacing Intel Real Sense camera with ROS
Working with a point cloud to a laser scan package
Interfacing Hokuyo Laser in ROS
Working with point cloud data
How to publish a point cloud
How to subscribe and process the point cloud
Writing data to a Point Cloud Data (PCD) file
Reading and publishing a point cloud from a PCD file
Working with AR Marker detection for object pose estimation
Using AprilTag with ROS
Questions
Summary
Chapter 11: Building and Interfacing Differential Drive Mobile Robot Hardware in ROS
Introducing to Chefbot – a DIY mobile robot and its hardware configuration
Flashing Chefbot firmware using Energia IDE
Serial data sending protocol from LaunchPad to PC
Serial data sending protocol from PC to Launchpad
Discussing Chefbot interface packages on ROS
Computing odometry from encoder ticks
Computing motor velocities from ROS twist message
Running the robot standalone launch file using C++ nodes
Configuring the Navigation stack for Chefbot
Configuring the gmapping node
Configuring the Navigation stack packages
Common configuration local_costmap and global_costmap
Configuring global costmap parameters
Configuring local costmap parameters
Configuring base local planner parameters
Configuring DWA local planner parameters
Configuring move_base node parameters
Understanding AMCL
Understanding RViz for working with the Navigation stack
2D Pose Estimate button
Visualizing the particle cloud
The 2D Nav Goal button
Displaying the static map
Displaying the robot footprint
Displaying the global and local cost map
Displaying the global plan, the local plan, and the planner plan
The current goal
Obstacle avoidance using the Navigation stack
Working with Chefbot simulation
Building a room in Gazebo
Adding model files to the Gazebo model folder
Sending a goal to the Navigation stack from a ROS node
Questions
Summary
Chapter 12: Exploring the Advanced Capabilities of ROS-MoveIt!
Motion planning using the move_group C++ interface
Motion planning a random path using MoveIt! C++ APIs
Motion planning a custom path using MoveIt! C++ APIs
Collision checking with a robot arm using MoveIt!
Adding a collision object to MoveIt!
Removing a collision object from the planning scene
Attaching a collision object to a robot link
Checking self-collisions using MoveIt! APIs
Working with perception using MoveIt! and Gazebo
Manipulating objects with MoveIt!
Working with a robot pick-and-place task using MoveIt!
Calculating grasp poses with GPD
Pick and place action in Gazebo and real robot
Understanding DYNAMIXEL ROS Servo controllers for robot hardware interfacing
The DYNAMIXEL servos
DYNAMIXEL-ROS interface
Interfacing 7 DOF DYNAMIXEL-based robotic arm to ROS MoveIt!
Creating a controller package for a COOL arm robot
MoveIt! configuration of the COOL Arm
Questions
Summary
Chapter 13: Using ROS in MATLAB and Simulink
Getting started with MATLAB and MATLAB-ROS
Getting started with the Robotic System Toolbox and ROS-MATLAB interface
Starting with ROS topics and MATLAB callback functions
Implementing an obstacle avoidance system for a Turtlebot robot
Getting started with ROS and Simulink
Creating a wave signal integrator in Simulink
Working with ROS messages in Simulink
Publishing a ROS message in Simulink
Subscribing to a ROS topic in Simulink
Developing a simple control system in Simulink
Configuring the Simulink model
Questions
Summary
Chapter 14: ROS for Industrial Robots
Understanding ROS-Industrial packages
Goals of ROS-Industrial
ROS-Industrial – a brief history
Benefits of ROS-Industrial
Installing ROS-Industrial packages
Block diagram of ROS-Industrial packages
Creating a URDF for an industrial robot
Creating the MoveIt! configuration for an industrial robot
Updating the MoveIt! configuration files
Testing the MoveIt! configuration
Installing ROS-Industrial packages of  Universal robotic arms
Installing the ROS interface of Universal Robots
Understanding the Moveit! configuration of a Universal Robot arm
Getting started with real Universal Robots hardware and ROS-I
Working with MoveIt! configuration of ABB robots
Understanding the ROS-Industrial robot support packages
Visualizing the ABB robot model in RViz
ROS-Industrial robot client package
Designing industrial robot client nodes
ROS-Industrial robot driver package
Understanding the MoveIt! IKFast plugin
Creating the MoveIt! IKFast plugin for the ABB IRB 6640 robot
Prerequisites for developing the MoveIt! IKFast plugin
OpenRave and IK Fast modules
MoveIt! IKFast
Installing the MoveIt! IKFast package
Installing OpenRave on Ubuntu 16.04
Creating the COLLADA file of a robot to work with OpenRave
Generating the IKFast CPP file for the IRB 6640 robot
Creating the MoveIt! IKFast plugin
Questions
Summary
Chapter 15: Troubleshooting and Best Practices in ROS
Setting up RoboWare Studio in Ubuntu
Installing/uninstalling RoboWare Studio
Getting started with RoboWare Studio
Create ROS packages in RoboWare Studio
Building ROS workspace in RoboWare Studio
Executing ROS nodes in RoboWare Studio
Starting ROS tools from the RoboWare interface
Handling active ROS topics, nodes, and services
Creating ROS nodes and classes with RoboWare tools
ROS package manager in RoboWare Studio
Best practices in ROS
ROS C++ coding style guide
Standard naming conventions used in ROS
Code license agreement
ROS code formatting
Console output
Best practices in the ROS package
Important troubleshooting tips in ROS
Using roswtf
Questions
Summary
Other Books You May Enjoy
Index

Citation preview

Mastering ROS for Robotics Programming Second Edition

%FTJHOCVJMEBOETJNVMBUFDPNQMFYSPCPUTVTJOHUIF3PCPU 0QFSBUJOH4ZTUFN

Lentin Joseph Jonathan Cacace

BIRMINGHAM - MUMBAI

Mastering ROS for Robotics Programming Second Edition Copyright a 2018 Packt Publishing All rights reserved. No part of this book may be reproduced, stored in a retrieval system, or transmitted in any form or by any means, without the prior written permission of the publisher, except in the case of brief quotations embedded in critical articles or reviews. Every effort has been made in the preparation of this book to ensure the accuracy of the information presented. However, the information contained in this book is sold without warranty, either express or implied. Neither the author, nor Packt Publishing or its dealers and distributors, will be held liable for any damages caused or alleged to have been caused directly or indirectly by this book. Packt Publishing has endeavored to provide trademark information about all of the companies and products mentioned in this book by the appropriate use of capitals. However, Packt Publishing cannot guarantee the accuracy of this information. Commissioning Editor: Vijin Boricha Acquisition Editor: Divya Poojari Content Development Editor: Eisha Dsouza Technical Editor: Naveenkumar Jain Copy Editor: Safis Editing Project Coordinator: Kinjal Bari Proofreader: Safis Editing Indexer: Pratik Shirodkar Graphics: Jisha Chirayil Production Coordinator: Arvindkumar Gupta First published: December 2015 Second edition: February 2018 Production reference: 1220218 Published by Packt Publishing Ltd. Livery Place 35 Livery Street Birmingham B3 2PB, UK. ISBN 978-1-78847-895-3

XXXQBDLUQVCDPN

NBQUJP

Mapt is an online digital library that gives you full access to over 5,000 books and videos, as well as industry leading tools to help you plan your personal development and advance your career. For more information, please visit our website.

Why subscribe? Spend less time learning and more time coding with practical eBooks and Videos from over 4,000 industry professionals Improve your learning with Skill Plans built especially for you Get a free eBook or video every month Mapt is fully searchable Copy and paste, print, and bookmark content

PacktPub.com Did you know that Packt offers eBook versions of every book published, with PDF and ePub files available? You can upgrade to the eBook version at XXX1BDLU1VCDPN and as a print book customer, you are entitled to a discount on the eBook copy. Get in touch with us at TFSWJDF!QBDLUQVCDPN for more details. At XXX1BDLU1VCDPN, you can also read a collection of free technical articles, sign up for a range of free newsletters, and receive exclusive discounts and offers on Packt books and eBooks.

Contributors About the authors Lentin Joseph is an author and robotics entrepreneur from India. He runs a robotics software company called Qbotics Labs in India. He has 7 years of experience in the robotics domain especially in Robot Operating System, Open-CV, and PCL. He has authored three books in ROS, namely, Learning Robotics using Python, Mastering ROS for Robotics Programming, and ROS Robotics Projects. He is currently pursuing his masters in Robotics from India and is also doing research in Robotics Institute, CMU, USA.

Jonathan Cacace was born in Naples, Italy, on December 13, 1987. He received his Masterbs degree in computer science, and a Ph.D. degree in Information and Automation Engineering, from the University of Naples Federico II. Currently, he is a postdoc at the PRISMA Lab of the University of Naples Federico II. He is involved in different research projects focused on industrial and service robotics in which he has developed several ROS-based applications integrating robot perception and control. I would like to express my thanks and gratitude to my friends, my parents and all the people I met during my life who contributed to enrich my life and my knowledge.

About the reviewer Ruixiang Du is a PhD candidate in Mechanical Engineering at Worcester Polytechnic Institute (WPI). He currently works in the Autonomy, Control and Estimation Laboratory with a research focus on the motion planning and control of autonomous mobile robots. He received a bachelorbs degree in Automation from North China Electric Power University in 2011 and a masterbs degree in Robotics Engineering from WPI in 2013. Ruixiang has general interests in robotics and in real-time and embedded systems. He has worked on various robotic projects with robot platforms ranging from medical robots, unmanned aerial/ground vehicles, to humanoid robots. He was a member of Team WPICMU for the DARPA Robotics Challenge.

Packt is searching for authors like you If you're interested in becoming an author for Packt, please visit BVUIPSTQBDLUQVCDPN and apply today. We have worked with thousands of developers and tech professionals, just like you, to help them share their insight with the global tech community. You can make a general application, apply for a specific hot topic that we are recruiting an author for, or submit your own idea.

Table of Contents Preface

1

Chapter 1: Introduction to ROS Why should we learn ROS? Why we prefer ROS for robots Why some do not prefer ROS for robots Understanding the ROS filesystem level

7

ROS packages ROS metapackages ROS messages The ROS services

Understanding the ROS computation graph level ROS nodes ROS messages ROS topics ROS services ROS bags The ROS Master Using the ROS parameter

ROS community level What are the prerequisites for starting with ROS? Running the ROS Master and the ROS parameter server Checking the roscore command output

Questions Summary Chapter 2: Getting Started with ROS Programming Creating a ROS package Working with ROS topics Creating ROS nodes Building the nodes

Adding custom msg and srv files

7 8 10 11 13 15 16 18 19 21 22 23 24 25 26 28 29 30 30 32 33 34 35 35 38 38 41 44

Table of Contents

Working with ROS services

47

Working with ROS actionlib

52 53 55

Creating the ROS action server Creating the ROS action client

Building the ROS action server and client

Creating launch files Applications of topics, services, and actionlib Maintaining the ROS package Releasing your ROS package Preparing the ROS package for the release Releasing our package Creating a Wiki page for your ROS package

Questions Summary

56 59 62 62 63 64 64 67 70 71

Chapter 3: Working with 3D Robot Modeling in ROS ROS packages for robot modeling Understanding robot modeling using URDF Creating the ROS package for the robot description Creating our first URDF model Explaining the URDF file Visualizing the 3D robot model in RViz Interacting with pan-and-tilt joints

72 73 74 78 78 81 83 84

Adding physical and collision properties to a URDF model Understanding robot modeling using xacro Using properties Using the math expression Using macros

85 86 87 87 87

Converting xacro to URDF Creating the robot description for a seven DOF robot manipulator Arm specification

88 89 90 90

Type of joints

Explaining the xacro model of the seven DOF arm Using constants Using macros

91 91 91

[ ii ]

Table of Contents

Including other xacro files Using meshes in the link Working with the robot gripper Viewing the seven DOF arm in RViz Understanding joint state publisher Understanding robot state publisher

Creating a robot model for the differential drive mobile robot Questions Summary Chapter 4: Simulating Robots Using ROS and Gazebo Simulating the robotic arm using Gazebo and ROS Creating the robotic arm simulation model for Gazebo Adding colors and textures to the Gazebo robot model Adding transmission tags to actuate the model Adding the gazebo_ros_control plugin Adding a 3D vision sensor to Gazebo

Simulating the robotic arm with Xtion Pro Visualizing the 3D sensor data

92 93 93 94 96 96 98 103 103 104 105 106 109 109 110 110 112 113

Moving robot joints using ROS controllers in Gazebo Understanding the ros_control packages Different types of ROS controllers and hardware interfaces How the ROS controller interacts with Gazebo Interfacing joint state controllers and joint position controllers to the arm Launching the ROS controllers with Gazebo Moving the robot joints

Simulating a differential wheeled robot in Gazebo Adding the laser scanner to Gazebo Moving the mobile robot in Gazebo Adding joint state publishers in the launch file

Adding the ROS teleop node Questions Summary

115 116 116 117 119 120 122 123 125 126 128 128 130 131

Chapter 5: Simulating Robots Using ROS and V-REP Setting up V-REP with ROS Understanding the vrep_plugin [ iii ]

132 133 137

Table of Contents

Interacting with V-REP using ROS services Interacting with V-REP using ROS topics

139 141

Simulating the robotic arm using V-REP and ROS

146

Adding the ROS interface to V-REP joint controllers

149

Simulating a differential wheeled robot in V-REP Adding a laser sensor to V-REP Adding a 3D vision sensor to V-REP

Questions Summary

153 156 159 161 161

Chapter 6: Using the ROS MoveIt! and Navigation Stack Installing MoveIt! MoveIt! architecture The move_group node Motion planning using MoveIt! Motion planning request adapters MoveIt! planning scene MoveIt! kinematics handling MoveIt! collision checking

Generating MoveIt! configuration package using the Setup Assistant tool Step 1 – Launching the Setup Assistant tool Step 2 – Generating the Self-Collision matrix Step 3 – Adding virtual joints Step 4 – Adding planning groups Step 5 – Adding the robot poses Step 6 – Setting up the robot end effector Step 7 – Adding passive joints Step 8 – Author information Step 9 – Generating configuration files

Motion planning of robot in RViz using MoveIt! configuration package Using the RViz Motion Planning plugin Interfacing the MoveIt! configuration package to Gazebo Step 1 – Writing the controller configuration file for MoveIt! Step 2 – Creating the controller launch files Step 3 – Creating the controller configuration file for Gazebo

[ iv ]

162 163 163 164 166 168 168 169 170 171 171 173 174 175 176 177 178 178 178 180 181 184 185 186 186

Table of Contents Step 4 – Creating the launch file for Gazebo trajectory controllers Step 5 – Debugging the Gazebo- MoveIt! interface

188 189

Understanding the ROS Navigation stack

191

ROS Navigation hardware requirements Working with Navigation packages

191

Understanding the move_base node Working of Navigation stack Localizing on the map Sending a goal and path planning Collision recovery behavior Sending the command velocity

Installing the ROS Navigation stack Building a map using SLAM Creating a launch file for gmapping Running SLAM on the differential drive robot Implementing autonomous navigation using amcl and a static map Creating an amcl launch file

Questions Summary

193 193 195 195 195 195 195 196 196 196 199 203 203 206 207

Chapter 7: Working with pluginlib, Nodelets, and Gazebo Plugins Understanding pluginlib Creating plugins for the calculator application using pluginlib Working with the pluginlib_calculator package Step 1 - Creating the calculator_base header file Step 2 - Creating the calculator_plugins header file Step 3 - Exporting plugins using the calculator_plugins.cpp Step 4 - Implementing the plugin loader using the calculator_loader.cpp Step 5 - Creating the plugin description file: calculator_plugins.xml Step 6 - Registering the plugin with the ROS package system Step 7 - Editing the CMakeLists.txt file Step 8 - Querying the list of plugins in a package Step 9 - Running the plugin loader

Understanding ROS nodelets

208 209 209 211 211 212 213 213 214 215 215 215 216 216

Creating a nodelet Step 1 - Creating a package for a nodelet Step 2 - Creating the hello_world.cpp nodelet Step 3 - Explanation of hello_world.cpp

[v]

217 217 217 217

Table of Contents Step 4 - Creating the plugin description file Step 5 - Adding the export tag in package.xml Step 6 - Editing CMakeLists.txt Step 7 - Building and running nodelets Step 8 - Creating launch files for nodelets

Understanding the Gazebo plugins Creating a basic world plugin

219 219 219 220 222 224 224

Questions Summary

228 229

Chapter 8: Writing ROS Controllers and Visualization Plugins Understanding ros_control packages The controller_interface package

230 232 232 233 233 234 234

Initializating the controller Starting the ROS controller Updating the ROS controller Stopping the controller

The controller_manager

234

Writing a basic joint controller in ROS Step 1 - Creating the controller package Step 2 – Creating the controller header file Step 3 – Creating the controller source file Step 4 – Explaining the controller source file Step 5 – Creating the plugin description file Step 6 – Updating package.xml Step 7 – Updating CMakeLists.txt Step 8 – Building the controller Step 9 – Writing the controller configuration file Step 10 – Writing the launch file for the controller Step 11 – Running the controller along with the seven dof arm in Gazebo

Understanding the ROS visualization tool (RViz) and its plugins Displays panel RViz toolbar Views Time panel Dockable panels

235 235 236 236 237 239 239 239 239 240 240 241 243 245 245 245 245 245

Writing an RViz plugin for teleoperation [ vi ]

246

Table of Contents

Methodology of building the RViz plugin Step 1 – Creating the RViz plugin package Step 2 – Creating the RViz plugin header file Step 3 – Creating the RViz plugin definition Step 4 – Creating the plugin description file Step 5 – Adding the export tags in package.xml Step 6 – Editing CMakeLists.txt Step 7 – Building and loading plugins

Questions Summary

246 247 247 249 250 250 251 252 254 254

Chapter 9: Interfacing I/O Boards, Sensors, and Actuators to ROS Understanding the Arduino-ROS interface What is the Arduino-ROS interface? Understanding the rosserial package in ROS Installing rosserial packages on Ubuntu 16.04

Understanding ROS node APIs in Arduino ROS - Arduino Publisher and Subscriber example Arduino-ROS, example - blink LED and push button Arduino-ROS, example - Accelerometer ADXL 335 Arduino-ROS, example - ultrasonic distance sensor Equations to find distance using the ultrasonic range sensor

Arduino-ROS example - Odometry Publisher

Interfacing non-Arduino boards to ROS Setting ROS on Odroid-XU4 and Raspberry Pi 2 How to install an OS image to Odroid-XU4 and Raspberry Pi 2 Installation in Windows Installation in Linux

Connecting to Odroid-XU4 and Raspberry Pi 2 from a PC Configuring an Ethernet hotspot for Odroid-XU4 and Raspberry Pi 2 Installing Wiring Pi on Odroid-XU4 Installing Wiring Pi on Raspberry Pi 2

Blinking LED using ROS on Raspberry Pi 2 Push button + blink LED using ROS on Raspberry Pi 2 Running examples in Raspberry Pi 2

Interfacing DYNAMIXEL actuators to ROS Questions

[ vii ]

255 256 257 258 260 265 267 271 274 278 279 282 284 284 287 287 289 289 290 292 294 295 297 300 302 302

Table of Contents

Summary

303

Chapter 10: Programming Vision Sensors Using ROS, Open CV, and PCL Understanding ROS – OpenCV interfacing packages Understanding ROS - PCL interfacing packages Installing ROS perception

304 305 305 306

Interfacing USB webcams in ROS Working with ROS camera calibration Converting images between ROS and OpenCV using cv_bridge Image processing using ROS and OpenCV Step 1 – Creating a ROS package for the experiment Step 2 – Creating source files Step 3 – Explanation of the code Publishing and subscribing images using image_transport Converting OpenCV to ROS images using cv_bridge Finding edges on the image Visualizing raw and edge-detected-images

Step 4 – Editing the CMakeLists.txt file Step 5 – Building and running an example

Interfacing Kinect and Asus Xtion Pro in ROS Interfacing Intel Real Sense camera with ROS Working with a point cloud to a laser scan package

Interfacing Hokuyo Laser in ROS Working with point cloud data How to publish a point cloud How to subscribe and process the point cloud Writing data to a Point Cloud Data (PCD) file Reading and publishing a point cloud from a PCD file

Working with AR Marker detection for object pose estimation Using AprilTag with ROS

308 312 315 316 316 316 316 317 318 320 320 320 321 322 325 328 331 333 334 336 337 338 341 342

Questions Summary

348 348

Chapter 11: Building and Interfacing Differential Drive Mobile Robot Hardware in ROS Introducing to Chefbot – a DIY mobile robot and its hardware configuration [ viii ]

349 350

Table of Contents

Flashing Chefbot firmware using Energia IDE Serial data sending protocol from LaunchPad to PC Serial data sending protocol from PC to Launchpad

Discussing Chefbot interface packages on ROS Computing odometry from encoder ticks Computing motor velocities from ROS twist message Running the robot standalone launch file using C++ nodes

Configuring the Navigation stack for Chefbot Configuring the gmapping node Configuring the Navigation stack packages Common configuration local_costmap and global_costmap Configuring global costmap parameters Configuring local costmap parameters Configuring base local planner parameters Configuring DWA local planner parameters Configuring move_base node parameters

Understanding AMCL Understanding RViz for working with the Navigation stack 2D Pose Estimate button Visualizing the particle cloud The 2D Nav Goal button Displaying the static map Displaying the robot footprint Displaying the global and local cost map Displaying the global plan, the local plan, and the planner plan The current goal

Obstacle avoidance using the Navigation stack Working with Chefbot simulation Building a room in Gazebo Adding model files to the Gazebo model folder

Sending a goal to the Navigation stack from a ROS node

Questions Summary

353 355 355 356 363 365 367 367 367 369 370 371 372 373 374 374 377 380 380 382 383 384 385 386 387 388 388 390 390 391 393 396 396

Chapter 12: Exploring the Advanced Capabilities of ROS-MoveIt! Motion planning using the move_group C++ interface Motion planning a random path using MoveIt! C++ APIs Motion planning a custom path using MoveIt! C++ APIs

[ ix ]

397 398 398 401

Table of Contents

Collision checking with a robot arm using MoveIt!

403

Adding a collision object to MoveIt! Removing a collision object from the planning scene Attaching a collision object to a robot link Checking self-collisions using MoveIt! APIs

403

Working with perception using MoveIt! and Gazebo

411

Manipulating objects with MoveIt!

408 408 409 420

Working with a robot pick-and-place task using MoveIt! Calculating grasp poses with GPD Pick and place action in Gazebo and real robot

Understanding DYNAMIXEL ROS Servo controllers for robot hardware interfacing The DYNAMIXEL servos DYNAMIXEL-ROS interface

420 425 428 429 429 430

Interfacing 7 DOF DYNAMIXEL-based robotic arm to ROS MoveIt! Creating a controller package for a COOL arm robot MoveIt! configuration of the COOL Arm

Questions Summary

431 432 436 438 439

Chapter 13: Using ROS in MATLAB and Simulink Getting started with MATLAB and MATLAB-ROS Getting started with the Robotic System Toolbox and ROS-MATLAB interface Starting with ROS topics and MATLAB callback functions Implementing an obstacle avoidance system for a Turtlebot robot

Getting started with ROS and Simulink Creating a wave signal integrator in Simulink Working with ROS messages in Simulink Publishing a ROS message in Simulink Subscribing to a ROS topic in Simulink

Developing a simple control system in Simulink Configuring the Simulink model

440 441 442 447 452 458 458 461 461 466 469 471

Questions Summary

474 474

Chapter 14: ROS for Industrial Robots [x]

475

Table of Contents

Understanding ROS-Industrial packages Goals of ROS-Industrial ROS-Industrial – a brief history Benefits of ROS-Industrial

476 476 477 477

Installing ROS-Industrial packages Block diagram of ROS-Industrial packages Creating a URDF for an industrial robot Creating the MoveIt! configuration for an industrial robot Updating the MoveIt! configuration files Testing the MoveIt! configuration

Installing ROS-Industrial packages of Universal robotic arms Installing the ROS interface of Universal Robots

Understanding the Moveit! configuration of a Universal Robot arm Getting started with real Universal Robots hardware and ROS-I Working with MoveIt! configuration of ABB robots Understanding the ROS-Industrial robot support packages Visualizing the ABB robot model in RViz

477 478 480 481 485 487 487 488 490 492 494 498 499

ROS-Industrial robot client package

500

Designing industrial robot client nodes

501

ROS-Industrial robot driver package Understanding the MoveIt! IKFast plugin Creating the MoveIt! IKFast plugin for the ABB IRB 6640 robot Prerequisites for developing the MoveIt! IKFast plugin OpenRave and IK Fast modules MoveIt! IKFast Installing the MoveIt! IKFast package Installing OpenRave on Ubuntu 16.04

Creating the COLLADA file of a robot to work with OpenRave Generating the IKFast CPP file for the IRB 6640 robot Creating the MoveIt! IKFast plugin

Questions Summary

502 505 505 506 506 506 507 507 509 511 512 514 515

Chapter 15: Troubleshooting and Best Practices in ROS Setting up RoboWare Studio in Ubuntu

[ xi ]

516 517

Table of Contents

Installing/uninstalling RoboWare Studio Getting started with RoboWare Studio Create ROS packages in RoboWare Studio Building ROS workspace in RoboWare Studio Executing ROS nodes in RoboWare Studio Starting ROS tools from the RoboWare interface Handling active ROS topics, nodes, and services Creating ROS nodes and classes with RoboWare tools ROS package manager in RoboWare Studio

Best practices in ROS

517 517 519 522 523 525 526 528 528 529

ROS C++ coding style guide Standard naming conventions used in ROS Code license agreement ROS code formatting Console output

Best practices in the ROS package Important troubleshooting tips in ROS Using roswtf

530 530 531 531 532 533 533 534

Questions Summary

537 537

Other Books You May Enjoy

538

Index

541

[ xii ]

Preface Robot Operating System is robotic middleware that helps developers to program robotic applications, and is widely used in robotics companies, research centers, and universities. Mastering ROS for Robotics Programming, Second Edition presents advanced concepts of the ROS framework and is particularly suitable for users who are already familiar with the basic concepts of ROS. However, a brief introduction of the basic ROS concepts is proposed in the first chapter in order to help new developers start with the examples in the book. Readers will be guided through the creation, the modeling model and design, new robots, as well as simulating and interfacing them with the ROS framework. They will use advanced simulation software to use ROS tools that allow robot navigation, manipulation, and sensor elaboration. Finally, the reader will learn how to handle important concepts such as ROS low-level controllers, nodelets, and plugins. The readers can work with almost all of the examples of the book using only a standard computer without any special hardware requirements. However, additional hardware components will be used in some chapters of the book to discuss how to use ROS with external sensors, actuators, and I/O boards. The book is organized as follows. After an introduction to the basic concepts of ROS, how to model and simulate a robot is discussed. Gazebo and the V-REP software simulator will be used to control and interact with the modeled robot. These simulators will be used to connect the robots with MoveIt! and navigation ROS package. ROS plugins, controllers, and nodelets are then discussed. Finally, the book discusses how to connect Matlab and Simulink software with ROS.

Who this book is for This book is meant to be used by passionate robotics developers or researchers who want to fully exploit the features of ROS. The book is also good for all the users who already are familiar with typical robotics applications or who want to start learning how to develop the world of ROS in an advanced manner, learning how to model, build, and control their own robots. A basic knowledge of GNU/Linux and C++ programming is strongly recommended if you want to easily comprehend the contents of the book.

Preface

What this book covers $IBQUFS, Introduction to ROS, gives you an understanding of the core underlying concepts

of ROS. $IBQUFS, Getting Started with ROS Programming, explains how to work with ROS

packages. $IBQUFS, Working with 3D Robot Modeling in ROS, discusses the design of two robots; one

is a seven Degree of Freedom (DOF) manipulator and the other is a differential drive robot. $IBQUFS, Simulating Robots Using ROS and Gazebo, discusses the simulation of a seven DOF arm, differential wheeled robots, and ROS controllers that help control robot joints in Gazebo. $IBQUFS, Simulating Robots Using ROS and V-REP, introduces using the V-REP simulator

and vrep_plugin to connect ROS with the simulation scene. Then the control of a seven DOF arm and a differential mobile robot is discussed. $IBQUFS, Using the ROS MoveiIt! and Navigation Stack, interfaces out-of-the-box

functionalities such as robot manipulation and autonomous navigation using ROS MoveIt! and Navigation stack. $IBQUFS, Working with Pluginlib, Nodelets, and Gazebo Plugins, shows some of the advanced concepts in ROS, such as ROS pluginlib, nodelets, and Gazebo plugins. We will discuss the functionalities and application of each concept and can practice one example to demonstrate its working. $IBQUFS, Writing ROS Controllers and Visualization Plugins, shows how to write a basic

ROS controller for PR2 robots and robots similar to PR2. After creating the controller, we will run the controller using the PR2 simulation in Gazebo. We will also see how to create plugin for RViz. $IBQUFS Interfacing I/O Boards, Sensor, and Actuators to ROS, discusses interfacing some

hardware components, such as sensors and actuators, with ROS. We will see the interfacing of sensors using I/O boards, such as Arduino, Raspberry Pi, and Odroid-XU4, with ROS. $IBQUFS, Programming Vision Sensors Using ROS, Open-CV and PCL, discusses how to

interface various vision sensors with ROS and program it using libraries such as Open Source Computer Vision (OpenCV) and Point Cloud Library (PCL), and working with AR Markers.

[2]

Preface $IBQUFS, Building and Interfacing Differential Drive Mobile Robot Hardware in ROS, helps

you to build autonomous mobile robot hardware with differential drive configuration and interface it with ROS. This chapter aims to give you an idea of building a custom mobile robot and interfacing it with ROS. $IBQUFS, Exploring the Advanced Capabilities of ROS-MoveIt!, discusses the capabilities of

MoveIt! such as collision avoidance, perception using 3D sensors, grasping, picking, and placing. After that, we can see how to interface of a robotic manipulator hardware with MoveIt! $IBQUFS, Using ROS in Matlab and Simulink, discusses how to connect Matlab and Simulink software with ROS. $IBQUFS, ROS for Industrial Robots, helps you understand and install ROS-Industrial packages in ROS. We can see how to develop an MoveIt! IKFast plugin for an industrial robot. $IBQUFS, Troubleshooting and Best Practices in ROS, discusses how to set the ROS development environment in Eclipse IDE, best practices in ROS, and troubleshooting tips in ROS.

To get the most out of this book In order to run the examples in this book, you need a standard PC running Linux OS. Ubuntu 16.04 is the suggested Linux distribution, but Debian 8 is supported as well. The suggested PC configuration requires at least 4 GB of RAM and a modern processor (Intel ifamily) to execute Gazebo simulations and image processing algorithms. Readers can even work in a virtual environment setup installing Linux OS on a virtual machine, using Virtual box or VMware software hosted on a Windows system. The disadvantage of this choice is that more computational power is needed to work with the examples and the reader could face issues when interfacing ROS with real hardware. The software needed to follow the book is ROS and Kinetic Kame. Additional software required is V-REP simulator, Git, Matlab, and Simulink. Finally, some chapters help readers to interface ROS with commercial hardware such as I/O boards (Arduino, Odroid, and Raspberry Pi computers), vison sensors (Kinect/Asus Xition Pro), and actuators. These are special hardware components that must be bought to run some examples of the book but are not strictly required to learn ROS.

[3]

Preface

Download the example code files You can download the example code files for this book from your account at XXXQBDLUQVCDPN. If you purchased this book elsewhere, you can visit XXXQBDLUQVCDPNTVQQPSU and register to have the files emailed directly to you. You can download the code files by following these steps: 1. 2. 3. 4.

Log in or register at XXXQBDLUQVCDPN. Select the SUPPORT tab. Click on Code Downloads & Errata. Enter the name of the book in the Search box and follow the onscreen instructions.

Once the file is downloaded, please make sure that you unzip or extract the folder using the latest version of: WinRAR/7-Zip for Windows Zipeg/iZip/UnRarX for Mac 7-Zip/PeaZip for Linux The code bundle for the book is also hosted on GitHub at IUUQTHJUIVCDPN1BDLU1VCMJTIJOH.BTUFSJOH304GPS3PCPUJDT1SPHSBNNJOH4FDPO E&EJUJPO. In case there's an update to the code, it will be updated on the existing GitHub

repository. We also have other code bundles from our rich catalog of books and videos available at IUUQTHJUIVCDPN1BDLU1VCMJTIJOH. Check them out!

Download the color images We also provide a PDF file that has color images of the screenshots/diagrams used in this book. You can download it here: IUUQXXXQBDLUQVCDPNTJUFTEFGBVMUGJMFTEPXOMPBET.BTUFSJOH304GPS3PCPUJDT1S PHSBNNJOH4FDPOE&EJUJPO@$PMPS*NBHFTQEG.

[4]

Preface

Conventions used There are a number of text conventions used throughout this book. $PEF*O5FYU: Indicates code words in text, database table names, folder names, filenames, file extensions, pathnames, dummy URLs, user input, and Twitter handles. Here is an example: "Mount the downloaded 8FC4UPSN ENH disk image file as another disk in your system."

A block of code is set as follows: MBVODI HSPVQOT QBSBNOBNFSPTWFSTJPODPNNBOESPTWFSTJPOSPTMBVODI QBSBNOBNFSPTEJTUSPDPNNBOESPTWFSTJPOE OPEFQLHSPTPVUUZQFSPTPVUOBNFSPTPVUSFTQBXOUSVF HSPVQ MBVODI

Any command-line input or output is written as follows: $ rostopic list $ cd

Bold: Indicates a new term, an important word, or words that you see onscreen. For example, words in menus or dialog boxes appear in the text like this. Here is an example: "On the main toolbar, select File | Open Workspace, and choose the directory representing the ROS workspace." Warnings or important notes appear like this.

Tips and tricks appear like this.

[5]

Preface

Get in touch Feedback from our readers is always welcome. General feedback: Email GFFECBDL!QBDLUQVCDPN and mention the book title in the subject of your message. If you have questions about any aspect of this book, please email us at RVFTUJPOT!QBDLUQVCDPN. Errata: Although we have taken every care to ensure the accuracy of our content, mistakes do happen. If you have found a mistake in this book, we would be grateful if you would report this to us. Please visit XXXQBDLUQVCDPNTVCNJUFSSBUB, selecting your book, clicking on the Errata Submission Form link, and entering the details. Piracy: If you come across any illegal copies of our works in any form on the Internet, we would be grateful if you would provide us with the location address or website name. Please contact us at DPQZSJHIU!QBDLUQVCDPN with a link to the material. If you are interested in becoming an author: If there is a topic that you have expertise in and you are interested in either writing or contributing to a book, please visit BVUIPSTQBDLUQVCDPN.

Reviews Please leave a review. Once you have read and used this book, why not leave a review on the site that you purchased it from? Potential readers can then see and use your unbiased opinion to make purchase decisions, we at Packt can understand what you think about our products, and our authors can see your feedback on their book. Thank you! For more information about Packt, please visit QBDLUQVCDPN.

[6]

1

Introduction to ROS The first two chapters of this book introduce basic ROS concepts and its package management system in order to refresh your memory about concepts you should already know. In this first chapter, we will go through ROS concepts such as the ROS Master, the ROS nodes, the ROS parameter server, ROS messages and services discussing what we need to install ROS and how to get started with the ROS master. In this chapter, we will cover the following topics: Why should we learn ROS? Why should we prefer or should not prefer ROS for robots? Getting started with the ROS filesystem level and its computation graph level. Understanding ROS framework elements. Getting started with the ROS master.

Why should we learn ROS? Robot Operating System (ROS) is a flexible framework, providing various tools and libraries to write robotic software. It offers several powerful features to help developers in such tasks as message passing, distributing computing, code reusing, and implementation of state-of-the-art algorithms for robotic applications. The ROS project was started in 2007, with the name Switchyard, by Morgan Quigley (IUUQXJLJPTSGPVOEBUJPOPSHNPSHBO), as part of the Stanford STAIR robot project. The main development of ROS happened at Willow Garage (IUUQTXXXXJMMPXHBSBHFDPN).

Introduction to ROS

Chapter 1

The ROS community is growing very fast, and there are many users and developers worldwide. Most of the high-end robotics companies are now porting their software to ROS. This trend is also visible in industrial robotics, in which companies are switching from proprietary robotic applications to ROS. The ROS industrial movement has gained momentum in the past few years, owing to the large amount of research done in that field. ROS Industrial can extend the advanced capabilities of ROS to manufacturing. The increasing applications of ROS can generate a lot of job opportunities in this field. So, after some years, a knowledge of ROS will be an essential requirement for a robotics engineer.

Why we prefer ROS for robots Imagine that we are going to build an autonomous mobile robot. Here are some of the reasons why people choose ROS over other robotic platforms, such as Player, YARP, Orocos, MRPT, and so on: High-end capabilities: ROS comes with ready-to-use capabilities. For example, Simultaneous Localization and Mapping (SLAM) and Adaptive Monte Carlo Localization (AMCL) packages in ROS can be used for performing autonomous navigation in mobile robots, and the .PWF*U package can be used for motion planning of robot manipulators. These capabilities can directly be used in our robot software without any hassle. These capabilities are its best form of implementation, so writing new code for existing capabilities is like reinventing the wheel. Also, these capabilities are highly configurable; we can fine-tune each capability using various parameters. Tons of tools: ROS is packed with tons of tools for debugging, visualizing, and performing a simulation. The tools, such as rqt_gui, RViz, and Gazebo, are some of the strong open source tools for debugging, visualization, and simulation. A software framework that has these many tools is very rare. Support for high-end sensors and actuators: ROS is packed with device drivers and interface packages of various sensors and actuators in robotics. The high-end sensors include Velodyne-LIDAR, Laser scanners, Kinect, and so on, and actuators such as DYNAMIXEL servos. We can interface these components to ROS without any hassle.

[8]

Introduction to ROS

Chapter 1

Inter-platform operability: The ROS message-passing middleware allows communication between different nodes. These nodes can be programmed in any language that has ROS client libraries. We can write high-performance nodes in C++ or C and other nodes in Python or Java. This kind of flexibility is not available in other frameworks. Modularity: One of the issues that can occur in most of the standalone robotic applications is that if any of the threads of main code crash, the entire robot application can stop. In ROS, the situation is different; we are writing different nodes for each process, and if one node crashes, the system can still work. Also, ROS provides robust methods to resume operations even if any sensors or motors are dead. Concurrent resource handling: Handling a hardware resource via more than two processes is always a headache. Imagine we want to process an image from a camera for face detection and motion detection; we can either write the code as a single entity that can do both, or we can write a single-threaded code for concurrency. If we want to add more than two features in threads, the application behavior will get complex and will be difficult to debug. But in ROS, we can access the devices using ROS topics from the ROS drivers. Any number of ROS nodes can subscribe to the image message from the ROS camera driver, and each node can perform different functionalities. It can reduce the complexity in computation and also increase the debug ability of the entire system. Active community: When we choose a library or software framework, especially from an open source community, one of the main factors that needs to be checked before using it is its software support and developer community. There is no guarantee of support from an open source tool. Some tools provide good support and some tools don't. In ROS, the support community is active. There is a web portal to handle the support queries from users too (IUUQBOTXFSTSPTPSH). It seems that the ROS community has a steady growth in developers worldwide. There are many reasons to choose ROS other than the preceding points. Next, we can check the various reasons why people don't use ROS. Here are some of the existing reasons.

[9]

Introduction to ROS

Chapter 1

Why some do not prefer ROS for robots Here are some of the reasons why some people do not prefer ROS for their robotic projects: Difficulty in learning: ROS can be difficult to learn. It has a steep learning curve and developers should become familiar with many new concepts to get benefits from the ROS framework. Difficulties in starting with simulation: The main simulator in ROS is Gazebo. Even though Gazebo works well, to get started with Gazebo is not an easy task. The simulator has no inbuilt features to program. Complete simulation is done only through coding in ROS. When we compare Gazebo with other simulators, such as V-REP and Webots, they have inbuilt functionalities to prototype and program the robot. They also have a rich GUI toolset support a wide variety of robots and have ROS interfaces too. These tools are proprietary but can deliver a decent job. The toughness of learning simulation using Gazebo and ROS is a reason for not using it in projects. Difficulties in robot modeling: The robot modeling in ROS is performed using URDF, which is an XML-based robot description. In short, we need to write the robot model as a description using URDF tags. In V-REP, we can directly build the 3D robot model in the GUI itself, or we can import the mesh. In ROS, we should write the robot model definitions using URDF tags. There is a SolidWorks plugin to convert a 3D model from SolidWorks to URDF, but if we use other 3D CAD tools, there are no options at all. Learning to model a robot in ROS will take a lot of time, and building using URDF tags is also time-consuming compared to other simulators. Potential limitations: Current ROS versions have some limitations. For example, there is a lack of a native real-time application development support or the complexity to implement robust multi-robot distributed applications. ROS in commercial robot products: When we deploy ROS on a commercial product, a lot of things need to be taken care of. One thing is the code quality. ROS code follows a standard coding style and keeps best practices for maintaining the code too. We have to check whether it satisfies the quality level required for our product. We might have to do additional work to improve the quality of the code. Most of the code in ROS is contributed by researchers from universities, so if we are not satisfied with the ROS code quality, it is better to write our own code, which is specific to the robot and only use the ROS core functionalities if required.

[ 10 ]

Introduction to ROS

Chapter 1

We now know where we have to use ROS and where we do not. If ROS is really required for your robot, let's start discussing ROS in more detail. First, we can see the underlying core concepts of ROS. There are mainly three levels in ROS: the filesystem level, computation graph level, and community level. We will briefly have a look at each level.

Understanding the ROS filesystem level ROS is more than a development framework. We can refer to ROS as a meta-operating system, since it offers not only tools and libraries but even OS-like functions, such as hardware abstraction, package management, and a developer toolchain. Like a real operating system, ROS files are organized on the hard disk in a particular manner, as depicted in the following figure:

(KIWTG415aNGU[UVGONGXGN

Here are the explanations for each block in the filesystem: Packages: The ROS packages are the most basic unit of the ROS software. They contain one or more ROS programs (nodes), libraries, configuration files, and so on, which are organized together as a single unit. Packages are the atomic build item and release item in the ROS software. Package manifest: The package manifest file is inside a package that contains information about the package, author, license, dependencies, compilation flags, and so on. The QBDLBHFYNM file inside the ROS package is the manifest file of that package.

[ 11 ]

Introduction to ROS

Chapter 1

Metapackages: The term metapackage refers to one or more related packages which can be loosely grouped together. In principle, metapackages are virtual packages that don't contain any source code or typical files usually found in packages. Metapackages manifest: The metapackage manifest is similar to the package manifest, the difference being that it might include packages inside it as runtime dependencies and declare an FYQPSU tag. Messages (NTH): The ROS messages are a type of information that is sent from one ROS process to the other. We can define a custom message inside the NTH folder inside a package (NZ@QBDLBHFNTH.Z.FTTBHF5ZQFNTH). The extension of the message file is NTH. Services (TSW): The ROS service is a kind of request/reply interaction between processes. The reply and request data types can be defined inside the TSW folder inside the package (NZ@QBDLBHFTSW.Z4FSWJDF5ZQFTSW). Repositories: Most of the ROS packages are maintained using a Version Control System (VCS), such as Git, Subversion (svn), Mercurial (hg), and so on. The collection of packages that share a common VCS can be called repositories. The package in the repositories can be released using a catkin release automation tool called CMPPN. The following screenshot gives you an idea of the files and folders of a package that we are going to create in the upcoming sections:

(KIWTG.KUVQHaNGUKPUKFGVJGGZGTEKUGRCEMCIG

[ 12 ]

Introduction to ROS

Chapter 1

ROS packages A typical structure of an ROS package is shown here:

(KIWTG5VTWEVWTGQHCV[RKECN%415RCEMCIG

We can discuss the use of each folder as follows: DPOGJH: All configuration files that are used in this ROS package are kept in this

folder. This folder is created by the user and it is a common practice to name the folder DPOGJH to keep the configuration files in it. JODMVEFQBDLBHF@OBNF: This folder consists of headers and libraries that we need to use inside the package. TDSJQU: This folder keeps executable Python scripts. In the block diagram, we can see two example scripts. TSD: This folder stores the C++ source codes. MBVODI: This folder keeps the launch files that are used to launch one or more ROS nodes. NTH: This folder contains custom message definitions. TSW: This folder contains the services definitions. BDUJPO: This folder contains the action files. We will see more about these kind of files in the next chapter. QBDLBHFYNM: This is the package manifest file of this package. $.BLF-JTUTUYU: This files contains the directives to compile the package.

[ 13 ]

Introduction to ROS

Chapter 1

We need to know some commands to create, modify, and work with the ROS packages. Here are some of the commands used to work with ROS packages: DBULJO@DSFBUF@QLH: This command is used to create a new package SPTQBDL: This command is used to get information about the package in the

filesystem DBULJO@NBLF: This command is used to build the packages in the workspace SPTEFQ: This command will install the system dependencies required for this package To work with packages, ROS provides a bash-like command called SPTCBTI (IUUQXJLJSPTPSHSPTCBTI), which can be used to navigate and manipulate the ROS package. Here are some of the SPTCBTI commands: SPTDE: This command is used to change the current directory using a package

name, stack name, or a special location. If we give the argument a package name, it will switch to that package folder. SPTDQ: This command is used to copy a file from a package. SPTFE: This command is used to edit a file using the vim editor. SPTSVO: This command is used to run an executable inside a package. The definition of QBDLBHFYNM of a typical package is shown in the following screenshot:

(KIWTG5VTWEVWTGQH

QBDLBHFYNM

[ 14 ]

Introduction to ROS

Chapter 1

The QBDLBHFYNM file consists of the package name, version of the package, the package description, author details, package build dependencies, and runtime dependencies. The CVJME@EFQFOE CVJME@EFQFOE tag includes the packages that are necessary to build the source code of the package. The packages inside the SVO@EFQFOE SVO@EFQFOE tags are necessary during runtime of the package node.

ROS metapackages Metapackages are specialized packages in ROS that only contain one file, that is, a QBDLBHFYNM file. They don't contain folders and files like a normal package. Metapackages simply group a set of multiple packages as a single logical package. In the QBDLBHFYNM file, the metapackage contains an FYQPSU tag, as shown here: FYQPSU NFUBQBDLBHF FYQPSU

Also, in metapackages, there are no CVJMEUPPM@EFQFOE dependencies for DBULJO; there are only SVO@EFQFOE dependencies, which are the packages grouped in the metapackage. The ROS navigation stack is a good example of metapackages. If ROS and its navigation package are installed, we can try the following command, by switching to the navigation metapackage folder: $ roscd navigation

Open QBDLBHFYNM using your favorite text editor (HFEJU in the following case): $ gedit package.xml

[ 15 ]

Introduction to ROS

Chapter 1

This is a lengthy file; here is a stripped-down version of it:

(KIWTG5VTWEVWTGQHOGVCRCEMCIG

QBDLBHFYNM

ROS messages The ROS nodes can write or read data that has a different type. The types of data are described using a simplified message description language, also called ROS messages. These datatype descriptions can be used to generate source code for the appropriate message type in different target languages. The data type description of ROS messages is stored in NTH files in the NTH subdirectory of a ROS package. Even though the ROS framework provides a large set of robotic-specific messages already implemented, developers can define their own message type inside their nodes. The message definition can consist of two types: GJFMET and DPOTUBOUT. The field is split into field types and field names. The field type is the data type of the transmitting message and field name is the name of it. The constants define a constant value in the NFTTBHF file.

[ 16 ]

Introduction to ROS

Chapter 1

Here is an example of message definitions: JOUOVNCFS TUSJOHOBNF GMPBUTQFFE

Here, the first part is the field type and the second is the field name. The field type is the data type and the field name can be used to access the value from the message. For example, we can use NTHOVNCFS for accessing the value of the number from the message. Here is a table showing some of the built-in field types that we can use in our message: Primitive type Serialization

C++

Python

CPPM 

Unsigned 8-bit int

VJOU@U 

CPPM

JOU

Signed 8-bit int

JOU@U

JOU

VJOU

Unsigned 8-bit int

VJOU@U

JOU 

JOU

Signed 16-bit int

JOU@U

JOU

VJOU

Unsigned 16-bit int

VJOU@U

JOU

JOU

Signed 32-bit int

JOU@U

JOU

VJOU

Unsigned 32-bit int

VJOU@U

JOU

JOU

Signed 64-bit int

JOU@U

MPOH

VJOU

Unsigned 64-bit int

VJOU@U

MPOH

GMPBU

32-bit IEEE float

GMPBU

GMPBU

GMPBU

64-bit IEEE float

EPVCMF

GMPBU

TUSJOH

ascii string(4)

TUETUSJOH

TUSJOH

UJNF

secs/nsecs unsigned 32-bit ints SPT5JNF

EVSBUJPO

secs/nsecs signed 32-bit ints

SPTQZ5JNF

SPT%VSBUJPO SPTQZ%VSBUJPO

[ 17 ]

Introduction to ROS

Chapter 1

Other kinds of messages are designed to cover a specific application necessity, such as exchanging common geometrical (HFPNFUSZ@NTHT) or sensor (TFOTPS@NTHT) information. A special type of ROS message is called a message header. Headers can carry information, such as time, frame of reference or GSBNF@JE, and sequence number. Using headers, we will get numbered messages and more clarity in who is sending the current message. The header information is mainly used to send data such as robot joint transforms (TF). Here is an example of the message header: VJOUTFR UJNFTUBNQ TUSJOHGSBNF@JE

The SPTNTH command tool can be used to inspect the message header and the field types. The following command helps to view the message header of a particular message: $ rosmsg show std_msgs/Header

This will give you an output like the preceding example message header. We will look at the SPTNTH command and how to work with custom message definitions further in the upcoming sections.

The ROS services The ROS services are a type request/response communication between ROS nodes. One node will send a request and wait until it gets a response from the other. The request/response communication is also using the ROS message description. Similar to the message definitions using the NTH file, we have to define the service definition in another file called TSW, which has to be kept inside the TSW subdirectory of the package. Similar to the message definition, a service description language is used to define the ROS service types. An example service description format is as follows: 3FRVFTUNFTTBHFUZQF TUSJOHTUS  3FTQPOTFNFTTBHFUZQF TUSJOHTUS

The first section is the message type of the request that is separated by  and in the next section is the message type of the response. In these examples, both 3FRVFTU and 3FTQPOTF are strings.

[ 18 ]

Introduction to ROS

Chapter 1

In the upcoming sections, we will look at how to work with ROS services.

Understanding the ROS computation graph level The computation in ROS is done using a network of a process called ROS nodes. This computation network can be called the computation graph. The main concepts in the computation graph are ROS Nodes, Master, Parameter server, Messages, Topics, Services, and Bags. Each concept in the graph is contributed to this graph in different ways. The ROS communication-related packages including core client libraries, such as SPTDQQ and SPTQZUIPO , and the implementation of concepts, such as topics, nodes, parameters, and services are included in a stack called SPT@DPNN (IUUQXJLJSPTPSHSPT@DPNN). This stack also consists of tools such as SPTUPQJD, SPTQBSBN, SPTTFSWJDF, and SPTOPEF to introspect the preceding concepts. The SPT@DPNN stack contains the ROS communication middleware packages and these packages are collectively called the ROS Graph layer:

(KIWTG5VTWEVWTGQHVJG415)TCRJNC[GT

[ 19 ]

Introduction to ROS

Chapter 1

The following are abstracts of each graph's concepts: Nodes: Nodes are the process that perform computation. Each ROS node is written using ROS client libraries. Using client library APIs, we can implement different ROS functionalities, such as the communication methods between nodes, which is particularly useful when different nodes of our robot must exchange information between them. Using the ROS communication methods, they can communicate with each other and exchange data. One of the aims of ROS nodes is to build simple processes rather than a large process with all the functionality. Being a simple structure, ROS nodes are easy to debug. Master: The ROS Master provides the name registration and lookup to the rest of the nodes. Nodes will not be able to find each other, exchange messages, or invoke services without a ROS Master. In a distributed system, we should run the master on one computer, and other remote nodes can find each other by communicating with this master. Parameter server: The parameter server allows you to keep the data to be stored in a central location. All nodes can access and modify these values. The parameter server is a part of the ROS Master. Messages: Nodes communicate with each other using messages. Messages are simply a data structure containing the typed field, which can hold a set of data, and that can be sent to another node. There are standard primitive types (integer, floating point, Boolean, and so on) and these are supported by ROS messages. We can also build our own message types using these standard types. Topics: Each message in ROS is transported using named buses called topics. When a node sends a message through a topic, then we can say the node is publishing a topic. When a node receives a message through a topic, then we can say that the node is subscribing to a topic. The publishing node and subscribing node are not aware of each other's existence. We can even subscribe a topic that might not have any publisher. In short, the production of information and consumption of it are decoupled. Each topic has a unique name, and any node can access this topic and send data through it as long as they have the right message type. Services: In some robot applications, the publish/subscribe communication model may not be suitable. For example, in some cases, we need a kind of request/response interaction, in which one node can ask for the execution of a fast procedure to another node; for example, asking for some quick calculation. The ROS service interaction is like a remote procedure call.

[ 20 ]

Introduction to ROS

Chapter 1

Logging: ROS provides a logging system for storing data, such as sensor data, which can be difficult to collect but is necessary for developing and testing robot algorithms: the bagfiles. Bagfiles are very useful features when we work with complex robot mechanisms. The following graph shows how the nodes communicate with each other using topics. The topics are mentioned in a rectangle and the nodes are represented in ellipses. The messages and parameters are not included in this graph. These kinds of graphs can be generated using a tool called SRU@HSBQI (IUUQXJLJSPTPSHSRU@HSBQI):

(KIWTG)TCRJQHEQOOWPKECVKQPDGVYGGPPQFGUWUKPIVQRKEU

ROS nodes ROS nodes are a process that perform computation using ROS client libraries such as SPTDQQ and SPTQZ. One node can communicate with other nodes using ROS Topics, Services, and Parameters. A robot might contain many nodes; for example, one node processes camera images, one node handles serial data from the robot, one node can be used to compute odometry, and so on.

[ 21 ]

Introduction to ROS

Chapter 1

Using nodes can make the system fault tolerant. Even if a node crashes, an entire robot system can still work. Nodes also reduce the complexity and increase debug-ability compared to monolithic code because each node is handling only a single function. All running nodes should have a name assigned to identify them from the rest of the system. For example, DBNFSB@OPEF could be a name of a node that is broadcasting camera images. There is a SPTCBTI tool to introspect ROS nodes. The SPTOPEF command can be used to get information about a ROS node. Here are the usages of SPTOPEF: SPTOPEFJOGP: This will print the information about the node SPTOPEFLJMM: This will kill a running node SPTOPEFMJTU: This will list the running nodes SPTOPEFNBDIJOF: This will list the nodes running on a

particular machine or a list of machines SPTOPEFQJOH: This will check the connectivity of a node SPTOPEFDMFBOVQ: This will purge the registration of unreachable nodes We will look at example nodes using the SPTDQQ client and will discuss the working of ROS nodes that use functionalities such ROS Topics, Service, Messages, and actionlib.

ROS messages ROS nodes communicate with each other by publishing messages to a topic. As we discussed earlier, messages are a simple data structure containing field types. The ROS message supports standard primitive datatypes and arrays of primitive types. Nodes can also exchange information using service calls. Services are also messages. The service message definitions are defined inside the TSW file. We can access the message definition using the following method. For example, to access TUE@NTHTNTH4USJOHNTH, we can use TUE@NTHT4USJOH. If we are using the SPTDQQ client, we have to include TUE@NTHT4USJOHI for the string message definition.

In addition to message data type, ROS uses an MD5 checksum comparison to confirm whether the publisher and subscriber exchange the same message data types.

[ 22 ]

Introduction to ROS

Chapter 1

ROS has inbuilt tools called SPTNTH to get information about ROS messages. Here are some parameters used along with SPTNTH: SPTNTHTIPX: This shows the message description SPTNTHMJTU: This lists all messages SPTNTHNE: This displays NETVN of a message SPTNTHQBDLBHF: This lists messages in a package SPTNTHQBDLBHFT: This lists packages that

contain messages

ROS topics ROS topics are named buses in which ROS nodes exchange messages. Topics can anonymously publish and subscribe, which means that the production of messages is decoupled from the consumption. The ROS nodes are not interested in knowing which node is publishing the topic or subscribing topics; they only look for the topic name and whether the message types of the publisher and subscriber are matching. The communication using topics are unidirectional. If we want to implement a request/response, such as communication, we have to switch to ROS services. The ROS nodes communicate with topics using TCP/IP-based transport known as TCPROS. This method is the default transport method used in ROS. Another type of communication is UDPROS, which has low-latency, loose transport, and is only suited for teleoperations. The ROS topic tool can be used to get information about ROS topics. Here is the syntax of this command: SPTUPQJDCXUPQJD: This command will display the bandwidth used by the

given topic. SPTUPQJDFDIPUPQJD: This command will print the content of the given topic in a human readable format. Users can use the "-p" option to print data in a csv format. SPTUPQJDGJOENFTTBHF@UZQF: This command will find topics using the given message type. SPTUPQJDI[UPQJD: This command will display the publishing rate of the given topic.

[ 23 ]

Introduction to ROS

Chapter 1

SPTUPQJDJOGPUPQJD: This command will print information about an

active topic. SPTUPQJDMJTU: This command will list all active topics in the ROS system. SPTUPQJDQVCUPQJDNFTTBHF@UZQFBSHT: This command can be used to publish a value to a topic with a message type. SPTUPQJDUZQFUPQJD: This will display the message type of the given topic.

ROS services When we need a request/response kind of communication in ROS, we have to use the ROS services. ROS topics can't implement natively such kind of communication because it is unidirectional. The ROS services are mainly used in a distributed system. The ROS services are defined using a pair of messages. We have to define a request datatype and a response datatype in a TSW file. The TSW files are kept in a TSW folder inside a package. In ROS services, one node acts as a ROS server in which the service client can request the service from the server. If the server completes the service routine, it will send the results to the service client. For example, consider a node able to provide the sum of two numbers received in input, implementing this functionality through a ROS service. The other nodes of our system might request the sum of two numbers via this service. Differently, topics are used to stream continuous data flow. The ROS service definition can be accessed by the following method; for example, if NZ@QBDLBHFTSW*NBHFTSW can be accessed by NZ@QBDLBHF*NBHF.

In ROS services also, there is an MD5 DIFDLTVN that checks in the nodes. If the sum is equal, then only the server responds to the client. There are two ROS tools to get information about the ROS service. The first tool is SPTTSW, which is similar to SPTNTH, and is used to get information about service types. The next command is SPTTFSWJDF, which is used to list and query about the running ROS services.

[ 24 ]

Introduction to ROS

Chapter 1

The following explain how to use the SPTTFSWJDF tool to get information about the running services: SPTTFSWJDFDBMMTFSWJDFBSHT: This tool will call the service using the

given arguments SPTTFSWJDFGJOETFSWJDF@UZQF: This command will find services in the given service type SPTTFSWJDFJOGPTFSWJDFT: This will print information about the given service SPTTFSWJDFMJTU: This command will list the active services running on the system SPTTFSWJDFUZQFTFSWJDF: This command will print the service type of a given service SPTTFSWJDFVSJTFSWJDF: This tool will print the service ROSRPC URI

ROS bags A bag file in ROS is for storing ROS message data from topics and services. The CBH extension is used to represent a bag file. Bag files are created using the SPTCBH command, which will subscribe one or more topics and store the message's data in a file as it's received. This file can play the same topics as they are recorded from or it can remap the existing topics too. The main application of SPTCBH is data logging. The robot data can be logged and can visualize and process offline. The SPTCBH command is used to work with SPTCBH files. Here are the commands to record and playback a bag file: SPTCBHSFDPSEP: This command

will record the given topics into a bag file that is given in the command. We can also record all topics using the B argument. SPTCBHQMBZ: This will playback the existing bag file.

[ 25 ]

Introduction to ROS

Chapter 1

Further details about this command can be found at: IUUQXJLJSPTPSHSPTCBH$PNNBOEMJOF

There is a GUI tool to handle the record and playback of bag files called SRU@CBH. To learn more about SRU@CBH, go to: IUUQXJLJSPTPSHSRU@CBH

The ROS Master The ROS Master is much like a DNS server, associating unique names and IDs to ROS elements active in our system. When any node starts in the ROS system, it will start looking for the ROS Master and register the name of the node in it. So, the ROS Master has the details of all the nodes currently running on the ROS system. When any details of the nodes change, it will generate a callback and update with the latest details. These node details are useful for connecting with each node. When a node starts publishing a topic, the node will give the details of the topic, such as name and data type, to the ROS Master. The ROS Master will check whether any other nodes are subscribed to the same topic. If any nodes are subscribed to the same topic, the ROS Master will share the node details of the publisher to the subscriber node. After getting the node details, these two nodes will interconnect using the TCPROS protocol, which is based on TCP/IP sockets. After connecting to the two nodes, the ROS Master has no role in controlling them. We might be able to stop either the publisher node or the subscriber node according to our requirement. If we stop any nodes, it will check with the ROS Master once again. This same method is used for the ROS services. The nodes are written using the ROS client libraries, such as SPTDQQ and SPTQZ. These clients interact with the ROS Master using XML Remote Procedure Call (XMLRPC)-based APIs, which act as the backend of the ROS system APIs. The 304@."45&3@63* environment variable contains the IP and port of the ROS Master. Using this variable, ROS nodes can locate the ROS Master. If this variable is wrong, the communication between nodes will not take place. When we use ROS in a single system, we can use the IP of a localhost or the name MPDBMIPTU itself. But in a distributed network, in which computation is on different physical computers, we should define 304@."45&3@63* properly; only then will the remote nodes be able find each other and communicate with each other. We need only one Master in a distributed system, and it should run on a computer in which all other computers can ping it properly to ensure that remote ROS nodes can access the Master.

[ 26 ]

Introduction to ROS

Chapter 1

The following diagram shows an illustration of how the ROS Master interacts with a publishing and subscribing node, with the publisher node publishing a string type topic with a )FMMP8PSME message and the subscriber node subscribing to this topic:

(KIWTG%QOOWPKECVKQPDGVYGGPVJG415/CUVGTCPF*GNNQ9QTNFRWDNKUJGTCPFUWDUETKDGT

When the publisher node starts publishing the )FMMP8PSME message in a particular topic, the ROS Master gets the details of the topic and details of the node. It will search whether any node is subscribing to the same topic. If there are no nodes subscribing to the same topic at that time, both nodes remain unconnected. If the publisher and subscriber nodes run at the same time, the ROS Master exchanges the details of the publisher to the subscriber and they will connect and can exchange data through ROS messages.

[ 27 ]

Introduction to ROS

Chapter 1

Using the ROS parameter When programming a robot, we might have to define robot parameters, such as robot controller gains P, I, and D. When the number of parameters increases, we might need to store them as files. In some situations, these parameters have to share between two or more programs too. In this case, ROS provides a parameter server, which is a shared server in which all ROS nodes can access parameters from this server. A node can read, write, modify, and delete parameter values from the parameter server. We can store these parameters in a file and load them into the server. The server can store a wide variety of data types and can even store dictionaries. The programmer can also set the scope of the parameter, that is, whether it can be accessed by only this node or all the nodes. The parameter server supports the following XMLRPC datatypes: 32-bit integers Booleans Strings Doubles ISO8601 dates Lists Base64-encoded binary data We can also store dictionaries on the parameter server. If the number of parameters is high, we can use a YAML file to save them. Here is an example of the YAML file parameter definitions: DBNFSBOBNF OJLPO TUSJOHUZQF DBNFSBGQTJOUFHFS DBNFSBFYQPTVSFGMPBU DBNFSBBDUJWFUSVFCPPMFBO

The SPTQBSBN tool is used to get and set the ROS parameter from the command line. The following are the commands to work with ROS parameters: SPTQBSBNTFU: This command will set a

value in the given parameter SPTQBSBNHFU: This command will retrieve a value from the given parameter

[ 28 ]

Introduction to ROS

Chapter 1

SPTQBSBNMPBE: This command will dump the existing ROS parameters to a YAML file SPTQBSBNEFMFUF: This command will delete the given parameter SPTQBSBNMJTU: This command will list existing parameter names The parameters can be changed dynamically during the execution of the node that uses these parameters, using the EZBNJD@SFDPOGJHVSF package (IUUQXJLJSPTPSHEZOBNJD@SFDPOGJHVSF).

ROS community level These are ROS resources that enable a new community for ROS to exchange software and knowledge. The various resources in these communities are as follows: Distributions: Similar to the Linux distribution, ROS distributions are a collection of versioned metapackages that we can install. The ROS distribution enables easier installation and collection of the ROS software. The ROS distributions maintain consistent versions across a set of software. Repositories: ROS relies on a federated network of code repositories, where different institutions can develop and release their own robot software components. The ROS Wiki: The ROS community Wiki is the main forum for documenting information about ROS. Anyone can sign up for an account and contribute their own documentation, provide corrections or updates, write tutorials, and more. Bug ticket system: If we find a bug in the existing software or need to add a new feature, we can use this resource. Mailing lists: The ROS-users mailing list is the primary communication channel about new updates to ROS, as well as a forum to ask questions about the ROS software. ROS Answers: This website resource helps to ask questions related to ROS. If we post our doubts on this site, other ROS users can see this and give solutions. Blog: The ROS blog updates with news, photos, and videos related to the ROS community (IUUQXXXSPTPSHOFXT).

[ 29 ]

Introduction to ROS

Chapter 1

What are the prerequisites for starting with ROS? Before getting started with ROS and trying the code in this book, the following prerequisites should be met: Ubuntu 16.04 LTS / Ubuntu 15.10 / Debian 8: ROS is officially supported by Ubuntu and Debian operating systems. We prefer to stick with the LTS version of Ubuntu, that is, Ubuntu 16.04. ROS kinetic desktop full installation: Install the full desktop installation of ROS. The version we prefer is ROS kinetic, the latest stable version. The following link gives you the installation instruction of the latest ROS distribution: IUUQXJLJSPTPSHLJOFUJD*OTUBMMBUJPO6CVOUV. Choose the SPT LJOFUJDEFTLUPQGVMM package from the repository list.

Running the ROS Master and the ROS parameter server Before running any ROS nodes, we should start the ROS Master and the ROS parameter server. We can start the ROS Master and the ROS parameter server by using a single command called SPTDPSF, which will start the following programs: ROS Master ROS parameter server SPTPVU logging nodes The SPTPVU node will collect log messages from other ROS nodes and store them in a log file, and will also re-broadcast the collected log message to another topic. The SPTPVU topic is published by ROS nodes by using ROS client libraries such as SPTDQQ and SPTQZ, and this topic is subscribed by the SPTPVU node which rebroadcasts the message in another topic called SPTPVU@BHH. This topic has an aggregate stream of log messages. The SPTDPSF command is a prerequisite before running any ROS node. The following screenshot shows the messages printing when we run the SPTDPSF command in a Terminal.

[ 30 ]

Introduction to ROS

Chapter 1

The following is a command to run SPTDPSF on a Linux Terminal: $ roscore

SPTDPSFEQOOCPF

(KIWTG6GTOKPCNOGUUCIGUYJKNGTWPPKPIVJG

The following are explanations of each section when executing SPTDPSF on the Terminal: In section 1, we can see a log file is created inside the _SPTMPH folder for collecting logs from ROS nodes. This file can be used for debugging purposes. In section 2, the command starts a ROS launch file called SPTDPSFYNM. When a launch file starts, it automatically starts the SPTNBTUFS and the ROS parameter server. The SPTMBVODI command is a Python script, which can start SPTNBTUFS and the ROS parameter server whenever it tries to execute a launch file. This section shows the address of the ROS parameter server within the port. In section 3, we can see the parameters such as SPTEJTUSP and SPTWFSTJPO displayed on the Terminal. These parameters are displayed when it executes SPTDPSFYNM. We look at SPTDPSFYNM and its details further in the next section. In section 4, we can see the SPTNBTUFS node is started using 304@."45&3@63*, which we defined earlier as an environment variable. In section 5, we can see the SPTPVU node is started, which will start subscribing the SPTPVU topic and rebroadcasting into SPTPVU@BHH.

[ 31 ]

Introduction to ROS

Chapter 1

The following is the content of SPTDPSFYNM: MBVODI HSPVQOT QBSBNOBNFSPTWFSTJPODPNNBOESPTWFSTJPOSPTMBVODI QBSBNOBNFSPTEJTUSPDPNNBOESPTWFSTJPOE OPEFQLHSPTPVUUZQFSPTPVUOBNFSPTPVUSFTQBXOUSVF HSPVQ MBVODI

When the SPTDPSF command is executed, initially, the command checks the command-line argument for a new port number for the SPTNBTUFS. If it gets the port number, it will start listening to the new port number; otherwise, it will use the default port. This port number and the SPTDPSFYNM launch file will pass to the SPTMBVODI system. The SPTMBVODI system is implemented in a Python module; it will parse the port number and launch the SPTDPSFYNM file. In the SPTDPSFYNM file, we can see the ROS parameters and nodes are encapsulated in a group XML tag with a  namespace. The group XML tag indicates that all the nodes inside this tag have the same settings. The two parameters called SPTWFSTJPO and SPTEJTUSP store the output of the SPTWFSTJPOSPTMBVODI and SPTWFSTJPOE commands using the DPNNBOE tag, which is a part of the ROS QBSBN tag. The DPNNBOE tag will execute the command mentioned on it and store the output of the command in these two parameters. The SPTNBTUFS and parameter server are executed inside SPTMBVODI modules by using the 304@."45&3@63* address. This is happening inside the SPTMBVODI Python module. The 304@."45&3@63* is a combination of the IP address and port in which SPTNBTUFS is going to listen. The port number can be changed according to the given port number in the SPTDPSF command.

Checking the roscore command output Let's check the ROS topics and ROS parameters created after running SPTDPSF. The following command will list the active topics on the Terminal: $ rostopic list

[ 32 ]

Introduction to ROS

Chapter 1

The list of topics is as follows, as per our discussion on the SPTPVU node subscribe SPTPVU topic. This has all the log messages from the ROS nodes and SPTPVU@BHH will rebroadcast the log messages: /rosout /rosout_agg

The following command lists the parameters available when running SPTDPSF. The following is the command to list the active ROS parameter: $ rosparam list

The parameters are mentioned here; they have the ROS distribution name, version, address of the SPTMBVODI server and SVO@JE, where SVO@JE is a unique ID associated with a particular run of SPTDPSF: /rosdistro /roslaunch/uris/host_robot_virtualbox__51189 /rosversion /run_id

The list of the ROS service generated during the running SPTDPSF can be checked using the following command: $ rosservice list

The list of services running is as follows: /rosout/get_loggers /rosout/set_logger_level

These ROS services are generated for each ROS node for setting the logging levels.

Questions After going through the chapter, you should now be able to answer the following questions: Why should we learn ROS? How does ROS differ from other robotic software platforms? What are the basic elements of ROS framework? What is the internal working of SPTDPSF?

[ 33 ]

Introduction to ROS

Chapter 1

Summary ROS is now a trending software framework among roboticists. Gaining knowledge in ROS is essential in the upcoming years if you are planning to build your career as a robotics engineer. In this chapter, we have gone through the basics of ROS, mainly to refresh the concepts if you have already learned ROS. We discussed the necessity of learning ROS and how it excels among the current robotics software platforms. We went through the basic concepts, such as the ROS Master, the parameter server, and SPTDPSF , and looked at the explanation of the working of SPTDPSF. In the next chapter, we will introduce the ROS package management, discussing some practical examples of the ROS communication system.

[ 34 ]

2

Getting Started with ROS Programming After discussing the basics of the ROS Master, the parameter server, and SPTDPSF , we can now start to create and build a ROS package. In this chapter, we will create different ROS nodes implementing the ROS communication system. Working with ROS packages, we will also refresh the concepts of ROS nodes, topics, messages, services, and actionlib. We will cover the following list of topics: Creating, compiling and running ROS packages. Working with standard and custom ROS messages. Working with ROS services and actionlib. Maintaining and releasing your ROS packages. Creating a wiki page for ROS packages.

Creating a ROS package The ROS packages are the basic unit of the ROS system. We can create a ROS package, build it, and release it to the public. The current distribution of ROS we are using is kinetic. We are using the DBULJO build system to build ROS packages. A build system is responsible for generating UBSHFUT (executable/libraries) from a raw source code that can be used by an end user. In older distributions, such as Electric and Fuerte, SPTCVJME was the build system. Because of the various flaws of SPTCVJME, DBULJO came into existence, which is basically based on Cross Platform Make (CMake). This has a lot of advantages, such as porting the package into another operating system, such as Windows. If an OS supports CMake and Python, DBULJO -based packages can be easily ported into it.

Getting Started with ROS Programming

Chapter 2

The first requirement work with ROS packages is to create a ROS DBULJO workspace. After installed ROS, we can create and build a DBULJO@XPSLTQBDF called DBULJO@XT: $ mkdir -p ~/catkin_ws/src

To compile the workspace, we should source the ROS environment, in order to get access to ROS functions: $ source /opt/ros/kinetic/setup.bash

Switch to the source, TSD folder previously created. $ cd ~/catkin_ws/src

Initialize a new DBULJO workspace: $ catkin_init_workspace

We can build the workspace even if there are no packages. We can use the following command to switch to the workspace folder: $ cd ~/catkin_ws

The DBULJO@NBLF command will build the following workspace: $ catkin_make

This last command will create a EFWFM and a CVJME directory in the catkin workspace. Inside the EFWFM folder different setup files are located. To add the created ROS workspace to the ROS environment, we should source one of this file. In addition, we can source the setup file of this workspace every time that a new CBTI session starts with the following command: $ echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc $ source ~/.bashrc

After setting the DBULJO workspace, we can create our own package that has sample nodes to demonstrate the working of ROS topics, messages, services, and actionlib. The DBULJO@DSFBUF@QLH command is used to create a ROS package. This command is used to create our package, in which we are going to create demos of various ROS concepts. Switch to the DBULJO workspace TSD folder and create the package, using the following command: catkin_create_pkg package_name [dependency1] [dependency2]

[ 36 ]

Getting Started with ROS Programming

Chapter 2

Source code folder: All ROS packages, either created from scratch or downloaded from other code repositories, must be placed in the TSD folder of the ROS workspace, otherwise they can not be recognized by the ROS system and compiled. Here is the command to create the sample ROS package: $ catkin_create_pkg mastering_ros_demo_pkg roscpp std_msgs actionlib actionlib_msgs

The dependencies in the packages are as follows: SPTDQQ: This is the C++ implementation of ROS. It is a ROS client library which

provides APIs to C++ developers to make ROS nodes with ROS topics, services, parameters, and so on. We are including this dependency because we are going to write a ROS C++ node. Any ROS package which uses the C++ node must add this dependency. TUE@NTHT: This package contains basic ROS primitive data types, such as integer, float, string, array, and so on. We can directly use these data types in our nodes without defining a new ROS message. BDUJPOMJC The BDUJPOMJC metapackage provides interfaces to create preemptible tasks in ROS nodes. We are creating BDUJPOMJC -based nodes in this package. So we should include this package to build the ROS nodes. BDUJPOMJC@NTHT: This package contains standard message definitions needed to interact with the action server and action client. After package creation, additional dependencies can be added manually by editing the $.BLF-JTUTUYU and QBDLBHFYNM files. We will get the following message if the package

has been successfully created:

(KIWTG6GTOKPCNOGUUCIGUYJKNGETGCVKPIC415RCEMCIG

[ 37 ]

Getting Started with ROS Programming

Chapter 2

After creating this package, build the package without adding any nodes, using the DBULJO@NBLF command. This command must be executed from the DBULJO workspace path. The following command shows you how to build our empty ROS package: ~/catkin_ws $ catkin_make

After a successful build, we can start adding nodes to the TSD folder of this package. The build folder in the CMake build files mainly contains executables of the nodes that are placed inside the DBULJO workspace TSD folder. The EFWFM folder contains bash script, header files, and executables in different folders generated during the build process. We can see how to make ROS nodes and build using DBULJO@NBLF.

Working with ROS topics Topics are the basic way of communicating between two nodes. In this section, we can see how the topics works. We are going to create two ROS nodes for publishing a topic and subscribing the same. Navigate to the NBTUFSJOH@SPT@EFNP@QLH folder, joining the TSD subdirectory for the source code. EFNP@UPQJD@QVCMJTIFSDQQ and EFNP@UPQJD@TVCTDSJCFSDQQ are the two sets of code that we are going to discuss.

Creating ROS nodes The first node we are going to discuss is EFNP@UPQJD@QVCMJTIFSDQQ. This node will publish an integer value on a topic called OVNCFST. Copy the current code into a new package or use this existing file from the code repository. Here is the complete code: JODMVEFSPTSPTI JODMVEFTUE@NTHT*OUI JODMVEFJPTUSFBN JOUNBJO JOUBSHDDIBS BSHW \ SPTJOJU BSHDBSHWEFNP@UPQJD@QVCMJTIFS SPT/PEF)BOEMFOPEF@PCK SPT1VCMJTIFSOVNCFS@QVCMJTIFS OPEF@PCKBEWFSUJTFTUE@NTHT*OU OVNCFST SPT3BUFMPPQ@SBUF  JOUOVNCFS@DPVOU XIJMF SPTPL  \

[ 38 ]

Getting Started with ROS Programming

Chapter 2

TUE@NTHT*OUNTH NTHEBUBOVNCFS@DPVOU 304@*/'0 ENTHEBUB OVNCFS@QVCMJTIFSQVCMJTI NTH SPTTQJO0ODF  MPPQ@SBUFTMFFQ  

OVNCFS@DPVOU ^ SFUVSO ^

Here is the detailed explanation of the preceding code: JODMVEFSPTSPTI JODMVEFTUE@NTHT*OUI JODMVEFJPTUSFBN

The SPTSPTI is the main header of ROS. If we want to use the SPTDQQ client APIs in our code, we should include this header. The TUE@NTHT*OUI is the standard message definition of the integer datatype. Here, we are sending an integer value through a topic. So we should need a message type for handling the integer data. TUE@NTHT contains the standard message definition of primitive datatypes. TUE@NTHT*OUI contains the integer message definition: SPTJOJU BSHDBSHWEFNP@UPQJD@QVCMJTIFS

This code will initialize a ROS node with a name. It should be noted that the ROS node should be unique. This line is mandatory for all ROS C++ nodes: SPT/PEF)BOEMFOPEF@PCK

This will create a /PEFIBOEMF object, which is used to communicate with the ROS system: SPT1VCMJTIFSOVNCFS@QVCMJTIFS OPEF@PCKBEWFSUJTFTUE@NTHT*OU OVNCFST

This will create a topic publisher and name the topic OVNCFST with a message type TUE@NTHT*OU. The second argument is the buffer size. It indicates how many messages need to be put in a buffer before sending. It should be set to high if the data sending rate is high: SPT3BUFMPPQ@SBUF 

[ 39 ]

Getting Started with ROS Programming

Chapter 2

This is used to set the frequency of sending data: XIJMF SPTPL  \

This is an infinite XIJMF loop, and it quits when we press Ctrl + C. The SPTPL  function returns zero when there is an interrupt; this can terminate this XIJMF loop: TUE@NTHT*OUNTH NTHEBUBOVNCFS@DPVOU

The first line creates an integer ROS message, and the second line assigns an integer value to the message. Here, EBUB is the field name of the NTH object: 304@*/'0 ENTHEBUB

This will print the message data. This line is used to log the ROS information: OVNCFS@QVCMJTIFSQVCMJTI NTH

This will publish the message to the topics OVNCFST: SPTTQJO0ODF 

This command will read and update all ROS topics. The node will not publish without a TQJO  or TQJO0ODF  function: MPPQ@SBUFTMFFQ 

This line will provide the necessary delay to achieve a frequency of 10 Hz. After discussing the publisher node, we can discuss the subscriber node, which is EFNP@UPQJD@TVCTDSJCFSDQQ. Copy the code to a new file or use the existing file. Here is the definition of the subscriber node: JODMVEFSPTSPTI JODMVEFTUE@NTHT*OUI JODMVEFJPTUSFBN WPJEOVNCFS@DBMMCBDL DPOTUTUE@NTHT*OU$POTU1USNTH\ 304@*/'0 3FDFJWFENTH EBUB ^ JOUNBJO JOUBSHDDIBS BSHW\ SPTJOJU BSHDBSHWEFNP@UPQJD@TVCTDSJCFS SPT/PEF)BOEMFOPEF@PCK SPT4VCTDSJCFSOVNCFS@TVCTDSJCFS

[ 40 ]

Getting Started with ROS Programming

Chapter 2

OPEF@PCKTVCTDSJCF OVNCFSTOVNCFS@DBMMCBDL SPTTQJO  SFUVSO ^

Here is the code explanation: JODMVEFSPTSPTI JODMVEFTUE@NTHT*OUI JODMVEFJPTUSFBN

This is the header needed for the subscribers: WPJEOVNCFS@DBMMCBDL DPOTUTUE@NTHT*OU$POTU1USNTH\ 304@*/'0 3FDJFWFENTH EBUB ^

This is a DBMMCBDL function that will execute whenever a data comes to the OVNCFST topic. Whenever a data reaches this topic, the function will call and extract the value and print it on the console: SPT4VCTDSJCFSOVNCFS@TVCTDSJCFS OPEF@PCKTVCTDSJCF OVNCFSTOVNCFS@DBMMCBDL

This is the subscriber, and here we are giving the topic name needed to subscribe, the buffer size, and the DBMMCBDL function. We are subscribing the OVNCFST topic and we have already seen the DBMMCBDL function in the preceding section: SPTTQJO 

This is an infinite loop in which the node will wait in this step. This code will fasten the callbacks whenever a data reaches the topic. The node will quit only when we press the Ctrl + C key.

Building the nodes We have to edit the $.BLF-JTUTUYU file in the package to compile and build the source code. Navigate to NBTUFSJOH@SPT@EFNP@QLH to view the existing $.BLF-JTUTUYU file. The following code snippet in this file is responsible for building these two nodes: JODMVEF@EJSFDUPSJFT

JODMVEF \DBULJO@*/$-6%&@%*34^ \#PPTU@*/$-6%&@%*34^ 

[ 41 ]

Getting Started with ROS Programming

Chapter 2

5IJTXJMMDSFBUFFYFDVUBCMFTPGUIFOPEFT BEE@FYFDVUBCMF EFNP@UPQJD@QVCMJTIFSTSDEFNP@UPQJD@QVCMJTIFSDQQ BEE@FYFDVUBCMF EFNP@UPQJD@TVCTDSJCFSTSDEFNP@UPQJD@TVCTDSJCFSDQQ 5IJTXJMMHFOFSBUFNFTTBHFIFBEFSGJMFCFGPSFCVJMEJOHUIFUBSHFU BEE@EFQFOEFODJFT EFNP@UPQJD@QVCMJTIFS NBTUFSJOH@SPT@EFNP@QLH@HFOFSBUF@NFTTBHFT@DQQ BEE@EFQFOEFODJFT EFNP@UPQJD@TVCTDSJCFS NBTUFSJOH@SPT@EFNP@QLH@HFOFSBUF@NFTTBHFT@DQQ 5IJTXJMMMJOLFYFDVUBCMFTUPUIFBQQSPQSJBUFMJCSBSJFT UBSHFU@MJOL@MJCSBSJFT EFNP@UPQJD@QVCMJTIFS\DBULJO@-*#3"3*&4^ UBSHFU@MJOL@MJCSBSJFT EFNP@UPQJD@TVCTDSJCFS\DBULJO@-*#3"3*&4^

We can add the preceding snippet to create a new a $.BLF-JTUTUYU file for compiling the two codes. The DBULJO@NBLF command is used to build the package. We can first switch to a workspace: $ cd ~/catkin_ws

Build NBTUFSJOH@SPT@EFNP@QBDLBHF as follows: $ catkin_make

We can either use the preceding command to build the entire workspace, or use the the  %$"5,*/@8)*5&-*45@1"$,"(&4 option. With this option, it is possible to set one or more packages to compile: $ catkin_make -DCATKIN_WHITELIST_PACKAGES="pkg1,pkg2,..."

Note that is necessary to revert this configuration to compile other packages or the entire workspace. This can be done using the following command: $ catkin_make -DCATKIN_WHITELIST_PACKAGES=""

If the building is done, we can execute the nodes. First, start SPTDPSF: $ roscore

Now run both commands in two shells. In the running publisher: $ rosrun mastering_ros_demo_package demo_topic_publisher

[ 42 ]

Getting Started with ROS Programming

Chapter 2

In the running subscriber: $ rosrun mastering_ros_demo_package demo_topic_subscriber

We can see the output as shown here:

(KIWTG4WPPKPIVQRKERWDNKUJGTCPFUWDUETKDGT

The following diagram shows how the nodes communicate with each other. We can see that the EFNP@UPQJD@QVCMJTIFS node publishes the OVNCFST topic and then subscribes to the EFNP@UPQJD@TVCTDSJCFS node:

(KIWTG)TCRJQHVJGEQOOWPKECVKQPDGVYGGPRWDNKUJGTCPFUWDUETKDGTPQFGU

We can use the SPTOPEF and SPTUPQJD tools to debug and understand the working of two nodes: SPTOPEFMJTU: This will list the active nodes. SPTOPEFJOGPEFNP@UPQJD@QVCMJTIFS: This will get the info of the

publisher node. SPTUPQJDFDIPOVNCFST: This will display the value sending through the OVNCFST topic. SPTUPQJDUZQFOVNCFST: This will print the message type of the OVNCFST topic.

[ 43 ]

Getting Started with ROS Programming

Chapter 2

Adding custom msg and srv files In this section, we will look at how to create custom messages and services definitions in the current package. The message definitions are stored in a NTH file and the service definitions are stored in a TSW file. These definitions inform ROS about the type of data and name of data to be transmitted from a ROS node. When a custom message is added, ROS will convert the definitions into equivalent C++ codes, which we can include in our nodes. We can start with message definitions. Message definitions have to be written in the NTH file and have to be kept in the NTH folder, which is inside the package. We are going to create a message file called EFNP@NTHNTH with the following definition: TUSJOHHSFFUJOH JOUOVNCFS

Until now, we have worked only with standard message definitions. Now, we have created our own definitions and can see how to use them in our code. The first step is to edit the QBDLBHFYNM file of the current package and uncomment the lines CVJME@EFQFOE NFTTBHF@HFOFSBUJPOCVJME@EFQFOE and FYFD@EFQFOE NFTTBHF@SVOUJNFFYFD@EFQFOE . Edit the current $.BLF-JTUTUYU and add the NFTTBHF@HFOFSBUJPO line, as follows: GJOE@QBDLBHF DBULJO3&26*3&%$0.10/&/54 SPTDQQ SPTQZ TUE@NTHT BDUJPOMJC BDUJPOMJC@NTHT NFTTBHF@HFOFSBUJPO 

[ 44 ]

Getting Started with ROS Programming

Chapter 2

Uncomment the following line and add the custom message file: BEE@NFTTBHF@GJMFT

'*-&4 EFNP@NTHNTH  (FOFSBUFBEEFENFTTBHFTBOETFSWJDFTXJUIBOZEFQFOEFODJFTMJTUFEIFSF HFOFSBUF@NFTTBHFT

%&1&/%&/$*&4 TUE@NTHT BDUJPOMJC@NTHT 

After these steps, we can compile and build the package: $ cd ~/catkin_ws/ $ catkin_make

To check whether the message is built properly, we can use the SPTNTH command: $ rosmsg show mastering_ros_demo_pkg/demo_msg

If the content shown by the command and the definition are the same, the procedure is correct. If we want to test the custom message, we can build a publisher and subscriber using the custom message type named EFNP@NTH@QVCMJTIFSDQQ and EFNP@NTH@TVCTDSJCFSDQQ. Navigate to the NBTUFSJOH@SPT@EFNP@QLH folder for these code. We can test the message by adding the following lines of code in $.BLF-JTUTUYU: BEE@FYFDVUBCMF EFNP@NTH@QVCMJTIFSTSDEFNP@NTH@QVCMJTIFSDQQ BEE@FYFDVUBCMF EFNP@NTH@TVCTDSJCFSTSDEFNP@NTH@TVCTDSJCFSDQQ BEE@EFQFOEFODJFT EFNP@NTH@QVCMJTIFS NBTUFSJOH@SPT@EFNP@QLH@HFOFSBUF@NFTTBHFT@DQQ BEE@EFQFOEFODJFT EFNP@NTH@TVCTDSJCFS NBTUFSJOH@SPT@EFNP@QLH@HFOFSBUF@NFTTBHFT@DQQ UBSHFU@MJOL@MJCSBSJFT EFNP@NTH@QVCMJTIFS\DBULJO@-*#3"3*&4^ UBSHFU@MJOL@MJCSBSJFT EFNP@NTH@TVCTDSJCFS\DBULJO@-*#3"3*&4^

[ 45 ]

Getting Started with ROS Programming

Chapter 2

Build the package using DBULJO@NBLF and test the node using the following commands. Run SPTDPSF: $ roscore

Start the custom message publisher node: $ rosrun mastering_ros_demo_pkg demo_msg_publisher

Start the custom message subscriber node: $ rosrun mastering_ros_demo_pkg demo_msg_subscriber

The publisher node publishes a string along with an integer, and the subscriber node subscribes the topic and prints the values. The output and graph are shown as follows:

(KIWTG4WPPKPIRWDNKUJGTCPFUWDUETKDGTWUKPIEWUVQOOGUUCIGFGaPKVKQPU

The topic in which the nodes are communicating is called EFNP@NTH@UPQJD. Here is the graph view of the two nodes:

(KIWTG)TCRJQHVJGEQOOWPKECVKQPDGVYGGPOGUUCIGRWDNKUJGTCPFUWDUETKDGT

Next, we can add TSW files to the package. Create a new folder called TSW in the current package folder and add a TSW file called EFNP@TSWTSW. The definition of this file is as follows: TUSJOHJO  TUSJOHPVU

[ 46 ]

Getting Started with ROS Programming

Chapter 2

Here, both the 3FRVFTU and 3FTQPOTF are strings. In the next step, we need to uncomment the following lines in QBDLBHFYNM as we did for the ROS messages: CVJME@EFQFOE NFTTBHF@HFOFSBUJPOCVJME@EFQFOE FYFD@EFQFOE NFTTBHF@SVOUJNFFYFD@EFQFOE

Take $.BLF-JTUTUYU and add NFTTBHF@SVOUJNF in DBULJO@QBDLBHF : DBULJO@QBDLBHF

$"5,*/@%&1&/%4SPTDQQSPTQZTUE@NTHTBDUJPOMJCBDUJPOMJC@NTHT NFTTBHF@SVOUJNF 

We need to follow the same procedure in generating services as we did for the ROS message. Apart from that, we need additional sections to be uncommented, as shown here: (FOFSBUFTFSWJDFTJOUIF TSW GPMEFS BEE@TFSWJDF@GJMFT

'*-&4 EFNP@TSWTSW 

After making these changes, we can build the package using DBULJO@NBLF and using the following command, we can verify the procedure: $ rossrv show mastering_ros_demo_pkg/demo_srv

If we see the same content as we defined in the file, we can confirm it's working.

Working with ROS services In this section, we are going to create ROS nodes, which can use the services definition that we defined already. The service nodes we are going to create can send a string message as a request to the server and the server node will send another message as a response. Navigate to NBTUFSJOH@SPT@EFNP@QLHTSD, and find nodes with the names EFNP@TFSWJDF@TFSWFSDQQ and EFNP@TFSWJDF@DMJFOUDQQ.

[ 47 ]

Getting Started with ROS Programming

Chapter 2

The EFNP@TFSWJDF@TFSWFSDQQ is the server, and its definition is as follows: JODMVEFSPTSPTI JODMVEFNBTUFSJOH@SPT@EFNP@QLHEFNP@TSWI JODMVEFJPTUSFBN JODMVEFTTUSFBN VTJOHOBNFTQBDFTUE CPPMEFNP@TFSWJDF@DBMMCBDL NBTUFSJOH@SPT@EFNP@QLHEFNP@TSW3FRVFTUSFR NBTUFSJOH@SPT@EFNP@QLHEFNP@TSW3FTQPOTFSFT\ TUETUSJOHTUSFBNTT TT3FDFJWFE)FSF SFTPVUTTTUS  304@*/'0 'SPN$MJFOU4FSWFSTBZT SFRJOD@TUS SFTPVUD@TUS  SFUVSOUSVF ^ JOUNBJO JOUBSHDDIBS BSHW \ SPTJOJU BSHDBSHWEFNP@TFSWJDF@TFSWFS SPT/PEF)BOEMFO SPT4FSWJDF4FSWFSTFSWJDFOBEWFSUJTF4FSWJDF EFNP@TFSWJDF EFNP@TFSWJDF@DBMMCBDL 304@*/'0 3FBEZUPSFDFJWFGSPNDMJFOU SPTTQJO  SFUVSO ^

Let's see an explanation of the code: JODMVEFSPTSPTI JODMVEFNBTUFSJOH@SPT@EFNP@QLHEFNP@TSWI JODMVEFJPTUSFBN JODMVEFTTUSFBN

Here, we included SPTSPTI, which is a mandatory header for a ROS CPP node. The NBTUFSJOH@SPT@EFNP@QLHEFNP@TSWI header is a generated header, which contains our service definition and we can use this in our code. The TTUSFBNI is for getting string streaming classes: CPPMEFNP@TFSWJDF@DBMMCBDL NBTUFSJOH@SPT@EFNP@QLHEFNP@TSW3FRVFTUSFR NBTUFSJOH@SPT@EFNP@QLHEFNP@TSW3FTQPOTFSFT \

[ 48 ]

Getting Started with ROS Programming

Chapter 2

This is the server callback function executed when a request is received on the server. The server can receive the request from clients with a message type of NBTUFSJOH@SPT@EFNP@QLHEFNP@TSW3FRVFTU and sends the response in the NBTUFSJOH@SPT@EFNP@QLHEFNP@TSW3FTQPOTF type: TUETUSJOHTUSFBNTT TT3FDFJWFE)FSF SFTPVUTTTUS 

In this code, the string data 3FDFJWFE)FSF is passing to the service 3FTQPOTF instance. Here, PVU is the field name of the response that we have given in EFNP@TSWTSW. This response will go to the service client node: SPT4FSWJDF4FSWFSTFSWJDFOBEWFSUJTF4FSWJDF EFNP@TFSWJDF EFNP@TFSWJDF@DBMMCBDL

This creates a service called EFNP@TFSWJDF and a callback function is executed when a request comes to this service. The callback function is EFNP@TFSWJDF@DBMMCBDL, which we saw in the preceding section. Next, we can see how EFNP@TFSWJDF@DMJFOUDQQ is working. Here is the definition of this code: JODMVEFSPTSPTI JODMVEFJPTUSFBN JODMVEFNBTUFSJOH@SPT@EFNP@QLHEFNP@TSWI JODMVEFJPTUSFBN JODMVEFTTUSFBN VTJOHOBNFTQBDFTUE JOUNBJO JOUBSHDDIBS BSHW \ SPTJOJU BSHDBSHWEFNP@TFSWJDF@DMJFOU SPT/PEF)BOEMFO SPT3BUFMPPQ@SBUF  SPT4FSWJDF$MJFOUDMJFOU OTFSWJDF$MJFOUNBTUFSJOH@SPT@EFNP@QLHEFNP@TSW EFNP@TFSWJDF XIJMF SPTPL  \ NBTUFSJOH@SPT@EFNP@QLHEFNP@TSWTSW TUETUSJOHTUSFBNTT TT4FOEJOHGSPN)FSF TSWSFRVFTUJOTTTUS  JG DMJFOUDBMM TSW \

[ 49 ]

Getting Started with ROS Programming

Chapter 2

304@*/'0 'SPN$MJFOU4FSWFSTBZT TSWSFRVFTUJOD@TUS TSWSFTQPOTFPVUD@TUS  ^ FMTF \ 304@&3303 'BJMFEUPDBMMTFSWJDF SFUVSO ^ SPTTQJO0ODF  MPPQ@SBUFTMFFQ  ^ SFUVSO ^

Let's explain the code: SPT4FSWJDF$MJFOUDMJFOU OTFSWJDF$MJFOUNBTUFSJOH@SPT@EFNP@QLHEFNP@TSW EFNP@TFSWJDF

This line creates a service client that has the message type NBTUFSJOH@SPT@EFNP@QLHEFNP@TSW and communicates to a ROS service named EFNP@TFSWJDF: NBTUFSJOH@SPT@EFNP@QLHEFNP@TSWTSW

This line will create a new service object instance: TUETUSJOHTUSFBNTT TT4FOEJOHGSPN)FSF TSWSFRVFTUJOTTTUS 

Fill the request instance with a string called 4FOEJOHGSPN)FSF: JG DMJFOUDBMM TSW \

This will send the service call to the server. If it is sent successfully, it will print the response and request; if it failed, it will do nothing: 304@*/'0 'SPN$MJFOU4FSWFSTBZT TSWSFRVFTUJOD@TUS TSWSFTQPOTFPVUD@TUS 

[ 50 ]

Getting Started with ROS Programming

Chapter 2

If the response is received, then it will print the request and the response. After discussing the two nodes, we can discuss how to build these two nodes. The following code is added to $.BLF-JTUTUYU to compile and build the two nodes: BEE@FYFDVUBCMF EFNP@TFSWJDF@TFSWFSTSDEFNP@TFSWJDF@TFSWFSDQQ BEE@FYFDVUBCMF EFNP@TFSWJDF@DMJFOUTSDEFNP@TFSWJDF@DMJFOUDQQ BEE@EFQFOEFODJFT EFNP@TFSWJDF@TFSWFS NBTUFSJOH@SPT@EFNP@QLH@HFOFSBUF@NFTTBHFT@DQQ BEE@EFQFOEFODJFT EFNP@TFSWJDF@DMJFOU NBTUFSJOH@SPT@EFNP@QLH@HFOFSBUF@NFTTBHFT@DQQ UBSHFU@MJOL@MJCSBSJFT EFNP@TFSWJDF@TFSWFS\DBULJO@-*#3"3*&4^ UBSHFU@MJOL@MJCSBSJFT EFNP@TFSWJDF@DMJFOU\DBULJO@-*#3"3*&4^

We can execute the following commands to build the code: $ cd ~/catkin_ws $ catkin_make

To start the nodes, first execute SPTDPSF and use the following commands: $ rosrun mastering_ros_demo_pkg demo_service_server $ rosrun mastering_ros_demo_pkg demo_service_client

(KIWTG4WPPKPI415UGTXKEGENKGPVCPFUGTXGTPQFGU

[ 51 ]

Getting Started with ROS Programming

Chapter 2

We can work with SPTTFSWJDF using the SPTTFSWJDF command: SPTTFSWJDFMJTU: This will list the current ROS services SPTTFSWJDFUZQFEFNP@TFSWJDF: This will print the message type of EFNP@TFSWJDF SPTTFSWJDFJOGPEFNP@TFSWJDF: This will print the information of EFNP@TFSWJDF

Working with ROS actionlib In ROS services, the user implements a request/reply interaction between two nodes, but if the reply takes too much time or the server is not finished with the given work, we have to wait until it completes, blocking the main application while waiting for the termination of the requested action. In addition, the calling client could be implemented to monitor the execution of the remote process. In these cases, we should implement our application using BDUJPOMJC. This is another method in ROS in which we can preempt the running request and start sending another one if the request is not finished on time as we expected. Actionlib packages provide a standard way to implement these kinds of preemptive tasks. Actionlib is highly used in robot arm navigation and mobile robot navigation. We can see how to implement an action server and action client implementation. There is another method in ROS in which we can preempt the running request and start sending another one if the request is not finished on time as we expected. Actionlib packages provide a standard way to implement these kinds of preemptive tasks. Actionlib is highly used in robot arm navigation and mobile robot navigation. We can see how to implement an action server and action client implementation. Like ROS services, in BDUJPOMJC, we have to specify the action specification. The action specification is stored inside the action file having an extension of BDUJPO. This file must be kept inside the BDUJPO folder, which is inside the ROS package. The BDUJPO file has the following parts: Goal: The action client can send a goal that has to be executed by the action server. This is similar to the request in the ROS service. For example, if a robot arm joint wants to move from 45 degrees to 90 degrees, the goal here is 90 degrees.

[ 52 ]

Getting Started with ROS Programming

Chapter 2

Feedback: When an action client sends a goal to the action server, it will start executing a callback function. Feedback is simply giving the progress of the current operation inside the callback function. Using the feedback definition, we can get the current progress. In the preceding case, the robot arm joint has to move to 90 degrees; in this case, the feedback can be the intermediate value between 45 and 90 degrees in which the arm is moving. Result: After completing the goal, the action server will send a final result of completion, it can be the computational result or an acknowledgment. In the preceding example, if the joint reaches 90 degrees it achieves the goal and the result can be anything indicating it finished the goal. We can discuss a demo action server and action client here. The demo action client will send a number as the goal. When an action server receives the goal, it will count from 0 to the goal number with a step size of 1 and with a 1 second delay. If it completes before the given time, it will send the result; otherwise, the task will be preempted by the client. The feedback here is the progress of counting. The action file of this task is as follows. The action file is named %FNP@BDUJPOBDUJPO: HPBMEFGJOJUJPO JOUDPVOU  SFTVMUEFGJOJUJPO JOUGJOBM@DPVOU  GFFECBDL JOUDVSSFOU@OVNCFS

Here, the count value is the goal in which the server has to count from zero to this number. GJOBM@DPVOU is the result, in which the final value after completion of a task and DVSSFOU@OVNCFS is the feedback value. It will specify how much the progress is. Navigate to NBTUFSJOH@SPT@EFNP@QLHTSD and you can find the action server node as EFNP@BDUJPO@TFSWFSDQQ and action client node as EFNP@BDUJPO@DMJFOUDQQ.

Creating the ROS action server In this section, we will discuss EFNP@BDUJPO@TFSWFSDQQ. The action server receives a goal value that is a number. When the server gets this goal value, it will start counting from zero to this number. If the counting is complete, it will successfully finish the action, if it is preempted before finishing, the action server will look for another goal value.

[ 53 ]

Getting Started with ROS Programming

Chapter 2

This code is a bit lengthy, so we can discuss the important code snippet of this code. Let's start with the header files: JODMVEFBDUJPOMJCTFSWFSTJNQMF@BDUJPO@TFSWFSI JODMVEFNBTUFSJOH@SPT@EFNP@QLH%FNP@BDUJPO"DUJPOI

The first header is the standard action library to implement an action server node. The second header is generated from the stored action files. It should include accessing our action definition: DMBTT%FNP@BDUJPO"DUJPO \

This class contains the action server definition: BDUJPOMJC4JNQMF"DUJPO4FSWFSNBTUFSJOH@SPT@EFNP@QLH%FNP@BDUJPO"DUJPO BT

Create a simple action server instance with our custom action message type: NBTUFSJOH@SPT@EFNP@QLH%FNP@BDUJPO'FFECBDLGFFECBDL

Create a feedback instance for sending feedback during the operation: NBTUFSJOH@SPT@EFNP@QLH%FNP@BDUJPO3FTVMUSFTVMU

Create a result instance for sending the final result: %FNP@BDUJPO"DUJPO TUETUSJOHOBNF BT OI@OBNFCPPTUCJOE %FNP@BDUJPO"DUJPOFYFDVUF$#UIJT@ GBMTF BDUJPO@OBNF OBNF

This is an action constructor, and an action server is created here by taking an argument such as /PEFIBOEMF, BDUJPO@OBNF, and FYFDVUF$#, where FYFDVUF$# is the action callback where all the processing is done: BTSFHJTUFS1SFFNQU$BMMCBDL CPPTUCJOE %FNP@BDUJPO"DUJPOQSFFNQU$# UIJT

This line registers a callback when the action is preempted. The QSFFNU$# is the callback name executed when there is a preempt request from the action client: WPJEFYFDVUF$# DPOTUNBTUFSJOH@SPT@EFNP@QLH%FNP@BDUJPO(PBM$POTU1US HPBM \ JG BTJT"DUJWF ]]BTJT1SFFNQU3FRVFTUFE SFUVSO

[ 54 ]

Getting Started with ROS Programming

Chapter 2

This is the callback definition which is executed when the action server receives a HPBM value. It will execute callback functions only after checking whether the action server is currently active or it is preempted already: GPS QSPHSFTTQSPHSFTTHPBM DPVOUQSPHSFTT

\ $IFDLGPSSPT JG SPTPL \

This loop will execute until the goal value is reached. It will continuously send the current progress as feedback: JG BTJT"DUJWF ]]BTJT1SFFNQU3FRVFTUFE \ SFUVSO ^

Inside this loop, it will check whether the action server is active or it is preempted. If it occurs, the function will return: JG HPBM DPVOUQSPHSFTT\ SFTVMUGJOBM@DPVOUQSPHSFTT BTTFU4VDDFFEFE SFTVMU ^

If the current value reaches the goal value, then it publishes the final result: %FNP@BDUJPO"DUJPOEFNP@BDUJPO@PCK SPTUIJT@OPEFHFU/BNF 

In NBJO ,we create an instance of %FNP@BDUJPO"DUJPO, which will start the action server.

Creating the ROS action client In this section, we will discuss the workings of an action client. EFNP@BDUJPO@DMJFOUDQQ is the action client node that will send the goal value consisting of a number which is the goal. The client is getting the goal value from the command-line arguments. The first command-line argument of the client is the goal value, and the second is the time of completion for this task. The goal value will be sent to the server and the client will wait until the given time, in seconds. After waiting, the client will check whether it completed or not; if it is not complete, the client will preempt the action.

[ 55 ]

Getting Started with ROS Programming

Chapter 2

The client code is a bit lengthy, so we will discuss the important sections of the code: JODMVEFBDUJPOMJCDMJFOUTJNQMF@BDUJPO@DMJFOUI JODMVEFBDUJPOMJCDMJFOUUFSNJOBM@TUBUFI JODMVEFNBTUFSJOH@SPT@EFNP@QLH%FNP@BDUJPO"DUJPOI

In the action client, we need to include BDUJPOMJCDMJFOUTJNQMF@BDUJPO@DMJFOUI to get the action client APIs, which are used to implement action clients: BDUJPOMJC4JNQMF"DUJPO$MJFOUNBTUFSJOH@SPT@EFNP@QLH%FNP@BDUJPO"DUJPO BD EFNP@BDUJPOUSVF

This will create an action client instance: BDXBJU'PS4FSWFS 

This line will wait for an infinite time if there is no action server running on the system. It will exit only when there is an action server running on the system: NBTUFSJOH@SPT@EFNP@QLH%FNP@BDUJPO(PBMHPBM HPBMDPVOUBUPJ BSHW BDTFOE(PBM HPBM

Create an instance of a goal, and send the goal value from the first command line argument: CPPMGJOJTIFE@CFGPSF@UJNFPVU BDXBJU'PS3FTVMU SPT%VSBUJPO BUPJ BSHW

This line will wait for the result from the server until the given seconds: BDDBODFM(PBM 

If it is not finished, it will preempt the action.

Building the ROS action server and client After creating these two files in the TSD folder, we have to edit the QBDLBHFYNM and $.BLF-JTUTUYU to build the nodes.

[ 56 ]

Getting Started with ROS Programming

Chapter 2

The QBDLBHFYNM file should contain message generation and runtime packages, as we did for ROS service and messages. We have to include the #PPTU library in $.BLF-JTUTUYU to build these nodes. Also, we have to add the action files that we wrote for this example: GJOE@QBDLBHF DBULJO3&26*3&%$0.10/&/54 SPTDQQ SPTQZ TUE@NTHT BDUJPOMJC BDUJPOMJC@NTHT NFTTBHF@HFOFSBUJPO 

We should pass BDUJPOMJC, BDUJPOMJC@NTHT, and NFTTBHF@HFOFSBUJPO in GJOE@QBDLBHF : 4ZTUFNEFQFOEFODJFTBSFGPVOEXJUI$.BLF TDPOWFOUJPOT GJOE@QBDLBHF #PPTU3&26*3&%$0.10/&/54TZTUFN

We should add #PPTU as a system dependency: (FOFSBUFBDUJPOTJOUIF BDUJPO GPMEFS BEE@BDUJPO@GJMFT

'*-&4 %FNP@BDUJPOBDUJPO 

We need to add our action file in BEE@BDUJPO@GJMFT : (FOFSBUFBEEFENFTTBHFTBOETFSWJDFTXJUIBOZEFQFOEFODJFTMJTUFEIFSF HFOFSBUF@NFTTBHFT

%&1&/%&/$*&4 TUE@NTHT BDUJPOMJC@NTHT 

[ 57 ]

Getting Started with ROS Programming

Chapter 2

We have to add BDUJPOMJC@NTHT in HFOFSBUF@NFTTBHFT : DBULJO@QBDLBHF

$"5,*/@%&1&/%4SPTDQQSPTQZTUE@NTHTBDUJPOMJCBDUJPOMJC@NTHT NFTTBHF@SVOUJNF  JODMVEF@EJSFDUPSJFT

JODMVEF \DBULJO@*/$-6%&@%*34^ \#PPTU@*/$-6%&@%*34^ 

We have to add #PPTU to include the directory: #VJMEJOHBDUJPOTFSWFSBOEBDUJPODMJFOU BEE@FYFDVUBCMF EFNP@BDUJPO@TFSWFSTSDEFNP@BDUJPO@TFSWFSDQQ BEE@FYFDVUBCMF EFNP@BDUJPO@DMJFOUTSDEFNP@BDUJPO@DMJFOUDQQ BEE@EFQFOEFODJFT EFNP@BDUJPO@TFSWFS NBTUFSJOH@SPT@EFNP@QLH@HFOFSBUF@NFTTBHFT@DQQ BEE@EFQFOEFODJFT EFNP@BDUJPO@DMJFOU NBTUFSJOH@SPT@EFNP@QLH@HFOFSBUF@NFTTBHFT@DQQ UBSHFU@MJOL@MJCSBSJFT EFNP@BDUJPO@TFSWFS\DBULJO@-*#3"3*&4^ UBSHFU@MJOL@MJCSBSJFT EFNP@BDUJPO@DMJFOU\DBULJO@-*#3"3*&4^

After DBULJO@NBLF, we can run these nodes using the following commands: Run SPTDPSF: $ roscore

Launch the action server node: $rosrun mastering_ros_demo_pkg demo_action_server

Launch the action client node: $rosrun mastering_ros_demo_pkg demo_action_client 10 1

[ 58 ]

Getting Started with ROS Programming

Chapter 2

The output of these process is shown as follows:

(KIWTG4WPPKPI415CEVKQPNKDUGTXGTCPFENKGPV

Creating launch files The launch files in ROS are a very useful feature for launching more than one node. In the preceding examples, we have seen a maximum of two ROS nodes, but imagine a scenario in which we have to launch 10 or 20 nodes for a robot. It will be difficult if we run each node in a terminal one by one. Instead, we can write all nodes inside an XML-based file called launch files and, using a command called roslaunch, we can parse this file and launch the nodes. The SPTMBVODI command will automatically start the ROS Master and the parameter server. So, in essence, there is no need to start the SPTDPSF command and individual node; if we launch the file, all operations will be done in a single command.

[ 59 ]

Getting Started with ROS Programming

Chapter 2

Let's start creating launch files. Switch to the package folder and create a new launch file called EFNP@UPQJDMBVODI to launch two ROS nodes that are publishing and subscribing an integer value. We keep the launch files in a MBVODI folder, which is inside the package: $ $ $ $

roscd mastering_ros_demo_pkg mkdir launch cd launch gedit demo_topic.launch

Paste the following content into the file: MBVODI OPEFOBNFQVCMJTIFS@OPEFQLHNBTUFSJOH@SPT@EFNP@QLH UZQFEFNP@UPQJD@QVCMJTIFSPVUQVUTDSFFO OPEFOBNFTVCTDSJCFS@OPEFQLHNBTUFSJOH@SPT@EFNP@QLH UZQFEFNP@UPQJD@TVCTDSJCFSPVUQVUTDSFFO MBVODI

Let's discuss what is in the code. The MBVODI MBVODI tags are the root element in a MBVODI file. All definitions will be inside these tags. The OPEF tag specifies the desired node to launch: OPEFOBNFQVCMJTIFS@OPEFQLHNBTUFSJOH@SPT@EFNP@QLH UZQFEFNP@UPQJD@QVCMJTIFSPVUQVUTDSFFO

The OBNF tag inside OPEF indicates the name of the node, QLH is the name of the package, and UZQF is the name of executable we are going to launch. After creating the launch file EFNP@UPQJDMBVODI, we can launch it using the following command: $ roslaunch mastering_ros_demo_pkg demo_topic.launch

[ 60 ]

Getting Started with ROS Programming

Chapter 2

Here is the output we get if the launch is successful:

EFNP@UPQJDNCWPEJaNG

(KIWTG6GTOKPCNOGUUCIGUYJKNGNCWPEJKPIVJG

We can check the list of nodes using: $ rosnode list

We can also view the log messages and debug the nodes using a GUI tool called SRU@DPOTPMF: $ rqt_console

We can see the logs generated by two nodes in this tool, as shown here:

(KIWTG.QIIKPIWUKPIVJG

SRU@DPOTPMFVQQN

[ 61 ]

Getting Started with ROS Programming

Chapter 2

Applications of topics, services, and actionlib Topics, services, and actionlib are used in different scenarios. We know topics are a unidirectional communication method, services are a bidirectional request/reply kind of communication, and actionlib is a modified form of ROS services in which we can cancel the executing process running on the server whenever required. Here are some of the areas where we use these methods: Topics: Streaming continuous data flow, such as sensor data. For example, stream joypad data to teleoperate a robot, publish robot odometry, publish video stream from a camera. Services: Executing procedures that terminate quickly. For example, save calibration parameter of sensors, save a map generated by the robot during its navigation, or load a parameter file. Actionlib: Execute long and complex actions managing their feedback. For example, navigate towards a target or plan a motion path. The complete source code of this project can be cloned from the following Git repository. The following command will clone the project repository: $ git clone https://github.com/jocacace/mastering_ros_demo_pkg.git

Maintaining the ROS package Most ROS packages are maintained using a Version Control System (VCS) such as Git, Subversion (svn), Mercurial (hg), and so on. A collection of packages that share a common VCS can be called a repository. The package in the repository can be released using a catkin release automation tool called bloom. Most ROS packages are released as open source with the BSD license. There are active developers around the globe who are contributing to the ROS platform. Maintaining packages is important for all software, especially open source applications. Open source software is maintained and supported by a community of developers. Creating a version control system for our package is essential if we want to maintain and accept a contribution from other developers. The preceding package is already updated in GitHub, and you can view the source code of the project at IUUQTHJUIVCDPNKPDBDBDFNBTUFSJOH@SPT@EFNP@QLH

[ 62 ]

Getting Started with ROS Programming

Chapter 2

Releasing your ROS package After creating a ROS package in GitHub, we can officially release our package. ROS provides detailed steps to release the ROS package using a tool called bloom (IUUQSPTJOGSBTUSVDUVSFHJUIVCJPCMPPN). Bloom is a release automation tool, designed to make platform-specific releases from the source projects. Bloom is designed to work best with the catkin project. The prerequisites for releasing the package are as follows: Install the Bloom tool Create a Git repository for the current package Create an empty Git repository for the release The following command will install bloom in Ubuntu: $ sudo apt-get install python-bloom

Create a Git repository for the current package. The repository that has the package is called the upstream repository. Here, we already created a repository at IUUQTHJUIVCDPNKPDBDBDFNBTUFSJOH@SPT@EFNP@QLH. Create an empty repository in Git for the release package. This repository is called the SFMFBTF repository. We have created a package called EFNP@QLHSFMFBTF. After meeting these prerequisites, we can start to create the release of the package. Navigate to the NBTUFSJOH@SPT@EFNP@QLH local repository where we push our package code to Git. Open a terminal inside this local repository and execute the following command: $ catkin_generate_changelog

The purpose of this command is to create a $)"/(&-0(STU file inside the local repository. After executing this command, it will show this option: Continue without -all option [y/N]. Give Z here It will create a $)"/(&-0(STU in the local repository. After the creation of the log file, we can update the Git repository by committing the changes: $ git add -A $ git commit -m 'Updated CHANGELOG.rst' $ git push -u origin master

[ 63 ]

Getting Started with ROS Programming

Chapter 2

Preparing the ROS package for the release In this step, we will check whether the package contains change logs, versions, and so on. The following command makes our package consistent and recommended for release. This command should execute from the local repository of the package: $ catkin_prepare_release

The command will set a version tag if there is no current version, and commit the changes in the upstream repository.

Releasing our package The following command starts the release. The syntax of this command is as follows: bloom-release --rosdistro --track repository_name $ bloom-release --rosdistro kinetic --track kinetic mastering_ros_demo_pkg

When this command is executed, it will go to the SPTEJTUSP (IUUQTHJUIVCDPNSPTSPTEJTUSP) package repository to get the package details. The SPTEJTUSP package in ROS contains an index file, which contains a list of all the packages in ROS. Currently, there is no index for our package because this is our first release, but we can add our package details to this index file called EJTUSJCVUJPOTZBNM. The following message will be displayed when there is no reference of the package in SPTEJTUSP:

[ 64 ]

Getting Started with ROS Programming

Chapter 2

(KIWTG6GTOKPCNOGUUCIGUYJGPVJGTGKUPQTGHGTGPEGQHVJGRCEMCIGKP

SPTEJTUSP

We should give the release repository in the terminal that is marked in red in the preceding screenshot. In this case, the URL was IUUQTHJUIVCDPNKPDBDBDFEFNP@QLHSFMFBTF:

(KIWTG+PRWVVKPIVJGTGNGCUGTGRQUKVQT[74.

[ 65 ]

Getting Started with ROS Programming

Chapter 2

In the upcoming steps, the wizard will ask for the repository name, upstream, URL, and so on. We can give these options and, finally, a pull request to SPTEJTUSP will be submitted, which is shown in the following screenshot:

(KIWTG5GPFKPICRWNNTGSWGUVVQTQUFKUVTQ

The QVMM request for this package can be viewed at IUUQTHJUIVCDPNSPTSPTEJTUSPQVMM. If it is accepted, it will merge to LJOFUJDEJTUSJCVUJPOZBNM, which contains the index of all packages in ROS.

[ 66 ]

Getting Started with ROS Programming

Chapter 2

The following screenshot displays the package as an index in LJOFUJDEJTUSJCVUJPOZBNM:

(KIWTG6JGFKUVTKDWVKQP[CONaNGQH415MKPGVKE

After this step, we can confirm that the package is released and officially added to the ROS index.

Creating a Wiki page for your ROS package ROS wiki allows users to create their own home pages to showcase their package, robot, or sensors. The official wiki page of ROS is XJLJSPTPSH. Now, we are going to create a wiki page for our package.

[ 67 ]

Getting Started with ROS Programming

Chapter 2

Downloading the example code: You can download the example code files from your account at: IUUQXXXQBDLUQVCDPN for all the Packt Publishing books you have purchased. If you purchased this book elsewhere, you can visit: IUUQXXXQBDLUQVCDPNTVQQPSU and register to have the files emailed directly to you. You can also download chapter codes from: IUUQTHJUIVCDPNKPDBDBDFNBTUFSJOH@SPT@OE@FEHJU this code repository contains the link to all other code repositories used in this book. The first step is to register in wiki using your e-mail address. Go to XJLJSPTPSH, and click on the Login button, as shown in the screenshot:

(KIWTG.QECVKPIVJGNQIKPQRVKQPHTQO415YKMK

[ 68 ]

Getting Started with ROS Programming

Chapter 2

After clicking on Login, you can register or directly log in with your details if you are already registered. After Login, press the username link on the right side of the wiki page, as shown in the following screenshot:

(KIWTG.QECVKPIVJGWUGTCEEQWPVDWVVQPHTQO415YKMK

[ 69 ]

Getting Started with ROS Programming

Chapter 2

After clicking on this link, you will get a chance to create a home page for your package; you will get a text editor with GUI to enter data into. The following screenshot shows you the page we created for this demo package:

(KIWTG%TGCVKPICPGYYKMKRCIG

Questions After going through the chapter, you should now be able to answer the following questions: Which kind of communication protocols between nodes are supported by ROS? What is the difference between SPTSVO and SPTMBVODI commands? How do ROS topics and services differ in their operations? How do ROS services and BDUJPOMJC differ in their operations?

[ 70 ]

Getting Started with ROS Programming

Chapter 2

Summary In this chapter, we provided different examples of ROS nodes in which ROS features such as ROS topics, services, and actions were implemented. We discussed how to create and compile ROS packages using custom and standard messages. After demonstrating the workings of each concept, we uploaded the package to GitHub and created a wiki page for the package. In the next chapter, we will discuss ROS robot modeling using URDF and YBDSP, and will design some robot models.

[ 71 ]

3

Working with 3D Robot Modeling in ROS The first phase of robot manufacturing is design and modeling. We can design and model a robot using CAD tools such as AutoCAD, SOLIDWORKS, and Blender. One of the main purposes of robot modeling is simulation. The robotic simulation tool can check for critical flaws in the robot's design and can confirm that the robot will work before it goes to the manufacturing phase. The virtual robot model must have all the characteristics of the real hardware. The shape of a robot may or may not look like the actual robot, but it must be abstract, which has all the physical characteristics of the actual robot. In this chapter, we are going to discuss the designing of two robots. One is a seven Degrees of Freedom (DOF) manipulator, and the other is a differential drive robot. In the upcoming chapters, we will look at simulation, how to build the real hardware, and interfacing with ROS. If we are planning to create the 3D model of the robot and simulate it using ROS, you need to learn about some ROS packages that help in robot designing. Creating a model for our robot in ROS is important for different reasons. For example, we can use this model to simulate and control the robot, visualize it, or use ROS tools to get information on the robotic structure and its kinematics. ROS has a standard meta package for designing and creating robot models called SPCPU@NPEFM, which consists of a set of packages, some of which are called VSEG, LEM@QBSTFS, SPCPU@TUBUF@QVCMJTIFS, and DPMMBEB@VSEG. These packages help us create the 3D robot model description with the exact characteristics of the real hardware.

Working with 3D Robot Modeling in ROS

Chapter 3

In this chapter, we will cover the following topics: ROS packages for robot modeling Creating the ROS package for the robot description Understanding robot modeling using URDF Understanding robot modeling using xacro Converting xacro to URDF Creating a robot description for a seven DOF robot manipulator Working with the joint state publisher and robot state publisher Creating robot description for a differential wheeled robot

ROS packages for robot modeling ROS provides some good packages that can be used to build 3D robot models. In this section, we will discuss some of the important ROS packages that are commonly used to build and model a robot: VSEG: The most important ROS package to model the robot is the VSEG package.

This package contains a C++ parser for the Unified Robot Description Format (URDF), which is an XML file representing a robot model. Other different components make up VSEG: VSEG@QBSTFS@QMVHJO: This package implements methods to fill URDF data structures VSEGEPN@IFBEFST: This component provides core data structure headers to use the urdf parser DPMMBEB@QBSTFS: This package populates data structures by parsing a Collada file VSEGEPN: This component populates data structures by parsing URDF files DPMMBEBEPN: This is a stand-alone component to convert Collada documents with 3D computer graphics software such as Maya, Blender, and Soft image

[ 73 ]

Working with 3D Robot Modeling in ROS

Chapter 3

We can define a robot model, sensors, and a working environment using URDF, and we can parse it using URDF parsers. We can only describe a robot in URDF that has a tree-like structure in its links, that is, the robot will have rigid links and will be connected using joints. Flexible links can't be represented using URDF. The URDF is composed using special XML tags, and we can parse these XML tags using parser programs for further processing. We can work on URDF modeling in the upcoming sections: KPJOU@TUBUF@QVCMJTIFS: This tool is very useful when designing robot models using URDF. This package contains a node called KPJOU@TUBUF@QVCMJTIFS,

which reads the robot model description, finds all joints, and publishes joint values to all nonfixed joints using GUI sliders. The user can interact with each robot joint using this tool and can visualize using RViz. While designing URDF, the user can verify the rotation and translation of each joint using this tool. We will talk more about the KPJOU@TUBUF@QVCMJTIFS node and its usage in the upcoming section. LEM@QBSTFS: Kinematic and Dynamics Library (KDL) is an ROS package that contains parser tools to build a KDL tree from the URDF representation. The kinematic tree can be used to publish the joint states and also to forward and inverse the kinematics of the robot. SPCPU@TUBUF@QVCMJTIFS: This package reads the current robot joint states and publishes the 3D poses of each robot link using the kinematics tree build from the URDF. The 3D pose of the robot is published as the UG (transform) ROS. The UG ROS publishes the relationship between the coordinates frames of a robot. YBDSP: Xacro stands for (XML Macros), and we can define how YBDSP is equal to URDF plus add-ons. It contains some add-ons to make URDF shorter and readable, and can be used for building complex robot descriptions. We can convert YBDSP to URDF at any time using ROS tools. We will learn more about YBDSP and its usage in the upcoming sections.

Understanding robot modeling using URDF We have discussed the VSEG package. In this section, we will look further into the 63%' 9.- tags, which help to model the robot. We have to create a file and write the relationship between each link and joint in the robot and save the file with the VSEG extension. URDF can represent the kinematic and dynamic description of the robot, the visual representation of the robot, and the collision model of the robot.

[ 74 ]

Working with 3D Robot Modeling in ROS

Chapter 3

The following tags are the commonly used URDF tags to compose a URDF robot model: MJOL: The MJOL tag represents a single link of a robot. Using this tag, we

can model a robot link and its properties. The modeling includes the size, the shape, and the color, and it can even import a 3D mesh to represent the robot link. We can also provide the dynamic properties of the link, such as the inertial matrix and the collision properties. The syntax is as follows: MJOLOBNFOBNFPGUIFMJOL  JOFSUJBM JOFSUJBM WJTVBM WJTVBM DPMMJTJPO DPMMJTJPO MJOL

The following is a representation of a single link. The Visual section represents the real link of the robot, and the area surrounding the real link is the Collision section. The Collision section encapsulates the real link to detect collision before hitting the real link:

(KIWTG8KUWCNK\CVKQPQHC74&(NKPM

KPJOU: The KPJOU tag represents a robot joint. We can specify the kinematics and

the dynamics of the joint, and set the limits of the joint movement and its velocity. The KPJOU tag supports the different types of joints, such as revolute, continuous, prismatic, fixed, floating, and planar. The syntax is as follows: KPJOUOBNFOBNFPGUIFKPJOU  QBSFOUMJOLMJOL DIJMEMJOLMJOL DBMJCSBUJPO

[ 75 ]

Working with 3D Robot Modeling in ROS

Chapter 3

EZOBNJDTEBNQJOH MJNJUFGGPSU KPJOU

A URDF joint is formed between two links; the first is called the Parent link, and the second is called the Child link. The following is an illustration of a joint and its link:

(KIWTG8KUWCNK\CVKQPQHC74&(LQKPV

SPCPU: This tag encapsulates the entire robot model that can be represented using URDF. Inside the SPCPU tag, we can define the name of the robot, the links, and

the joints of the robot. The syntax is as follows: SPCPUOBNFOBNFPGUIFSPCPU  MJOL MJOL MJOL MJOL KPJOU KPJOU KPJOU KPJOU SPCPU

[ 76 ]

Working with 3D Robot Modeling in ROS

Chapter 3

A robot model consists of connected links and joints. Here is a visualization of the robot model:

(KIWTG8KUWCNK\CVKQPQHCTQDQVOQFGNJCXKPILQKPVUCPFNKPMU

HB[FCP: This tag is used when we include the simulation parameters of the Gazebo simulator inside the URDF. We can use this tag to include HB[FCP plugins, HB[FCP material properties, and so on. The following shows an example using HB[FCP tags: HB[FCPSFGFSFODFMJOL@ NBUFSJBM (B[FCP#MBDLNBUFSJBM HB[FCP

You can find more URDF tags at IUUQXJLJSPTPSHVSEG9.-.

[ 77 ]

Working with 3D Robot Modeling in ROS

Chapter 3

Creating the ROS package for the robot description Before creating the URDF file for the robot, let's create an ROS package in the DBULJO workspace so that the robot model keeps using the following command: $ catkin_create_pkg mastering_ros_robot_description_pkg roscpp tf geometry_msgs urdf rviz xacro

The package mainly depends on the VSEG and YBDSP packages. If these packages have not been installed on to your system, you can install them using the package manager: $sudo apt-get install ros-kinetic-urdf $sudo apt-get install ros-kinetic-xacro

We can create the VSEG file of the robot inside this package and create launch files to display the created VSEG in RViz. The full package is available on the following Git repository; you can clone the repository for a reference to implement this package, or you can get the package from the book's source code: $ git clone https://github.com/jocacace/mastering_ros_robot_description_pkg.git

Before creating the VSEG file for this robot, let's create three folders called VSEG, NFTIFT, and MBVODI inside the package folder. The VSEG folder can be used to keep the VSEG and YBDSP files that we are going to create. The NFTIFT folder keeps the meshes that we need to include in the VSEG file, and the MBVODI folder keeps the ROS launch files.

Creating our first URDF model After learning about URDF and its important tags, we can start some basic modeling using URDF. The first robot mechanism that we are going to design is a pan-and-tilt mechanism, as shown in the following figure.

[ 78 ]

Working with 3D Robot Modeling in ROS

Chapter 3

There are three links and two joints in this mechanism. The base link is static, and all the other links are mounted on it. The first joint can pan on its axis, and the second link is mounted on the first link, and it can tilt on its axis. The two joints in this system are of a revolute type:

(KIWTG8KUWCNK\CVKQPQHCRCPCPFVKNVOGEJCPKUOKP48K\

Let's see the URDF code of this mechanism. Navigate to the NBTUFSJOH@SPT@SPCPU@EFTDSJQUJPO@QLHVSEG directory and open QBO@UJMUVSEG: 
YNMWFSTJPO
SPCPUOBNFQBO@UJMU MJOLOBNFCBTF@MJOL WJTVBM HFPNFUSZ DZMJOEFSMFOHUISBEJVT HFPNFUSZ PSJHJOSQZYZ[

[ 79 ]

Working with 3D Robot Modeling in ROS

Chapter 3

NBUFSJBMOBNFZFMMPX DPMPSSHCB NBUFSJBM WJTVBM MJOL KPJOUOBNFQBO@KPJOUUZQFSFWPMVUF QBSFOUMJOLCBTF@MJOL DIJMEMJOLQBO@MJOL PSJHJOYZ[ BYJTYZ[ KPJOU MJOLOBNFQBO@MJOL WJTVBM HFPNFUSZ DZMJOEFSMFOHUISBEJVT HFPNFUSZ PSJHJOSQZYZ[ NBUFSJBMOBNFSFE DPMPSSHCB NBUFSJBM WJTVBM MJOL KPJOUOBNFUJMU@KPJOUUZQFSFWPMVUF QBSFOUMJOLQBO@MJOL DIJMEMJOLUJMU@MJOL PSJHJOYZ[ BYJTYZ[ KPJOU MJOLOBNFUJMU@MJOL WJTVBM HFPNFUSZ DZMJOEFSMFOHUISBEJVT HFPNFUSZ PSJHJOSQZYZ[ NBUFSJBMOBNFHSFFO DPMPSSHCB NBUFSJBM WJTVBM MJOL SPCPU

[ 80 ]

Working with 3D Robot Modeling in ROS

Chapter 3

Explaining the URDF file When we check the code, we can add a SPCPU tag at the top of the description: 
YNMWFSTJPO
SPCPUOBNFQBO@UJMU

The SPCPU tag defines the name of the robot that we are going to create. Here, we named the robot QBO@UJMU. If we check the sections after the SPCPU tag definition, we can see link and joint definitions of the pan-and-tilt mechanism: MJOLOBNFCBTF@MJOL WJTVBM HFPNFUSZ DZMJOEFSMFOHUISBEJVT HFPNFUSZ PSJHJOSQZYZ[ NBUFSJBMOBNFZFMMPX DPMPSSHCB NBUFSJBM WJTVBM MJOL

The preceding code snippet is the CBTF@MJOL definition of the pan-and-tilt mechanism. The WJTVBM tag describes the visual appearance of the link, which is shown on the robot simulation. We can define the link geometry (DZMJOEFS, CPY, TQIFSF, or NFTI) and the material (DPMPS and UFYUVSF) of the link using this tag: KPJOUOBNFQBO@KPJOUUZQFSFWPMVUF QBSFOUMJOLCBTF@MJOL DIJMEMJOLQBO@MJOL PSJHJOYZ[ BYJTYZ[ KPJOU

In the preceding code snippet, we define, a joint with a unique name and its joint type. The joint type we used here is SFWPMVUF, and the parent and child links are CBTF@MJOL and QBO@MJOL, respectively. The joint origin is also specified inside this tag. Save the preceding URDF code as QBO@UJMUVSEG and check whether the VSEG contains errors using the following command: $ check_urdf pan_tilt.urdf

[ 81 ]

Working with 3D Robot Modeling in ROS

Chapter 3

The DIFDL@VSEG command will parse the VSEG tag and show an error, if there are any. If everything is OK, it will output the following: robot name is: pan_tilt ---------- Successfully Parsed XML --------------root Link: base_link has 1 child(ren) child(1): pan_link  child(1): tilt_link

If we want to view the structure of the robot links and joints graphically, we can use a command tool called VSEG@UP@HSBQIJ[: $ urdf_to_graphiz pan_tilt.urdf

This command will generate two files: QBO@UJMUHW and QBO@UJMUQEG. We can view the structure of this robot using this command: $ evince pan_tilt.pdf

We will get the following output:

(KIWTG)TCRJQHLQKPVCPFNKPMUKPVJGRCPCPFVKNVOGEJCPKUO

[ 82 ]

Working with 3D Robot Modeling in ROS

Chapter 3

Visualizing the 3D robot model in RViz After designing the URDF, we can view it on RViz. We can create a WJFX@EFNPMBVODI launch file and put the following code into the MBVODI folder. Navigate to the NBTUFSJOH@SPT@SPCPU@EFTDSJQUJPO@QLHMBVODI directory for the code: MBVODI BSHOBNFNPEFM QBSBNOBNFSPCPU@EFTDSJQUJPOUFYUGJMF GJOE NBTUFSJOH@SPT@SPCPU@EFTDSJQUJPO@QLHVSEGQBO@UJMUVSEG QBSBNOBNFVTF@HVJWBMVFUSVF OPEFOBNFKPJOU@TUBUF@QVCMJTIFSQLHKPJOU@TUBUF@QVCMJTIFS UZQFKPJOU@TUBUF@QVCMJTIFS OPEFOBNFSPCPU@TUBUF@QVCMJTIFSQLHSPCPU@TUBUF@QVCMJTIFS UZQFTUBUF@QVCMJTIFS OPEFOBNFSWJ[QLHSWJ[UZQFSWJ[BSHTE GJOE NBTUFSJOH@SPT@SPCPU@EFTDSJQUJPO@QLHVSEGSWJ[SFRVJSFEUSVF MBVODI

We can launch the model using the following command: $ roslaunch mastering_ros_robot_description_pkg view_demo.launch

If everything works fine, we will get a pan-and-tilt mechanism in RViz, as shown here:

(KIWTG,QKPVNGXGNQHRCPCPFVKNVOGEJCPKUO

[ 83 ]

Working with 3D Robot Modeling in ROS

Chapter 3

Interacting with pan-and-tilt joints We can see that an extra GUI came along with RViz; it contains sliders to control the pan joints and the tilt joints. This GUI is called the Joint State Publisher node, from the KPJOU@TUBUF@QVCMJTIFS package: OPEFOBNFKPJOU@TUBUF@QVCMJTIFSQLHKPJOU@TUBUF@QVCMJTIFS UZQFKPJOU@TUBUF@QVCMJTIFS

We can include this node in the MBVODI file, using this statement. The limits of pan-and-tilt should be mentioned inside the KPJOU tag: KPJOUOBNFQBO@KPJOUUZQFSFWPMVUF QBSFOUMJOLCBTF@MJOL DIJMEMJOLQBO@MJOL PSJHJOYZ[ BYJTYZ[ MJNJUFGGPSUWFMPDJUZMPXFSVQQFS EZOBNJDTEBNQJOHGSJDUJPO KPJOU

MJNJUFGGPSUWFMPDJUZMPXFSVQQFS defines

the limits of effort, the velocity, and the angle limits. The effort is the maximum force supported by this joint; MPXFS and VQQFS indicate the lower and upper limit of the joint in the radian for the revolute type joint, and meters for prismatic joints. The velocity is the maximum joint velocity:

(KIWTG,QKPVNGXGNQHRCPCPFVKNVOGEJCPKUO

[ 84 ]

Working with 3D Robot Modeling in ROS

Chapter 3

The preceding screenshot shows the GUI of KPJOU@TUBUF@QVCMJTIFS, with sliders and current joint values shown in the box.

Adding physical and collision properties to a URDF model Before simulating a robot in a robot simulator, such as Gazebo or V-REP, we need to define the robot link's physical properties, such as geometry, color, mass, and inertia, as well as the collision properties of the link. We will only get good simulation results if we define all these properties inside the robot model. URDF provides tags to include all these parameters and code snippets of CBTF@MJOL contained in these properties, as given here: MJOL  DPMMJTJPO HFPNFUSZ DZMJOEFSMFOHUISBEJVT HFPNFUSZ PSJHJOSQZYZ[ DPMMJTJPO JOFSUJBM NBTTWBMVF JOFSUJBJYYJYZJY[JZZJZ[J[[ JOFSUJBM  MJOL

Here, we define the collision geometry as cylinder and the mass as 1 kg, and we also set the inertial matrix of the link. The DPMMJTJPO and JOFSUJB parameters are required in each link, otherwise Gazebo will not load the robot model properly.

[ 85 ]

Working with 3D Robot Modeling in ROS

Chapter 3

Understanding robot modeling using xacro The flexibility of URDF reduces when we work with complex robot models. Some of the main features that URDF is missing are simplicity, reusability, modularity, and programmability. If someone wants to reuse a URDF block 10 times in his robot description, he can copy and paste the block 10 times. If there is an option to use this code block and make multiple copies with different settings, it will be very useful while creating the robot description. The URDF is a single file and we can't include other URDF files inside it. This reduces the modular nature of the code. All code should be in a single file, which reduces the code's simplicity. Also, if there is some programmability, such as adding variables, constants, mathematical expressions, and conditional statements, in the description language, it will be more userfriendly. The robot modeling using xacro meets all of these conditions. Some of the main features of xacro are as follows: Simplify URDF: The xacro is the cleaned-up version of URDF. It creates macros inside the robot description and reuses the macros. This can reduce the code length. Also, it can include macros from other files and make the code simpler, more readable, and more modular. Programmability: The xacro language supports a simple programming statement in its description. There are variables, constants, mathematical expressions, conditional statements, and so on that make the description more intelligent and efficient. We can say that xacro is an updated version of URDF, and we can convert the xacro definition to URDF whenever it is necessary, using some ROS tools. We can talk about the same description of pan-and-tilt using xacro. Navigate to NBTUFSJOH@SPT@SPCPU@EFTDSJQUJPO@QLHVSEG, and the file name is QBO@UJMUYBDSP. Instead of VSEG, we need to use the YBDSP extension for xacro files. Here is the explanation of the xacro code: 
YNMWFSTJPO
SPCPUYNMOTYBDSPIUUQXXXSPTPSHXJLJYBDSPOBNFQBO@UJMU

[ 86 ]

Working with 3D Robot Modeling in ROS

Chapter 3

These lines specify a namespace that is needed in all xacro files for parsing the xacro file. After specifying the namespace, we need to add the name of the xacro file.

Using properties Using xacro, we can declare constants or properties that are the named values inside the xacro file, which can be used anywhere in the code. The main use of these constant definitions is, instead of giving hardcoded values on links and joints, we can keep constants, and it will be easier to change these values rather than finding the hardcoded values and replacing them. An example of using properties is given here. We declare the base link and the pan link's length and radius. So, it will be easy to change the dimension here rather than changing the values in each one: YBDSPQSPQFSUZOBNFCBTF@MJOL@MFOHUIWBMVF YBDSPQSPQFSUZOBNFCBTF@MJOL@SBEJVTWBMVF YBDSPQSPQFSUZOBNFQBO@MJOL@MFOHUIWBMVF YBDSPQSPQFSUZOBNFQBO@MJOL@SBEJVTWBMVF

We can use the value of the variable by replacing the hardcoded value with the following definition: DZMJOEFSMFOHUI\QBO@MJOL@MFOHUI^ SBEJVT\QBO@MJOL@SBEJVT^

Here, the old value, , is replaced with \QBO@MJOL@MFOHUI^, and  is replaced with \QBO@MJOL@SBEJVT^.

Using the math expression We can build mathematical expressions inside \^ using basic operations such as , , ,  , unary minus, and parenthesis. Exponentiation and modulus are not supported yet. The following is a simple math expression used inside the code: DZMJOEFSMFOHUI\QBO@MJOL@MFOHUI^ SBEJVT\QBO@MJOL@SBEJVT ^

[ 87 ]

Working with 3D Robot Modeling in ROS

Chapter 3

Using macros One of the main features of xacro is that it supports macros. We can use xacro to reduce the length of complex definitions. Here is a YBDSP definition we used in our code for inertial: YBDSPNBDSPOBNFJOFSUJBM@NBUSJYQBSBNTNBTT JOFSUJBM NBTTWBMVF\NBTT^ JOFSUJBJYYJYZJY[ JZZJZ[J[[ JOFSUJBM YBDSPNBDSP

Here, the macro is named JOFSUJBM@NBUSJY, and its parameter is mass. The mass parameter can be used inside the inertial definition using \NBTT^. We can replace each inertial code with a single line, as given here: YBDSPJOFSUJBM@NBUSJYNBTT

The xacro definition improved the code readability and reduced the number of lines compared to VSEG. Next, we will look at how to convert xacro to a URDF file.

Converting xacro to URDF After designing the xacro file, we can use the following command to convert it to a URDF file: $ rosrun xacro xacro pan_tilt.xacro --inorder > pan_tilt_generated.urdf

The JOPSEFS option has been recently introduced in ROS to increase the power of the conversion tool. It allows us to process the document in read order, adding more features than there were in older ROS versions. We can use the following line in the ROS launch file for converting xacro to URDF and use it as a SPCPU@EFTDSJQUJPO parameter: QBSBNOBNFSPCPU@EFTDSJQUJPODPNNBOE GJOEYBDSPYBDSPJOPSEFS  GJOENBTUFSJOH@SPT@SPCPU@EFTDSJQUJPO@QLHVSEGQBO@UJMUYBDSP 

[ 88 ]

Working with 3D Robot Modeling in ROS

Chapter 3

We can view the xacro of pan-and-tilt by making a launch file, and it can be launched using the following command: $ roslaunch mastering_ros_robot_description_pkg view_pan_tilt_xacro.launch

Creating the robot description for a seven DOF robot manipulator Now, we can create some complex robots using URDF and xacro. The first robot we are going to deal with is a seven DOF robotic arm, which is a serial link manipulator with multiple serial links. The seven DOF arm is kinematically redundant, which means it has more joints and DOF than required to achieve its goal position and orientation. The advantage of redundant manipulators is that we can have more joint configuration for a desired goal position and orientation. It will improve the flexibility and versatility of the robot movement and can implement effective collision-free motion in a robotic workspace. Let's start creating the seven DOF arm; the final output model of the robot arm is shown here (the various joints and links in the robot are also marked on the image):

(KIWTG,QKPVUCPF.KPMUQHUGXGP&1(CTOTQDQV

[ 89 ]

Working with 3D Robot Modeling in ROS

Chapter 3

The preceding robot is described using xacro. We can take the actual description file from the cloned repository. We can navigate to the VSEG folder inside the cloned package and open the TFWFO@EPG@BSNYBDSP file. We will copy and paste the description to the current package and discuss the major section of this robot description.

Arm specification Here is the robot arm specification of this seven DOF arm: Degrees of freedom: 7 Length of the arm: 50 cm Reach of the arm: 35 cm Number of links: 12 Number of joints: 11

Type of joints Here is the list of joints containing the joint name and its type of robot: Joint number Joint name

Joint type Angle limits (in degrees)

1

CPUUPN@KPJOU

Fixed

--

2

TIPVMEFS@QBO@KPJOU

Revolute

-150 to 114

3

TIPVMEFS@QJUDI@KPJOU Revolute

-67 to 109

4

FMCPX@SPMM@KPJOU

Revolute

-150 to 41

5

FMCPX@QJUDI@KPJOU

Revolute

-92 to 110

6

XSJTU@SPMM@KPJOU

Revolute

-150 to 150

7

XSJTU@QJUDI@KPJOU

Revolute

92 to 113

8

HSJQQFS@SPMM@KPJOU

Revolute

-150 to 150

9

GJOHFS@KPJOU

Prismatic 0 to 3 cm

10

GJOHFS@KPJOU

Prismatic 0 to 3 cm

We design the xacro of the arm using the preceding specifications; next up is the explanation of the xacro arm file.

[ 90 ]

Working with 3D Robot Modeling in ROS

Chapter 3

Explaining the xacro model of the seven DOF arm We will define ten links and nine joints on this robot, and two links and two joints in the robot gripper. Let's start by looking at the xacro definition: 
YNMWFSTJPO
SPCPUOBNFTFWFO@EPG@BSNYNMOTYBDSPIUUQSPTPSHXJLJYBDSP

Because we are writing a xacro file, we should mention the xacro namespace to parse the file.

Using constants We use constants inside this xacro to make robot descriptions shorter and more readable. Here, we define the degree to the radian conversion factor, PI value, length, height, and width of each of the links: QSPQFSUZOBNFEFH@UP@SBEWBMVF QSPQFSUZOBNF.@1*WBMVF QSPQFSUZOBNFFMCPX@QJUDI@MFOWBMVF QSPQFSUZOBNFFMCPX@QJUDI@XJEUIWBMVF QSPQFSUZOBNFFMCPX@QJUDI@IFJHIUWBMVF

Using macros We define macros in this code to avoid repeatability and to make the code shorter. Here are the macros we have used in this code: YBDSPNBDSPOBNFJOFSUJBM@NBUSJYQBSBNTNBTT JOFSUJBM NBTTWBMVF\NBTT^ JOFSUJBJYYJYZJY[JZZJZ[ J[[ JOFSUJBM YBDSPNBDSP

[ 91 ]

Working with 3D Robot Modeling in ROS

Chapter 3

This is the definition of the JOFSUJBMNBUSJY macro, in which we can use NBTT as its parameter: YBDSPNBDSPOBNFUSBOTNJTTJPO@CMPDLQBSBNTKPJOU@OBNF USBOTNJTTJPOOBNFUSBO UZQF USBOTNJTTJPO@JOUFSGBDF4JNQMF5SBOTNJTTJPOUZQF KPJOUOBNF\KPJOU@OBNF^ IBSEXBSF*OUFSGBDF 1PTJUJPO+PJOU*OUFSGBDFIBSEXBSF*OUFSGBDF KPJOU BDUVBUPSOBNFNPUPS IBSEXBSF*OUFSGBDF 1PTJUJPO+PJOU*OUFSGBDFIBSEXBSF*OUFSGBDF NFDIBOJDBM3FEVDUJPO NFDIBOJDBM3FEVDUJPO BDUVBUPS USBOTNJTTJPO YBDSPNBDSP

In the section of the code, we can see the definition using the USBOTNJTTJPO tag. The USBOTNJTTJPO tag relates a joint to an actuator. It defines the type of transmission that we are using in a particular joint, as well as the type of motor and its parameters. It also defines the type of hardware interface we use when we interface with the ROS controllers.

Including other xacro files We can extend the capabilities of the robot xacro by including the xacro definition of sensors using the YBDSPJODMVEF tag. The following code snippet shows how to include a sensor definition in the robot xacro: YBDSPJODMVEFGJMFOBNF GJOE NBTUFSJOH@SPT@SPCPU@EFTDSJQUJPO@QLHVSEGTFOTPSTYUJPO@QSP@MJWFVSEGYBDSP 

Here, we include a xacro definition of a sensor called Asus Xtion pro, and this will be expanded when the xacro file is parsed. Using  GJOE NBTUFSJOH@SPT@SPCPU@EFTDSJQUJPO@QLHVSEGTFOTPSTYUJPO@QSP@MJWFVSEGY BDSP, we can access the xacro definition of the sensor, where GJOE is to locate the current NBTUFSJOH@SPT@SPCPU@EFTDSJQUJPO@QLH package. We will talk more about vision-processing in $IBQUFS, Building and Interfacing Differential Drive Mobile Robot Hardware in ROS.

[ 92 ]

Working with 3D Robot Modeling in ROS

Chapter 3

Using meshes in the link We can insert a primitive shape in to a link, or we can insert a mesh file using the NFTI tag. The following example shows how to insert a mesh into the vision sensor: WJTVBM PSJHJOYZ[SQZ HFPNFUSZ NFTIGJMFOBNF QBDLBHFNBTUFSJOH@SPT@SPCPU@EFTDSJQUJPO@QLHNFTIFTTFOTPSTYUJPO@QSP@MJW FYUJPO@QSP@MJWFEBF HFPNFUSZ NBUFSJBMOBNF%BSL(SFZ WJTVBM

Working with the robot gripper The gripper of the robot is designed for the picking and placing of blocks; the gripper is in the simple linkage category. There are two joints for the gripper, and each joint is prismatic. Here is the KPJOU definition of one gripper joint: KPJOUOBNFGJOHFS@KPJOUUZQFQSJTNBUJD QBSFOUMJOLHSJQQFS@SPMM@MJOL DIJMEMJOLHSJQQFS@GJOHFS@MJOL PSJHJOYZ[ BYJTYZ[ MJNJUFGGPSUMPXFSVQQFSWFMPDJUZ TBGFUZ@DPOUSPMMFSL@QPTJUJPO L@WFMPDJUZ TPGU@MPXFS@MJNJU\^ TPGU@VQQFS@MJNJU\^ EZOBNJDTEBNQJOHGSJDUJPO KPJOU

Here, the first gripper joint is formed by HSJQQFS@SPMM@MJOL and HSJQQFS@GJOHFS@MJOL, and the second joint is formed by HSJQQFS@SPMM@MJOL and HSJQQFS@GJOHFS@MJOL.

[ 93 ]

Working with 3D Robot Modeling in ROS

Chapter 3

The following graph shows how the gripper joints are connected in HSJQQFS@SPMM@MJOL:

(KIWTG)TCRJQHVJGGPFGbGEVQTUGEVKQPQHVJGUGXGP&1(CTOTQDQV

Viewing the seven DOF arm in RViz After talking about the robot model, we can view the designed xacro file in RViz and control each joint using the KPJOUTUBUFQVCMJTIFS node and publish the robot state using 3PCPU4UBUF1VCMJTIFS. The preceding task can be performed using a launch file called WJFX@BSNMBVODI, which is inside the MBVODI folder of this package: MBVODI BSHOBNFNPEFM 1BSTJOHYBDSPBOEMPBEJOHSPCPU@EFTDSJQUJPOQBSBNFUFS QBSBNOBNFSPCPU@EFTDSJQUJPODPNNBOE GJOEYBDSPYBDSPJOPSEFS  GJOENBTUFSJOH@SPT@SPCPU@EFTDSJQUJPO@QLHVSEGTFWFO@EPG@BSNYBDSP 4FUUJOHHVJQBSBNFUFSUPUSVFGPSEJTQMBZKPJOUTMJEFSGPSHFUUJOH

[ 94 ]

Working with 3D Robot Modeling in ROS

Chapter 3

KPJOUDPOUSPM QBSBNOBNFVTF@HVJWBMVFUSVF 4UBSUJOH+PJOUTUBUFQVCMJTIFSOPEFXIJDIXJMMQVCMJTIUIFKPJOU WBMVFT OPEFOBNFKPJOU@TUBUF@QVCMJTIFSQLHKPJOU@TUBUF@QVCMJTIFS UZQFKPJOU@TUBUF@QVCMJTIFS 4UBSUJOHSPCPUTUBUFQVCMJTIXIJDIXJMMQVCMJTIDVSSFOUSPCPUKPJOU TUBUFTVTJOHUG OPEFOBNFSPCPU@TUBUF@QVCMJTIFSQLHSPCPU@TUBUF@QVCMJTIFS UZQFTUBUF@QVCMJTIFS -BVODIWJTVBMJ[BUJPOJOSWJ[ OPEFOBNFSWJ[QLHSWJ[UZQFSWJ[BSHTE GJOE NBTUFSJOH@SPT@SPCPU@EFTDSJQUJPO@QLHVSEGSWJ[SFRVJSFEUSVF MBVODI

Create the following launch file inside the MBVODI folder, and build the package using the DBULJO@NBLF command. Launch the VSEG using the following command: $ roslaunch mastering_ros_robot_description_pkg view_arm.launch

The robot will be displayed on RViz, with the joint state publisher GUI:

(KIWTG5GXGP&1(CTOKP48K\YKVJLQKPVAUVCVGARWDNKUJGT

[ 95 ]

Working with 3D Robot Modeling in ROS

Chapter 3

We can interact with the joint slider and move the joints of the robot. Next, we will talk about what KPJOUTUBUFQVCMJTIFS can do.

Understanding joint state publisher Joint state publisher is one of the ROS packages that is commonly used to interact with each joint of the robot. The package contains the KPJOU@TUBUF@QVCMJTIFS node, which will find the nonfixed joints from the URDF model and publish the joint state values of each joint in the TFOTPS@NTHT+PJOU4UBUF message format. In the preceding launch file, WJFX@BSNMBVODI, we started the KPJOU@TUBUF@QVCMJTIFS node and set a parameter called VTF@HVJ to USVF, as follows: QBSBNOBNFVTF@HVJWBMVFUSVF 4UBSUJOH+PJOUTUBUFQVCMJTIFSOPEFXIJDIXJMMQVCMJTIUIFKPJOU WBMVFT OPEFOBNFKPJOU@TUBUF@QVCMJTIFSQLHKPJOU@TUBUF@QVCMJTIFS UZQFKPJOU@TUBUF@QVCMJTIFS

If we set VTF@HVJ to USVF, the KPJOU@TUBUF@QVCMJTIFS node displays a slider-based control window to control each joint. The lower and upper value of a joint will be taken from the lower and upper values associated with the MJNJU tag used inside the KPJOU tag. The preceding screenshot shows the robot model in RViz, along with a user interface to change the position of robot joints, which states with the VTF@HVJ parameter set to USVF. We can find more on the KPJOUTUBUFQVCMJTIFS package at IUUQXJLJSPTPSHKPJOU@TUBUF@QVCMJTIFS.

Understanding robot state publisher The SPCPUTUBUFQVCMJTIFS package helps to publish the state of the robot to UG. This package subscribes to joint states of the robot and publishes the 3D pose of each link using the kinematic representation from the URDF model. We can implement the SPCPUTUBUF QVCMJTIFS node using the following line inside the launch file: 4UBSUJOHSPCPUTUBUFQVCMJTIXIJDIXJMMQVCMJTIUG OPEFOBNFSPCPU@TUBUF@QVCMJTIFSQLHSPCPU@TUBUF@QVCMJTIFS UZQFTUBUF@QVCMJTIFS

[ 96 ]

Working with 3D Robot Modeling in ROS

Chapter 3

In the preceding launch file, WJFX@BSNMBVODI, we started this node to publish the UG of the arm. We can visualize the transformation of the robot by clicking the UG option on RViz, shown as follows:

(KIWTG6(XKGYQHUGXGP&1(CTOKP48K\

The KPJOU@TUBUF@QVCMJTIFS and SPCPU@TUBUF@QVCMJTIFS packages are installed along with the ROS desktop's installations. After creating the robot description of the seven DOF arm, we can talk about how to make a mobile robot with differential wheeled mechanisms.

[ 97 ]

Working with 3D Robot Modeling in ROS

Chapter 3

Creating a robot model for the differential drive mobile robot A differential wheeled robot will have two wheels connected on opposite sides of the robot chassis, which is supported by one or two caster wheels. The wheels will control the speed of the robot by adjusting individual velocity. If the two motors are running at the same speed, the wheels will move forward or backward. If one wheel is running slower than the other, the robot will turn to the side of the lower speed. If we want to turn the robot to the left side, we reduce the velocity of the left wheel, and vice versa. There are two supporting wheels, called caster wheels, that will support the robot and rotate freely based on the movement of the main wheels. The URDF model of this robot is present in the cloned ROS package. The final robot model is shown here:

(KIWTG&KbGTGPVKCNFTKXGOQDKNGTQDQV

The preceding robot has five joints and links. The two main joints connect the wheels to the robot, while the others are fixed joints connecting the caster wheels and the base footprint to the body of the robot.

[ 98 ]

Working with 3D Robot Modeling in ROS

Chapter 3

The preceding robot has five joints and five links. The two main joints are two-wheel joints, and the other three joints are two fixed joints by caster wheels, and one fixed joint by base foot print to the base link of the robot. Here is the connection graph of this robot:

(KIWTG)TCRJKECNTGRTGUGPVCVKQPQHNKPMUCPFLQKPVUHQTVJGFKbGTGPVKCNFTKXGOQDKNGTQDQV

We can go through the important section of code in the URDF file. The URDF file, called EJGG@XIFFMFE@SPCPUYBDSP, is placed inside the VSEG folder of the cloned ROS package.

[ 99 ]

Working with 3D Robot Modeling in ROS

Chapter 3

The first section of the URDF file is given here. The robot is named EJGGFSFOUJBM@XIFFMFE@SPCPU, and it also includes a URDF file, called XIFFMVSEGYBDSP. This xacro file contains the definition of the wheel and its transmission; if we use this xacro file, then we can avoid writing two definitions for the two wheels. We use this xacro definition because the two wheels are identical in shape and size: 
YNMWFSTJPO
SPCPUOBNFEJGGFSFOUJBM@XIFFMFE@SPCPU YNMOTYBDSPIUUQSPTPSHXJLJYBDSP YBDSPJODMVEFGJMFOBNF GJOE NBTUFSJOH@SPT@SPCPU@EFTDSJQUJPO@QLHVSEGXIFFMVSEGYBDSP

The definition of a wheel inside XIFFMVSEGYBDSP is given here. We can mention whether the wheel has to be placed to the left, right, front, or back. Using this macro, we can create a maximum of four wheels but, for now, we require only two: YBDSPNBDSPOBNFXIFFMQBSBNTGCMSQBSFOUUSBOTMBUF9USBOTMBUF: GMJQ: GCGSPOUCBDLMSMFGUSJHIU MJOLOBNF\GC^@\MS^@XIFFM

We also mention the Gazebo parameters required for simulation. Mentioned here are the Gazebo parameters associated with a wheel. We can mention the frictional co-efficient and the stiffness co-efficient using the HB[FCPSFGFSFODF tag: HB[FCPSFGFSFODF\GC^@\MS^@XIFFM NVWBMVF NVWBMVF LQWBMVF LEWBMVF GEJSWBMVF NBUFSJBM (B[FCP(SFZNBUFSJBM UVSO(SBWJUZ0GG GBMTFUVSO(SBWJUZ0GG HB[FCP

The joints that we define for a wheel are continuous joints because there is no limit in the XIFFM joint. The QBSFOUMJOL here is the robot base, and the DIJMEMJOL is each wheel: KPJOUOBNF\GC^@\MS^@XIFFM@KPJOUUZQFDPOUJOVPVT QBSFOUMJOL\QBSFOU^ DIJMEMJOL\GC^@\MS^@XIFFM PSJHJOYZ[\USBOTMBUF9

[ 100 ]

Working with 3D Robot Modeling in ROS

Chapter 3

We also need to mention the USBOTNJTTJPO tag of each wheel; the macro of the wheel is as follows: 5SBOTNJTTJPOJTJNQPSUBOUUPMJOLUIFKPJOUTBOEUIFDPOUSPMMFS  USBOTNJTTJPOOBNF\GC^@\MS^@XIFFM@KPJOU@USBOT UZQF USBOTNJTTJPO@JOUFSGBDF4JNQMF5SBOTNJTTJPOUZQF KPJOUOBNF\GC^@\MS^@XIFFM@KPJOU BDUVBUPSOBNF\GC^@\MS^@XIFFM@KPJOU@NPUPS IBSEXBSF*OUFSGBDF &GGPSU+PJOU*OUFSGBDFIBSEXBSF*OUFSGBDF NFDIBOJDBM3FEVDUJPO NFDIBOJDBM3FEVDUJPO BDUVBUPS USBOTNJTTJPO YBDSPNBDSP SPCPU

In EJGG@XIFFMFE@SPCPUYBDSP, we can use the following lines to use the macros defined inside XIFFMVSEGYBDSP: XIFFMGCGSPOUMSSJHIUQBSFOUCBTF@MJOLUSBOTMBUF9 USBOTMBUF:GMJQ: XIFFMGCGSPOUMSMFGUQBSFOUCBTF@MJOLUSBOTMBUF9 USBOTMBUF:GMJQ:

Using the preceding lines, we define the wheels on the left and right of the robot base. The robot base is cylindrical, as shown in the preceding figure. The inertia calculating macro is given here. This xacro snippet will use the mass, radius, and height of the cylinder to calculate inertia using this equation: .BDSPGPSDBMDVMBUJOHJOFSUJBPGDZMJOEFS NBDSPOBNFDZMJOEFS@JOFSUJBQBSBNTNSI JOFSUJBJYY\N  S S I I^JYZJY[ JZZ\N  S S I I^JZ[ J[[\N S S^ NBDSP

The launch file definition for displaying this root model in RViz is given here. The launch file is named WJFX@NPCJMF@SPCPUMBVODI: MBVODI BSHOBNFNPEFM 1BSTJOHYBDSPBOETFUUJOHSPCPU@EFTDSJQUJPOQBSBNFUFS QBSBNOBNFSPCPU@EFTDSJQUJPODPNNBOE GJOEYBDSPYBDSPJOPSEFS  GJOENBTUFSJOH@SPT@SPCPU@EFTDSJQUJPO@QLHVSEGEJGG@XIFFMFE@SPCPUYBDSP  4FUUJOHHVJQBSBNFUFSUPUSVFGPSEJTQMBZKPJOUTMJEFS QBSBNOBNFVTF@HVJWBMVFUSVF

[ 101 ]

Working with 3D Robot Modeling in ROS

Chapter 3

4UBSUJOH+PJOUTUBUFQVCMJTIFSOPEFXIJDIXJMMQVCMJTIUIFKPJOU WBMVFT OPEFOBNFKPJOU@TUBUF@QVCMJTIFSQLHKPJOU@TUBUF@QVCMJTIFS UZQFKPJOU@TUBUF@QVCMJTIFS 4UBSUJOHSPCPUTUBUFQVCMJTIXIJDIXJMMQVCMJTIUG OPEFOBNFSPCPU@TUBUF@QVCMJTIFSQLHSPCPU@TUBUF@QVCMJTIFS UZQFTUBUF@QVCMJTIFS -BVODIWJTVBMJ[BUJPOJOSWJ[ OPEFOBNFSWJ[QLHSWJ[UZQFSWJ[BSHTE GJOE NBTUFSJOH@SPT@SPCPU@EFTDSJQUJPO@QLHVSEGSWJ[SFRVJSFEUSVF MBVODI

The only difference between the arm URDF file is the change in the name; the other sections are the same. We can view the mobile robot using the following command: $ roslaunch mastering_ros_robot_description_pkg view_mobile_robot.launch

The screenshot of the robot in RViz is as follows:

(KIWTG8KUWCNK\KPIOQDKNGTQDQVKP48K\YKVJLQKPVUVCVGRWDNKUJGT

[ 102 ]

Working with 3D Robot Modeling in ROS

Chapter 3

Questions What are the packages used for robot modeling in ROS? What are the important URDF tags used for robot modeling? What are the reasons for using xacro over URDF? What is the function of the joint state publisher and robot state publisher packages? What is the function of the transmission tag in URDF?

Summary In this chapter, we mainly looked at the importance of robot modeling and how we can model a robot in ROS. We talked more about the SPCPU@NPEFM meta package and the packages inside SPCPU@NPEFM, such as VSEG, YBDSP, and KPJOU@TUBUF@QVCMJTIFS. We discussed URDF, xacro, and the main URDF tags that we are going to use. We also created a sample model in URDF and xacro and discussed the difference between the two. After that, we created a complex robotic manipulator with seven DOF and looked at the usage of the KPJOUTUBUFQVCMJTIFS and SPCPUTUBUFQVCMJTIFS packages. Towards the end of the chapter, we reviewed the designing procedure of a differential drive mobile robot using xacro. In the next chapter, we will look at the simulation of these robots using Gazebo.

[ 103 ]

4

Simulating Robots Using ROS and Gazebo After designing the 3D model of a robot, the next phase is its simulation. Robot simulation will give you an idea about the working of robots in a virtual environment. We are going to use the Gazebo (IUUQXXXHB[FCPTJNPSH) simulator to simulate the seven DOF arms and the mobile robot. Gazebo is a multi-robot simulator for complex indoor and outdoor robotic simulation. We can simulate complex robots, robot sensors, and a variety of 3D objects. Gazebo already has simulation models of popular robots, sensors, and a variety of 3D objects in their repository (IUUQTCJUCVDLFUPSHPTSGHB[FCP@NPEFMT). We can directly use these models without having to create new ones. Gazebo has a good interface in ROS, which exposes the whole control of Gazebo in ROS. We can install Gazebo without ROS, and we should install the ROS-Gazebo interface to communicate from ROS to Gazebo. In this chapter, we will discuss the simulation of seven DOF arms and differential wheeled robots. We will discuss ROS controllers that help to control the robot's joints in Gazebo.

Simulating Robots Using ROS and Gazebo

Chapter 4

We will cover the following topics in this chapter: Understanding robotic simulation and Gazebo Simulation a model of a robotic arm for Gazebo Simulating the robotic arm with an rgb-d sensor Moving robot joints using ROS controllers in Gazebo Simulating a differential wheeled robot in Gazebo Teleoperating a mobile robot in Gazebo

Simulating the robotic arm using Gazebo and ROS In the previous chapter, we designed a seven-DOF arm. In this section, we will simulate the robot in Gazebo using ROS. Before starting with Gazebo and ROS, we should install the following packages to work with Gazebo and ROS: $ sudo apt-get install ros-kinetic-gazebo-ros-pkgs ros-kinetic-gazebo-msgs ros-kinetic-gazebo-plugins ros-kinetic-gazebo-ros-control

The default version installed from kinetic ROS packages is Gazebo 7.0. The use of each package is as follows: HB[FCP@SPT@QLHT: This contains wrappers and tools for interfacing ROS with

Gazebo HB[FCPNTHT: This contains messages and service data structures for interfacing with Gazebo from ROS HB[FCPQMVHJOT: This contains Gazebo plugins for sensors, actuators, and so on. HB[FCPSPTDPOUSPM: This contains standard controllers to communicate between ROS and Gazebo

[ 105 ]

Simulating Robots Using ROS and Gazebo

Chapter 4

After installation, check whether Gazebo is properly installed using the following commands: $ roscore & rosrun gazebo_ros gazebo

These commands will open the Gazebo GUI. If we have the Gazebo simulator, we can proceed to develop the simulation model of the seven-DOF arm for Gazebo.

Creating the robotic arm simulation model for Gazebo We can create the simulation model for a robotic arm by updating the existing robot description by adding simulation parameters. We can create the package needed to simulate the robotic arm using the following command: $ catkin_create_pkg seven_dof_arm_gazebo gazebo_msgs gazebo_plugins gazebo_ros gazebo_ros_control mastering_ros_robot_description_pkg

Alternatively, the full package is available in the following Git repository; you can clone the repository for a reference to implement this package, or you can get the package from the book's source code: $ git clone

https://github.com/jocacace/seven_dof_arm_gazebo.git

You can see the complete simulation model of the robot in the TFWFO@EPG@BSNYBDSP file, placed in the NBTUFSJOH@SPT@SPCPU@EFTDSJQUJPO@QLHVSEG folder. The file is filled with URDF tags, which are necessary for the simulation. We will define the sections of collision, inertial, transmission, joints, links, and Gazebo.

[ 106 ]

Simulating Robots Using ROS and Gazebo

Chapter 4

To launch the existing simulation model, we can use the TFWFO@EPG@BSN@HB[FCP package, which has a launch file called TFWFO@EPG@BSN@XPSMEMBVODI. The file definition is as follows: MBVODI UIFTFBSFUIFBSHVNFOUTZPVDBOQBTTUIJTMBVODIGJMFGPSFYBNQMF QBVTFEUSVF BSHOBNFQBVTFEEFGBVMUGBMTF BSHOBNFVTF@TJN@UJNFEFGBVMUUSVF BSHOBNFHVJEFGBVMUUSVF BSHOBNFIFBEMFTTEFGBVMUGBMTF BSHOBNFEFCVHEFGBVMUGBMTF 8FSFTVNFUIFMPHJDJOFNQUZ@XPSMEMBVODI JODMVEFGJMF GJOEHB[FCP@SPTMBVODIFNQUZ@XPSMEMBVODI BSHOBNFEFCVHWBMVF BSHEFCVH BSHOBNFHVJWBMVF BSHHVJ BSHOBNFQBVTFEWBMVF BSHQBVTFE BSHOBNFVTF@TJN@UJNFWBMVF BSHVTF@TJN@UJNF BSHOBNFIFBEMFTTWBMVF BSHIFBEMFTT JODMVEF -PBEUIF63%'JOUPUIF3041BSBNFUFS4FSWFS QBSBNOBNFSPCPU@EFTDSJQUJPODPNNBOE GJOEYBDSPYBDSPJOPSEFS  GJOENBTUFSJOH@SPT@SPCPU@EFTDSJQUJPO@QLHVSEGTFWFO@EPG@BSNYBDSP  3VOBQZUIPOTDSJQUUPUIFTFOEBTFSWJDFDBMMUPHB[FCP@SPTUP TQBXOB63%'SPCPU OPEFOBNFVSEG@TQBXOFSQLHHB[FCP@SPTUZQFTQBXO@NPEFM SFTQBXOGBMTFPVUQVUTDSFFO BSHTVSEGNPEFMTFWFO@EPG@BSNQBSBNSPCPU@EFTDSJQUJPO MBVODI

Launch the following command and check what you get: $ roslaunch seven_dof_arm_gazebo seven_dof_arm_world.launch

[ 107 ]

Simulating Robots Using ROS and Gazebo

Chapter 4

You can see the robotic arm in Gazebo, as shown in the following figure; if you get this output, without any errors, you are done:

(KIWTG5KOWNCVKQPQHUGXGP&1(CTOKP)C\GDQ

Let's discuss the robot simulation model files in detail.

[ 108 ]

Simulating Robots Using ROS and Gazebo

Chapter 4

Adding colors and textures to the Gazebo robot model We can see in the simulated robot that each link has different colors and textures. The following tags inside the xacro file provide textures and colors to robot links: HB[FCPSFGFSFODFCPUUPN@MJOL NBUFSJBM (B[FCP8IJUFNBUFSJBM HB[FCP HB[FCPSFGFSFODFCBTF@MJOL NBUFSJBM (B[FCP8IJUFNBUFSJBM HB[FCP HB[FCPSFGFSFODFTIPVMEFS@QBO@MJOL NBUFSJBM (B[FCP3FENBUFSJBM HB[FCP

Adding transmission tags to actuate the model To actuate the robot using ROS controllers, we should define the USBOTNJTTJPO element to link actuators to joints. Here is the macro defined for transmission: YBDSPNBDSPOBNFUSBOTNJTTJPO@CMPDLQBSBNTKPJOU@OBNF USBOTNJTTJPOOBNFUSBO UZQF USBOTNJTTJPO@JOUFSGBDF4JNQMF5SBOTNJTTJPOUZQF KPJOUOBNF\KPJOU@OBNF^ IBSEXBSF*OUFSGBDF IBSEXBSF@JOUFSGBDF1PTJUJPO+PJOU*OUFSGBDFIBSEXBSF*OUFS GBDF KPJOU BDUVBUPSOBNFNPUPS NFDIBOJDBM3FEVDUJPO NFDIBOJDBM3FEVDUJPO BDUVBUPS USBOTNJTTJPO YBDSPNBDSP

Here, KPJOUOBNF is the joint in which we link the actuators. The UZQF element is the type of transmission. Currently, USBOTNJTTJPO@JOUFSGBDF/4JNQMF5SBOTNJTTJPO is only supported. The IBSEXBSF*OUFSGBDF element is the type of hardware interface to load (position, velocity, or effort interfaces). In the proposed example, a position control hardware interface has been used. The hardware interface is loaded by the HB[FCP@SPT@DPOUSPM plugin; we look at this plugin in the next section.

[ 109 ]

Simulating Robots Using ROS and Gazebo

Chapter 4

Adding the gazebo_ros_control plugin After adding the transmission tags, we should add the HB[FCP@SPT@DPOUSPM plugin in the simulation model to parse the transmission tags and assign appropriate hardware interfaces and the control manager. The following code adds the HB[FCP@SPT@DPOUSPM plugin to the xacro file: SPT@DPOUSPMQMVHJO HB[FCP QMVHJOOBNFHB[FCP@SPT@DPOUSPMGJMFOBNFMJCHB[FCP@SPT@DPOUSPMTP SPCPU/BNFTQBDF TFWFO@EPG@BSNSPCPU/BNFTQBDF QMVHJO HB[FCP

Here, the QMVHJO element specifies the plugin name to be loaded, which is MJCHB[FCP@SPT@DPOUSPMTP. The SPCPU/BNFTQBDF element can be given as the name of the robot; if we are not specifying the name, it will automatically load the name of the robot from the URDF. We can also specify the controller update rate (DPOUSPM1FSJPE ), the location of SPCPU@EFTDSJQUJPO (URDF) on the parameter server (SPCPU1BSBN ), and the type of robot hardware interface (SPCPU4JN5ZQF ). The default hardware interfaces are +PJOU4UBUF*OUFSGBDF, &GGPSU+PJOU*OUFSGBDF, and 7FMPDJUZ+PJOU*OUFSGBDF.

Adding a 3D vision sensor to Gazebo In Gazebo, we can simulate the robot movement and its physics; we can also simulate sensors too. To build a sensor in Gazebo, we must model the behavior of that sensor in Gazebo. There are some prebuilt sensor models in Gazebo that can be used directly in our code without writing a new model. Here, we are adding a 3D vision sensor (commonly known as an rgb-d sensor) called the Asus Xtion Pro model in Gazebo. The sensor model is already implemented in the HB[FCP@SPT@QLHT/HB[FCP@QMVHJOT ROS package, which we have already installed in our ROS system. Each model in Gazebo is implemented as a Gazebo-ROS plugin, which can be loaded by inserting it into the URDF file.

[ 110 ]

Simulating Robots Using ROS and Gazebo

Chapter 4

Here is how we include a Gazebo definition and a physical robot model of Xtion Pro in the TFWFO@EPG@BSN@XJUI@SHCEYBDSP robot xacro file: YBDSPJODMVEFGJMFOBNF GJOE NBTUFSJOH@SPT@SPCPU@EFTDSJQUJPO@QLHVSEGTFOTPSTYUJPO@QSP@MJWFVSEGYBDSP 

Inside YUJPO@QSP@MJWFVSEGYBDSP, we can see the following lines: 
YNMWFSTJPO
SPCPUYNMOTYBDSPIUUQSPTPSHXJLJYBDSP YBDSPJODMVEFGJMFOBNF GJOE NBTUFSJOH@SPT@SPCPU@EFTDSJQUJPO@QLHVSEGTFOTPSTYUJPO@QSP@MJWFHB[FCPYBD SP  YBDSPNBDSPOBNFYUJPO@QSP@MJWFQBSBNTOBNFQBSFOU PSJHJO PQUJDBM@PSJHJO  MJOLOBNF\OBNF^@MJOL  WJTVBM PSJHJOYZ[SQZ HFPNFUSZ NFTI GJMFOBNFQBDLBHFNBTUFSJOH@SPT@SPCPU@EFTDSJQUJPO@QLHNFTIFTTFOTPSTYUJP O@QSP@MJWFYUJPO@QSP@MJWFEBF HFPNFUSZ NBUFSJBMOBNF%BSL(SFZ WJTVBM MJOL SPCPU

Here, we can see it includes another file called YUJPO@QSP@MJWFHB[FCPYBDSP, which consists of the complete Gazebo definition of Xtion Pro. We can also see a macro definition named YUJPO@QSP@MJWF, which contains the complete model definition of Xtion Pro, including links and joints: NFTI GJMFOBNFQBDLBHFNBTUFSJOH@SPT@SPCPU@EFTDSJQUJPO@QLHNFTIFTTFOTPSTYUJP O@QSP@MJWFYUJPO@QSP@MJWFEBF

[ 111 ]

Simulating Robots Using ROS and Gazebo

Chapter 4

In the macro definition, we are importing a mesh file of the Asus Xtion Pro, which will be shown as the camera link in Gazebo. In the NBTUFSJOH@SPT@SPCPU@EFTDSJQUJPO@QLHVSEGTFOTPSTYUJPO@QSP@MJWFHB[F CPYBDSP file, we can set the Gazebo-ROS plugin of Xtion Pro. Here, we will define the plugin as macro with RGB and depth camera support. Here is the plugin definition: QMVHJOOBNF\OBNF^@GSBNF@DPOUSPMMFS GJMFOBNFMJCHB[FCP@SPT@PQFOOJ@LJOFDUTP BMXBZT0O USVFBMXBZT0O VQEBUF3BUF VQEBUF3BUF DBNFSB/BNF \OBNF^DBNFSB/BNF JNBHF5PQJD/BNF SHCJNBHF@SBXJNBHF5PQJD/BNF QMVHJO

The plugin filename of Xtion Pro is MJCHB[FCP@SPT@PQFOOJ@LJOFDUTP, and we can define the plugin parameters, such as the camera name, image topics, and so on.

Simulating the robotic arm with Xtion Pro Now that we have learned about the camera plugin definition in Gazebo, we can launch the complete simulation using the following command: $ roslaunch seven_dof_arm_gazebo seven_dof_arm_with_rgbd_world.launch

[ 112 ]

Simulating Robots Using ROS and Gazebo

Chapter 4

We can see the robot model with a sensor on the top of the arm, as shown here:

(KIWTG5KOWNCVKQPQHUGXGP&1(CTOYKVJ#UWU:VKQP2TQKP)C\GDQ

We can now work with the simulated rgb-d sensor as if it were directly plugged into our computer. So we can check whether it provides the correct image output.

Visualizing the 3D sensor data After launching the simulation using the preceding command, we can check topics generated by the sensor plugin:

(KIWTGSHCEKOCIGVQRKEUIGPGTCVGFD[)C\GDQ

[ 113 ]

Simulating Robots Using ROS and Gazebo

Chapter 4

Let's view the image data of a 3D vision sensor using the following tool called JNBHF@WJFX: View the RGB raw image: $ rosrun image_view image_view image:=/rgbd_camera/rgb/image_raw

View the IR raw image: $ rosrun image_view image_view image:=/rgbd_camera/ir/image_raw

View the depth image: $ rosrun image_view image_view image:=/rgbd_camera/depth/image_raw

Here is the screenshot with all these images:

(KIWTG8KGYKPIKOCIGUQHVJGSHCEUGPUQTKP)C\GDQ

We can also view the point cloud data of this sensor in RViz. Launch RViz using the following command: $ rosrun rviz rviz -f /rgbd_camera_optical_frame

[ 114 ]

Simulating Robots Using ROS and Gazebo

Chapter 4

Add a PointCloud2 display type and set the Topic as SHCE@DBNFSBEFQUIQPJOUT. We will get a point cloud view as follows:

(KIWTG8KGYKPIRQKPVENQWFFCVCHTQOCPSHCEUGUQTKP48K\

Moving robot joints using ROS controllers in Gazebo In this section, we are going to discuss how to move each joint of the robot in Gazebo. To move each joint, we need to assign an ROS controller. In particular, for each joint we need to attach a controller that is compatible with the hardware interface mentioned inside the USBOTNJTTJPO tags. An ROS controller mainly consists of a feedback mechanism that can receive a set point and control the output using the feedback from the actuators. The ROS controller interacts with the hardware using the hardware interface. The main function of the hardware interface is to act as a mediator between ROS controllers and the real or simulated hardware, allocating the resources to control it considering the data generated by the ROS controller.

[ 115 ]

Simulating Robots Using ROS and Gazebo

Chapter 4

In this robot, we have defined the position controllers, velocity controllers, effort controllers, and so on. The ROS controllers are provided by a set of packages called SPT@DPOUSPM. For a proper understanding of how to configure ROS controllers for the arm, we should understand its concepts. We will discuss more on the SPT@DPOUSPM packages, different types of ROS controllers, and how an ROS controller interacts with the Gazebo simulation.

Understanding the ros_control packages The SPT@DPOUSPM packages have the implementation of robot controllers, controller managers, hardware interfaces, different transmission interfaces, and control toolboxes. The SPT@DPOUSPMT packages are composed of the following individual packages: DPOUSPM@UPPMCPY: This package contains common modules (PID and Sine) that

can be used by all controllers DPOUSPMMFS@JOUFSGBDF: This package contains the JOUFSGBDF base class for controllers DPOUSPMMFS@NBOBHFS: This package provides the infrastructure to MPBE, VOMPBE, TUBSU, and TUPQ controllers DPOUSPMMFS@NBOBHFS@NTHT: This package provides the message and service definition for the controller manager IBSEXBSF@JOUFSGBDF: This contains the base class for the hardware interfaces USBOTNJTTJPO@JOUFSGBDF: This package contains the interface classes for the USBOTNJTTJPO interface (differential, four bar linkage, joint state, position, and velocity)

Different types of ROS controllers and hardware interfaces Let's see the list of ROS packages that contain the standard ROS controllers: KPJOU@QPTJUJPO@DPOUSPMMFS: This is a simple implementation of the joint

position controller KPJOU@TUBUF@DPOUSPMMFS: This is a controller to publish joint states KPJOU@FGGPSU@DPOUSPMMFS: This is an implementation of the joint effort (force) controller

[ 116 ]

Simulating Robots Using ROS and Gazebo

Chapter 4

The following are some of the commonly used hardware interfaces in ROS: +PJOU$PNNBOE*OUFSGBDFT: This will send the commands to the hardware &GGPSU+PJOU*OUFSGBDF: This will send the FGGPSU command 7FMPDJUZ+PJOU*OUFSGBDF: This will send the WFMPDJUZ command 1PTJUJPO+PJOU*OUFSGBDF: This will send the QPTJUJPO command +PJOU4UBUF*OUFSGBDFT: This will retrieve the joint states from the actuators

encoder

How the ROS controller interacts with Gazebo Let's see how an ROS controller interacts with Gazebo. The following figure shows the interconnection of the ROS controller, robot hardware interface, and simulator/real hardware:

(KIWTG+PVGTCEVKPI415EQPVTQNNGTUYKVJ)C\GDQ

We can see the third-party tools, the OBWJHBUJPO and .PWF*U packages. These packages can give the goal (set point) to the mobile robot controllers and robotic arm controllers. These controllers can send the position, velocity, or effort to the robot hardware interface.

[ 117 ]

Simulating Robots Using ROS and Gazebo

Chapter 4

The hardware interface allocates each resource to the controllers and sends values to each resource. The communications between the robot controllers and robot hardware interfaces are shown in the following diagram:

(KIWTG+NNWUVTCVKQPQH415EQPVTQNNGTUCPFJCTFYCTGKPVGTHCEGU

The hardware interface is decoupled from actual hardware and simulation. The values from the hardware interface can be fed to Gazebo for simulation or to the actual hardware itself. The hardware interface is a software representation of the robot and its abstract hardware. The resource of the hardware interfaces are actuators, joints, and sensors. Some resources are read-only, such as joint states, IMU, force-torque sensors, and so on, and some are read and write compatible, such as position, velocity, and effort joints.

[ 118 ]

Simulating Robots Using ROS and Gazebo

Chapter 4

Interfacing joint state controllers and joint position controllers to the arm Interfacing robot controllers to each joint is a simple task. The first task is to write a configuration file for two controllers. The joint state controllers will publish the joint states of the arm and the joint position controllers can receive a goal position for each joint and can move each joint. We will find the configuration file for the controller at TFWFO@EPG@BSN@HB[FCP@DPOUSPMZBNM in the TFWFO@EPG@BSN@HB[FCPDPOGJH folder. Here is the configuration file definition: TFWFO@EPG@BSN 1VCMJTIBMMKPJOUTUBUFT KPJOU@TUBUF@DPOUSPMMFS UZQFKPJOU@TUBUF@DPOUSPMMFS+PJOU4UBUF$POUSPMMFS QVCMJTI@SBUF 1PTJUJPO$POUSPMMFST KPJOU@QPTJUJPO@DPOUSPMMFS UZQFQPTJUJPO@DPOUSPMMFST+PJOU1PTJUJPO$POUSPMMFS KPJOUTIPVMEFS@QBO@KPJOU QJE\QJE^ KPJOU@QPTJUJPO@DPOUSPMMFS UZQFQPTJUJPO@DPOUSPMMFST+PJOU1PTJUJPO$POUSPMMFS KPJOUTIPVMEFS@QJUDI@KPJOU QJE\QJE^ KPJOU@QPTJUJPO@DPOUSPMMFS UZQFQPTJUJPO@DPOUSPMMFST+PJOU1PTJUJPO$POUSPMMFS KPJOUFMCPX@SPMM@KPJOU QJE\QJE^ KPJOU@QPTJUJPO@DPOUSPMMFS UZQFQPTJUJPO@DPOUSPMMFST+PJOU1PTJUJPO$POUSPMMFS KPJOUFMCPX@QJUDI@KPJOU QJE\QJE^ KPJOU@QPTJUJPO@DPOUSPMMFS UZQFQPTJUJPO@DPOUSPMMFST+PJOU1PTJUJPO$POUSPMMFS KPJOUXSJTU@SPMM@KPJOU QJE\QJE^ KPJOU@QPTJUJPO@DPOUSPMMFS UZQFQPTJUJPO@DPOUSPMMFST+PJOU1PTJUJPO$POUSPMMFS KPJOUXSJTU@QJUDI@KPJOU QJE\QJE^ KPJOU@QPTJUJPO@DPOUSPMMFS UZQFQPTJUJPO@DPOUSPMMFST+PJOU1PTJUJPO$POUSPMMFS

[ 119 ]

Simulating Robots Using ROS and Gazebo

Chapter 4

KPJOUHSJQQFS@SPMM@KPJOU QJE\QJE^

We can see that all the controllers are inside the namespace TFWFO@EPG@BSN, and the first line represents the joint state controllers, which will publish the joint state of the robot at the rate of 50 Hz. The remaining controllers are joint position controllers, which are assigned to the first seven joints, and they also define the PID gains.

Launching the ROS controllers with Gazebo If the controller configuration is ready, we can build a launch file that starts all the controllers along with the Gazebo simulation. Navigate to the TFWFO@EPG@BSN@HB[FCPMBVODI directory and open the TFWFO@EPG@BSN@HB[FCP@DPOUSPMMBVODI file: MBVODI -BVODI(B[FCP JODMVEFGJMF GJOE TFWFO@EPG@BSN@HB[FCPMBVODITFWFO@EPG@BSN@XPSMEMBVODI

-PBEKPJOUDPOUSPMMFSDPOGJHVSBUJPOTGSPN:".-GJMFUPQBSBNFUFS TFSWFS SPTQBSBNGJMF GJOE TFWFO@EPG@BSN@HB[FCPDPOGJHTFWFO@EPG@BSN@HB[FCP@DPOUSPMZBNM DPNNBOEMPBE

MPBEUIFDPOUSPMMFST OPEFOBNFDPOUSPMMFS@TQBXOFSQLHDPOUSPMMFS@NBOBHFSUZQFTQBXOFS SFTQBXOGBMTF PVUQVUTDSFFOOTTFWFO@EPG@BSNBSHTKPJOU@TUBUF@DPOUSPMMFS KPJOU@QPTJUJPO@DPOUSPMMFS KPJOU@QPTJUJPO@DPOUSPMMFS KPJOU@QPTJUJPO@DPOUSPMMFS KPJOU@QPTJUJPO@DPOUSPMMFS KPJOU@QPTJUJPO@DPOUSPMMFS KPJOU@QPTJUJPO@DPOUSPMMFS KPJOU@QPTJUJPO@DPOUSPMMFS

DPOWFSUKPJOUTUBUFTUP5'USBOTGPSNTGPSSWJ[FUD OPEFOBNFSPCPU@TUBUF@QVCMJTIFSQLHSPCPU@TUBUF@QVCMJTIFS

[ 120 ]

Simulating Robots Using ROS and Gazebo

Chapter 4

UZQFSPCPU@TUBUF@QVCMJTIFS SFTQBXOGBMTFPVUQVUTDSFFO SFNBQGSPNKPJOU@TUBUFTUPTFWFO@EPG@BSNKPJOU@TUBUFT OPEF MBVODI

The launch files start the Gazebo simulation of the arm, load the controller configuration, load the joint state controller and joint position controllers, and, finally, run the robot state publisher, which publishes the joint states and TF. Let's check the controller topics generated after running this launch file: $ roslaunch seven_dof_arm_gazebo seven_dof_arm_gazebo_control.launch

If the command is successful, we can see these messages in the Terminal:

(KIWTG6GTOKPCNOGUUCIGUYJKNGNQCFKPIVJG415EQPVTQNNGTUQHUGXGP&1(CTO

Here are the topics generated from the controllers when we run this launch file:

(KIWTG2QUKVKQPEQPVTQNNGTEQOOCPFVQRKEUIGPGTCVGFD[VJG415EQPVTQNNGTU

[ 121 ]

Simulating Robots Using ROS and Gazebo

Chapter 4

Moving the robot joints After finishing the preceding topics, we can start commanding positions to each joint. To move a robot joint in Gazebo, we should publish a desired joint value with a message type TUE@NTHT'MPBU to the joint position controller command topics. Here is an example of moving the fourth joint to 1.0 radians: $ rostopic pub /seven_dof_arm/joint4_position_controller/command std_msgs/Float64 1.0

(KIWTG/QXKPICLQKPVQHVJGCTOKP)C\GDQ

We can also view the joint states of the robot by using the following command: $ rostopic echo /seven_dof_arm/joint_states

[ 122 ]

Simulating Robots Using ROS and Gazebo

Chapter 4

Simulating a differential wheeled robot in Gazebo We have seen the simulation of the robotic arm. In this section, we can set up the simulation for the differential wheeled robot that we designed in the previous chapter. You will get the EJGG@XIFFMFE@SPCPUYBDSP mobile robot description from the NBTUFSJOH@SPT@SPCPU@EFTDSJQUJPO@QLHVSEG folder. Let's create a launch file to spawn the simulation model in Gazebo. As we did for the robotic arm, we can create a 304 package to launch a Gazebo simulation using the same dependencies of the TFWFO@EPG@BSN@HB[FCP package, clone the entire package from the following Git repository, or get the package from the book's source code: $ git clone

https://github.com/jocacace/diff_wheeled_robot_gazebo.git

Navigate to the EJGG@XIFFMFE@SPCPU@HB[FCPMBVODI directory and take the EJGG@XIFFMFE@HB[FCPMBVODI file. Here is the definition of this launch: MBVODI UIFTFBSFUIFBSHVNFOUTZPVDBOQBTTUIJTMBVODIGJMFGPSFYBNQMF QBVTFEUSVF BSHOBNFQBVTFEEFGBVMUGBMTF BSHOBNFVTF@TJN@UJNFEFGBVMUUSVF BSHOBNFHVJEFGBVMUUSVF BSHOBNFIFBEMFTTEFGBVMUGBMTF BSHOBNFEFCVHEFGBVMUGBMTF 8FSFTVNFUIFMPHJDJOFNQUZ@XPSMEMBVODI JODMVEFGJMF GJOEHB[FCP@SPTMBVODIFNQUZ@XPSMEMBVODI BSHOBNFEFCVHWBMVF BSHEFCVH BSHOBNFHVJWBMVF BSHHVJ BSHOBNFQBVTFEWBMVF BSHQBVTFE BSHOBNFVTF@TJN@UJNFWBMVF BSHVTF@TJN@UJNF BSHOBNFIFBEMFTTWBMVF BSHIFBEMFTT JODMVEF VSEGYNMSPCPUEFTDSJQUJPOMPBEFEPOUIF1BSBNFUFS4FSWFS QBSBNOBNFSPCPU@EFTDSJQUJPODPNNBOE GJOEYBDSPYBDSPJOPSEFS  GJOE NBTUFSJOH@SPT@SPCPU@EFTDSJQUJPO@QLHVSEGEJGG@XIFFMFE@SPCPUYBDSP  3VOBQZUIPOTDSJQUUPUIFTFOEBTFSWJDFDBMMUPHB[FCP@SPTUP TQBXOB63%'SPCPU OPEFOBNFVSEG@TQBXOFSQLHHB[FCP@SPTUZQFTQBXO@NPEFM

[ 123 ]

Simulating Robots Using ROS and Gazebo

Chapter 4

SFTQBXOGBMTFPVUQVUTDSFFO BSHTVSEGNPEFMEJGG@XIFFMFE@SPCPUQBSBNSPCPU@EFTDSJQUJPO MBVODI

To launch this file, we can use the following command: $ roslaunch diff_wheeled_robot_gazebo diff_wheeled_gazebo.launch

You will see the following robot model in Gazebo. If you get this model, you have successfully finished the first phase of the simulation:

(KIWTG&KbGTGPVKCNYJGGNGFTQDQVKP)C\GDQ

After successful simulation, let's add the laser scanner to the robot.

[ 124 ]

Simulating Robots Using ROS and Gazebo

Chapter 4

Adding the laser scanner to Gazebo We add the laser scanner on the top of Gazebo to perform high-end operations, such as autonomous navigation or map creation using this robot. Here, we should add the following extra code section to EJGG@XIFFMFE@SPCPUYBDSP to add the laser scanner to the robot: MJOLOBNFIPLVZP@MJOL WJTVBM PSJHJOYZ[SQZ HFPNFUSZ CPYTJ[F\IPLVZP@TJ[F^\IPLVZP@TJ[F^\IPLVZP@TJ[F^ HFPNFUSZ NBUFSJBMOBNF#MVF WJTVBM MJOL KPJOUOBNFIPLVZP@KPJOUUZQFGJYFE PSJHJOYZ[\CBTF@SBEJVTIPLVZP@TJ[F^ \CBTF@IFJHIU IPLVZP@TJ[F^SQZ QBSFOUMJOLCBTF@MJOL DIJMEMJOLIPLVZP@MJOL KPJOU HB[FCPSFGFSFODFIPLVZP@MJOL NBUFSJBM (B[FCP#MVFNBUFSJBM UVSO(SBWJUZ0GG GBMTFUVSO(SBWJUZ0GG TFOTPSUZQFSBZOBNFIFBE@IPLVZP@TFOTPS QPTF \IPLVZP@TJ[F^QPTF WJTVBMJ[F GBMTFWJTVBMJ[F VQEBUF@SBUF VQEBUF@SBUF SBZ TDBO IPSJ[POUBM TBNQMFT TBNQMFT SFTPMVUJPO SFTPMVUJPO NJO@BOHMF NJO@BOHMF NBY@BOHMF NBY@BOHMF IPSJ[POUBM TDBO SBOHF NJO NJO NBY NBY SFTPMVUJPO SFTPMVUJPO SBOHF SBZ QMVHJOOBNFHB[FCP@SPT@IFBE@IPLVZP@DPOUSPMMFS GJMFOBNFMJCHB[FCP@SPT@MBTFSTP UPQJD/BNF TDBOUPQJD/BNF

[ 125 ]

Simulating Robots Using ROS and Gazebo

Chapter 4

GSBNF/BNF IPLVZP@MJOLGSBNF/BNF QMVHJO TFOTPS HB[FCP

In this section, we use the Gazebo ROS plugin file called MJCHB[FCP@SPT@MBTFSTP to simulate the laser scanner. The complete code can be found in the EJGG@XIFFMFE@SPCPU@XJUI@MBTFSYBDSP description file in the NBTUFSJOH@SPT@SPCPU@EFTDSJQUJPO@QLHVSEG directory. We can view the laser scanner data by adding some objects in the simulation environment. Here, we add some cylinders around the robot and can see the corresponding laser view in the next section of the figure:

(KIWTG&KbGTGPVKCNFTKXGTQDQVKPTCPFQOQDLGEVKP)C\GDQ

The laser scanner plugin publishes laser data ( TFOTPS@NTHT-BTFS4DBO ) into the TDBO topic.

Moving the mobile robot in Gazebo The robot we are working with is a differential robot with two wheels and two caster wheels. The complete characteristics of the robot should model as the Gazebo-ROS plugin for the simulation. Luckily, the plugin for a basic differential drive is already implemented. To move the robot in Gazebo, we should add a Gazebo-ROS plugin file called MJCHB[FCP@SPT@EJGG@ESJWFTP to get the differential drive behavior in this robot.

[ 126 ]

Simulating Robots Using ROS and Gazebo

Chapter 4

Here is the complete code snippet of the definition of this plugin and its parameters: %JGGFSFOUJBMESJWFDPOUSPMMFS HB[FCP QMVHJOOBNFEJGGFSFOUJBM@ESJWF@DPOUSPMMFS GJMFOBNFMJCHB[FCP@SPT@EJGG@ESJWFTP SPT%FCVH-FWFM %FCVHSPT%FCVH-FWFM QVCMJTI8IFFM5' GBMTFQVCMJTI8IFFM5' SPCPU/BNFTQBDF SPCPU/BNFTQBDF QVCMJTI5G QVCMJTI5G QVCMJTI8IFFM+PJOU4UBUF GBMTFQVCMJTI8IFFM+PJOU4UBUF BMXBZT0O USVFBMXBZT0O VQEBUF3BUF VQEBUF3BUF MFGU+PJOU GSPOU@MFGU@XIFFM@KPJOUMFGU+PJOU SJHIU+PJOU GSPOU@SJHIU@XIFFM@KPJOUSJHIU+PJOU XIFFM4FQBSBUJPO \ CBTF@SBEJVT^XIFFM4FQBSBUJPO XIFFM%JBNFUFS \ XIFFM@SBEJVT^XIFFM%JBNFUFS CSPBEDBTU5' CSPBEDBTU5' XIFFM5PSRVF XIFFM5PSRVF XIFFM"DDFMFSBUJPO XIFFM"DDFMFSBUJPO DPNNBOE5PQJD DNE@WFMDPNNBOE5PQJD PEPNFUSZ'SBNF PEPNPEPNFUSZ'SBNF PEPNFUSZ5PQJD PEPNPEPNFUSZ5PQJD SPCPU#BTF'SBNF CBTF@GPPUQSJOUSPCPU#BTF'SBNF

QMVHJO HB[FCP

We can provide the parameters such as the wheel joints of the robot (joints should be of a continuous type), wheel separation, wheel diameters, odometry topic, and so on, in this plugin. An important parameter that we need to move the robot is: DPNNBOE5PQJD DNE@WFMDPNNBOE5PQJD

This parameter is the command velocity topic to the plugin, which is basically a 5XJTU message in ROS (TFOTPS@NTHT/5XJTU). We can publish the 5XJTU message into the DNE@WFM topic, and we can see the robot start to move from its position.

[ 127 ]

Simulating Robots Using ROS and Gazebo

Chapter 4

Adding joint state publishers in the launch file After adding the differential drive plugin, we need to join state publishers to the existing launch file, or we can build a new one. You can see the new final launch file, EJGG@XIFFMFE@HB[FCP@GVMMMBVOD , in EJGG@XIFFMFE@SPCPU@HB[FCPMBVODI. The launch file contains joint state publishers, which help to visualize in RViz. Here are the extra lines added in this launch file for the joint state publishing: OPEFOBNFKPJOU@TUBUF@QVCMJTIFSQLHKPJOU@TUBUF@QVCMJTIFS UZQFKPJOU@TUBUF@QVCMJTIFS OPEF TUBSUSPCPUTUBUFQVCMJTIFS OPEFQLHSPCPU@TUBUF@QVCMJTIFSUZQFSPCPU@TUBUF@QVCMJTIFS OBNFSPCPU@TUBUF@QVCMJTIFSPVUQVUTDSFFO QBSBNOBNFQVCMJTI@GSFRVFODZUZQFEPVCMFWBMVF OPEF

Adding the ROS teleop node The ROS teleop node publishes the ROS 5XJTU command by taking keyboard inputs. From this node, we can generate both linear and angular velocity, and there is already a standard teleop node implementation available; we can simply reuse the node. The teleop is implemented in the EJGG@XIFFMFE@SPCPU@DPOUSPM package. The script folder contains the EJGG@XIFFMFE@SPCPU@LFZ node, which is the teleop node. As per usual, you can get this package from the code provided with the book or download it from the following link: $ git clone

https://github.com/jocacace/diff_wheeled_robot_control.git

To successfully compile and use this package, you may need to install the KPZ@OPEF package: $ sudo apt-get install ros-kinetic-joy

[ 128 ]

Simulating Robots Using ROS and Gazebo

Chapter 4

Here is the launch file called LFZCPBSE@UFMFPQMBVODI to start the teleop node: MBVODI EJGGFSFOUJBM@UFMFPQ@LFZBMSFBEZIBTJUTPXOCVJMUJOWFMPDJUZ TNPPUIFS OPEFQLHEJGG@XIFFMFE@SPCPU@DPOUSPMUZQFEJGG@XIFFMFE@SPCPU@LFZ OBNFEJGG@XIFFMFE@SPCPU@LFZPVUQVUTDSFFO QBSBNOBNFTDBMF@MJOFBSWBMVFUZQFEPVCMF QBSBNOBNFTDBMF@BOHVMBSWBMVFUZQFEPVCMF SFNBQGSPNUVSUMFCPU@UFMFPQ@LFZCPBSEDNE@WFMUPDNE@WFM OPEF MBVODI

Let's start moving the robot. Launch Gazebo with complete simulation settings, using the following command: $ roslaunch diff_wheeled_robot_gazebo diff_wheeled_gazebo_full.launch

Start the teleop node: $ roslaunch diff_wheeled_robot_control keyboard_teleop.launch

Start RViz to visualize the robot state and laser data: $ rosrun rviz rviz

Add 'JYFE'SBNFPEPN, add -BTFS4DBO , and the topic as TDBO to view the laser scan data, and add the 3PCPUNPEFM to view the robot model. In the teleop terminal, we can use some keys (U, I, O, J, K, L, M, ",", ".") for direction adjustment and other keys (q, z, w, x, e, c, k, space key) for speed adjustments. Here is the screenshot showing the robot moving in Gazebo using the teleop and its visualization in RViz.

[ 129 ]

Simulating Robots Using ROS and Gazebo

Chapter 4

We can add primitive shapes from the Gazebo toolbar to the robot environment or we can add objects from the online library, which is on the left-side panel:

(KIWTG/QXKPIFKbGTGPVKCNFTKXGTQDQVKP)C\GDQWUKPIVGNGQRGTCVKQP

The robot will only move when we press the appropriate key inside the teleop node terminal. If this terminal is not active, pressing the key will not move the robot. If everything works well, we can explore the area using the robot and visualizing the laser data in RViz.

Questions Why do we perform robotic simulation? How can we add sensors into a Gazebo simulation? What are the different types of ROS controllers and hardware interfaces? How can we move the mobile robot in a Gazebo simulation?

[ 130 ]

Simulating Robots Using ROS and Gazebo

Chapter 4

Summary After designing the robot, the next phase is its simulation. There are a lot of uses in simulation. We can validate a robot design, and at the same time, we can work with a robot without having its real hardware. There are some situations when we need to work without having a robot hardware. Simulators are useful in all these situations. In this chapter, we were trying to simulate two robots, one was a robotic arm with sevenDOF and the other was a differential wheeled mobile robot. We started with the robotic arm, and discussed the additional Gazebo tags needed to launch the robot in Gazebo. We discussed how to add a 3D vision sensor to the simulation. Later, we created a launch file to start Gazebo with a robotic arm and discussed how to add controllers to each joint. We added the controllers and worked with each joint. Like the robotic arm, we created the URDF for the Gazebo simulation and added the necessary Gazebo-ROS plugin for the laser scanner and differential drive mechanism. After completing the simulation model, we launched the simulation using a custom launch file. Finally, we looked at how to move the robot using the teleop node. We can learn more about the robotic arm and mobile robots, which are supported by ROS, from the following link: IUUQXJLJSPTPSH3PCPUT. In the next chapter, we will see how to simulate robots using another famous robotics simulation: V-REP.

[ 131 ]

5

Simulating Robots Using ROS and V-REP Having learned how to simulate robots with Gazebo, in this chapter, we will discuss how to use another powerful and famous simulation software: V-REP (Virtual Robot Experimentation Platform, IUUQXXXDPQQFMJBSPCPUJDTDPN). V-REP is a multi-platform robotic simulator developed by Coppelia Robotics. It offers many simulation models of popular industrial and mobile robots ready to be used, and different functionalities that can be easily integrated and combined through a dedicated API. In addition, V-REP can operate with ROS using a communication interface that allows us to control the simulation scene and the robots via topics and services. Like Gazebo, V-REP can be used as a standalone software, while an external plugin must be installed to work with ROS. In this chapter, we will learn how to set up the V-REP simulator and install the ROS communication bridge, discussing some initial codes to understand how it works. We will show how to interact with V-REP using services and topics and how to import and interface a new robot model using the URDF file. Finally, we will discuss how to interact with popular mobile robots imported from the V-REP model database, enriching it with additional sensors.

Simulating Robots Using ROS and V-REP

Chapter 5

We will cover the following topics in this chapter: Setting up V-REP with ROS Understanding WSFQ@QMVHJO Interacting with V-REP using ROS Importong a robot model using an URDF file Implementing a ROS interface to simulate a robotic arm in V-REP Controlling a mobile robot using V-REP Adding additional sensors to simulated robots

Setting up V-REP with ROS Before starting to work with V-REP, we need to install it in our system and compile the ROS packages needed to establish the communication bridge between ROS and the simulation scene. V-REP is a cross-platform software, available for different operating systems, such as Windows, macOS, and Linux. It is developed by Coppelia Robotics GmbH and is distributed with both free educational and commercial licenses. Download the last version of the V-REP simulator from the Coppelia Robotics download page: IUUQXXXDPQQFMJBSPCPUJDTDPN EPXOMPBETIUNM, choosing the Linux version of the V-REP PRO EDU software. In this chapter, we will refer to the V-REP version W. You can download this version, if already available on the website, using the following command in any desired directory: $ wget http://coppeliarobotics.com/files/V-REP_PRO_EDU_V3_4_0_Linux.tar.gz

After completing the download, extract the archive: $ tar -zxvf V-REP_PRO_EDU_V3_4_0_Linux.tar.gz

To easily access V-REP resources, it is convenient to set the 73&1@3005 environmental variable that points to the V-REP main folder: $ echo "export VREP_ROOT=/path/to/v_rep/folder >> ~/.bashrc"

[ 133 ]

Simulating Robots Using ROS and V-REP

Chapter 5

V-REP offers the following modes to control simulated robots from external applications: Remote API: The V-REP remote API is composed of several functions that can be called from external applications developed in C/C++, Python, Lua, or Matlab. The remote API interacts with V-REP over the network, using socket communication. You can integrate the remote API in your C++ or Python nodes, in order to connect ROS with the simulation scene. The list of all remote APIs available in V-REP can be found on the Coppelia Robotics website: IUUQXXX DPQQFMJBSPCPUJDTDPNIFMQ'JMFTFOSFNPUF"QJ'VODUJPOTIUN. To use the remote API, you must implement both client and server sides: V-REP Client: The client side resides in the external application. It can be implemented in a ROS node or in a standard program written in one of the supported programming languages. V-REP Server: This side is implemented in V-REP scripts and allows the simulator to receive external data to interact with the simulation scene. RosPlugin: The V-REP RosPlugin implements a high-level abstraction that directly connects the simulated object scene with the ROS communication system. Using this plugin, you can automatically apply subscribed messages and publish topics from scene objects to get information or control simulated robots. RosInterface: Introduced in the latest versions of V-REP, this interface will substitute the RosPlugin in the future versions. Differently from the RosPlugin, this module duplicates the C++ API functions to allow ROS and V-REP communication. In this book, we will discuss how to interact with V-REP using the RosPlugin. The first thing to do is to compile the ROS communication bridge. We must add two packages to our ROS workspace: WSFQ@DPNNPO and WSFQ@QMVHJO. As usual, you can clone the entire package from the following GitHub repository or obtain the package from the book's source code: $ git clone https://github.com/jocacace/vrep_common.git $ git clone https://github.com/jocacace/vrep_plugin.git

Compile the packages with the DBULJO@NBLF command. If everything goes right, the compilation will create the WSFQ@QMVHJO shared library: MJCW@SFQ&YU3PTTP. This file is located in the EFWFMMJCdirectory in the ROS workspace. To enable V-REP to use this library, we need to copy it into the main WSFQ folder: $ cp devel/lib/libv_repExtRos.so $VREP_ROOT

[ 134 ]

Simulating Robots Using ROS and V-REP

Chapter 5

This library allows V-REP to connect with an active instance of roscore at startup, transforming it in a ROS node linked to the framework. So, to connect V-REP with ROS, an instance of roscore must be executed before launching V-REP. To test that everything is working properly, start SPTDPSF and launch the V-REP software: $ roscore & $VREP_ROOT/vrep.sh

During the startup, all V-REP plugins installed in the system will be loaded. We can check if the WSFQ@QMVHJO is loaded, as shown in the following figure:

(KIWTG2NWIKPUNQCFKPIFWTKPI84'2UVCTVWR

In addition, after starting the V-REP program, a new topic is published containing information about the simulation status. Listing the active topics, we can check that the WSFQJOGP topic is published. As shown in the next figure, this message provides information about the state of the simulation, if it is running or not, and information about the simulation time:

(KIWTG%QPVGPVQHXTGRKPHQOGUUCIG

[ 135 ]

Simulating Robots Using ROS and V-REP

Chapter 5

To explore WSFQ@QMVHJO functionalities, we can have a look at the QMVHJO@QVCMJTIFS@TVCTDSJCFSUUU scene, located in the WSFQ@EFNP@QLHTDFOF folder. To open this scene, use the main drop-down menu and select the entry: File | Open Scene. This simulation is based on the SPT5PQJD1VCMJTIFS"OE4VCTDSJCFSUUU scene proposed in older V-REP versions. After opening this scene, the simulation windows should appear, as in the following image:

QMVHJO@QVCMJTIFS@TVCTDSJCFSUUUUKOWNCVKQPUEGPG

(KIWTG

In this scene, a robot is equipped with two cameras: one active camera acquiring images from the environment, publishing the video stream on a specific topic, and a passive camera, that only acquires the video stream from the same topic. We can press the QMBZ button on the main bar of the V-REP interface.

[ 136 ]

Simulating Robots Using ROS and V-REP

Chapter 5

After that the simulation starts; this is what will happen:

(KIWTG+OCIGRWDNKUJGFCPFUWDUETKDGTGZCORNG

In this simulation, the passive camera displays the image published from the active one, receiving vision data directly from the ROS framework. We can also visualize the video stream published by V-REP using the JNBHF@WJFX package: $ rosrun image_view image_view image:=/camera/image_raw

Understanding the vrep_plugin The WSFQ@QMVHJO is part of the V-REP API framework. Even though the plugin is correctly installed in your system, the load operation will fail if the roscore was not running at that time. A pop-up error will inform users if the simulations scenes need the WSFQ@QMVHJO because the roscore was not running before running the simulator, or it is not installed in the system:

[ 137 ]

Simulating Robots Using ROS and V-REP

(KIWTG'TTQTFKURNC[GFYJGP

Chapter 5

WSFQ@QMVHJOKUWUGFYKVJQWVTWPPKPIVJGTQUEQTG

After starting up V-REP with the WSFQ@QMVHJO correctly loaded, V-REP will act as a ROS node called WSFQ. This can be shown using the following command: $ rosnode list

WSFQ@QMVHJO

(KIWTG.KUVQHCEVKXG415PQFGUCHVGTTWPPKPI84'2YKVJ

Other ROS nodes can communicate with V-REP in the following ways: WSFQ@QMVHJO offers ROS services. Different services are available after launching

V-REP to control the simulation scene or its state. WSFQ@QMVHJO can be enabled to subscribe or advertise topics. As a normal ROS node, simulation models can communicate via topics. We can start interacting with V-REP using services. Let's create a ROS package with the following dependencies: $ catkin_create_pkg vrep_demo_pkg roscpp vrep_common std_msgs geometry_msgs

[ 138 ]

Simulating Robots Using ROS and V-REP

Chapter 5

Alternatively, it is possible to clone the entire package from the following GitHub repository or get the entire package from the book's source code: $ git clone https://github.com/jocacace/vrep_demo_pkg.git

Interacting with V-REP using ROS services As a first example, we will use ROS services to start and stop the simulation scene. To do this, we must call WSFQTJN3PT4UBSU4JNVMBUJPO and theWSFQTJN3PT4UPQ4JNVMBUJPO services respectively. We will discuss the source code of the TUBSU@TUPQ@TDFOFDQQ file located in the WSFQ@EFNP@QLHTSD directory: JODMVEFSPTSPTI JODMVEFWSFQ@DPNNPOTJN3PT4UBSU4JNVMBUJPOI JODMVEFWSFQ@DPNNPOTJN3PT4UPQ4JNVMBUJPOI JOUNBJO JOUBSHDDIBS BSHW\ SPTJOJU BSHDBSHWTUBSU@TUPQ@WSFQ@OPEF SPT/PEF)BOEMFO SPT4FSWJDF$MJFOUTUBSU@WSFQ@DMJFOU OTFSWJDF$MJFOUWSFQ@DPNNPOTJN3PT4UBSU4JNVMBUJPO WSFQTJN3PT4UBSU4JNVM BUJPO WSFQ@DPNNPOTJN3PT4UBSU4JNVMBUJPOTUBSU@TSW SPT4FSWJDF$MJFOUTUPQ@WSFQ@DMJFOU OTFSWJDF$MJFOUWSFQ@DPNNPOTJN3PT4UPQ4JNVMBUJPO WSFQTJN3PT4UPQ4JNVMBU JPO WSFQ@DPNNPOTJN3PT4UPQ4JNVMBUJPOTUPQ@TSW 304@*/'0 4UBSUJOH7SFQTJNVMBUJPO JG TUBSU@WSFQ@DMJFOUDBMM TUBSU@TSW\ JG TUBSU@TSWSFTQPOTFSFTVMU\ 304@*/'0 4JNVMBUJPOTUBSUFEXBJUTFDPOETCFGPSFTUPQ JU TMFFQ  JG TUPQ@WSFQ@DMJFOUDBMM TUPQ@TSW\ JG TUPQ@TSWSFTQPOTFSFTVMU\ 304@*/'0 4JNVMBUJPOTUPQQFE ^ ^ FMTF 304@&3303 'BJMFEUPDBMMWSFQTJN3PT4UPQ4JNVMBUJPO TFSWJDF ^

[ 139 ]

Simulating Robots Using ROS and V-REP

Chapter 5

^ FMTF 304@&3303 'BJMFEUPDBMMWSFQTJN3PT4UBSU4JNVMBUJPOTFSWJDF ^

Let's see the explanation of the code: SPT4FSWJDF$MJFOUTUBSU@WSFQ@DMJFOU OTFSWJDF$MJFOUWSFQ@DPNNPOTJN3PT4UBSU4JNVMBUJPO WSFQTJN3PT4UBSU4JNVM BUJPO WSFQ@DPNNPOTJN3PT4UBSU4JNVMBUJPOTUBSU@TSW SPT4FSWJDF$MJFOUTUPQ@WSFQ@DMJFOU OTFSWJDF$MJFOUWSFQ@DPNNPOTJN3PT4UPQ4JNVMBUJPO WSFQTJN3PT4UPQ4JNVMBU JPO WSFQ@DPNNPOTJN3PT4UPQ4JNVMBUJPOTUPQ@TSW

Here, we declare the service client objects, as already seen in Chapter 1. These services communicate with the WSFQ@DPNNPOTJN3PT4UBSU4JNVMBUJPO and WSFQ@DPNNPOTJN3PT4UBSU4JNVMBUJPONFTTBHF types respectively. These services do not require any input value, while returning the success or failure of the start/stop operation. If the start operation is executed without errors, we can stop the simulation after a certain time, as shown in the following code: 304@*/'0 4UBSUJOH7SFQTJNVMBUJPO JG TUBSU@WSFQ@DMJFOUDBMM TUBSU@TSW\ JG TUBSU@TSWSFTQPOTFSFTVMU\ 304@*/'0 4JNVMBUJPOTUBSUFEXBJUTFDPOETCFGPSFTUPQJU TMFFQ  JG TUPQ@WSFQ@DMJFOUDBMM TUPQ@TSW\ JG TUPQ@TSWSFTQPOTFSFTVMU\ 304@*/'0 4JNVMBUJPOTUPQQFE ^ ^ FMTF 304@&3303 'BJMFEUPDBMMWSFQTJN3PT4UPQ4JNVMBUJPO TFSWJDF ^ ^ FMTF 304@&3303 'BJMFEUPDBMMWSFQTJN3PT4UBSU4JNVMBUJPOTFSWJDF

[ 140 ]

Simulating Robots Using ROS and V-REP

Chapter 5

We can now use another service, WSFQTJN3PT"EE4UBUVTCBS.FTTBHF, to publish messages to the status bar of V-REP. We can improve the previous code with the following lines, as reported in the TUBSU@TUPQ@TDFOF@XJUI@NTHDQQ file: JOUDOU XIJMF DOU

\ TUETUSJOHTUSFBNTT TT4JNVMBUJPOXIJMFTUPQJODOUTFDPOET NTH@TSWSFRVFTUNFTTBHFTTTUS  JG NTH@DMJFOUDBMM NTH@TSW\ 304@8"3/ 'BJMFEUPDBMMWSFQTJN3PT"EE4UBUVTCBS.FTTBHF TFSWJDF ^ TMFFQ  ^

Here the TJN3PT"EE4UBUVTCBS.FTTBHF service is used to display how many seconds remain before stopping the simulation. We can test this behavior by compiling and running the TUBSU@TUPQ@TDFOF@XJUI@NTHDQQ node: $ rosrun vrep_demo_pkg start_stop_scene_with_msg

The output of this node on the V-REP window is shown in the following screenshot:

(KIWTG/GUUCIGUUGPVVQ84'2UVCVWUDCTXKC

[ 141 ]

WSFQ@QMVHJO

Simulating Robots Using ROS and V-REP

Chapter 5

Interacting with V-REP using ROS topics We will now discuss how to use topics to communicate with V-REP. This is useful when we want to send information to the objects of the simulation, or retrieve data generated by robots. While services are enabled when V-REP starts, topic communication happens only on demand, initializing publisher and subscriber variables in the simulation scene. The most common way to program the simulation scene of V-REP is via Lua scripts. Every object of the scene can be associated to a script that is automatically invoked when a simulation starts and is cyclically executed during the simulation time. In the next example, we will create a scene with two objects. One will be programmed to receive a float data from a specific topic, while the other one republishes the same data on another topic. Use the drop-down menu on the Scene hierarchy panel, select the entry: Add | Dummy. We can create two objects, a EVNNZ@QVCMJTIFS and a EVNNZ@TVCTDSJCFS and associate a script for each of them. Use the right mouse button on the created objects, and select the entry Add | Associated child script | Non-threaded, as shown in the following figure:

(KIWTG#UUQEKCVKPICPQPVJTGCFGFUETKRVVQ84'2QDLGEV

[ 142 ]

Simulating Robots Using ROS and V-REP

Chapter 5

Alternatively, we can directly load the simulation scene by opening the EFNP@QVCMJTIFS@TVCTDSJCFSUUU file located in the WSFQ@EFNP@QLHTDFOF directory. Let's see the content of the script associated to the EVNNZ@TVCTDSJCFS object: JG TJN@DBMM@UZQFTJN@DIJMETDSJQUDBMM@JOJUJBMJ[BUJPOUIFO MPDBMNPEVMF/BNF MPDBMNPEVMF7FSTJPO MPDBMJOEFY MPDBMQMVHJO/PU'PVOEUSVF XIJMFNPEVMF/BNFEP NPEVMF/BNFNPEVMF7FSTJPOTJN(FU.PEVMF/BNF JOEFY JG NPEVMF/BNF 3PT UIFO QMVHJO/PU'PVOEGBMTF FOE JOEFYJOEFY  FOE JG QMVHJO/PU'PVOEUIFO TJN%JTQMBZ%JBMPH &SSPS  304QMVHJOXBTOPUGPVOEO4JNVMBUJPOXJMMOPU SVOQSPQFSMZ TJN@EMHTUZMF@PLGBMTFOJM\^\^ FMTF &OBCMFTVCTDSJCFSUPWSFQ@EFNPGMPBU@JOUPQJD TJN&YU304@FOBCMF4VCTDSJCFS WSFQ@EFNPGMPBU@JOTJNSPT@TUSNDNE@TFU@GMPBU @TJHOBMJO FOE FOE JG TJN@DBMM@UZQFTJN@DIJMETDSJQUDBMM@BDUVBUJPOUIFO FOE JG TJN@DBMM@UZQFTJN@DIJMETDSJQUDBMM@TFOTJOHUIFO FOE JG TJN@DBMM@UZQFTJN@DIJMETDSJQUDBMM@DMFBOVQUIFO FOE

Each Lua script linked to V-REP objects contains the following four sections: TJN@DIJMETDSJQUDBMM@JOJUJBMJ[BUJPO: This section is executed only the

first time that the simulation starts. TJN@DIJMETDSJQUDBMM@BDUVBUJPO: This section is cyclically called at the same frame rate of the simulation. Users can put here the code that controls the actuation of the robot. TJN@DIJMETDSJQUDBMM@TFOTJOH: This part will be executed in each simulation step, during the sensing phase of a simulation step. TJN@DIJMETDSJQUDBMM@DMFBOVQ: This section is called just before the simulation ends.

[ 143 ]

Simulating Robots Using ROS and V-REP

Chapter 5

Let's see the explanation of the preceding code: JG TJN@DBMM@UZQFTJN@DIJMETDSJQUDBMM@JOJUJBMJ[BUJPOUIFO MPDBMNPEVMF/BNF MPDBMNPEVMF7FSTJPO MPDBMJOEFY MPDBMQMVHJO/PU'PVOEUSVF XIJMFNPEVMF/BNFEP NPEVMF/BNFNPEVMF7FSTJPOTJN(FU.PEVMF/BNF JOEFY JG NPEVMF/BNF 3PT UIFO QMVHJO/PU'PVOEGBMTF FOE JOEFYJOEFY  FOE

In the initialization part, we check if the WSFQ@QMVHJO is installed in the system, otherwise, an error is displayed: TJN&YU304@FOBCMF4VCTDSJCFS WSFQ@EFNPGMPBU@JOTJNSPT@TUSNDNE@TFU@GMPBU @TJHOBMJO

This activates the subscriber of the input float value on the WSFQ@EFNPGMPBU@JO topic. The TJN&YU304@FOBCMF4VCTDSJCFS function expects as parameters the name of the topic, the desired queue size, the desired type to stream, and three enabling parameters. These parameters specify the item upon which the data should be applied. For example, if we want to set the position of a joint object, the first parameter will be the object handle, while the other parameters will not be used. In our case, we want to save the value received from the topic into the variable JO. Let's now see the content of the script associated to the EVNNZ@QVCMJTIFS object: JG TJN@DBMM@UZQFTJN@DIJMETDSJQUDBMM@JOJUJBMJ[BUJPOUIFO $IFDLJGUIFSFRVJSFEQMVHJOJTUIFSF MJCW@SFQ&YU3PTTPPS MJCW@SFQ&YU3PTEZMJC MPDBMNPEVMF/BNF MPDBMNPEVMF7FSTJPO MPDBMJOEFY MPDBMQMVHJO/PU'PVOEUSVF XIJMFNPEVMF/BNFEP NPEVMF/BNFNPEVMF7FSTJPOTJN(FU.PEVMF/BNF JOEFY JG NPEVMF/BNF 3PT UIFO QMVHJO/PU'PVOEGBMTF FOE JOEFYJOEFY  FOE JG QMVHJO/PU'PVOEUIFO

[ 144 ]

Simulating Robots Using ROS and V-REP

Chapter 5

TJN%JTQMBZ%JBMPH &SSPS  304QMVHJOXBTOPUGPVOEO4JNVMBUJPO XJMMOPUSVOQSPQFSMZ TJN@EMHTUZMF@PLGBMTFOJM \^\^ FMTF TJN&YU304@FOBCMF1VCMJTIFS WSFQ@EFNPGMPBU@PVU TJNSPT@TUSNDNE@HFU@GMPBU@TJHOBMPVU FOE FOE FOE JG TJN@DBMM@UZQFTJN@DIJMETDSJQUDBMM@BDUVBUJPOUIFO (FUWBMVFPGJOQVUTJHOBMBOEQVCMJTIJUPOWSFQ@EFNPGMPBU@PVUUPQJD EBUBTJN(FU'MPBU4JHOBM JO  JG EBUBUIFO TJN4FU'MPBU4JHOBM PVUEBUB TJN"EE4UBUVTCBS.FTTBHF EBUB FOE FOE JG TJN@DBMM@UZQFTJN@DIJMETDSJQUDBMM@TFOTJOHUIFO 1VUZPVSNBJO4&/4*/(DPEFIFSF FOE JG TJN@DBMM@UZQFTJN@DIJMETDSJQUDBMM@DMFBOVQUIFO 1VUTPNFSFTUPSBUJPODPEFIFSF &OE

The code is explained here: TJN&YU304@FOBCMF1VCMJTIFS WSFQ@EFNPGMPBU@PVU TJNSPT@TUSNDNE@HFU@GMPBU@TJHOBMPVU

In this line, after checking the correct installation of the WSFQ@QMVHJO, we will enable the publisher of the float value. After this line, the script publishes continuously the value of the variable PVU: JG TJN@DBMM@UZQFTJN@DIJMETDSJQUDBMM@BDUVBUJPOUIFO (FUWBMVFPGJOQVUTJHOBMBOEQVCMJTIJUPOWSFQ@EFNPGMPBU@PVUUPQJD EBUBTJN(FU'MPBU4JHOBM JO  JG EBUBUIFO TJN4FU'MPBU4JHOBM PVUEBUB TJN"EE4UBUVTCBS.FTTBHF EBUB FOE FOE

Finally, we set the value of the out variable with the one received from the WSFQ@EFNPGMPBU@JOUPQJD, stored in the JO variable. Note that JO and PVU are special global variables accessible from all scripts of the scene. These variables in V-REP are called signals.

[ 145 ]

Simulating Robots Using ROS and V-REP

Chapter 5

After running the simulator, we can check that everything works correctly, publishing a desired number on the input topic and monitoring the output from the output topic, using the following commands: $ rostopic pub /vrep_demo/float_in std_msgs/Float32 "data: 2.0" -r 12 $ rostopic echo /vrep_demo/float_out

(KIWTG4WPPKPI84'2RWDNKUJGTCPFUWDUETKDGTFGOQ

Simulating the robotic arm using V-REP and ROS In the previous chapter, we used Gazebo to import and simulate the seven-DOF arm designed in $IBQUFS, Working with 3D Robot Modeling in ROS. Here, we will do the same thing using V-REP. The first step to simulate our seven-DOF arm is to import it in the simulation scene. V-REP allows you to import new robots using URDF files; for this reason, we must convert the xacro model of the arm in a URDF file, saving the generated URDF file in the URDF folder of the WSFQ@EFNP@QLH package: $ rosrun xacro xacro seven_dof_arm.xacro -inorder > /path/to/vrep_demo_pkg/urdf/seven_dof_arm.urdf

[ 146 ]

Simulating Robots Using ROS and V-REP

Chapter 5

We can now import the robot model using the URDF import plugin. Select from the main drop-down menu the entry Plugins | URDF import and press the import button, choosing the default import options from the dialog window. Finally, select the desired file to import and the seven-DOF arm will appear in the scene:

(KIWTG5KOWNCVKQPQHUGXGP&1(CTOKP84'2

All the components of the robot are now imported into the scene, as we can see from the Scene hierarchy panel, in which are reported the set of robot joints and links defined in the URDF file.

[ 147 ]

Simulating Robots Using ROS and V-REP

Chapter 5

Even if the robot has been correctly imported, is not ready to be controlled yet. To actuate the robot, we need to enable all robot motors from the Joint Dynamic Properties panel. Until the motor is disabled, it is not possible to move it during the simulation. To enable the motor of a joint, open the Scene object proprieties panel, selecting its entry from the main drop-down menu: Tools | Scene object proprieties. You can also open this dialog with a double-click on an object icon in the TDFOFIJFSBSDIZ. From this new window, open the dynamic properties dialog and enable the motor and the control loop of the joint, selecting the controller type. By default, the motor is controlled via a PID, as shown in the following figure:

(KIWTG5EGPG1DLGEV2TQRTKGVKGUCPF,QKPV&[PCOKE2TQRTKGVKGUFKCNQIU

To increase the performance of the control loop, PID gains should be properly tuned. After enabling motors and control loops for all robot joints, we can check that everything has been configured correctly. Run the simulation and set a target position from the Scene Object Proprieties panel.

[ 148 ]

Simulating Robots Using ROS and V-REP

Chapter 5

Here is an example of moving the fourth joint to 1.0 radians:

(KIWTG/QXKPICLQKPVQHVJGCTOHTQO84'21DLGEV2TQRTKGVKGUFKCNQI

Adding the ROS interface to V-REP joint controllers In this section, we will learn how to interface the seven-DOF arm with the WSFQ@QMVHJO to stream the state of its joints and receive the control input via topics. As already seen in the previous example, select a component of the robot (for example the CBTF@MJOL@SFTQPOEBCMF) and create a Lua script that will manage the communication between V-REP and ROS. Here is the script source code: JG TJN@DBMM@UZQFTJN@DIJMETDSJQUDBMM@JOJUJBMJ[BUJPOUIFO $IFDLJGUIFSFRVJSFEQMVHJOJTUIFSF MJCW@SFQ&YU3PTTPPS MJCW@SFQ&YU3PTEZMJC MPDBMNPEVMF/BNF MPDBMNPEVMF7FSTJPO MPDBMJOEFY MPDBMQMVHJO/PU'PVOEUSVF

[ 149 ]

Simulating Robots Using ROS and V-REP

Chapter 5

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

[ 150 ]

Simulating Robots Using ROS and V-REP

Chapter 5

TJNSPT@TUSNDNE@TFU@KPJOU@UBSHFU@QPTJUJPOXSJTU@SPMM@IBOEMF  TJN&YU304@FOBCMF4VCTDSJCFS WSFQ@EFNPTFWFO@EPG@BSNXSJTU@QJUDIDUSM  TJNSPT@TUSNDNE@TFU@KPJOU@UBSHFU@QPTJUJPOXSJTU@QJUDI@IBOEMF  TJN&YU304@FOBCMF4VCTDSJCFS WSFQ@EFNPTFWFO@EPG@BSNHSJQQFS@SPMMDUSM  TJNSPT@TUSNDNE@TFU@KPJOU@UBSHFU@QPTJUJPOHSJQQFS@SPMM@IBOEMF  FOE FOE JG TJN@DBMM@UZQFTJN@DIJMETDSJQUDBMM@DMFBOVQUIFO FOE JG TJN@DBMM@UZQFTJN@DIJMETDSJQUDBMM@TFOTJOHUIFO FOE JG TJN@DBMM@UZQFTJN@DIJMETDSJQUDBMM@BDUVBUJPOUIFO FOE

Let's look at the following the explanation of the code. After we have checked the correct installation of WSFQ@QMVHJO, we initialize an object handler for each joint of the arm: TIPVMEFS@QBO@IBOEMFTJN(FU0CKFDU)BOEMF TIPVMEFS@QBO@KPJOU  TIPVMEFS@QJUDI@IBOEMFTJN(FU0CKFDU)BOEMF TIPVMEFS@QJUDI@KPJOU  FMCPX@SPMM@IBOEMFTJN(FU0CKFDU)BOEMF FMCPX@SPMM@KPJOU  FMCPX@QJUDI@IBOEMFTJN(FU0CKFDU)BOEMF FMCPX@QJUDI@KPJOU  XSJTU@SPMM@IBOEMFTJN(FU0CKFDU)BOEMF XSJTU@SPMM@KPJOU  XSJTU@QJUDI@IBOEMFTJN(FU0CKFDU)BOEMF XSJTU@QJUDI@KPJOU  HSJQQFS@SPMM@IBOEMFTJN(FU0CKFDU)BOEMF HSJQQFS@SPMM@KPJOU 

Here, we use the TJN(FU0CKFDU)BOEMF function, whose argument is the name of the object as it appear in the scene hierarchy panel that we want to handle. We can now enable the joint state publishers: TJN&YU304@FOBCMF1VCMJTIFS WSFQ@EFNPTFWFO@EPG@BSNTIPVMEFS@QBOTUBUF  TJNSPT@TUSNDNE@HFU@KPJOU@TUBUFTIPVMEFS@QBO@IBOEMF  TJN&YU304@FOBCMF1VCMJTIFS WSFQ@EFNPTFWFO@EPG@BSNTIPVMEFS@QJUDITUBUF  TJNSPT@TUSNDNE@HFU@KPJOU@TUBUFTIPVMEFS@QJUDI@IBOEMF  TJN&YU34@FOBCMF1VCMJTIFS WSFQ@EFNPTFWFO@EPG@BSNFMCPX@SPMMTUBUF  TJNSPT@TUSNDNE@HFU@KPJOU@TUBUFFMCPX@SPMM@IBOEMF  TJN&YU304@FOBCMF1VCMJTIFS WSFQ@EFNPTFWFO@EPG@BSNFMCPX@QJUDITUBUF  TJNSPT@TUSNDNE@HFU@KPJOU@TUBUFFMCPX@QJUDI@IBOEMF  TJN&YU304@FOBCMF1VCMJTIFS WSFQ@EFNPTFWFO@EPG@BSNXSJTU@SPMMTUBUF  TJNSPT@TUSNDNE@HFU@KPJOU@TUBUFXSJTU@SPMM@IBOEMF  TJN&YU304@FOBCMF1VCMJTIFS WSFQ@EFNPTFWFO@EPG@BSNXSJTU@QJUDITUBUF  TJNSPT@TUSNDNE@HFU@KPJOU@TUBUFXSJTU@QJUDI@IBOEMF  TJN&YU304@FOBCMF1VCMJTIFS WSFQ@EFNPTFWFO@EPG@BSNHSJQQFS@SPMMTUBUF  TJNSPT@TUSNDNE@HFU@KPJOU@TUBUFHSJQQFS@SPMM@IBOEMF 

[ 151 ]

Simulating Robots Using ROS and V-REP

Chapter 5

This code uses the TJN&YU304@FOBCMF1VCMJTIFS function with the TJNSPT@TUSNDNE@HFU@KPJOU@TUBUF argument, allowing V-REP to stream the state of the joint specified via its object handler (the fourth parameter in the function), using a TFOTPS@NTHT+PJOU4UBUF message type:

(KIWTG,QKPVUVCVGRWDNKUJGFD[

WSFQ@QMVHJO

TJN&YU304@FOBCMF4VCTDSJCFS WSFQ@EFNPTFWFO@EPG@BSNTIPVMEFS@QBODUSM  TJNSPT@TUSNDNE@TFU@KPJOU@UBSHFU@QPTJUJPOTIPVMEFS@QBO@IBOEMF  TJN&YU304@FOBCMF4VCTDSJCFS WSFQ@EFNPTFWFO@EPG@BSNTIPVMEFS@QJUDIDUSM  TJNSPT@TUSNDNE@TFU@KPJOU@UBSHFU@QPTJUJPOTIPVMEFS@QJUDI@IBOEMF  TJN&YU304@FOBCMF4VCTDSJCFS WSFQ@EFNPTFWFO@EPG@BSNFMCPX@SPMMDUSM  TJNSPT@TUSNDNE@TFU@KPJOU@UBSHFU@QPTJUJPOFMCPX@SPMM@IBOEMF  TJN&YU304@FOBCMF4VCTDSJCFS WSFQ@EFNPTFWFO@EPG@BSNFMCPX@QJUDIDUSM  TJNSPT@TUSNDNE@TFU@KPJOU@UBSHFU@QPTJUJPOFMCPX@QJUDI@IBOEMF  TJN&YU304@FOBCMF4VCTDSJCFS WSFQ@EFNPTFWFO@EPG@BSNXSJTU@SPMMDUSM  TJNSPT@TUSNDNE@TFU@KPJOU@UBSHFU@QPTJUJPOXSJTU@SPMM@IBOEMF  TJN&YU304@FOBCMF4VCTDSJCFS WSFQ@EFNPTFWFO@EPG@BSNXSJTU@QJUDIDUSM  TJNSPT@TUSNDNE@TFU@KPJOU@UBSHFU@QPTJUJPOXSJTU@QJUDI@IBOEMF  TJN&YU304@FOBCMF4VCTDSJCFS WSFQ@EFNPTFWFO@EPG@BSNHSJQQFS@SPMMDUSM  TJNSPT@TUSNDNE@TFU@KPJOU@UBSHFU@QPTJUJPOHSJQQFS@SPMM@IBOEMF 

Finally, here we enable the arm to get the user control input. The TJN&YU304@FOBCMF4VCTDSJCFS functions are called with the TJNSPT@TUSNDNE@TFU@KPJOU@UBSHFU@QPTJUJPO command, enabling the arm to subscribe to a set of float streams. The received values will be automatically applied to the joints specified via the object handlers. As usual, we can test that everything works fine by setting a target position to one of the joints of the robot: $ rostopic pub /vrep_demo/seven_dof_arm/wrist_pitch/ctrl std_msgs/Float64 "data: 1.0"

[ 152 ]

Simulating Robots Using ROS and V-REP

Chapter 5

Simulating a differential wheeled robot in VREP After simulating a robotic arm, we will now discuss how to set up a simulation scene to control a mobile robot. In this case, we will use one of the simulation models already implemented in V-REP. To import a model from the V-REP database, select the desired model class and object from the Model Browser panel and drag it into the scene. For our simulation, we will simulate the Pioneer 3dx, one of the most popular differential wheeled mobile robots used as a research platform:

(KIWTG2KQPGGTFZKP84'2UKOWNCVKQPUEGPG

By default, the Pioneer robot is equipped with 16 sonar sensors, both forward and rear facing. In the next section, we will discuss how to equip the robot with other sensors. To actuate the robot using ROS, we should add a script that receives the desired linear and angular velocity and convert it in wheel velocities. We can use the same script to enable sensor data streaming via a ROS topic.

[ 153 ]

Simulating Robots Using ROS and V-REP

Chapter 5

Here is the complete code of the script associated to 1JPOFFS@QEY object in the Scene Hierarchy panel. Part of this code is released with the V-REP simulator and is already available when the 1JPOFFS@QEY is imported in the simulation scene: JG TJN@DBMM@UZQFTJN@DIJMETDSJQUDBMM@JOJUJBMJ[BUJPOUIFO VTFOTPST\^ TPOBSQVCMJTIFS\^ GPSJEP VTFOTPSTTJN(FU0CKFDU)BOEMF 1JPOFFS@QEY@VMUSBTPOJD4FOTPSJ TPOBSQVCMJTIFSTJN&YU304@FOBCMF1VCMJTIFS TPOBS J TJNSPT@TUSNDNE@SFBE@QSPYJNJUZ@TFOTPSVTFOTPST  FOE NPUPS-FGUTJN(FU0CKFDU)BOEMF 1JPOFFS@Q[email protected] NPUPS3JHIUTJN(FU0CKFDU)BOEMF 1JPOFFS@Q[email protected] *OQVU TJN&YU304@FOBCMF4VCTDSJCFS MJOFBS@WFM  TJNSPT@TUSNDNE@TFU@GMPBU@TJHOBM WM  TJN&YU304@FOBCMF4VCTDSJCFS BOHVMBS@WFM  TJNSPT@TUSNDNE@TFU@GMPBU@TJHOBM WB  PVUQVU SPCPU)BOEMFSTJN(FU0CKFDU)BOEMF 1JPOFFS@QEY CPEZQPTJUJPO PEPN1VCMJTIFSTJN&YU304@FOBCMF1VCMJTIFS PEPNFUSZ TJNSPT@TUSNDNE@HFU@PE PN@EBUBSPCPU)BOEMFS  BYFT@MFOHUI XIFFM@SBEJVT FOE JG TJN@DBMM@UZQFTJN@DIJMETDSJQUDBMM@DMFBOVQUIFO FOE JG TJN@DBMM@UZQFTJN@DIJMETDSJQUDBMM@BDUVBUJPOUIFO MPDBMW@MTJN(FU'MPBU4JHOBM  WM  MPDBMW@BTJN(FU'MPBU4JHOBM  WB  JGOPUW@MUIFO W@M FOE JGOPUW@BUIFO W@B FOE MPDBMW@MFGU MPDBMW@SJHIU W@MFGU XIFFM@SBEJVT W@MBYFT@MFOHUI W@B W@SJHIU XIFFM@SBEJVT W@M BYFT@MFOHUI W@B

[ 154 ]

Simulating Robots Using ROS and V-REP

Chapter 5

TJN4FU+PJOU5BSHFU7FMPDJUZ NPUPS-FGUW@MFGU TJN4FU+PJOU5BSHFU7FMPDJUZ NPUPS3JHIUW@SJHIU FOE

Here is the explanation of the code: GPSJEP VTFOTPSTTJN(FU0CKFDU)BOEMF 1JPOFFS@QEY@VMUSBTPOJD4FOTPSJTPOBSQVCM JTIFSTJN&YU304@FOBCMF1VCMJTIFS TPOBS J TJNSPT@TUSNDNE@SFBE@QSPYJNJUZ@TFOTPSVTFOTPST  FOE NPUPS-FGUTJN(FU0CKFDU)BOEMF 1JPOFFS@Q[email protected] NPUPS3JHIUTJN(FU0CKFDU)BOEMF 1JPOFFS@Q[email protected]

Here, we stream the sonar data and initialize the handlers of the wheels to be controlled: TJN&YU304@FOBCMF4VCTDSJCFS MJOFBS@WFM  TJNSPT@TUSNDNE@TFU@GMPBU@TJHOBM WM  TJN&YU304@FOBCMF4VCTDSJCFS BOHVMBS@WFM  TJNSPT@TUSNDNE@TFU@GMPBU@TJHOBM WB  SPCPU)BOEMFSTJN(FU0CKFDU)BOEMF 1JPOFFS@QEY CPEZQPTJUJPO PEPN1VCMJTIFSTJN&YU304@FOBCMF1VCMJTIFS PEPNFUSZ TJNSPT@TUSNDNE@HFU@PE PN@EBUBSPCPU)BOEMFS 

This allows the robot to receive the desired cartesian velocities from the ROS topic and stream its odometry. The TJNSPT@TUSNDNE@TFU@GMPBU@TJHOBM command is used to read a float value from the MJOFBS@WFM and BOHVMBS@WFM topics, while the TJNSPT@TUSNDNE@HFU@PEPN@EBUB command enables the streaming of the odometry data of the robot via a OBW@NTHT0EPN message. In the actuation part of the script, we can calculate the velocity of the wheels: MPDBMW@MTJN(FU'MPBU4JHOBM  WM  MPDBMW@BTJN(FU'MPBU4JHOBM  WB 

[ 155 ]

Simulating Robots Using ROS and V-REP

Chapter 5

This block of code retrieves the value of signals of the desired linear and angular velocities from the input topic: JGOPUW@MUIFO W@M FOE JGOPUW@BUIFO W@B FOE

Since, by default, signals are created with null values, it is recommended to check that they have been initialized before we use them: W@MFGU XIFFM@SBEJVT W@MBYFT@MFOHUI W@B W@SJHIU XIFFM@SBEJVT W@M BYFT@MFOHUI W@B TJN4FU+PJOU5BSHFU7FMPDJUZ NPUPS-FGUW@MFGU TJN4FU+PJOU5BSHFU7FMPDJUZ NPUPS3JHIUW@SJHIU

Finally, we can calculate and set the wheel velocities needed to actuate the robot with the desired control input. In our simulation, a wheel is represented by a simple joint. To move it, we can use TJN4FU+PJOU5BSHFU7FMPDJUZ, which sets the desired target velocity to the joint. After running the simulation, the user should be able to read the sonar values and the position of the robot, calculated via the robot odometry, and set the linear and angular velocity: $ rostopic pub /linear_vel std_msgs/Float32 "data: 0.2"

This will apply a linear velocity of 0.2 m/s to the robot.

Adding a laser sensor to V-REP An important feature of robotic simulators is the possibility to simulate sensors as well as robots. We are now going to add a laser sensor to the 1JPOFFS@EY. V-REP offers different models of vision, inertial, and proximity sensors. These can be selected from the Model Browser panel in the components | sensors section.

[ 156 ]

Simulating Robots Using ROS and V-REP

Chapter 5

To add a MBTFS to the robot, select and drag and drop )PLVZP@63(@-9@6(@304 into the scene. It's useful to put the sensor as a parent of the robot to be facilitated in its correct positioning on the robot frame. After importing the sensor, the 4DFOF)JFSBSDIZ should appear, as show in the following figure:

(KIWTG5EGPG*KGTCTEJ[RCPGNYKVJVJG2KQPGGTGSWKRRGFYKVJJQMW[QNCUGT

[ 157 ]

Simulating Robots Using ROS and V-REP

Chapter 5

We can position the sensor on the robot frame using the 0CKFDU*UFN QPTJUJPOPSJFOUBUJPO window, accessible via the toolbar. This laser sensor is already suitable to stream laser data on the GSPOU@TDBO topic, using a TFOTPS@NTHT-BTFS4DBO message. After running the simulation, we can see the data generated by the laser scanner in RViz:

(KIWTG.CUGTUECPPGTFCVCXKUWCNK\GFKP84'2CPF48K\

[ 158 ]

Simulating Robots Using ROS and V-REP

Chapter 5

Adding a 3D vision sensor to V-REP In this section, we will add another sensor to our mobile robot: an rgb-d sensor, like the one already used in $IBQUFS, Simulating Robots Using ROS and Gazebo. V-REP already has a pre-built model for this sensor, but, unlike the laser scanner, it is not directly integrated with ROS. For this reason, let's modify the script associated to this sensor to stream its data via topics. Add the sensor selecting the kinect model from the Model Browser panel and drop it in the robot components. Position the kinect in the desired location and the associated script in the following way: JG TJN@DBMM@UZQFTJN@DIJMETDSJQUDBMM@JOJUJBMJ[BUJPOUIFO EFQUI$BNTJN(FU0CKFDU)BOEMF LJOFDU@EFQUI  EFQUI7JFXTJN'MPBUJOH7JFX"EE  DPMPS$BNTJN(FU0CKFDU)BOEMF LJOFDU@SHC  DPMPS7JFXTJN'MPBUJOH7JFX"EE  HMBTTTJN(FU0CKFDU)BOEMF LJOFDU@HMBTT  LJOFDUTJN(FU0CKFDU)BOEMF LJOFDU  FOE JG TJN@DBMM@UZQFTJN@DIJMETDSJQUDBMM@TFOTJOHUIFO SHC5PQJD/BNFTJN&YU304@FOBCMF1VCMJTIFS SHCJNBHF@SBX TJNSPT@TUSNDNE@HF U@WJTJPO@TFOTPS@JNBHFDPMPS$BN  %FQUI5PQJD/BNFTJN&YU304@FOBCMF1VCMJTIFS EFQUIJNBHF@SBX TJNSPT@TUSNDN E@HFU@WJTJPO@TFOTPS@JNBHFEFQUI$BN  JOGP5PQJD/BNFTJN&YU304@FOBCMF1VCMJTIFS DBNFSB*OGP TJNSPT@TUSNDNE@HFU@ WJTJPO@TFOTPS@JOGPDPMPS$BN  FOE JG TJN@DBMM@UZQFTJN@DIJMETDSJQUDBMM@DMFBOVQUIFO FOE

Here, in the TJN@DIJMETDSJQUDBMM@TFOTJOH section, we add the code to stream data generated by the rgb-d sensor: the colored image and the depth image, and the information concerning the camera calibration. We can run the simulation, as shown here: $ rosrun image_view image_view image:=/rgb/image_raw

[ 159 ]

Simulating Robots Using ROS and V-REP

Chapter 5

And we get the following image:

(KIWTG4)$CPFFGRVJKOCIGUUVTGCOGFD[MKPGEVUGPUQTKP84'2

We have now simulated a differential wheeled mobile robot equipped with different kinds of sensors fully integrated with ROS. This robot model is saved in the NPCJMF@SPCPUUUU simulation scene provided with the WSFQ@EFNP@QLH package.

[ 160 ]

Simulating Robots Using ROS and V-REP

Chapter 5

Questions We should now be able to answer the following questions: How do V-REP and ROS communicate? In what way is it possible to control a V-REP simulation with ROS? How can we import new robot models in V-REP and integrate them with ROS? How can we integrate new V-REP model sensors with ROS? How can we move the mobile robot in a V-REP simulation?

Summary In this chapter, we mainly replicated things already done in the previous chapter with Gazebo, using another simulator: V-REP. V-REP is a multi-platform simulation software that integrates different technologies and is very versatile. With respect to Gazebo, V-REP could appear easier to use for new users. In this chapter, we simulated two robots, one imported using the URDF file of the sevenDOF arm designed in previous chapters, and the other was a popular differential wheeled robot provided by V-REP simulation models. We learned how to interface and control robot joints of our model with ROS and how to move a differential drive mobile robot using topics. In addition, we discussed how to add different type of sensors in our simulation scene, improving the equipment of the simulated mobile robot with a laser and a 3D vision sensor. Finally, we discussed how to connect a rgb-d sensor simulated in V-REP with the ROS framework. In the next chapter, we will see how to interface the robotic arm with the ROS MoveIt package and the mobile robot with the Navigation stack.

[ 161 ]

6

Using the ROS MoveIt! and Navigation Stack In the previous chapters, we have been discussing the design and simulation of a robotic arm and mobile robot. We controlled each joint of the robotic arm in Gazebo using the ROS controller and moved the mobile robot inside Gazebo using the teleop node. In this chapter, we are going to address the motion planning problem. Moving a robot by directly controlling its joints manually might be a difficult task, especially if we want to add position or velocity constraints to the robot motion. Similarly, driving a mobile robot, avoiding obstacles, requires the planning of a path. For this reason, we will solve these problems using the ROS MoveIt! and Navigation stack. MoveIt! is a set of packages and tools for doing mobile manipulation in ROS. The official web page (IUUQNPWFJUSPTPSH) contains the documentations, the list of robots using MoveIt!, and various examples to demonstrate pick and place, grasping, simple motion planning using inverse kinematics, and so on. MoveIt! contains state-of-the-art software for motion planning, manipulation, 3D perception, kinematics, collision checking, control, and navigation. Apart from the command line interface, MoveIt! has some good GUI to interface a new robot to MoveIt!. Also, there is a RViz plugin, which enables motion planning from RViz itself. We will also see how to motion plan our robot using MoveIt! C++ APIs.

Using the ROS MoveIt! and Navigation Stack

Chapter 6

Next is the Navigation stack, another set of powerful tools and libraries to work mainly for mobile robot navigation. The Navigation stack contains ready-to-use navigation algorithms which can be used in mobile robots, especially for differential wheeled robots. Using these stacks, we can make the robot autonomous, and that is the final concept that we are going to see in the Navigation stack. The first section of this chapter will discuss more on the MoveIt! package, installation, and architecture. After discussing the main concepts of MoveIt!, we will see how to create a MoveIt! package for our robotic arm, which can provide collision-aware path planning to our robot. Using this package, we can perform motion planning (inverse kinematics) in RViz, and can interface to Gazebo or the real robot for executing the paths. After discussing the interfacing, we will discuss more about the Navigation stack and see how to perform autonomous navigation using SLAM (Simultaneous Localization And Mapping) and amcl (Adaptive Monte Carlo Localization).

Installing MoveIt! Let's start with installing MoveIt!. The installation procedure is very simple and is just a single command. Using the following commands, we install the MoveIt! core, a set of plugins ad planners for ROS Kinetic: $ sudo apt-get install ros-kinetic-moveit ros-kinetic-moveit-plugins roskinetic-moveit-planners

MoveIt! architecture Let's start with MoveIt! and its architecture. Understanding the architecture of MoveIt! helps to program and interface the robot to MoveIt!. We will quickly go through the architecture and the important concepts of MoveIt!, and start interfacing and programming our robots. Here is the MoveIt! architecture, included in their official web page, at IUUQNPWFJUSPTPSHEPDVNFOUBUJPODPODFQUT:

[ 163 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

(KIWTG/QXG+VCTEJKVGEVWTGFKCITCO

The move_group node We can say that NPWF@HSPVQ is the heart of MoveIt!, as this node acts as an integrator of the various components of the robot and delivers actions/services according to the user's needs. From the architecture, it's clear that the NPWF@HSPVQ node collects robot information such as point cloud, joint state of the robot, and transform (TF) of the robot in the form of topics and services.

[ 164 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

From the parameter server, it collects the robot kinematics data, such as SPCPU@EFTDSJQUJPO (URDF), Semantic Robot Description Format (SRDF), and the configuration files. The SRDF file and the configuration files are generated while we generate a MoveIt! package for our robot. The configuration files contains the parameter file for setting joint limits, perception, kinematics, end effector, and so on. We will see the files when we discuss generating the MoveIt! package for our robot. When MoveIt! gets all this information about the robot and its configuration, we can say it is properly configured and we can start commanding the robot from the user interfaces. We can either use C++ or Python MoveIt! APIs to command the NPWF@HSPVQ node to perform actions such as pick/place, IK, and FK, among others. Using the RViz motion planning plugin, we can command the robot from the RViz GUI itself. As we already discussed, the NPWF@HSPVQ node is an integrator; it does not run any kind of motion planning algorithms directly, but instead connects all the functionalities as plugins. There are plugins for kinematics solvers, motion planning, and so on. We can extend the capabilities through these plugins. After motion planning, the generated trajectory talks to the controllers in the robot using the 'PMMPX+PJOU5SBKFDUPSZ"DUJPO interface. This is an action interface in which an action server is run on the robot, and NPWF@OPEF initiates an action client which talks to this server

and executes the trajectory on the real robot/Gazebo simulator. At the end of the MoveIt! discussion, we will see how to connect MoveIt! with RViz GUI to Gazebo. The following screenshot shows a robotic arm that is controlling from RViz and the trajectory is executed inside Gazebo:

[ 165 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

(KIWTG6TCLGEVQT[HTQO48K\)7+GZGEWVKPIKP)C\GDQ

Motion planning using MoveIt! Assume that we know the starting pose of the robot, a desired goal pose of the robot, the geometrical description of the robot, and geometrical description of the world, then motion planning is the technique to find an optimum path that moves the robot gradually from the start pose to the goal pose, while never touching any obstacles in the world and without colliding with the robot links. In this context, the robot geometry is described via the URDF file. We can also create a description file for the robot environment and use laser or vision sensors of the robot to map its operative space, in order to avoid static and dynamic obstacles during the execution of planned paths.

[ 166 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

In the case of the robotic arm, the motion planner should find a trajectory (consisting of joint spaces of each joint) in which the links of the robot should never collide with the environment, avoid self-collision (collision between two robot links), and not violate the joint limits. MoveIt! can talk to the motion planners through the plugin interface. We can use any motion planner by simply changing the plugin. This method is highly extensible so we can try our own custom motion planners using this interface. The move_group node talks to the motion planner plugin via the ROS action/services. The default planner for the NPWF@HSPVQ node is OMPL (IUUQPNQMLBWSBLJMBCPSH). To start motion planning, we should send a motion planning request to the motion planner which specified our planning requirements. The planning requirement may be setting a new goal pose of the end-effector; for example, for a pick and place operation. We can set additional kinematic constraints for the motion planners. The following are some inbuilt constraints in MoveIt!: Position constraints: These restrict the position of a link Orientation constraints: These restrict the orientation of a link Visibility constraints: These restrict a point on the link to be visible in an area (view of a sensor) Joint constraints: These restrict a joint within its joint limits User-specified constraints: Using these constraints, the user can define his own constraints using the callback functions Using these constraints, we can send a motion planning request and the planner will generate a suitable trajectory according to the request. The NPWF@HSPVQ node will generate the suitable trajectory from the motion planner which obeys all the constraints. This can be sent to robot joint trajectory controllers.

[ 167 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

Motion planning request adapters The planning request adapters help to pre-process the motion planning request and postprocess the motion planning response. One of the uses of pre-processing requests is that it helps to correct if there is a violation in the joint states and, for the post-processing, it can convert the path generated by the planner to a time-parameterized trajectory. The following are some of the default planning request adapters in MoveIt!: FixStartStateBounds: If a joint state is slightly outside the joint limits, then this adapter can fix the initial joint limits within the limits. FixWorkspaceBounds: This specifies a workspace for planning with a cube size of 10 m x 10 m x 10 m. FixStartStateCollision: This adapter samples a new collision free configuration if the existing joint configuration is in collision. It makes a new configuration by changing the current configuration by a small factor called KJHHMF@GBDUPS. FixStartStatePathConstraints: This adapter is used when the initial pose of the robot does not obey the path constraints. In this, it finds a near pose which satisfies the path constraints and uses that pose as the initial state. AddTimeParameterization: This adapter parameterizes the motion plan by applying the velocity and acceleration constraints.

MoveIt! planning scene The term "planning scene" is used to represent the world around the robot and store the state of the robot itself. The planning scene monitor inside NPWF@HSPVQ maintains the planning scene representation. The NPWF@HSPVQ node consists of another section called the world geometry monitor, which builds the world geometry from the sensors of the robot and from the user input.

[ 168 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

The planning scene monitor reads the joint_states topic from the robot, and the sensor information and world geometry from the world geometry monitor. The world scene monitor reads from the occupancy map monitor, which uses 3D perception to build a 3D representation of the environment, called Octomap. Octomaps can be generated from point clouds, which are handled by a point cloud occupancy map update plugin and depth images handled by a depth image occupancy map updater. The following image shows the representation of the planning scene from the MoveIt! official wiki (IUUQNPWFJUSPTPSHEPDVNFOUBUJPODPODFQUT):

(KIWTG/QXG+VRNCPPKPIUEGPGQXGTXKGYFKCITCO

MoveIt! kinematics handling MoveIt! provides a great flexibility to switch the inverse kinematics algorithms using the robot plugins. Users can write their own IK solver as a MoveIt! plugin and switch from the default solver plugin whenever required. The default IK solver in MoveIt! is a numerical jacobian-based solver.

[ 169 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

Compared to the analytic solvers, the numerical solver can take time to solve IK. The package called IKFast can be used to generate a C++ code for solving IK using analytical methods, which can be used for different kinds of robot manipulator and perform better if the DOF is less than 6. This C++ code can also be converted into the MoveIt! plugin by using some ROS tool. We will look at this procedure in the upcoming chapters. Forward kinematics and finding jacobians are already integrated to the MoveIt! RobotState class, so we don't need to use plugins for solving FK.

MoveIt! collision checking The CollisionWorld object inside MoveIt! is used to find collisions inside a planning scene which is using the Flexible Collision Library (FCL) package as a backend. MoveIt! supports collision checking for different types of objects, such as meshes, primitive shapes such as boxes, cylinders, cones, spheres, and Octomap. Collision checking is one of the computationally expensive tasks during motion planning. To reduce this computation, MoveIt! provides a matrix called ACM (Allowed Collision Matrix). It contains a binary value corresponding to the need to check for a collision between two pairs of bodies. If the value of the matrix is 1, it means collision of the corresponding pair is not needed. We can set the value as 1 where the bodies are always so far that they would never collide with each other. Optimizing ACM can reduce the total computation needed for collision avoidance. After discussing the basic concepts in MoveIt!, we can now discuss how to interface a robotic arm into MoveIt!. To interface a robot arm in MoveIt!, we need to satisfy the components that we saw in Figure 1. The NPWF@HSPVQ node essentially requires parameters, such as URDF, SRDF, config files, and joint states topics, along with TF from a robot to start with motion planning. MoveIt! provides a GUI-based tool called Setup Assistant to generate all these elements. The following section describes the procedure to generate a MoveIt! configuration from the Setup Assistant tool.

[ 170 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

Generating MoveIt! configuration package using the Setup Assistant tool The MoveIt! Setup Assistant is a graphical user interface for configuring any robot to MoveIt!. Basically, this tool generates SRDF, configuration files, launch files, and scripts generating from the robot URDF model, which is required to configure the NPWF@HSPVQ node. The SRDF file contains details about the arm joints, end-effector joints, virtual joints, and the collision link pairs, which are configured during the MoveIt! configuration process using the Setup Assistant tool. The configuration file contains details about the kinematic solvers, joint limits, controllers, and so on, which are also configured and saved during the configuration process. Using the generated configuration package of the robot, we can work with motion planning in RViz without the presence of a real robot or simulation interface. Let's start the configuration wizard, and we can see the step-by-step procedure to build the configuration package of our robotic arm.

Step 1 ` Launching the Setup Assistant tool To start the MoveIt! Setup Assistant tool, we can use the following command: $ roslaunch moveit_setup_assistant setup_assistant.launch

This will bring up a window with two choices: Create New MoveIt! Configuration Package or Edit Existing MoveIt! Configuration Package. Here we are creating a new package, so we need that option. If we have a MoveIt! package already, then we can select the second option.

[ 171 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

Click on the Create New MoveIt! Configuration Package button, which will display a new screen, as shown next:

(KIWTG/QXG+V5GVWR#UUKUVCPV

In this step, the wizard asks for the URDF model of the new robot. To give the URDF file, click on the Browse button and navigate to NBTUFSJOH@SPT@SPCPU@EFTDSJQUJPO@QLHVSEGTFWFO@EPG@BSNVSEG. Choose this file and press the Load button to load the URDF. We can either give the robot model as pure URDF or xacro; if we give xacro, the tool will convert to RDF internally.

[ 172 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

If the robot model is successfully parsed, we can see the robot model on the window, as shown in the following screenshot:

(KIWTG5WEEGUUHWNN[RCTUKPIVJGTQDQVOQFGNKPVJG5GVWR#UUKUVCPVVQQN

Step 2 ` Generating the Self-Collision matrix We can now start to navigate all the panels of the window to properly configure our robot. In the Self-Collisions tab, MoveIt! searches for a pair of links on the robot which can be safely disabled from the collision checking. These can reduce the processing time. This tool analyzes each link pair and categorizes the links as always in collision, never in collision, default in collision, adjacent links disabled, and sometimes in collision, and it disables the pair of links which makes any kind of collision. The following image shows the SelfCollisions window:

[ 173 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

(KIWTG4GIGPGTCVKPIVJG5GNH%QNNKUKQPOCVTKZ

The sampling density is the number of random positions to check for self-collision. If the density is large, computation will be high but self-collision will be less. The default value is 10,000. We can see the disabled pair of links by pressing the Regenerate Default Collision Matrix button; it will take a few seconds to list out the disabled pair of links.

Step 3 ` Adding virtual joints Virtual joints attach the robot to the world. They are not mandatory for a static robot which does not move. We need virtual joints when the base position of the arm is not fixed. For example, if a robot arm is fixed on a mobile robot, we should define a virtual joint with respect to the odometry frame (PEPN). In the case of our robot, we are not creating virtual joints.

[ 174 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

Step 4 ` Adding planning groups A planning group is basically a group of joints/links in a robotic arm which plans together to achieve a goal position of a link or the end effector. We must create two planning groups, one for the arm and one for the gripper. Click on the Planning Groups tab on the left side and click on the Add Group button. You will see the following screen, which has the settings of the BSN group:

(KIWTG#FFKPIVJGRNCPPKPIITQWRQHVJGCTO

Here, we are giving Group Name as arm, and Kinematic Solver as LEM@LJOFNBUJDT@QMVHJO,%-,JOFNBUJDT1MVHJO, which is the default numerical IK solver with MoveIt!. We can keep the other parameters as the default values. In addition, we can choose different ways to add elements in a planning group. For example, we could specify the joints of the group, add its links, or directly specify a kinematic chain.

[ 175 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

Inside the arm group, first we have to add a kinematic chain, starting from CBTF@MJOL as the first link to the HSBTQJOH@GSBNF. Add a group called HSJQQFS and we don't need to have a kinematic solver for the HSJQQFS group. Inside this group, we can add the joints and links of the gripper. These settings are shown next:

(KIWTG#FFKPIVJGRNCPPKPIITQWRQHVJGCTOCPFITKRRGT

Step 5 ` Adding the robot poses In this step, we can add certain fixed poses in the robot configuration. For example, we can assign a home position or a pick/place position in this step. The advantage is that, while programming with MoveIt! APIs, we can directly call these poses, which are also called group states. These have many applications in the pick/place and grasping operation. The robot can switch to the fixed poses without any hassle.

[ 176 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

Step 6 ` Setting up the robot end effector In this step, we name the robot end effector and assign the end-effector group, the parent link, and the parent group. We can add any number of end effectors to this robot. In our case, it's a gripper designed for pick and place operations. Click on the Add End Effector button and name the end effector as SPCPU@FFG, the planning group as gripper, which we have already created, the parent link as HSBTQJOH@GSBNF, and the parent group as BSN:

(KIWTG#FFKPIGPFGbGEVQTU

[ 177 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

Step 7 ` Adding passive joints In this step, we can specify the passive joints in the robot. Passive joints mean that the joints do not have any actuators. Caster wheels are one of the examples of passive joints. The planner will ignore these kinds of joints during motion planning.

Step 8 ` Author information In this step, the author of the robotic model can add personal information, his name and email address, required by catkin to release the model to the ROS community.

Step 9 ` Generating configuration files We are almost done. We are in the final stage, that is, generating the configuration files. In this step, the tool will generate a configuration package which contains the file needed to interface MoveIt!. Click on the Browse button to locate a folder to save the configuration file that is going to be generated by the Setup Assistant tool. Here we can see the files are generating inside a folder called TFWFO@EPG@BSN@DPOGJH. You can BEE@DPOGJH or @HFOFSBUFE along with the robot name for the configuration package. Click on the Generate Package button, and it will generate the files to the given folder. If the process is successful, we can click on Exit Setup Assistant, which will exit us from the tool.

[ 178 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

The following screenshot shows the generation process:

(KIWTG)GPGTCVKPIVJG/QXG+VEQPaIWTCVKQPRCEMCIG

After generating the MoveIt! configuration package, we can copy it into our DBULJO workspace. In the following section, we are going to work with this package. As usual, the model of the robot created can be downloaded from the following GitHub repository or can be obtained from the book's source code: $ git clone https://github.com/jocacace/seven_dof_arm_config

[ 179 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

Motion planning of robot in RViz using MoveIt! configuration package MoveIt! provides a plugin for RViz, which allows it to create new planning scenes where robot works, generate motion plans, and add new objects, visualize the planning output and can directly interact with the visualized robot. The MoveIt! configuration package consists of configuration files and launch files to start motion planning in RViz. There is a demo launch file in the package to explore all the functionalities of this package. The following is the command to invoke the demo launch file: $ roslaunch seven_dof_arm_config demo.launch

If everything works fine, we will get the following screen of RViz being loaded with the .PUJPO1MBOOJOH plugin provided by MoveIt!:

(KIWTG/QXG+V48K\RNWIKP

[ 180 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

Using the RViz Motion Planning plugin From the preceding figure , we can see that the RViz-Motion Planning plugin is loaded on the left side of the screen. There are several tabs on the Motion Planning window, such as Context, Planning, and so on. The default tab is the Context tab and we can see the default Planning Library as OMPL, which is shown in green. It indicates that MoveIt! successfully loaded the motion planning library. If it is not loaded, we can't perform motion planning. Next is the Planning tab. This is one of the frequently used tabs used to assign the Start State, Goal State, Plan a path, and Execute the path. Shown next is the GUI of the Planning tab:

(KIWTG/QXG+V48K\2NCPPKPIVCD

We can assign the start state and the goal state of the robot under the Query panel. Using the Plan button, we can plan the path from the start to the goal state, and if the planning is successful, we can execute it. By default, execution is done on fake controllers. We can change these controllers into trajectory controllers for executing the planned trajectory in Gazebo or the real robot.

[ 181 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

We can set the starting and the goal position of the robot end effector by using the interactive marker attached on the arm gripper. We can translate and rotate the marker pose, and, if there is a planning solution, we can see an arm in orange color. In some situations, the arm will not move even the end-effector marker pose moves, and if the arm does not come to the marker position, we can assume that there is no IK solution in that pose. We may need more DOF to reach there or there might be some collision between the links. The following screenshots show a valid goal pose and an invalid goal pose:

(KIWTG#XCNKFRQUGCPFCPKPXCNKFRQUGQHVJGTQDQVKP48K\

The green colored arm represents the starting position of the arm, and the orange color represents the goal position. In the first figure, if we press the Plan button, MoveIt! plans a path from start to goal. In the second image, we can observe two things. First, one of the links of the orange arm is red, which means that the goal pose is in a self-collided state. Secondly, look at the end- effector marker; it is far from the actual end effector and it has also turned red.

[ 182 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

We can also work with some quick motion planning using random valid options in the start state and the goal state. If we select the goal state as random valid and press the Update button, it will generate a random valid goal pose. Click on the Plan button and we can see the motion planning. We can customize the RViz visualization using the various options in the .PUJPO1MBOOJOH plugin. Shown next are some of the options of this plugin:

(KIWTG5GVVKPIUQHVJG/QVKQP2NCPPKPIRNWIKPQP48K\

[ 183 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

The first marked area is Scene Robot, which will show the robot model; if it is unchecked, we won't see any robot model. The second marked area is the Trajectory Topic, in which RViz gets the visualization trajectory. If we want to animate the motion planning and want to display the motion trails, we should enable this option. One of the other sections in the plugin settings is shown in the following image:

(KIWTG2NCPPKPI4GSWGUVUGVVKPIKP/QVKQP2NCPPKPIRNWIKP

In the preceding figure, we can see the Query Start State and the Query Goal State options. These options can visualize the start pose and the goal pose of the arm, which we saw in Figure 13. Show Workspace visualizes the cubic workspace (world geometry) around the robot. The visualization can help to debug our motion-planning algorithm and understand the robot motion behavior in detail. In the next section, we will see how to interface the MoveIt! configuration package to Gazebo. This will execute the trajectory generated by MoveIt! in Gazebo.

Interfacing the MoveIt! configuration package to Gazebo We have already worked with the Gazebo simulation of this arm and attached controllers to it. For interfacing the arm in MoveIt! to Gazebo, we need a trajectory controller which has the 'PMMPX+PJOU5SBKFDUPSZ"DUJPO interface, as we mentioned in the MoveIt! architecture. The following is the procedure to interface MoveIt! to Gazebo.

[ 184 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

Step 1 ` Writing the controller configuration file for MoveIt! The first step is to create a configuration file for talking with the trajectory controllers in Gazebo from MoveIt!. The controller configuration file called DPOUSPMMFSTZBNM has to be created inside the DPOGJH folder of the TFWFO@EPG@BSN@DPOGJH package. Given next is an example of the DPOUSPMMFSTZBNM definition: DPOUSPMMFS@NBOBHFS@OTDPOUSPMMFS@NBOBHFS DPOUSPMMFS@MJTU OBNFTFWFO@EPG@BSNTFWFO@EPG@BSN@KPJOU@DPOUSPMMFS BDUJPO@OTGPMMPX@KPJOU@USBKFDUPSZ UZQF'PMMPX+PJOU5SBKFDUPSZ EFGBVMUUSVF KPJOUT TIPVMEFS@QBO@KPJOU TIPVMEFS@QJUDI@KPJOU FMCPX@SPMM@KPJOU FMCPX@QJUDI@KPJOU XSJTU@SPMM@KPJOU XSJTU@QJUDI@KPJOU HSJQQFS@SPMM@KPJOU OBNFTFWFO@EPG@BSNHSJQQFS@DPOUSPMMFS BDUJPO@OTGPMMPX@KPJOU@USBKFDUPSZ UZQF'PMMPX+PJOU5SBKFDUPSZ EFGBVMUUSVF KPJOUT GJOHFS@KPJOU GJOHFS@KPJOU

The controller configuration file contains the definition of the two controller interfaces; one is for arm and the other is for gripper. The type of action used in the controllers is 'PMMPX+PJOU5SBKFDUPSZ, and the action namespace is GPMMPX@KPJOU@USBKFDUPSZ. We have to list out the joints under each group. The EFGBVMUUSVF indicates that it will use the default controller, which is the primary controller in MoveIt! for communicating with the set of joints.

[ 185 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

Step 2 ` Creating the controller launch files Next, we have to create a new launch file called TFWFO@EPG@BSN@NPWFJU@DPOUSPMMFS@NBOBHFSMBVODI , which can start the trajectory controllers. The name of the file starts with the robot name, which is added with @NPWFJU@DPOUSPMMFS@NBOBHFS. The following is the TFWFO@EPG@BSN@DPOGJHMBVODI TFWFO@EPG@BSN@NPWFJU@DPOUSPMMFS@NBOBHFSMBVODI launch file definition: MBVODI 4FUUIFQBSBNUIBUUSBKFDUPSZ@FYFDVUJPO@NBOBHFSOFFETUPGJOEUIF DPOUSPMMFSQMVHJO BSHOBNFNPWFJU@DPOUSPMMFS@NBOBHFS EFGBVMUNPWFJU@TJNQMF@DPOUSPMMFS@NBOBHFS.PWF*U4JNQMF$POUSPMMFS.BOBHFS QBSBNOBNFNPWFJU@DPOUSPMMFS@NBOBHFSWBMVF BSH NPWFJU@DPOUSPMMFS@NBOBHFS MPBEDPOUSPMMFS@MJTU BSHOBNFVTF@DPOUSPMMFS@NBOBHFSEFGBVMUUSVF QBSBNOBNFVTF@DPOUSPMMFS@NBOBHFSWBMVF BSH VTF@DPOUSPMMFS@NBOBHFS -PBEKPJOUDPOUSPMMFSDPOGJHVSBUJPOTGSPN:".-GJMFUPQBSBNFUFS TFSWFS SPTQBSBNGJMF GJOETFWFO@EPG@BSN@DPOGJHDPOGJHDPOUSPMMFSTZBNM MBVODI

This launch file starts the .PWF*U4JNQMF$POUSPMMFS.BOBHFS and loads the joint trajectory controllers defined inside DPOUSPMMFSTZBNM.

Step 3 ` Creating the controller configuration file for Gazebo After creating the MoveIt! files, we have to create the Gazebo controller configuration file and the launch file. Create a new file called USBKFDUPSZ@DPOUSPMZBNM , which contains the list of the Gazebo ROS controllers that need to be loaded along with Gazebo.

[ 186 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

You will get this file from the UIFTFWFO@EPG@BSN@HB[FCP package created in $IBQUFS , Simulating Robots Using ROS and Gazebo in the DPOGJH folder. The following is the definition of this file: TFWFO@EPG@BSN TFWFO@EPG@BSN@KPJOU@DPOUSPMMFS UZQFQPTJUJPO@DPOUSPMMFST+PJOU5SBKFDUPSZ$POUSPMMFS KPJOUT TIPVMEFS@QBO@KPJOU TIPVMEFS@QJUDI@KPJOU FMCPX@SPMM@KPJOU FMCPX@QJUDI@KPJOU XSJTU@SPMM@KPJOU XSJTU@QJUDI@KPJOU HSJQQFS@SPMM@KPJOU HBJOT TIPVMEFS@QBO@KPJOU\QJEJ@DMBNQ^ TIPVMEFS@QJUDI@KPJOU\QJEJ@DMBNQ^ FMCPX@SPMM@KPJOU\QJEJ@DMBNQ^ FMCPX@QJUDI@KPJOU\QJEJ@DMBNQ^ XSJTU@SPMM@KPJOU\QJEJ@DMBNQ^ XSJTU@QJUDI@KPJOU\QJEJ@DMBNQ^ HSJQQFS@SPMM@KPJOU\QJEJ@DMBNQ^

HSJQQFS@DPOUSPMMFS UZQFQPTJUJPO@DPOUSPMMFST+PJOU5SBKFDUPSZ$POUSPMMFS KPJOUT GJOHFS@KPJOU GJOHFS@KPJOU HBJOT GJOHFS@KPJOU\QEJJ@DMBNQ^ GJOHFS@KPJOU\QEJJ@DMBNQ^

Here, we created a QPTJUJPO@DPOUSPMMFST+PJOU5SBKFDUPSZ$POUSPMMFS , which has an action interface of 'PMMPX+PJOU5SBKFDUPSZ for both the BSN and the HSJQQFS. We also defined the PID gain associated with each joint, which can provide a smooth motion.

[ 187 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

Step 4 ` Creating the launch file for Gazebo trajectory controllers After creating the configuration file, we can load the controllers along with Gazebo. We have to create a launch file which launches Gazebo, the trajectory controllers, and the MoveIt! interface in a single command. The launch file TFWFO@EPG@BSN@CSJOHVQ@NPWFJUMBVODI contains the definition to launch all these commands: MBVODI -BVODI(B[FCP JODMVEFGJMF GJOE TFWFO@EPG@BSN@HB[FCPMBVODITFWFO@EPG@BSN@XPSMEMBVODI SPT@DPOUSPMTFWFOEPGBSNMBVODIGJMF JODMVEFGJMF GJOE TFWFO@EPG@BSN@HB[FCPMBVODITFWFO@EPG@BSN@HB[FCP@TUBUFTMBVODI SPT@DPOUSPMUSBKFDUPSZDPOUSPMEPGBSNMBVODIGJMF JODMVEFGJMF GJOE TFWFO@EPG@BSN@HB[FCPMBVODITFWFO@EPG@BSN@USBKFDUPSZ@DPOUSPMMFSMBVODI NPWFJUMBVODIGJMF JODMVEFGJMF GJOE TFWFO@EPG@BSN@DPOGJHMBVODINPWFJU@QMBOOJOH@FYFDVUJPOMBVODI OPEFOBNFKPJOU@TUBUF@QVCMJTIFSQLHKPJOU@TUBUF@QVCMJTIFS UZQFKPJOU@TUBUF@QVCMJTIFS QBSBNOBNFVTF@HVJWBMVFGBMTF SPTQBSBN QBSBNTPVSDF@MJTU SPTQBSBN OPEF MBVODI

This launch file spawns the robot model in Gazebo, publishes the joint states, attaches the position controller, attaches the trajectory controller, and, finally, launches NPWFJU@QMBOOJOH@FYFDVUJPOMBVODI inside the MoveIt! package for starting the MoveIt! nodes along with RViz. We may need to load the .PUJPO1MBOOJOH plugin in RViz if it is not loaded by default. We can start motion planning inside RViz and execute in Gazebo using the following single command: $ roslaunch seven_dof_arm_gazebo seven_dof_arm_bringup_moveit.launch

[ 188 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

Note that, before properly launching the planning scene, we should use the following command to install some packages needed by MoveIt! to use ROS controllers: $ sudo apt-get install ros-kinetic-joint-state-controller ros-kineticposition-controllers ros-kinetic-joint-trajectory-controller

After we have installed the preceding packages, we can launch the planning scene. This will launch RViz and Gazebo, and we can do motion planning inside RViz. After motion planning, click on the Execute button to send the trajectory to the Gazebo controllers:

(KIWTG)C\GDQVTCLGEVQT[EQPVTQNNGTUGZGEWVKPIVJGVTCLGEVQT[HTQO/QXG+V

Step 5 ` Debugging the Gazebo- MoveIt! interface In this section, we will discuss some of the common issues and debugging techniques in this interface. If the trajectory is not executing on Gazebo, first list the topics: $ rostopic list

[ 189 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

If the Gazebo controllers are started properly, we will get the following joint trajectory topics in the list:

(KIWTG6QRKEUHTQOVJG)C\GDQ415VTCLGEVQT[EQPVTQNNGTU

We can see GPMMPX@KPJOU@USBKFDUPSZ for the gripper and the BSN group. If the controllers are not ready, the trajectory will not execute in Gazebo. Also, check the terminal message while starting the launch file:

(KIWTG6JG6GTOKPCNOGUUCIGUJQYKPIUWEEGUUHWNVTCLGEVQT[GZGEWVKQP

[ 190 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

In the Figure 18, the first section shows that the .PWF*U4JNQMF$POUSPMMFS.BOBHFS was able to connect with the Gazebo controller and if it couldn't connect to controller, it shows that it can't connect to the controller. The second section shows a successful motion planning. If the motion planning is not successful, MoveIt! will not send the trajectory to Gazebo. In the next section, we will discuss the ROS Navigation stack and look at the requirements needed to interface the Navigation stack to the Gazebo simulation.

Understanding the ROS Navigation stack The main aim of the ROS Navigation package is to move a robot from the start position to the goal position, without making any collision with the environment. The ROS Navigation package comes with an implementation of several navigation-related algorithms which can easily help implement autonomous navigation in the mobile robots. The user only needs to feed the goal position of the robot and the robot odometry data from sensors such as wheel encoders, IMU, and GPS, along with other sensor data streams, such as laser scanner data or 3D point cloud from sensors such as Kinect. The output of the Navigation package will be the velocity commands that will drive the robot to the given goal position. The Navigation stack contains the implementation of the standard algorithms, such as SLAM, A *(star), Dijkstra, amcl, and so on, which can directly be used in our application.

ROS Navigation hardware requirements The ROS Navigation stack is designed as generic. There are some hardware requirements that should be satisfied by the robot. The following are the requirements: The Navigation package will work better in differential drive and holonomic (total DOF of robot equals to controllable DOF of robots). Also, the mobile robot should be controlled by sending velocity commands in the form of: Y WFMPDJUZ, ZWFMPDJUZ (linear velocity), and UIFUBWFMPDJUZ (angular velocity). The robot should be equipped with a vision (rgb-d) or laser sensor to build the map of the environment. The Navigation stack will perform better for square and circular shaped mobile bases. It will work on an arbitrary shape, but performance is not guaranteed.

[ 191 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

The following are the basic building blocks of the Navigational stack taken from the ROS website (IUUQXJLJSPTPSHOBWJHBUJPO5VUPSJBMT3PCPU4FUVQ). We can see the purposes of each block and how to configure the Navigation stack for a custom robot:

(KIWTG0CXKICVKQPUVCEMUGVWRFKCITCO

According to the Navigation setup diagram, for configuring the Navigation package for a custom robot, we must provide functional blocks that interface to the Navigation stack. The following are the explanations of all the blocks which are provided as input to the Navigational stack: Odometry source: Odometry data of a robot gives the robot position with respect to its starting position. The main odometry sources are wheel encoders, IMU, and 2D/3D cameras (visual odometry). The odom value should publish to the Navigation stack, which has a message type of OBW@NTHT0EPNFUSZ. The PEPN message can hold the position and the velocity of the robot. Odometry data is a mandatory input to the Navigational stack. Sensor source: We have to provide laser scan data or point cloud data to the Navigation stack for mapping the robot environment. This data, along with odometry, combines to build the global and local cost map of the robot. The main sensors used here are Laser Range finders or Kinect 3D sensors. The data should be of type TFOTPS@NTHT-BTFS4DBO or TFOTPS@NTHT1PJOU$MPVE.

[ 192 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

sensor transforms/tf: The robot should publish the relationship between the robot coordinate frame using ROS tf. base_controller: The main function of the base controller is to convert the output of the Navigation stack, which is a twist (HFPNFUSZ@NTHT5XJTU) message, and convert it into corresponding motor velocities of the robot. The optional nodes of the Navigation stack are BNDM and map server, which allow localization of the robot and help to save/load the robot map.

Working with Navigation packages Before working with the Navigation stack, we were discussing MoveIt! and the NPWF@HSPVQ node. In the Navigation stack, also, there is a node like the NPWF@HSPVQ node, called the NPWF@CBTF node. From Figure 19, it is clear that the NPWF@CBTF node takes input from sensors, joint states, TF, and odometry, which is very similar to the NPWF@HSPVQ node that we saw in MoveIt!. Let's see more about the NPWF@CBTF node.

Understanding the move_base node The NPWF@CBTF node is from a package called NPWF@CBTF. The main function of this package is to move a robot from its current position to a goal position with the help of other navigation nodes. The NPWF@CBTF node inside this package links the global-planner and the local-planner for the path planning, connecting to the rotate-recovery package if the robot is stuck in some obstacle and connecting global costmap and local costmap for getting the map. The NPWF@CBTF node is basically an implementation of 4JNQMF"DUJPO4FSWFS, which takes a goal pose with message type (HFPNFUSZ@NTHT1PTF4UBNQFE). We can send a goal position to this node using a 4JNQMF"DUJPO$MJFOU node. The NPWF@CBTF node subscribes the goal from a topic called move_CBTF@TJNQMFHPBM, which is the input of the Navigation stack, as shown in the previous diagram. When this node receives a goal pose, it links to components such as HMPCBM@QMBOOFS, MPDBM@QMBOOFS, SFDPWFSZ@CFIBWJPS, HMPCBM@DPTUNBQ, and MPDBM@DPTUNBQ, generates the output, which is the command velocity (geometry_msgs/Twist), and sends it to the base controller for moving the robot for achieving the goal pose.

[ 193 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

The following is the list of all the packages which are linked by the NPWF@CBTF node: global-planner: This package provides libraries and nodes for planning the optimum path from the current position of the robot to the goal position, with respect to the robot map. This package has the implementation of path-finding algorithms, such as A*, Dijkstra, and so on, for finding the shortest path from the current robot position to the goal position. local-planner: The main function of this package is to navigate the robot in a section of the global path planned using the global planner. The local planner will take the odometry and sensor reading, and send an appropriate velocity command to the robot controller for completing a segment of the global path plan. The base local planner package is the implementation of the trajectory rollout and dynamic window algorithms. rotate-recovery: This package helps the robot to recover from a local obstacle by performing a 360 degree rotation. clear-costmap-recovery: This package is also for recovering from a local obstacle by clearing the costmap by reverting the current costmap used by the Navigation stack to the static map. costmap-2D: The main use of this package is to map the robot environment. The robot can only plan a path with respect to a map. In ROS, we create 2D or 3D occupancy grid maps, which is a representation of the environment in a grid of cells. Each cell has a probability value that indicates whether the cell is occupied or not. The costmap-2D package can build the grid map of the environment by subscribing sensor values of the laser scan or point cloud and also the odometry values. There are global cost maps for global navigation and local cost maps for local navigation. The following are the other packages which are interfaced to the NPWF@CBTF node: map-server: The map-server package allows us to save and load the map generated by the costmap-2D package. AMCL: AMCL (Adaptive Monte Carlo Localization) is a method to localize the robot in a map. This approach uses a particle filter to track the pose of the robot with respect to the map, with the help of probability theory. In the ROS system, AMCL accepts a TFOTPS@NTHT-BTFS4DBO to create the map. gmapping: The gmapping package is an implementation of an algorithm called Fast SLAM, which takes the laser scan data and odometry to build a 2D occupancy grid map. After discussing each functional block of the Navigation stack, let's see how it really works.

[ 194 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

Working of Navigation stack In the previous section, we saw the functionalities of each block in the ROS Navigation stack. Let's check how the entire system works. The robot should publish a proper odometry value, TF information, and sensor data from the laser, and have a base controller and map of the surroundings. If all these requirements are satisfied, we can start working with the Navigation package.

Localizing on the map The first step the robot is going to perform is localizing itself on the map. The AMCL package will help to localize the robot on the map.

Sending a goal and path planning After getting the current position of the robot, we can send a goal position to the move_base node. The move_base node will send this goal position to a global planner, which will plan a path from the current robot position to the goal position. This plan is with respect to the global costmap, which is feeding from the map server. The global planner will send this path to the local planner, which executes each segment of the global plan. The local planner gets the odometry and the sensor value from the move_base node and finds a collision-free local plan for the robot. The local planner is associated with the local costmap, which can monitor the obstacle(s) around the robot.

Collision recovery behavior The global and local costmap are tied with the laser scan data. If the robot is stuck somewhere, the Navigation package will trigger the recovery behavior nodes, such as the clear costmap recovery or rotate recovery nodes.

Sending the command velocity The local planner generates the command velocity in the form of a twist message that contains linear and angular velocity (HFPNFUSZ@NTHT5XJTU), to the robot base controller. The robot base controller converts the twist message to the equivalent motor speed.

[ 195 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

Installing the ROS Navigation stack The ROS desktop full installation will not install the ROS Navigation stack. We must install the Navigation stack separately, using the following command: $ sudo apt-get install ros-kinetic-navigation

After installing the Navigation package, let's start learning how to build a map of the robot environment. The robot we are using here is the differential wheeled robot that we discussed in the previous chapter. This robot satisfies all the three requirements of the Navigation stack.

Building a map using SLAM The ROS Gmapping package is a wrapper of the open source implementation of SLAM, called OpenSLAM (IUUQTXXXPQFOTMBNPSHHNBQQJOHIUNM). The package contains a node called TMBN@HNBQQJOH, which is the implementation of SLAM and helps to create a 2D occupancy grid map from the laser scan data and the mobile robot pose. The basic hardware requirement for doing SLAM is a laser scanner which is horizontally mounted on the top of the robot, and the robot odometry data. In this robot, we have already satisfied these requirements. We can generate the 2D map of the environment, using the HNBQQJOH package through the following procedure. Before operating with Gmapping, we need to install it using the following command: $ sudo apt-get install ros-kinetic-gmapping

Creating a launch file for gmapping The main task while creating a launch file for the HNBQQJOH process is to set the parameters for the TMBN@HNBQQJOH node and the NPWF@CBTF node. The TMBN@HNBQQJOH node is the core node inside the ROS Gmapping package. The TMBN@HNBQQJOH node subscribes the laser data (TFOTPS@NTHT-BTFS4DBO) and the TF data, and publishes the occupancy grid map data as output (OBW@NTHT0DDVQBODZ(SJE). This node is highly configurable and we can fine tune the parameters to improve the mapping accuracy. The parameters are mentioned at IUUQXJLJSPTPSHHNBQQJOH.

[ 196 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

The next node we have to configure is the NPWF@CBTF node. The main parameters we need to configure are the global and local costmap parameters, the local planner, and the NPWF@CBTF parameters. The parameters list is very lengthy. We are representing these parameters in several YAML files. Each parameter is included in the QBSBN folder inside the EJGG@XIFFMFE@SPCPU@HB[FCP package. The following is the HNBQQJOHMBVODI file used in this robot. The launch file is placed in the EJGG@XIFFMFE@SPCPU@HB[FCPMBVODI folder: MBVODI BSHOBNFTDBO@UPQJDEFGBVMUTDBO %FGJOJOHQBSBNFUFSTGPSTMBN@HNBQQJOHOPEF OPEFQLHHNBQQJOHUZQFTMBN@HNBQQJOHOBNFTMBN@HNBQQJOH PVUQVUTDSFFO QBSBNOBNFCBTF@GSBNFWBMVFCBTF@GPPUQSJOU QBSBNOBNFPEPN@GSBNFWBMVFPEPN QBSBNOBNFNBQ@VQEBUF@JOUFSWBMWBMVF QBSBNOBNFNBY6SBOHFWBMVF QBSBNOBNFNBY3BOHFWBMVF QBSBNOBNFTJHNBWBMVF QBSBNOBNFLFSOFM4J[FWBMVF QBSBNOBNFMTUFQWBMVF QBSBNOBNFBTUFQWBMVF QBSBNOBNFJUFSBUJPOTWBMVF QBSBNOBNFMTJHNBWBMVF QBSBNOBNFPHBJOWBMVF QBSBNOBNFMTLJQWBMVF QBSBNOBNFNJOJNVN4DPSFWBMVF QBSBNOBNFTSSWBMVF QBSBNOBNFTSUWBMVF QBSBNOBNFTUSWBMVF QBSBNOBNFTUUWBMVF QBSBNOBNFMJOFBS6QEBUFWBMVF QBSBNOBNFBOHVMBS6QEBUFWBMVF QBSBNOBNFUFNQPSBM6QEBUFWBMVF QBSBNOBNFSFTBNQMF5ISFTIPMEWBMVF QBSBNOBNFQBSUJDMFTWBMVF QBSBNOBNFYNJOWBMVF QBSBNOBNFZNJOWBMVF QBSBNOBNFYNBYWBMVF QBSBNOBNFZNBYWBMVF QBSBNOBNFEFMUBWBMVF QBSBNOBNFMMTBNQMFSBOHFWBMVF

[ 197 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

QBSBNOBNFMMTBNQMFTUFQWBMVF QBSBNOBNFMBTBNQMFSBOHFWBMVF QBSBNOBNFMBTBNQMFTUFQWBMVF SFNBQGSPNTDBOUP BSHTDBO@UPQJD OPEF %FGJOJOHQBSBNFUFSTGPSNPWF@CBTFOPEF OPEFQLHNPWF@CBTFUZQFNPWF@CBTFSFTQBXOGBMTFOBNFNPWF@CBTF PVUQVUTDSFFO SPTQBSBNGJMF GJOE EJGG@XIFFMFE@SPCPU@HB[FCPQBSBNDPTUNBQ@DPNNPO@QBSBNTZBNMDPNNBOEMPBE OTHMPCBM@DPTUNBQ SPTQBSBNGJMF GJOE EJGG@XIFFMFE@SPCPU@HB[FCPQBSBNDPTUNBQ@DPNNPO@QBSBNTZBNMDPNNBOEMPBE OTMPDBM@DPTUNBQ SPTQBSBNGJMF GJOE EJGG@XIFFMFE@SPCPU@HB[FCPQBSBNMPDBM@DPTUNBQ@QBSBNTZBNMDPNNBOEMPBE  SPTQBSBNGJMF GJOE EJGG@XIFFMFE@SPCPU@HB[FCPQBSBNHMPCBM@DPTUNBQ@QBSBNTZBNMDPNNBOEMPBE  SPTQBSBNGJMF GJOE EJGG@XIFFMFE@SPCPU@HB[FCPQBSBNCBTF@MPDBM@QMBOOFS@QBSBNTZBNM DPNNBOEMPBE SPTQBSBNGJMF GJOE EJGG@XIFFMFE@SPCPU@HB[FCPQBSBNEXB@MPDBM@QMBOOFS@QBSBNTZBNM DPNNBOEMPBE SPTQBSBNGJMF GJOE EJGG@XIFFMFE@SPCPU@HB[FCPQBSBNNPWF@CBTF@QBSBNTZBNMDPNNBOEMPBE OPEF MBVODI

[ 198 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

Running SLAM on the differential drive robot We can build the ROS package called EJGG@XIFFMFE@SPCPU@HB[FCP and can run the HNBQQJOHMBVODI file for building the map. The following are the commands to start with the mapping procedure. Start the robot simulation by using the Willow Garage world: $ roslaunch diff_wheeled_robot_gazebo diff_wheeled_gazebo_full.launch

Start the HNBQQJOH launch file with the following command: $ roslaunch diff_wheeled_robot_gazebo gmapping.launch

If the HNBQQJOH launch file is working fine, we will get the following kind of output on the Terminal:

(KIWTG6GTOKPCNOGUUCIGUFWTKPIIOCRRKPI

Start the keyboard teleoperation for manually navigating the robot around the environment. The robot can map its environment only if it covers the entire area: $ roslaunch diff_wheeled_robot_control keyboard_teleop.launch

[ 199 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

The current Gazebo view of the robot and the robot environment is shown next. The environment is with obstacles around the robot:

(KIWTG5KOWNCVKQPQHVJGTQDQVWUKPIVJG9KNNQY)CTCIGYQTNF

We can launch RViz and add a display type called Map and the topic name as NBQ.

[ 200 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

We can start moving the robot inside the world by using key board teleoperation, and we can see a map building according to the environment. The following image shows the completed map of the environment shown in RViz:

(KIWTG%QORNGVGFOCRQHVJGTQQOKP48K\

We can save the built map using the following command. This command will listen to the map topic and save into the image. The map server package does this operation: $ rosrun map_server map_saver -f willo

[ 201 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

Here XJMMP is the name of the map file. The map file is stored as two files: one is the YAML file, which contains the map metadata and the image name, and second is the image, which has the encoded data of the occupancy grid map. The following is the screenshot of the preceding command, running without any errors:

(KIWTG6GTOKPCNUETGGPUJQVYJKNGUCXKPICOCR

The saved encoded image of the map is shown next. If the robot gives accurate robot odometry data, we will get this kind of precise map similar to the environment. The accurate map improves the navigation accuracy through efficient path planning:

(KIWTG6JGUCXGFOCR

The next procedure is to localize and navigate in this static map.

[ 202 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

Implementing autonomous navigation using amcl and a static map The ROS amcl package provides nodes for localizing the robot on a static map. The amcl node subscribes the laser scan data, laser scan based maps, and the TF information from the robot. The amcl node estimates the pose of the robot on the map and publishes its estimated position with respect to the map. If we create a static map from the laser scan data, the robot can autonomously navigate from any pose of the map using amcl and the move_base nodes. The first step is to create a launch file for starting the amcl node. The amcl node is highly customizable; we can configure it with a lot of parameters. The list of parameters are available at the ROS package site (IUUQXJLJSPTPSHBNDM).

Creating an amcl launch file A typical BNDM launch file is given next. The amcl node is configured inside the BNDMMBVODIYNM file, which is in the EJGG@XIFFMFE@SPCPU@HB[FCPMBVODIJODMVEF package. The NPWF@CBTF node is also configured separately in the NPWF@CBTFMBVODIYNM file. The map file we created in the HNBQQJOH process is loaded here, using the NBQ@TFSWFS node: MBVODI .BQTFSWFS BSHOBNFNBQ@GJMFEFGBVMU GJOE EJGG@XIFFMFE@SPCPU@HB[FCPNBQTUFTUZBNM OPEFOBNFNBQ@TFSWFSQLHNBQ@TFSWFSUZQFNBQ@TFSWFSBSHT BSH NBQ@GJMF

JODMVEFGJMF GJOE EJGG@XIFFMFE@SPCPU@HB[FCPMBVODIJODMVEFTBNDMMBVODIYNM BSHOBNFJOJUJBM@QPTF@YWBMVF BSHOBNFJOJUJBM@QPTF@ZWBMVF BSHOBNFJOJUJBM@QPTF@BWBMVF JODMVEF JODMVEFGJMF GJOE EJGG@XIFFMFE@SPCPU@HB[FCPMBVODIJODMVEFTNPWF@CBTFMBVODIYNM

[ 203 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

MBVODI

The following is the code snippet of BNDMMBVODIYNM. This file is a bit lengthy, as we have to configure a lot of parameters for the BNDM node: MBVODI BSHOBNFVTF@NBQ@UPQJDEFGBVMUGBMTF BSHOBNFTDBO@UPQJDEFGBVMUTDBO BSHOBNFJOJUJBM@QPTF@YEFGBVMU BSHOBNFJOJUJBM@QPTF@ZEFGBVMU BSHOBNFJOJUJBM@QPTF@BEFGBVMU OPEFQLHBNDMUZQFBNDMOBNFBNDM QBSBNOBNFVTF@NBQ@UPQJDWBMVF BSHVTF@NBQ@UPQJD 1VCMJTITDBOTGSPNCFTUQPTFBUBNBYPG)[ QBSBNOBNFPEPN@NPEFM@UZQFWBMVFEJGG QBSBNOBNFPEPN@BMQIBWBMVF QBSBNOBNFHVJ@QVCMJTI@SBUFWBMVF QBSBNOBNFMBTFS@NBY@CFBNTWBMVF QBSBNOBNFMBTFS@NBY@SBOHFWBMVF

After creating this launch file, we can start the amcl node, using the following procedure: Start the simulation of the robot in Gazebo: $ roslaunch diff_wheeled_robot_gazebo diff_wheeled_gazebo_full.launch

Start the BNDM launch file, using the following command: $ roslaunch diff_wheeled_robot_gazebo amcl.launch

If the BNDM launch file is correctly loaded, the Terminal shows the following message:

(KIWTG6GTOKPCNUETGGPUJQVYJKNGGZGEWVKPICOEN

[ 204 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

If amcl is working fine, we can start commanding the robot to go into a position on the map using RViz, as shown in the following figure. In the figure, the arrow indicates the goal position. We have to enable LaserScan, Map, and Path visualizing plugins in RViz for viewing the laser scan, the global/local costmap, and the global/local paths. Using the 2D NavGoal button in RViz, we can command the robot to go to a desired position. The robot will plan a path to that point and give velocity commands to the robot controller to reach that point:

(KIWTG#WVQPQOQWUPCXKICVKQPWUKPICOENCPFVJGOCR

In the preceding image, we can see that we have placed a random obstacle in the robot's path, and that the robot has planned a path to avoid the obstacle. We can view the amcl particle cloud around the robot by adding a Pose Array on RViz and the topic is /particle_cloud. The following image shows the amcl particle around the robot:

[ 205 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

(KIWTG6JGCOENRCTVKENGENQWFCPFQFQOGVT[

Questions What is the main purpose of MoveIt! packages? What is the importance of the NPWF@HSPVQ node in MoveIt!? What is the purpose of the move_base node in the Navigation stack? What are the functions of the SLAM and amcl packages?

[ 206 ]

Using the ROS MoveIt! and Navigation Stack

Chapter 6

Summary This chapter offered a brief overview of MoveIt! and the Navigation stack of ROS, and demonstrated its capabilities using Gazebo simulation of a robotic arm mobile base. The chapter started with a MoveIt! overview and discussed detailed concepts about MoveIt!. After discussing MoveIt!, we interfaced MoveIt! and Gazebo. After interfacing, we executed the trajectory from MoveIt! on Gazebo. The next section was about the ROS Navigation stack. We discussed its concepts and workings as well. After discussing the concepts, we tried to interface our robot in Gazebo to the Navigation stack and build a map using SLAM. After doing SLAM, we performed autonomous navigation using amcl and the static map. In the next chapter, we will discuss pluginlib, nodelets, and controllers.

[ 207 ]

7

Working with pluginlib, Nodelets, and Gazebo Plugins

In the previous chapter, we discussed the interfacing and simulation of the robotic arm mobile robot to the ROS MoveIt! and Navigation stack. In this chapter, we will look at some of the advanced concepts in ROS, such as the ROS QMVHJOMJC, nodelets, and Gazebo plugins. We will discuss the functionalities and applications of each concept and will look at an example to demonstrate it's working. We have used Gazebo plugins in the previous chapters to get the sensor and robot behavior inside the Gazebo simulator. In this chapter, we are going to see how to create it. We will also discuss a modified form of ROS nodes called ROS nodelets. These features in ROS are implemented using a plugin architecture called QMVHJOMJC. In this chapter, we will discuss the following topics: Understanding QMVHJOMJC Implementing a sample plugin using QMVHJOMJC Understanding ROS nodelets Implementing a sample nodelet Understanding and creating a Gazebo plugin

Working with pluginlib, Nodelets, and Gazebo Plugins

Chapter 7

Understanding pluginlib Plugins are a commonly used term in the computer world. They are modular pieces of software that can add a new feature to the existing software application. The advantage of plugins is that we don't need to write all the features in the main software; instead, we can make an infrastructure on the main software to accept new plugins to it. Using this method, we can extend the capabilities of the software to any level. We need plugins for our robotics application too. When we are going to build a complex ROS-based application for a robot, plugins will be a good choice to extend the capabilities of the application. The ROS system provides a plugin framework called QMVHJOMJC to dynamically load/unload plugins, which can be a library or class. QMVHJOMJC represents a set of a C++ library, which helps to write plugins and load/unload whenever we need to. Plugin files are runtime libraries, such as shared objects (TP) or dynamic link libraries (%--), which are built without linking to the main application code. Plugins are separate entities that do not have any dependencies with the main software. The main advantage of plugins is that we can expand the application capabilities without making many changes in the main application code. We can create a simple plugin using QMVHJOMJC and can see all the procedures involved in creating a plugin using ROS pluginlib. Here, we are going to create a simple calculator application using QMVHJOMJC. We are adding each functionality of the calculator using plugins.

Creating plugins for the calculator application using pluginlib Creating a calculator application using plugins is a slightly tedious task compared to writing a single code. The aim of this example, however, is to show how to add new features to a calculator without modifying the main application code. In this example, we will see a computer application that loads plugins to perform each operation. Here, we only implement the main operations ,such as addition, subtraction, multiplication, and division. We can expand to any level by writing individual plugins for each operation.

[ 209 ]

Working with pluginlib, Nodelets, and Gazebo Plugins

Chapter 7

Before going on to create the plugin definition, we can access the calculator code from the QMVHJOMJC@DBMDVMBUPS folder for reference. We are going to create an ROS package called QMVHJOMJC@DBMDVMBUPS to build these plugins and the main calculator application. The following diagram shows how the calculator plugins and application are organized inside the QMVHJOMJC@DBMDVMBUPS ROS package:

(KIWTG1TICPK\CVKQPQHRNWIKPUKPVJGECNEWNCVQTCRRNKECVKQP

We can see the list of plugins of the calculator and a plugin base class called DBMD@GVODUJPOT. The plugin base class implements the common functionalities that are required by these plugins.

[ 210 ]

Working with pluginlib, Nodelets, and Gazebo Plugins

Chapter 7

This is how, we can create the ROS package and start developing plugins for the main calculator application.

Working with the pluginlib_calculator package For a quick start, we can use the existing ROS plugin package, QMVHJOMJC@DBMDVMBUPS. If we want to create this package from scratch, we can use the following command: $ catkin_create_pkg pluginlib_calculator pluginlib roscpp std_msgs

The main dependency of this package is QMVHJOMJC. We can discuss the main source files in this package to build plugins. However, you can get the plugin code from the code provided with this book or download it at this link: $ git clone https://github.com/jocacace/plugin_calculator

4UFQ$SFBUJOHUIFDBMDVMBUPS@CBTFIFBEFS`MF The DBMDVMBUPS@CBTFI file is present in the QMVHJOMJC@DBMDVMBUPSJODMVEFQMVHJOMJC@DBMDVMBUPS folder, and the main purpose of this file is to declare functions/methods that are commonly used by the plugins: OBNFTQBDFDBMDVMBUPS@CBTF \ DMBTTDBMD@GVODUJPOT \

Inside this code, we declare a class called DBMD@GVODUJPOT that encapsulates methods used by the plugins. This class is included in a namespace called DBMDVMBUPS@CBTF. We can add more classes inside this namespace to expand the functionalities of this base class: WJSUVBMWPJEHFU@OVNCFST EPVCMFOVNCFSEPVCMFOVNCFS WJSUVBMEPVCMFPQFSBUJPO 

These are the main methods implemented inside the DBMD@GVODUJPO class. The HFU@OVNCFS  function can retrieve two numbers as input to the calculator, and the PQFSBUJPO  function defines the mathematical operation we want to perform.

[ 211 ]

Working with pluginlib, Nodelets, and Gazebo Plugins

Chapter 7

4UFQ$SFBUJOHUIFDBMDVMBUPS@QMVHJOTIFBEFS`MF The DBMDVMBUPS@QMVHJOTI file is present in the QMVHJOMJC@DBMDVMBUPSJODMVEFQMVHJOMJC@DBMDVMBUPS folder, and the main purpose of this file is to define complete functions of the calculator plugins, which are named as "EE, 4VC, .VM, and %JW. Here is the explanation of this code: JODMVEFQMVHJOMJC@DBMDVMBUPSDBMDVMBUPS@CBTFI JODMVEFDNBUI OBNFTQBDFDBMDVMBUPS@QMVHJOT \ DMBTT"EEQVCMJDDBMDVMBUPS@CBTFDBMD@GVODUJPOT \

This header file includes the DBMDVMBUPS@CBTFI for accessing the basic functionalities of a calculator. Each plugin is defined as a class, and it inherits the DBMD@GVODUJPOT class from the DBMDVMBUPS@CBTFI class: DMBTT"EEQVCMJDDBMDVMBUPS@CBTFDBMD@GVODUJPOT \ QVCMJD "EE  \ OVNCFS@ OVNCFS@ ^ WPJEHFU@OVNCFST EPVCMFOVNCFSEPVCMFOVNCFS \ USZ\ OVNCFS@OVNCFS OVNCFS@OVNCFS ^ DBUDI JOUF \ TUEDFSS&YDFQUJPOXIJMFJOQVUUJOHOVNCFSTTUEFOEM ^ ^ EPVCMFPQFSBUJPO  \ SFUVSO OVNCFS@ OVNCFS@ ^

[ 212 ]

Working with pluginlib, Nodelets, and Gazebo Plugins

Chapter 7

QSJWBUF EPVCMFOVNCFS@ EPVCMFOVNCFS@ ^ ^

In this code, we can see definitions of inherited HFU@OVNCFST  and PQFSBUJPOT  functions. The HFU@OVNCFS  retrieves two number inputs and PQFSBUJPOT  performs the desired operation. In this case, it performs additional operations. We can see all other plugin definitions inside this header file.

4UFQ&YQPSUJOHQMVHJOTVTJOHUIFDBMDVMBUPS@QMVHJOTDQQ To load the class of plugins dynamically, we must export each class using a special macro called 1-6(*/-*#@&91035@$-"44. This macro must be present in any CPP file that consists of plugin classes. We have already defined the plugin class, and, in this file, we are going to define the macro statement only. Locate the DBMDVMBUPS@QMVHJOTDQQ file from the QMVHJOMJC@DBMDVMBUPSTSD folder. Here is how we export each plugin: JODMVEFQMVHJOMJCDMBTT@MJTU@NBDSPTI JODMVEFQMVHJOMJC@DBMDVMBUPSDBMDVMBUPS@CBTFI JODMVEFQMVHJOMJC@DBMDVMBUPSDBMDVMBUPS@QMVHJOTI 1-6(*/-*#@&91035@$-"44 DBMDVMBUPS@QMVHJOT"EE DBMDVMBUPS@CBTFDBMD@GVODUJPOT

Inside 1-6(*/-*#@&91035@$-"44, we need to provide the class name of the plugin and the base class.

4UFQ*NQMFNFOUJOHUIFQMVHJOMPBEFSVTJOHUIFDBMDVMBUPS@MPBEFSDQQ This plugin loader node loads each plugin and inputs the number to each plugin and fetches the result from the plugin. We can locate the DBMDVMBUPS@MPBEFSDQQ file from the QMVHJOMJC@DBMDVMBUPSTSD folder. Here is the explanation of this code: JODMVEFCPPTUTIBSFE@QUSIQQ JODMVEFQMVHJOMJCDMBTT@MPBEFSI JODMVEFQMVHJOMJC@DBMDVMBUPSDBMDVMBUPS@CBTFI

[ 213 ]

Working with pluginlib, Nodelets, and Gazebo Plugins

Chapter 7

These are the necessary header files to load the plugins: QMVHJOMJC$MBTT-PBEFSDBMDVMBUPS@CBTFDBMD@GVODUJPOT DBMD@MPBEFS QMVHJOMJC@DBMDVMBUPSDBMDVMBUPS@CBTFDBMD@GVODUJPOT

The QMVHJOMJC provides the $MBTT-PBEFS class, which is inside DMBTT@MPBEFSI, to load classes at runtime. We need to provide a name for the loader and the calculator base class as arguments: CPPTUTIBSFE@QUSDBMDVMBUPS@CBTFDBMD@GVODUJPOT BEE DBMD@MPBEFSDSFBUF*OTUBODF QMVHJOMJC@DBMDVMBUPS"EE

This will create an instance of the BEE class using the $MBTT-PBEFS object: BEE HFU@OVNCFST  EPVCMFSFTVMUBEE PQFSBUJPO 

These lines give an input and perform the operations in the plugin instance.

4UFQ$SFBUJOHUIFQMVHJOEFTDSJQUJPO`MFDBMDVMBUPS@QMVHJOTYNM After creating the calculator loader code, next we must describe the list of plugins inside this package in an XML file called the 1MVHJO%FTDSJQUJPO'JMF. The plugin description file contains all the information about the plugins inside a package, such as the name of the classes, types of classes, base class, and so on. The plugin description is an important file for plugin-based packages, because it helps the ROS system to automatically discover, load, and reason about the plugin. It also holds information such as the description of the plugin. The following code shows the plugin description file of our package called DBMDVMBUPS@QMVHJOTYNM, which is stored along with the $.BLF-JTUTUYU and QBDLBHFYNM files. You can get this file from the package folder itself.

Here is the explanation of this file: MJCSBSZQBUIMJCMJCQMVHJOMJC@DBMDVMBUPS DMBTTOBNFQMVHJOMJC@DBMDVMBUPS"EEUZQFDBMDVMBUPS@QMVHJOT"EE CBTF@DMBTT@UZQFDBMDVMBUPS@CBTFDBMD@GVODUJPOT EFTDSJQUJPO 5IJTJTBBEEQMVHJOEFTDSJQUJPO DMBTT

This code is for the "EE plugin and it defines the library path of the plugin, the class name, the class type, the base class, and the description.

[ 214 ]

Working with pluginlib, Nodelets, and Gazebo Plugins

Chapter 7

4UFQ3FHJTUFSJOHUIFQMVHJOXJUIUIF304QBDLBHFTZTUFN For QMVHJOMJC to find all plugin-based packages in the ROS system, we should export the plugin description file inside QBDLBHFYNM. If we do not include this plugin, the ROS system won't find the plugins inside the package. Here, we add the export tag to QBDLBHFYNM , as follows: FYQPSU QMVHJOMJC@DBMDVMBUPSQMVHJO\QSFGJY^DBMDVMBUPS@QMVHJOTYNM FYQPSU

To work this export command properly, we should insert the following lines in QBDLBHFYNM: CVJME@EFQFOE QMVHJOMJC@DBMDVMBUPSCVJME@EFQFOE SVO@EFQFOE QMVHJOMJC@DBMDVMBUPSSVO@EFQFOE

The current package should directly depend on itself, both at the time of building and also at runtime.

4UFQ&EJUJOHUIF$.BLF-JTUTUYU`MF Another difference with respect to a common ROS node regards the compilation directives included in the $.BLF-JTUTUYU file. To build the calculator plugins and loader nodes, we should add the following lines in $.BLF-JTUTUYU: QMVHJOMJC@UVUPSJBMTMJCSBSZ BEE@MJCSBSZ QMVHJOMJC@DBMDVMBUPSTSDDBMDVMBUPS@QMVHJOTDQQ UBSHFU@MJOL@MJCSBSJFT QMVHJOMJC@DBMDVMBUPS\DBULJO@-*#3"3*&4^ DBMDVMBUPS@MPBEFSFYFDVUBCMF BEE@FYFDVUBCMF DBMDVMBUPS@MPBEFSTSDDBMDVMBUPS@MPBEFSDQQ UBSHFU@MJOL@MJCSBSJFT DBMDVMBUPS@MPBEFS\DBULJO@-*#3"3*&4^

We are almost done with all the settings, and now it's time to build the package using the DBULJO@NBLF command.

4UFQ2VFSZJOHUIFMJTUPGQMVHJOTJOBQBDLBHF If the package is built properly, we can execute the loader. The following command will query the plugins inside a package: $ rospack plugins --attrib=plugin pluginlib_calculator

[ 215 ]

Working with pluginlib, Nodelets, and Gazebo Plugins

Chapter 7

We will get the following result if everything is built properly:

(KIWTG6JGTGUWNVQHVJGRNWIKPSWGT[

4UFQ3VOOJOHUIFQMVHJOMPBEFS After launched the roscore, we can execute the DBMDVMBUPS@MPBEFS using the following command: $ rosrun pluginlib_calculator calculator_loader

The following screenshot shows the output of this command, to check whether everything is working fine. The loader gives both inputs as  and we are getting the proper result, as shown, using plugins in the screenshot:

(KIWTG4GUWNVQHVJGRNWIKPNQCFGTPQFG

In the next section, we will look at a new concept called nodelets and discuss how to implement them.

Understanding ROS nodelets Nodelets are specific ROS nodes designed to run multiple algorithms within the same process in an efficient way, executing each process as threads. The threaded nodes can communicate with each other efficiently without overloading the network, with zero copy transport between two nodes. These threaded nodes can communicate with external nodes too. As we did using pluginlib, in nodelets, we can also dynamically load each class as a plugin, which has a separate namespace. Each loaded class can act as separate nodes, which are on a single process called nodelet.

[ 216 ]

Working with pluginlib, Nodelets, and Gazebo Plugins

Chapter 7

Nodelets are used when the volume of data transferred between nodes are very high; for example, in transferring data from 3D sensors or cameras. Next, we will look at how to create a nodelet.

Creating a nodelet In this section, we are going to create a basic nodelet that can subscribe a string topic called NTH@JO and publish the same string (TUE@NTHT4USJOH) on the topic NTH@PVU.

Step 1 - Creating a package for a nodelet We can create a package called OPEFMFU@IFMMP@XPSME , using the following command to create our nodelet: $ catkin_create_pkg nodelet_hello_world nodelet roscpp std_msgs

Otherwise, we can use the existing package from OPEFMFU@IFMMP@XPSME, or download it from the following link: $ git clone https://github.com/jocacace/nodelet_hello_world

Here, the main dependency of this package is the OPEFMFU package, which provides APIs to build a ROS nodelet.

Step 2 - Creating the hello_world.cpp nodelet Now, we are going to create the nodelet code. Create a folder called TSD inside the package and create a file called IFMMP@XPSMEDQQ. You will get the existing code from the OPEFMFU@IFMMP@XPSMETSD folder.

Step 3 - Explanation of hello_world.cpp Here is the explanation of the code: JODMVEFQMVHJOMJCDMBTT@MJTU@NBDSPTI JODMVEFOPEFMFUOPEFMFUI JODMVEFSPTSPTI JODMVEFTUE@NTHT4USJOHI JODMVEFTUEJPI

[ 217 ]

Working with pluginlib, Nodelets, and Gazebo Plugins

Chapter 7

These are the header files of this code. We should include DMBTT@MJTU@NBDSPI and OPEFMFUI to access the QMVHJOMJC APIs and nodelets APIs: OBNFTQBDFOPEFMFU@IFMMP@XPSME \ DMBTT)FMMPQVCMJDOPEFMFU/PEFMFU \

Here, we create a nodelet class called )FMMP, which inherits a standard nodelet base class. All nodelet classes should inherit from the nodelet base class and be dynamically loadable using QMVHJOMJC. Here, the )FMMP class is going to be used for dynamic loading: WJSUVBMWPJEPO*OJU  \ SPT/PEF)BOEMFQSJWBUF@OIHFU1SJWBUF/PEF)BOEMF  /0%&-&5@%( *OJUJBMJ[FEUIF/PEFMFU QVCQSJWBUF@OIBEWFSUJTFTUE@NTHT4USJOH NTH@PVU TVCQSJWBUF@OITVCTDSJCF NTH@JO)FMMPDBMMCBDLUIJT ^

This is the initialization function of a nodelet. This function should not block or do significant work. Inside the function, we are creating a node handle object, topic publisher, and subscriber on the topic NTH@PVU and NTH@JO respectively. There are macros to print debug messages while executing a nodelet. Here, we use /0%&-&5@%( to print debug messages in the console. The subscriber is tied up with a callback function called DBMMCBDL , which is inside the )FMMP class: WPJEDBMMCBDL DPOTUTUE@NTHT4USJOH$POTU1USJOQVU \ TUE@NTHT4USJOHPVUQVU PVUQVUEBUBJOQVU EBUB /0%&-&5@%( .FTTBHFEBUBTPVUQVUEBUBD@TUS  304@*/'0 .FTTBHFEBUBTPVUQVUEBUBD@TUS  QVCQVCMJTI PVUQVU ^

In the DBMMCBDL  function, it will print the messages from the NTH@JO topic and publish to the NTH@PVU topic: 1-6(*/-*#@&91035@$-"44 OPEFMFU@IFMMP@XPSME)FMMPOPEFMFU/PEFMFU

Here, we are exporting )FMMP as a plugin for the dynamic loading.

[ 218 ]

Working with pluginlib, Nodelets, and Gazebo Plugins

Chapter 7

Step 4 - Creating the plugin description file Like the QMVHJOMJC example, we have to create a plugin description file inside the OPEFMFU@IFMMP@XPSME package. The plugin description file, IFMMP@XPSMEYNM, is as follows: MJCSBSZQBUIMJCOPEFMFU@IFMMP@XPSME DMBTTOBNFOPEFMFU@IFMMP@XPSME)FMMPUZQFOPEFMFU@IFMMP@XPSME)FMMP CBTF@DMBTT@UZQFOPEFMFU/PEFMFU EFTDSJQUJPO "OPEFUPSFQVCMJTIBNFTTBHF EFTDSJQUJPO DMBTT MJCSBSZ

Step 5 - Adding the export tag in package.xml We need to add the export tag in QBDLBHFYNM and add build and run dependencies: FYQPSU OPEFMFUQMVHJO\QSFGJY^IFMMP@XPSMEYNM FYQPSU CVJME@EFQFOE OPEFMFU@IFMMP@XPSMECVJME@EFQFOE SVO@EFQFOE OPEFMFU@IFMMP@XPSMESVO@EFQFOE

Step 6 - Editing CMakeLists.txt We need to add additional lines of code in $.BLF-JTUTUYU to build a nodelet package. Here are the extra lines. You will get the complete $.BLF-JTUTUYU file from the existing package itself: %FDMBSFBDQQMJCSBSZ BEE@MJCSBSZ OPEFMFU@IFMMP@XPSME TSDIFMMP@XPSMEDQQ  4QFDJGZMJCSBSJFTUPMJOLBMJCSBSZPSFYFDVUBCMFUBSHFUBHBJOTU UBSHFU@MJOL@MJCSBSJFT OPEFMFU@IFMMP@XPSME \DBULJO@-*#3"3*&4^ 

[ 219 ]

Working with pluginlib, Nodelets, and Gazebo Plugins

Chapter 7

Step 7 - Building and running nodelets After following this procedure, we can build the package using DBULJO@NBLF and, if the build is successful, we can generate the shared object MJCOPEFMFU@IFMMP@XPSMETP file, which represents the plugin. The first step in running nodelets is to start the nodelet manager. A nodelet manager is a C++ executable program, which will listen to the ROS services and dynamically load nodelets. We can run a standalone manager or can embed it within a running node. The following commands can start the nodelet manager: Start SPTDPSF: $ roscore

Start the nodelet manager, using the following command: $ rosrun nodelet nodelet manager __name:=nodelet_manager

If the nodelet manager runs successfully, we will get a message, as shown here:

(KIWTG4WPPKPIVJGPQFGNGVOCPCIGT

After launching the nodelet manager, we can start the nodelet by using the following command: $ rosrun nodelet nodelet load nodelet_hello_world/Hello nodelet_manager __name:=nodelet1

When we execute the preceding command, the nodelet contacts the nodelet manager to instantiate an instance of the OPEFMFU@IFMMP@XPSME)FMMP nodelet with a name of OPEFMFU. The following screenshot shows the message when we load the nodelet:

(KIWTG4WPPKPIVJGPQFGNGV

[ 220 ]

Working with pluginlib, Nodelets, and Gazebo Plugins

Chapter 7

The topics generated after running this nodelet and the list of nodes are shown here:

(KIWTG6JGNKUVQHVQRKEUQHVJGPQFGNGV

We can test the node by publishing a string to the OPEFMFUNTH@JO topic and check whether we receive the same message in OPEFMFUNTH@PVU. The following command publishes a string to OPEFMFUNTH@JO: $ rostopic pub /nodelet1/msg_in std_msgs/String "Hello"

(KIWTG2WDNKUJKPICPFUWDUETKDKPIWUKPIVJGPQFGNGV

We can echo the NTH@PVU topic and can confirm whether the code is working properly. Here, we have seen that a single instance of the )FMMP  class is created as a node. We can create multiple instances of the )FMMP  class with different node names inside this nodelet.

[ 221 ]

Working with pluginlib, Nodelets, and Gazebo Plugins

Chapter 7

Step 8 - Creating launch files for nodelets We can also write launch files to load more than one instance of the nodelet class. The following launch file will load two nodelets, with the names UFTU and UFTU, and we can save it with the name IFMMP@XPSMEMBVODI: MBVODI 4UBSUFEOPEFMFUNBOBHFS OPEFQLHOPEFMFUUZQFOPEFMFUOBNFTUBOEBMPOF@OPEFMFU BSHTNBOBHFSPVUQVUTDSFFO 4UBSUJOHGJSTUOPEFMFU OPEFQLHOPEFMFUUZQFOPEFMFUOBNFUFTUBSHTMPBE OPEFMFU@IFMMP@XPSME)FMMPTUBOEBMPOF@OPEFMFUPVUQVUTDSFFO OPEF 4UBSUJOHTFDPOEOPEFMFU OPEFQLHOPEFMFUUZQFOPEFMFUOBNFUFTUBSHTMPBE OPEFMFU@IFMMP@XPSME)FMMPTUBOEBMPOF@OPEFMFUPVUQVUTDSFFO OPEF MBVODI

The preceding launch can be launched with the following command: $ roslaunch nodelet_hello_world hello_world.launch

The following message will show up on the Terminal if it is launched successfully:

(KIWTG.CWPEJKPIOWNVKRNGKPUVCPEGUQHVJG*GNNQ ENCUU

[ 222 ]

Working with pluginlib, Nodelets, and Gazebo Plugins

Chapter 7

The list of topics and nodes are shown here. We can see two nodelets instantiated and we can see their topics too:

(KIWTG6QRKEUIGPGTCVGFD[VJGOWNVKRNGKPUVCPEGUQH*GNNQ ENCUU

Topics are generated by the multiple instances of the )FMMP  class. We can see the interconnection between these nodelets using the SRU@HSBQI tool. Open SRU@HVJ: $ rosrun rqt_gui rqt_gui

Load the Node Graph plugin from the following option, Plugins | Introspection | Node Graph , and you will get the graph as shown in the following figure:

(KIWTG#VYQPQFGKPUVCPEGQHCPQFGNGV

Alternatively, you can directly load the SRU@OPEF plugin: $ rosrun rqt_graph rqt_graph

[ 223 ]

Working with pluginlib, Nodelets, and Gazebo Plugins

Chapter 7

Understanding the Gazebo plugins Gazebo plugins help us to control the robot models, sensors, world properties, and even the way Gazebo runs. Like pluginlib and nodelets, Gazebo plugins are a set of C++ code, which can be dynamically loaded/unloaded from the Gazebo simulator. Using plugins, we can access all the components of Gazebo, and also it is independent of ROS, so that it can share with people who are not using ROS. We can mainly classify the plugins as follows: The world plugin: Using the world plugin, we can control the properties of a specific world in Gazebo. We can change the physics engine, the lighting, and other world properties using this plugin. The model plugin: The model plugin is attached to a specific model in Gazebo and controls its properties. The parameters, such as the joint state of the model, control of the joints, and so on, can be controlled using this plugin. The sensor plugin: The sensor plugins are for modeling sensors, such as camera, IMU, and so on, in Gazebo. The system plugin: The system plugin is started along with the Gazebo startup. A user can control a system-related function in Gazebo using this plugin. The visual plugin: The visual property of any Gazebo component can be accessed and controlled using the visual plugin. Before starting development with Gazebo plugins, we might need to install some packages. The Gazebo version installed along with ROS Kinetic is 7.0, so you might need to install its development package in Ubuntu using the following command: $ sudo apt-get install libgazebo7-dev

The Gazebo plugins are independent of ROS and we don't need ROS libraries to build a plugin.

Creating a basic world plugin We will look at a basic Gazebo world plugin and try to build and load it in Gazebo. Create a folder called HB[FCP@CBTJD@XPSME@QMVHJO in a desired folder and create a CPP file called IFMMP@XPSMEDD: $ mkdir gazebo_basic_world_plugin && cd gazebo_basic_world_plugin $ nano hello_world.cc

[ 224 ]

Working with pluginlib, Nodelets, and Gazebo Plugins

Chapter 7

The definition of IFMMP@XPSMEDD is as follows: (B[FCPIFBEFSGPSHFUUJOHDPSFHB[FCPGVODUJPOT JODMVEFHB[FCPHB[FCPII "MMHB[FCPQMVHJOTTIPVMEIBWFHB[FCPOBNFTQBDF OBNFTQBDFHB[FCP \ 5IFDVTUPN8PSMEQMVHJO5VUPSJBMTJTJOIFSJUJOHGSPNTUBOEBSE XPSME1MVHJO&BDIXPSMEQMVHJOIBTUPJOIFSJUJOHGSPNTUBOEBSEQMVHJOUZQF DMBTT8PSME1MVHJO5VUPSJBMQVCMJD8PSME1MVHJO \ QVCMJD8PSME1MVHJO5VUPSJBM 8PSME1MVHJO  \ QSJOUG )FMMP8PSME=O ^

5IF-PBEGVODUJPODBOSFDFJWFUIF4%'FMFNFOUT QVCMJDWPJE-PBE QIZTJDT8PSME1US@XPSMETEG&MFNFOU1US@TEG \ ^ ^ 3FHJTUFSJOH8PSME1MVHJOXJUI4JNVMBUPS (;@3&(*45&3@803-%@1-6(*/ 8PSME1MVHJO5VUPSJBM ^

The header file used in this code is HB[FCPHB[FCPII  . The header contains core functionalities of Gazebo. Other headers are as follows: HB[FCPQIZTJDTQIZTJDTII: This is the Gazebo header for accessing the

physics engine parameters HB[FCPSFOEFSJOHSFOEFSJOHII: This is the Gazebo header for handling rendering parameters HB[FCPTFOTPSTTFOTPSTII: This is the header for handling sensors At the end of the code, we must export the plugin using the following statements.

[ 225 ]

Working with pluginlib, Nodelets, and Gazebo Plugins

Chapter 7

The (;@3&(*45&3@803-%@1-6(*/ 8PSME1MVHJO5VUPSJBM macro will register and export the plugin as a world plugin. The following macros are used to register for sensors, models, and so on: (;@3&(*[email protected]%&-@1-6(*/: This is the export macro for the Gazebo robot

model (;@3&(*45&3@4&/403@1-6(*/: This is the export macro for the Gazebo sensor model (;@3&(*45&3@4:45&.@1-6(*/: This is the export macro for the Gazebo system (;@3&(*45&3@7*46"-@1-6(*/: This is the export macro for Gazebo visuals After setting the code, we can make the $.BLF-JTUTUYU for compiling the source. The following is the source of $.BLF-JTUTUYU: OBOPHB[FCP@CBTJD@XPSME@QMVHJO$.BLF-JTUTUYU DNBLF@NJOJNVN@SFRVJSFE 7&34*0/'"5"-@&3303 TFU $.",&@$99@'-"(4TUED

Y\$.",&@$99@'-"(4^ GJOE@QBDLBHF #PPTU3&26*3&%$0.10/&/54TZTUFN JODMVEF@EJSFDUPSJFT \#PPTU@*/$-6%&@%*34^ MJOL@EJSFDUPSJFT \#PPTU@-*#3"3:@%*34^ JODMVEF 'JOE1LH$POGJH JG 1,(@$0/'*(@'06/% QLH@DIFDL@NPEVMFT (";�HB[FCP FOEJG  JODMVEF@EJSFDUPSJFT \(";�@*/$-6%&@%*34^ MJOL@EJSFDUPSJFT \(";�@-*#3"3:@%*34^ BEE@MJCSBSZ IFMMP@XPSME4)"3&%IFMMP@XPSMEDD UBSHFU@MJOL@MJCSBSJFT IFMMP@XPSME\(";�@-*#3"3*&4^\#PPTU@-*#3"3*&4^

Create a CVJME folder for storing the shared object: $ mkdir build && cd build

After switching to the build folder, execute the following command to compile and build the source code: $ cmake ../ $ make

[ 226 ]

Working with pluginlib, Nodelets, and Gazebo Plugins

Chapter 7

After building the code, we will get a shared object called MJCIFMMP@XPSMETP and we have to export the path of this shared object in (";�@1-6(*/@1"5) and add it to the CBTISD file: FYQPSU (";�@1-6(*/@1"5)\(";�@1-6(*/@1"5)^QBUIUPHB[FCP@CBTJD@XPSME@QMVHJO CVJME

After setting the Gazebo plugin path and reloading the CBTISD file, we can use it inside the URDF file or the SRDF file. The following is a sample world file called IFMMPXPSME, which includes this plugin: OBOPHB[FCP@CBTJD@XPSME@QMVHJOIFMMPXPSME 
YNMWFSTJPO
TEGWFSTJPO XPSMEOBNFEFGBVMU QMVHJOOBNFIFMMP@XPSMEGJMFOBNFMJCIFMMP@XPSMETP XPSME TEG

This file is also contained in the code provided with this book and in the following Git repository: $ git clone https://github.com/jocacace/gazebo_basic_world_plugin

Run the Gazebo server and load this world file: $ cd gazebo_basic_world_plugin $ gzserver hello.world --verbose

(KIWTG6JG)C\GDQYQTNFRNWIKPRTKPVKPI*GNNQ9QTNF

The Gazebo world plugin prints )FMMP8PSME. We will source the code for various Gazebo plugins from the Gazebo repository.

[ 227 ]

Working with pluginlib, Nodelets, and Gazebo Plugins

Chapter 7

We can check IUUQTCJUCVDLFUPSHPTSGHB[FCPbrowse for the source code., take the examples folder and then the plugins, as shown in the following figure:

(KIWTG6JGNKUVQH)C\GDQRNWIKPUKPVJGTGRQUKVQT[

Questions What is QMVHJOMJC and what are its main applications? What is the main application of nodelets? What are the different types of Gazebo plugins? What is the function of the model plugin in Gazebo?

[ 228 ]

Working with pluginlib, Nodelets, and Gazebo Plugins

Chapter 7

Summary In this chapter, we covered some advanced concepts, such as the QMVHJOMJC , OPEFMFUT , and Gazebo plugins, which can be used to add more functionalities to a complex ROS application. We discussed the basics of QMVHJOMJC and saw an example using it. After covering QMVHJOMJC, we looked at the ROS nodelets, which are widely used in highperformance applications. Also, we looked at an example using the ROS nodelets. Finally, we came to the Gazebo plugins that are used to add functionalities to Gazebo simulators. In the next chapter, we will discuss more on the RViz plugin and the ROS controllers.

[ 229 ]

8

Writing ROS Controllers and Visualization Plugins In the last chapter, we discussed pluginlib, nodelets, and Gazebo plugins. The base library for making plugins in ROS is pluginlib, and the same library can be used in nodelets and Gazebo plugins. In this chapter, we will continue with pluginlib-based concepts, such as ROS controllers and RViz plugins. We have already worked with ROS controllers and have reused some standard controllers, such as joint state, position, and trajectory controllers in Chapter 4: Simulating Robots Using ROS and Gazebo. In this chapter, we will see how to write a basic ROS controller for a generic robot. We will implement a desired controller for our TFWFO@EPG@BSN robot, developed in previous chapters, executing it in the Gazebo simulator. The RViz plugins can add more functionality to RViz, and in this chapter we will look at how to create a basic RViz plugin. The detailed topics that we are going to discuss in this chapter are as follows: Understanding packages required for ROS controller development Setting the ROS controller development environment Understanding SPT@DPOUSPM packages Writing and running a basic ROS controller Writing and running an RViz plugin Let us see how to develop an ROS controller; the first step is to understand the dependency packages required to start building custom controllers.

Writing ROS Controllers and Visualization Plugins

Chapter 8

The main set of packages used to develop a controller generic to all robots is contained in the SPT@DPOUSPM stack. This is a rewritten version of the QS@NFDIBOJTN, containing useful libraries to write low-level controllers for PR2 robots (IUUQTXXXXJMMPXHBSBHFDPNQBHFTQSPWFSWJFX) used in the past version of ROS. In ROS Kinetic, QS@NFDIBOJTN has been substituted by the SPT@DPOUSPM stack. The following is the description of some useful packages that help us to write robot controllers: SPT@DPOUSPM: This package takes as input the joint state data directly from the

robot's actuators and a desired set point, generating the output to send to its motors. The output is usually represented by the join position, velocity, or effort. DPOUSPMMFS@NBOBHFS: The controller manager can load and manage multiple controllers and can work them in a real-time compatible loop. DPOUSPMMFS@JOUFSGBDF: This is the controller base class package from which all custom controllers should inherit the controller base class. The controller manager will only load the controller if it inherits from this package. IBSEXBSF@JOUFSGBDF: This package represents the interface between the implemented controller and hardware of the robot. Using this interface, controllers can directly access cyclically the hardware components. KPJOU@MJNJUT@JOUFSGBDF: This package allows us to set joint limits to safely work with our robot. Joint limits are also included in the URDF of the robot. This package is different than the URDF, because it allows us to additionally specify acceleration and jerk limits. In addition, the position, velocity, and effort values contained within the URDF model can be overridden using this package. Commands sent to the hardware are filtered according to the specified joint limits. SFBMUJNF@UPPMT: This contains a set of tools that can be used from a hard realtime thread, if the operating system supports real-time behavior. The tools currently only provide the real-time publisher, which makes it possible to publish messages to an ROS topic in real time. Because we have already worked with SPT@DPOUSPM in $IBQUFS, Simulating Robots Using ROS and Gazebo, everything should be already installed on to our system. Otherwise, to operate this package, we should install the following ROS packages from the Ubuntu/Debian repositories: $ sudo apt-get install ros-kinetic-ros-control ros-kinetic-ros-controllers

[ 231 ]

Writing ROS Controllers and Visualization Plugins

Chapter 8

Before writing the ROS controller, it will be good to understand the use of each package of the SPT@DPOUSPM stack.

Understanding ros_control packages The SPT@DPOUSPM stack contains packages for writing ROS low-level controllers. The first package that we are going to discuss is the controller interface package.

The controller_interface package The basic ROS low-level controller that we want to implement must inherit a base class called DPOUSPMMFS@JOUFSGBDF$POUSPMMFS from this package. This represents a base class containing four fundamental functions: JOJU  , TUBSU , VQEBUF , and TUPQ . The basic structure of the $POUSPMMFS class is given as follows: OBNFTQBDFDPOUSPMMFS@JOUFSGBDF \ DMBTT$POUSPMMFS \ QVCMJD WJSUVBMCPPMJOJU IBSEXBSF@JOUFSGBDF SPCPU)8 SPT/PEF)BOEMFOI WJSUVBMWPJETUBSUJOH  WJSUVBMWPJEVQEBUF  WJSUVBMWPJETUPQQJOH  ^ ^

The workflow of the controller class is shown as follows:

[ 232 ]

Writing ROS Controllers and Visualization Plugins

Chapter 8

Initializating the controller The first function executing when a controller is loaded is JOJU . The JOJU  function will not start running the controller, it will just initialize it. The initialization can take any amount of time before starting the controllers. The declaration of the JOJU function is given as follows: WJSUVBMCPPMJOJU IBSXBSF@JOUFSGBDF SPCPU)8SPT/PEF)BOEMFOI

The function arguments are given as follows: IBSEXBSF@JOUFSGBDF SPCPU)8: This variable represents the specific

hardware interface used by the controller to develop. ROS contains a list of already-implemented hardware interfaces, such as: Joint Command Interfaces (effort, velocity, and position) Joint State Interfaces Actuator State Interfaces We can even create our own hardware interface. In the next example, a Position Joint Interface will be used. SPT/PEF)BOEMFOI: The controller can read the robot configuration and even advertise topics using this /PEFIBOEMF. The JOJU  method only executes once while the controller is loaded by the controller manager. If the JOJU  method is not successful, it will unload from the controller manager. We can write a custom message if any error occurs inside the JOJU  method.

Starting the ROS controller The TUBSUJOH  method executes once just before running the controller. This method will only execute once before updating and running the controller. The TUBSUJOH  method declaration is given as follows: WJSUVBMWPJETUBSUJOH 

The controller can also call the TUBSUJOH  method when it restarts the controller without unloading it.

[ 233 ]

Writing ROS Controllers and Visualization Plugins

Chapter 8

Updating the ROS controller The VQEBUF  function is the most important method that makes the controller alive. The VQEBUF method, by default, executes the code inside it at a rate of 1,000 Hz. It means the controller completes one execution within 1 millisecond. WJSUVBMWPJEVQEBUF 

Stopping the controller This method will call when a controller is stopped. The TUPQQJOH  method will execute as the last VQEBUF  call and only executes once. The TUPQQJOH  method will not fail and return nothing too. The following is the declaration of the TUPQQJOH  method: WJSUVBMWPJETUPQQJOH 

The controller_manager The DPOUSPMMFS@NBOBHFS package can load and unload the desired controller. The controller manager also ensures that the controller will not set a goal value that is less than or greater than the safety limits of the joint. The controller manager also publishes the states of the joint in the KPJOU@TUBUF (TFOTPS@NTHT+PJOU4UBUF) topic at a default rate of 100 Hz. The following figure shows the basic workflow of a controller manager:

The controller manager can load and unload a plugin. When a controller is loaded by the controller manager, it will first initialize it, but it will not start running. After loading the controller, we can start and stop the controller. When we start the controller, it will run the controller, and when we stop it, it will simply stop. Stopping doesn't mean it is unloaded. But if the controller is unloaded from the controller manager, we can't access the controller.

[ 234 ]

Writing ROS Controllers and Visualization Plugins

Chapter 8

Writing a basic joint controller in ROS The basic prerequisites for writing an ROS controller are already installed. We have discussed the underlying concepts of controllers. Now we can start creating a package for our own controller. We are going to develop a controller that can access a joint of the robot and move the robot in a sinusoidal fashion. In particular, the first joint of the TFWFO@EPG@BSN will follow a sinusoidal motion. The procedure of building a controller is similar to other plugins development that we have seen earlier. The list of procedures to create an ROS controller is given as follows: 1. 2. 3. 4. 5. 6. 7.

Create an ROS package with the necessary dependencies. Write controller code in C++. Register or export the C++ class as a plugin. Define the plugin definition in an XML file. Edit the $.BLF-JTUTUYU and QBDLBHFYNM files for exporting the plugin. Write the configuration for our controller. Load the controller using the controller manager.

Step 1 - Creating the controller package The first step is to create the controller package with all its dependencies. The following command can create a package for the controller called NZ@DPOUSPMMFS: $ catkin_create_pkg my_controller roscpp pluginlib controller_interface

We will get the existing package from the NZ@DPOUSPMMFS folder of the code provided with this book or clone the package from the following Git repository: $ git clone https://github.com/jocacace/my_controller.git

[ 235 ]

Writing ROS Controllers and Visualization Plugins

Chapter 8

Step 2 ` Creating the controller header file We will get the header file NZ@DPOUSPMMFSI from the NZ@DPOUSPMMFSTSD folder. Given in the following is the header file definition of NZ@DPOUSPMMFSI. As already stated, in this header we are going to implement the functions contained in the DPOUSPMMFS@JOUFSGBDF$POUSPMMFS class: JODMVEFDPOUSPMMFS@JOUFSGBDFDPOUSPMMFSI JODMVEFIBSEXBSF@JOUFSGBDFKPJOU@DPNNBOE@JOUFSGBDFI JODMVEFQMVHJOMJCDMBTT@MJTU@NBDSPTI

OBNFTQBDFNZ@DPOUSPMMFS@OT\ DMBTT.Z$POUSPMMFS$MBTTQVCMJD DPOUSPMMFS@JOUFSGBDF$POUSPMMFSIBSEXBSF@JOUFSGBDF1PTJUJPO+PJOU*OUFSGBDF \ QVCMJD CPPMJOJU IBSEXBSF@JOUFSGBDF1PTJUJPO+PJOU*OUFSGBDF IX SPT/PEF)BOEMFO WPJEVQEBUF DPOTUSPT5JNFUJNFDPOTUSPT%VSBUJPOQFSJPE WPJETUBSUJOH DPOTUSPT5JNFUJNF WPJETUPQQJOH DPOTUSPT5JNFUJNF QSJWBUF IBSEXBSF@JOUFSGBDF+PJOU)BOEMFKPJOU@ EPVCMFJOJU@QPT@ ^ ^

In the preceding code, we can see the controller class, .Z$POUSPMMFS$MBTT and we are inheriting the base class, DPOUSPMMFS@JOUFSGBDF$POUSPMMFS. We can see that each function inside the $POUSPMMFS class is overriding in our class.

Step 3 ` Creating the controller source file Create a folder called TSD inside the package and create a C++ file called NZ@DPOUSPMMFS@GJMFDQQ, which is the class definition of the preceding header. Given in the following is the definition of NZ@DPOUSPMMFS@GJMFDQQ, which has to be saved inside the TSD folder: JODMVEFNZ@DPOUSPMMFSI

[ 236 ]

Writing ROS Controllers and Visualization Plugins

Chapter 8

OBNFTQBDFNZ@DPOUSPMMFS@OT\ $POUSPMMFSJOJUJBMJ[BUJPO CPPM.Z$POUSPMMFS$MBTTJOJU IBSEXBSF@JOUFSGBDF1PTJUJPO+PJOU*OUFSGBDF IXSPT/PEF)BOEMFO \ 3FUSJFWFUIFKPJOUPCKFDUUPDPOUSPM TUETUSJOHKPJOU@OBNF JG OIHFU1BSBN KPJOU@OBNFKPJOU@OBNF\ 304@&3303 /PKPJOU@OBNFTQFDJGJFE SFUVSOGBMTF ^ KPJOU@IX HFU)BOEMF TIPVMEFS@QBO@KPJOU SFUVSOUSVF ^ $POUSPMMFSTUBSUVQ WPJE.Z$POUSPMMFS$MBTTTUBSUJOH DPOTUSPT5JNFUJNF\ (FUJOJUJBMQPTJUJPOUPVTFJOUIFDPOUSPMQSPDFEVSF JOJU@QPT@KPJOU@HFU1PTJUJPO  ^ $POUSPMMFSSVOOJOH WPJE.Z$POUSPMMFS$MBTTVQEBUF DPOTUSPT5JNFUJNFDPOTUSPT%VSBUJPO QFSJPE \ 1FSGPSNBTJOVTPJEBMNPUJPOGPSKPJOUTIPVMEFS@QBO@KPJOU EPVCMFEQPTJOJU@QPT@  TJO SPT5JNFOPX UP4FD  EPVCMFDQPTKPJOU@HFU1PTJUJPO  KPJOU@TFU$PNNBOE  DQPTEQPT"QQMZDPNNBOEUPUIFTFMFDUFE KPJOU  ^ $POUSPMMFSFYJUJOH WPJE.Z$POUSPMMFS$MBTTTUPQQJOH DPOTUSPT5JNFUJNF\^ ^ 3FHJTUFSUIFQMVHJO1-6(*/-*#@&91035@$-"44 NZ@OBNFTQBDF.Z1MVHJO CBTF@DMBTT@OBNFTQBDF1MVHJO#BTF$MBTT 1-6(*/-*#@&91035@$-"44 NZ@DPOUSPMMFS@OT.Z$POUSPMMFS$MBTT DPOUSPMMFS@JOUFSGBDF$POUSPMMFS#BTF

Step 4 ` Explaining the controller source file In this section, we can see the explanation of each section of the code: $POUSPMMFSJOJUJBMJ[BUJPOJOOPOSFBMUJNF

[ 237 ]

Writing ROS Controllers and Visualization Plugins

Chapter 8

CPPM.Z$POUSPMMFS$MBTTJOJU IBSEXBSF@JOUFSGBDF1PTJUJPO+PJOU*OUFSGBDF IXSPT/PEF)BOEMFO \

The preceding is the JOJU  function definition of the controller. This will be called when a controller is loaded by the controller manager. Inside the JOJU  function, we are creating an instance of the state of the robot (IX) and /PEF)BOEMF, and we also get the manager of the joint interacting with the controller. In our example, we defined the joint to control in the NZ@DPOUPMMFSZBNM file, loading the joint name into the ROS parameter server. This function returns the success or the failure in the controller initialization: JG OIHFU1BSBN KPJOU@OBNFKPJOU@OBNF\ 304@&3303 /PKPJOU@OBNFTQFDJGJFE SFUVSOGBMTF ^

This code will create a joint state object for a desired joint. Here is an instance of the IBSEXBSF@JOUFSGBDF class. KPJOU@OBNF is the desired joint in which we are attaching the controller: $POUSPMMFSTUBSUVQJOSFBMUJNF WPJE.Z$POUSPMMFS$MBTTTUBSUJOH DPOTUSPT5JNFUJNF \ JOJU@QPT@KPJOU@HFU1PTJUJPO  ^

After the controller is loaded, the next step is to start it. The preceding function will execute when we start a controller. In this function, it will retrieve the current position of the joint, storing its value into the JOJU@QPT@ variable: $POUSPMMFSVQEBUFMPPQJOSFBMUJNF WPJE.Z$POUSPMMFS$MBTTVQEBUF DPOTUSPT5JNFUJNFDPOTUSPT%VSBUJPO QFSJPE \ 1FSGPSNBTJOVTPJEBMNPUJPOGPSKPJOUTIPVMEFS@QBO@KPJOU EPVCMFEQPTJOJU@QPT@  TJO SPT5JNFOPX UP4FD  EPVCMFDQPTKPJOU@HFU1PTJUJPO  KPJOU@TFU$PNNBOE  DQPTEQPT"QQMZDPNNBOEUPUIFTFMFDUFE KPJOU  ^

This is the VQEBUF function of the controller, which will continuously move the joint in a sinusoidal fashion.

[ 238 ]

Writing ROS Controllers and Visualization Plugins

Chapter 8

Step 5 ` Creating the plugin description file We can define the plugin definition file, which is given in the following code. The plugin file is being saved inside the package folder under the name of DPOUSPMMFS@QMVHJOTYNM: MJCSBSZQBUIMJCMJCNZ@DPOUSPMMFS@MJC DMBTTOBNFNZ@DPOUSPMMFS@OT.Z$POUSPMMFS$MBTT UZQFNZ@DPOUSPMMFS@OT.Z$POUSPMMFS$MBTT CBTF@DMBTT@UZQFDPOUSPMMFS@JOUFSGBDF$POUSPMMFS#BTF MJCSBSZ

Step 6 ` Updating package.xml We need to update QBDLBHFYNM for pointing the DPOUSPMMFS@QMVHJOTYNM file: FYQPSU DPOUSPMMFS@JOUFSGBDFQMVHJO\QSFGJY^DPOUSPMMFS@QMVHJOTYNM FYQPSU

Step 7 ` Updating CMakeLists.txt After doing all these things, we can compose $.BLF-JTUTUYU of the package: NZ@DPOUSPMMFS@GJMFMJCSBSZ BEE@MJCSBSZ NZ@DPOUSPMMFS@MJCTSDNZ@DPOUSPMMFSDQQ UBSHFU@MJOL@MJCSBSJFT NZ@DPOUSPMMFS@MJC\DBULJO@-*#3"3*&4^

Step 8 ` Building the controller After completing $.BLF-JTUTUYU, we can build our controller using the DBULJO@NBLF command. After building, check that the controller is configured as a plugin using the SPTQBDL command, as shown here: $ rospack plugins --attrib=plugin controller_interface

With this command, all the controllers related to DPOUSPMMFS@JOUFSGBDF will be listed.

[ 239 ]

Writing ROS Controllers and Visualization Plugins

Chapter 8

If everything has been performed correctly, the output may look like the following:

Step 9 ` Writing the controller configuration file After proper installation of the controller, we can configure it and run it. The first procedure is to create the configuration file of the controller that consists of the controller type, joint name, joint limits, and so on. The configuration file is saved as a YAML file that must be saved inside the package. We are creating a YAML file with the name of NZ@DPOUSPMMFSZBNM, and the definition is given as follows: 'JMFMPBEFEEVSJOH(B[FCPTUBSUVQ NZ@DPOUSPMMFS@OBNF UZQFNZ@DPOUSPMMFS@OT.Z$POUSPMMFS$MBTT KPJOU@OBNFFMCPX@QJUDI@KPJOU

This file is the configuration of the controller. In particular, this file contains the type of the controller represented by the name of the class compiled with the controller source code and the set of parameters to pass to the controller. In our case, this is the name of the joint to control.

Step 10 ` Writing the launch file for the controller The joint assigned for showing the working of this controller is TIPVMEFS@QBO@KPJOU of the robot TFWFO@EPG@BSN. After creating the YAML file, we can create a launch file inside the launch folder, which can load the controller configuration file and run the controller. The launch file is called NZ@DPOUSPMMFSMBVODI, which is given as follows: 
YNMWFSTJPO
MBVODI JODMVEFGJMF GJOENZ@DPOUSPMMFSMBVODITFWFO@EPG@BSN@XPSMEMBVODI  SPTQBSBNGJMF GJOENZ@DPOUSPMMFSNZ@DPOUSPMMFSZBNM DPNNBOEMPBE OPEFOBNFNZ@DPOUSPMMFS@TQBXOFSQLHDPOUSPMMFS@NBOBHFS UZQFTQBXOFSSFTQBXOGBMTF

[ 240 ]

Writing ROS Controllers and Visualization Plugins

Chapter 8

PVUQVUTDSFFOBSHTNZ@DPOUSPMMFS@OBNF MBVODI

In the following code, we explain the launch file: MBVODI JODMVEFGJMF GJOENZ@DPOUSPMMFSMBVODITFWFO@EPG@BSN@XPSMEMBVODI 

Here we run the Gazebo simulator, launching a modified version of TFWFO@EPG@BSN: SPTQBSBNGJMF GJOENZ@DPOUSPMMFSNZ@DPOUSPMMFSZBNMDPNNBOEMPBE

Then, we load the developed controller. Finally, we spawn the controller: OPEFOBNFNZ@DPOUSPMMFS@TQBXOFSQLHDPOUSPMMFS@NBOBHFS UZQFTQBXOFSSFTQBXOGBMTF PVUQVUTDSFFOBSHTNZ@DPOUSPMMFS@OBNF

In this way, the DPOUSPMMFS@NBOBHFS will run the controller specified in the BSHT list. In our case, only the NZ@DPOUSPMMFS@OBNF is executed through the JOJU , the TUBSU , and the VQEBUF  functions implemented by the controller.

Step 11 ` Running the controller along with the seven dof arm in Gazebo After creating the controller launch files, we should test it on our robot. We can launch the Gazebo simulation using the following command: $ roslaunch my_controller my_controller.launch

[ 241 ]

Writing ROS Controllers and Visualization Plugins

Chapter 8

When we launch the simulation, all of the controllers associated with the robot also get started. The purpose of our controller is to move the FMCPX@QJUDI@KPJOU of the TFWFO@EPG@BSN, as defined in the controller. If everything is properly working, the elbow of the robot should start to move in a sinusoidal way:

If there are existing controllers handling this same joint, our controller can't work properly. To avoid this situation, we need to stop the controller that is handling the same joint of the robot. A set of services are exposed by the DPOUSPMMFS@NBOBHFS to manage the controllers of the robot. For example, we can use the following command to check the state of the controllers loaded in the system: $ rosservice call /controller_manager/list_controllers

The output of this command is shown in the following screenshot:

[ 242 ]

Writing ROS Controllers and Visualization Plugins

Chapter 8

In the previous screenshot, you can see that our controller (NZ@DPOUSPMMFS@OBNF) is running. We can stop it using the DPOUSPMMFS@NBOBHFSTXJUDI@DPOUSPMMFS service, as shown in the following screenshot:

Consider that in this example we are exploiting the HB[FCP@SPT@DPOUSPM plugin to run our controller. This plugin represents the hardware interface of our robot in the simulated scene. In the case of a real robot, we should write our own hardware interface to apply control data to robot actuators. In conclusion, SPT@DPOUSPM implements a standard set of generic controllers, such as FGGPSU@DPOUSPMMFST, KPJOU@TUBUF@DPOUSPMMFST, QPTJUJPO@DPOUSPMMFST, and velocity controllers for any kind of robots. We have already used these ROS controllers from SPT@DPOUSPM in $IBQUFS, Working with 3D Robot Modeling in ROS. SPT@DPOUSPM is still in development. Here we used SPT@DPOUSPM to develop a simple dedicated position controller for our TFWFO@EPG@BSN robot. You can check the availability of new controllers through the wiki page of SPT@DPOUSPM at IUUQTHJUIVCDPNSPTDPOUSPMTSPT@DPOUSPMXJLJ.

Understanding the ROS visualization tool (RViz) and its plugins The RViz tool is an official 3D visualization tool of ROS. Almost all kinds of data from sensors can be viewed through this tool. RViz will be installed along with the ROS desktop full installation. Let's launch RViz and see the basic components present in RViz.

[ 243 ]

Writing ROS Controllers and Visualization Plugins

Chapter 8

Start SPTDPSF: $ roscore

Start SWJ[: $ rviz

The important sections of the RViz GUI are marked, and the uses of each section are shown in the following screenshot:

[ 244 ]

Writing ROS Controllers and Visualization Plugins

Chapter 8

Displays panel The panel on the left side of the RViz is called the Displays panel. The Displays panel contains a list of the display plugins of RViz and its properties. The main use of display plugins is to visualize different types of ROS messages, mainly sensor data in the RViz 3D viewport. There are lots of display plugins already present in RViz for viewing images from the camera, and for viewing the 3D point cloud, LaserScan, robot model, TF, and so on. Plugins can be added by pressing the Add button on the left panel. We can also write our own display plugin and add it there.

RViz toolbar There are set of tools present in the RViz toolbar for manipulating the 3D viewport. The toolbar is present at the top of RViz. There are tools present for interacting with the robot model, modifying the camera view, giving navigation goals, and giving robot 2D pose estimations. We can add our own custom tools to the toolbar in the form of plugins.

Views The Views panel is placed on the right side of RViz. Using the Views panel, we can save different views of the 3D viewport and switch to each view by loading the saved configuration.

Time panel The Time panel displays the simulator time elapsed and is mainly useful if there is a simulator running along with RViz. We can also reset to the RViz initial setting using this panel.

Dockable panels The preceding toolbar and panels belong to dockable panels. We can create our own dockable panels as an RViz plugin. We are going to create a dockable panel that has an RViz plugin for robot teleoperation.

[ 245 ]

Writing ROS Controllers and Visualization Plugins

Chapter 8

Writing an RViz plugin for teleoperation In this chapter, we design a teleoperation commander in which we can manually enter the teleoperation topic, linear velocity, and angular velocity, as shown in the following screenshot:

The following is a detailed procedure on how to build this plugin.

Methodology of building the RViz plugin Before starting to build this plugin, we should know how to do it. The standard method to build an ROS plugin is applicable for this plugin too. The difference is that the RViz plugin is GUI based. The RViz is written using a GUI framework called Qt, so we need to create a GUI in Qt and, using Qt APIs, we have to get the GUI values and send them to the ROS system. The following steps describe how this teleoperation RViz plugin is going to work: 1. The dockable panel will have a Qt GUI interface, and the user can input the topic, linear velocity, and angular velocity of teleoperation from the GUI. 2. Collect the user input from GUI using Qt signals and slots, and publish the values using the ROS subscribe-and-publish method. (The Qt signals and slots are a trigger-invoke technique available in Qt. When a signal/trigger is generated by a GUI field, it can invoke a slot or function, such as a callback mechanism.) 3. Here, also, we can use the same procedure to build a plugin as we discussed earlier.

[ 246 ]

Writing ROS Controllers and Visualization Plugins

Chapter 8

Now we can see the step-by-step procedure to build this plugin.

Step 1 ` Creating the RViz plugin package Let's create a new package for creating the teleop plugin: $ catkin_create_pkg rviz_telop_commander roscpp rviz std_msgs

The package is mainly dependent on the SWJ[ package. RViz is built using Qt libraries, so we don't need to include additional Qt libraries in the package. In the Ubuntu 16.04 version, we need to use Qt5 libraries.

Step 2 ` Creating the RViz plugin header file Let's create a new header inside the TSD folder called UFMFPQ@QBEI. You will get this source code from the existing package. This header file consists of the class and methods declaration for the plugin. The following is the explanation of this header file: JODMVEFSPTSPTI JODMVEFSPTDPOTPMFI JODMVEFSWJ[QBOFMI

The preceding code is the header file required to build this plugin; we need ROS headers for publishing the UFMFPQ topic and SWJ[QBOFMI for getting the base class of the RViz panel for creating a new panel: DMBTT5FMFPQ1BOFMQVCMJDSWJ[1BOFM \

This is a plugin class and it is inherited from the SWJ[1BOFM base class: 2@0#+&$5 QVCMJD

This class is using the Qt signal and slots, and it's also a subclass of QObject in Qt. In that case, we should use the 2@0#+&$5 macro: 5FMFPQ1BOFM 28JEHFU QBSFOU

[ 247 ]

Writing ROS Controllers and Visualization Plugins

Chapter 8

This is the constructor of the 5FMFPQ1BOFM  class, and we are initializing a 28JEHFU class to 0. We are using the 28JEHFU instance inside the 5FMFPQ1BOFM class for implementing the GUI of the UFMFPQ plugin: WJSUVBMWPJEMPBE DPOTUSWJ[$POGJHDPOGJH WJSUVBMWPJETBWF SWJ[$POGJHDPOGJHDPOTU

The following shows how to override the SWJ[1BOFM functions for saving and loading the RViz config file: QVCMJD2@4-054

After this line, we can define some public Qt slots: WPJETFU5PQJD DPOTU24USJOHUPQJD

When we enter the topic name in the GUI and press &OUFS, this slot will be called and will create the topic publisher in the given name: QSPUFDUFE2@4-054 WPJETFOE7FM  WPJEVQEBUF@-JOFBS@7FMPDJUZ  WPJEVQEBUF@"OHVMBS@7FMPDJUZ  WPJEVQEBUF5PQJD 

These are the protected slots for sending velocity, updating linear velocity and angular velocity, and updating the topic name, when we change the name of the existing topic: 2-JOF&EJU PVUQVU@UPQJD@FEJUPS@ 2-JOF&EJU PVUQVU@UPQJD@FEJUPS@ 2-JOF&EJU PVUQVU@UPQJD@FEJUPS@

We are creating the Qt -JOF&EJU object to create three text fields in the plugin to receive the topic name, linear velocity, and angular velocity: SPT1VCMJTIFSWFMPDJUZ@QVCMJTIFS@ SPT/PEF)BOEMFOI@

These are the publisher object and the /PEFIBOEMF object for publishing topics and handling an ROS node.

[ 248 ]

Writing ROS Controllers and Visualization Plugins

Chapter 8

Step 3 ` Creating the RViz plugin definition In this step, we will create the main C++ file that contains the definition of the plugin. The file is UFMFPQ@QBEDQQ, and you will get it from the TSD package folder. The main responsibilities of this file are as follows: It acts as a container for a Qt GUI element, such as 2-JOF&EJU, to accept text entries It publishes the command velocity using the ROS publisher It saves and restores the RViz config files The following is the explanation for each section of the code: 5FMFPQ1BOFM5FMFPQ1BOFM 28JEHFU QBSFOU SWJ[1BOFM QBSFOU MJOFBS@WFMPDJUZ@  BOHVMBS@WFMPDJUZ@ \

This is the constructor and initialize SWJ[1BOFM with 28JEHFU, setting linear and angular velocity as : 27#PY-BZPVU UPQJD@MBZPVUOFX27#PY-BZPVU UPQJD@MBZPVU BEE8JEHFU OFX2-BCFM 5FMFPQ5PQJD PVUQVU@UPQJD@FEJUPS@OFX2-JOF&EJU UPQJD@MBZPVU BEE8JEHFU PVUQVU@UPQJD@FEJUPS@

This will add a new 2-JOF&EJU widget on the panel for handling the topic name. Similarly, two other 2-JOF&EJU widgets handle linear velocity and angular velocity. 25JNFS PVUQVU@UJNFSOFX25JNFS UIJT

This will create a Qt UJNFS object for updating a function that is publishing the velocity topic: DPOOFDU PVUQVU@UPQJD@FEJUPS@4*(/"- FEJUJOH'JOJTIFE UIJT4-05

VQEBUF5PQJD  DPOOFDU PVUQVU@UPQJD@FEJUPS@4*(/"- FEJUJOH'JOJTIFE UIJT4-05

VQEBUF5PQJD  DPOOFDU PVUQVU@UPQJD@FEJUPS@4*(/"- FEJUJOH'JOJTIFE UIJT4-05

VQEBUF@-JOFBS@7FMPDJUZ  DPOOFDU PVUQVU@UPQJD@FEJUPS@4*(/"- FEJUJOH'JOJTIFE UIJT4-05

VQEBUF@"OHVMBS@7FMPDJUZ 

[ 249 ]

Writing ROS Controllers and Visualization Plugins

Chapter 8

This will connect a Qt signal to the slots. Here, the signal is triggered when FEJUJOH'JOJTIFE  returns USVF, and the 4MPU here is VQEBUF5PQJD . When the editing inside a Qt -JOF&EJU is finished by pressing the Enter key, the signal will trigger, and the corresponding slot will execute. Here, this slot will set the topic name, angular velocity, and linear velocity value from the text field of the plugin: DPOOFDU PVUQVU@UJNFS4*(/"- UJNFPVU UIJT4-05 TFOE7FM  PVUQVU@UJNFS TUBSU 

These lines generate a signal when the Qt UJNFS times out. The timer will time out in each 100 ms and execute a slot called TFOE7FM , which will publish the velocity topic. We can see the definition of each slot after this section. This code is self-explanatory and, finally, we can see the following code to export it as a plugin: JODMVEFQMVHJOMJCDMBTT@MJTU@NBDSPTI 1-6(*/-*#@&91035@$-"44 SWJ[@UFMPQ@DPNNBOEFS5FMFPQ1BOFMSWJ[1BOFM

Step 4 ` Creating the plugin description file The definition of QMVHJO@EFTDSJQUJPOYNM is given as follows: MJCSBSZQBUIMJCMJCSWJ[@UFMPQ@DPNNBOEFS DMBTTOBNFSWJ[@UFMPQ@DPNNBOEFS5FMFPQ UZQFSWJ[@UFMPQ@DPNNBOEFS5FMFPQ1BOFM CBTF@DMBTT@UZQFSWJ[1BOFM EFTDSJQUJPO "QBOFMXJEHFUBMMPXJOHTJNQMFEJGGESJWFTUZMFSPCPUCBTFDPOUSPM EFTDSJQUJPO DMBTT MJCSBSZ

Step 5 ` Adding the export tags in package.xml We have to update the QBDLBHFYNM file to include the plugin description. The following is the update of QBDLBHFYNM: FYQPSU SWJ[QMVHJO\QSFGJY^QMVHJO@EFTDSJQUJPOYNM FYQPSU

[ 250 ]

Writing ROS Controllers and Visualization Plugins

Chapter 8

Step 6 ` Editing CMakeLists.txt We need to add extra lines to the $.BLF-JTUTUYU definition, as given in the following code: GJOE@QBDLBHF 2U$0.10/&/54$PSF8JEHFUT3&26*3&% TFU 25@-*#3"3*&42U8JEHFUT DBULJO@QBDLBHF

-*#3"3*&4\130+&$5@/".&^ $"5,*/@%&1&/%4SPTDQQ SWJ[  JODMVEF@EJSFDUPSJFT JODMVEF \DBULJO@*/$-6%&@%*34^ \#PPTU@*/$-6%&@%*34^  MJOL@EJSFDUPSJFT

\DBULJO@-*#3"3:@%*34^ \#PPTU@-*#3"3:@%*34^  BEE@EFGJOJUJPOT %25@/0@,&:803%4 25@83"1@$11 .0$@'*-&4 TSDUFMFPQ@QBEI 015*0/4%#0045@55@)"4@01&3"503@)11@*/$-6%&% %#0045@-&9*$"-@$"45@*/$-6%&%  TFU 4063$&@'*-&4 TSDUFMFPQ@QBEDQQ \.0$@'*-&4^  BEE@MJCSBSZ \130+&$5@/".&^\4063$&@'*-&4^ UBSHFU@MJOL@MJCSBSJFT \130+&$5@/".&^\25@-*#3"3*&4^ \DBULJO@-*#3"3*&4^

You will get the complete $.BLF-JTUTUYU from the SWJ[@UFMPQ@DPNNBOEFS package released with the code book, or you can clone the entire package from the following Git repositories: $ git clone https://github.com/jocacace/rviz_teleop_commander.git

[ 251 ]

Writing ROS Controllers and Visualization Plugins

Chapter 8

Step 7 ` Building and loading plugins After creating these files, build a package using DBULJO@NBLF. If the build is successful, we can load the plugin in RViz itself. Take the RViz and load the panel by going to Menu Panel | Add New Panel. We will get a panel such as the following:

[ 252 ]

Writing ROS Controllers and Visualization Plugins

Chapter 8

If we load the Teleop plugin from the list, we will get a panel such as the following:

We can put the Teleop Topic name and values inside the Linear Velocity and Angular Velocity, and we can echo the Teleop Topic and get the topic values such as the following:

[ 253 ]

Writing ROS Controllers and Visualization Plugins

Chapter 8

Questions What are the list of packages needed for writing a low-level controller in ROS? What are the different processes happening inside an ROS controller? What are the main packages of the SPT@DPOUSPM stack? What are the different types of RViz plugins?

Summary In this chapter, we discussed creating plugins for the ROS visualization tool (RViz) and writing basic ROS controllers. We have already worked with default controllers in ROS, and, in this chapter, we developed a custom controller for moving joints. After building and testing the controller, we looked at RViz plugins. We created a new RViz panel for teleoperation. We can manually enter the topic name; we need the twist messages and to enter the linear and angular velocity in the panel. This panel is useful for controlling robots without starting another teleoperation node. In the next chapter, we will discuss interfacing I/O boards and running ROS in embedded systems.

[ 254 ]

9

Interfacing I/O Boards, Sensors, and Actuators to ROS In the previous chapters, we discussed different kinds of plugin frameworks that are used in ROS. In this chapter, we are going to discuss the interfacing of some hardware components, such as sensors and actuators, to ROS. We will look at the interfacing of sensors using I/O boards, such as Arduino, Raspberry Pi, and Odroid-XU4 to ROS, and we will discuss interfacing smart actuators, such as DYNAMIXEL, to ROS. The following is the detailed list of topics that we are going to cover in this chapter: Understanding the Arduino-ROS interface Setting up the Arduino-ROS interface packages Arduino-ROS examples: Chatter and Talker, blink LED and push button, Accelerometer ADXL 335, ultrasonic distance sensors, and Odometry Publisher Interfacing a non-Arduino board to ROS Setting ROS on Odroid-XU4 and Raspberry Pi 2 Working with Raspberry Pi and Odroid GPIOs using ROS Interfacing DYNAMIXEL actuators to ROS

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

Understanding the Arduino-ROS interface Let's see what Arduino is first. Arduino is one of the most popular open source development boards in the market. The ease of programming and the cost effectiveness of the hardware have made Arduino a big success. Most of the Arduino boards are powered by Atmel microcontrollers, which are available from 8-bit to 32-bit, with clock speeds from 8 MHz to 84 MHz. Arduino can be used for the quick prototyping of robots. The main applications of Arduino in robotics are interfacing sensors and actuators, used for communicating with PCs for receiving high-level commands and sending sensor values to PCs using the 6"35 protocol. There are different varieties of Arduino available in the market. Selecting one board for our purpose will be dependent on the nature of our robotic application. Let's see some boards which we can use for beginners, intermediate, and high-end users:

(KIWTG&KbGTGPVXGTUKQPUQHVJG#TFWKPQDQCTF

In the following table, we will look at each Arduino board specification in brief and see where it can be deployed: Boards

Arduino UNO

Arduino Mega 2560

Arduino Due

Processor

ATmega328P

ATmega2560

ATSAM3X8E

Operating/Input Voltage

5V / 7-12 V

5V/ 7-12V

3.3V / 7 - 12 V

[ 256 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

CPU Speed

16 MHz

16 MHz

84 MHz

Analog In/Out

6/0

16/0

12/2

Digital IO/PWM

14/6

54/15

54/12

EEPROM[KB]

1

4

-

SRAM [KB]

2

8

96

Flash [KB]

32

256

512

USB

Regular

Regular

2 Micro

UART

1

4

4

Application

Basic robotics and sensor interfacing

Intermediate robotic application-level application

High-end robotics application

Let's look at how to interface Arduino to ROS.

What is the Arduino-ROS interface? Most of the communication between PCs and I/O boards in robots will be through the UART protocol. When both the devices communicate with each other, there should be some program in both the sides that can translate the serial commands from each of these devices. We can implement our own logic to receive and transmit the data from board to PC and vice versa. The interfacing code can be different in each I/O board because there are no standard libraries to do this communication. The Arduino-ROS interface is a standard way of communication between the Arduino boards and the PC. Currently, this interface is exclusive for Arduino. We may need to write custom nodes to interface with other I/O boards. We can use the similar C++ APIs of ROS used in the PC in the Arduino IDE also for programming the Arduino board. Detailed information about the interfacing package follows.

[ 257 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

Understanding the rosserial package in ROS The SPTTFSJBM package is a set of standardized communication protocols implemented for communicating from ROS to character devices, such as serial ports, and sockets, and vice versa. The SPTTFSJBM protocol can convert the standard ROS messages and services data types to embedded device equivalent data types. It also implements multi-topic support by multiplexing the serial data from a character device. The serial data is sent as data packets by adding header and tail bytes on the packet. The packet representation is shown next:

(KIWTGTQUUGTKCNRCEMGVTGRTGUGPVCVKQP

The function of each byte follows: Sync Flag: This is the first byte of the packet, which is always YGG Sync Flag/Protocol version: This byte was YGG on ROS Groovy and after that it is set to YGF Message Length: This is the length of the packet Checksum Over Message Length: This is the checksum of length for finding packet corruption Topic ID: This is the ID allocated for each topic; the range  is allocated for the system-related functionalities

[ 258 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

Serialized Message data: This is the data associated with each topic Checksum of Topic ID and Message data: This is the checksum for the topic and its serial data for finding the packet, DPSSVQUJPO The checksum of length is computed using the following equation: Checksum = 255 - ( (Topic ID Low Byte + Topic ID High Byte + ... data byte values) % 256) The ROS client libraries, such as SPTDQQ, SPTQZ, and SPTMJTQ, enable us to develop ROS nodes that can run from various devices. One of the ports of the ROS clients that enables us to run a ROS node from the embedded devices, such as Arduino and embedded Linux based boards, is called the SPTTFSJBM@DMJFOU library. Using the SPTTFSJBM@DMJFOU libraries, we can develop the ROS nodes from Arduino, embedded Linux platforms, and Windows. The following is the list of SPTTFSJBM@DMJFOU libraries for each of these platforms: SPTTFSJBM@BSEVJOP: This SPTTFSJBM@DMJFOU works on Arduino platforms,

such as Arduino UNO, Leonardo, Mega, and Due series for advance robotic projects SPTTFSJBM@FNCFEEFEMJOVY: This client supports embedded Linux platforms, such as VEXPro, Chumby alarm clock, WRT54GL router, and so on SPTTFSJBM@XJOEPXT: This is a client for the Windows platform In the PC side, we need some other packages to decode the serial message and convert to exact topics from the SPTTFSJBM@DMJFOU libraries. The following packages help in decoding the serial data: SPTTFSJBM@QZUIPO: This is the recommended PC-side node for handling serial

data from a device. The receiving node is completely written in Python. SPTTFSJBM@TFSWFS: This is a C++ implementation of SPTTFSJBM in the PC side. The inbuilt functionalities are less compared to SPTTFSJBM@QZUIPO, but it can be used for high-performance applications. We are mainly focusing on running the ROS nodes from Arduino. First, we will see how to set up the SPTTFSJBM packages, and then discuss how to set up the SPTTFSJBM@BSEVJOP client in the Arduino IDE.

[ 259 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

Installing rosserial packages on Ubuntu 16.04 To operate with Arduino on Ubuntu 16.04, we must install SPTTFSJBM ROS packages and then set up the Arduino client, installing the libraries needed to communicate with ROS. We can install the SPTTFSJBM packages on Ubuntu using the following commands: 1. Install the SPTTFSJBM package binaries, using BQUHFU: $ sudo apt-get install ros-kinetic-rosserial-arduino ros-kineticrosserial-embeddedlinux ros-kinetic-rosserial-windows ros-kineticrosserial-server ros-kinetic-rosserial-python

2. For installing the SPTTFSJBM@DMJFOU library called SPT@MJC in Arduino, we must download the latest Arduino IDE for Linux 32/64 bit. The following is the link for downloading the Arduino IDE: IUUQTXXXBSEVJOPDDFONBJO TPGUXBSF. Here we download the Linux 64-bit version and copy the Arduino IDE folder to the Ubuntu desktop. Arduino requires Java runtime support to run it. If it is not installed, we can install it using the following command: $ sudo apt-get install java-common

3. After installing Java runtime, we can switch the BSEVJOP folder using the following command: $ cd ~/Desktop/arduino-1.8.5

4. Start Arduino, using the following command: $./arduino

[ 260 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Shown next is the Arduino IDE window:

(KIWTG#TFWKPQ+&'

[ 261 ]

Chapter 9

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

5. Go to File | Preference for configuring the sketchbook folder of Arduino. The Arduino IDE stores the sketches to this location. We created a folder called "SEVJOP in the user IPNF folder and set this folder as the Sketchbook location:

(KIWTG2TGHGTGPEGUQH#TFWKPQ+&'

[ 262 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

We can see a folder called MJCSBSJFT inside the "SEVJOP folder. Switch to this folder, using the following command: $ cd ~/Arduino1/libraries/

If there is no MJCSBSJFT folder, we can create a new one. After switching into this folder, we can generate SPT@MJC , using a script called NBLF@MJCSBSJFTQZ, which is present inside the SPTTFSJBM@BSEVJOP package. SPT@MJC is the SPTTFSJBM@DMJFOU for Arduino, which provides the ROS client APIs inside an Arduino IDE environment: $ rosrun rosserial_arduino make_libraries.py

SPTTFSJBM@BSEVJOP is the ROS client for BSEVJOP , which can communicate using UART, and can publish topics, services, TF, and so on, like a ROS node. The NBLF@MJCSBSJFTQZ

script will generate a wrapper of the ROS messages and services which are optimized for Arduino data types. These ROS messages and services will convert into Arduino C/C++ code equivalent, as shown next: Conversion of ROS messages: ros_package_name/msg/Test.msg

--> ros_package_name::Test

Conversion of ROS services: ros_package_name/srv/Foo.srv

--> ros_package_name::Foo

For example, if we include JODMVEFTUE@NTHT6*OUI , we can instantiate the TUE@NTHT6*OU number.

[ 263 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

If the NBLF@MJCSBSJFTQZ script works fine, a folder called SPT@MJC will generate inside the MJCSBSJFT folder. Restart the Arduino IDE and we will see SPT@MJC examples as follows:

(KIWTG.KUVQH#TFWKPQTQUANKDGZCORNGU

We can take any example and make sure that it is building properly to ensure that the SPT@MJC APIs are working fine. The necessary APIs required for building ROS Arduino nodes are discussed next.

[ 264 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

Understanding ROS node APIs in Arduino The following is a basic structure of the ROS Arduino node. We can see the function of each line of code: JODMVEFSPTI SPT/PEF)BOEMFOI WPJETFUVQ \ OIJOJU/PEF  ^ WPJEMPPQ \ OITQJO0ODF  ^

The creation of /PEF)BOEMF in Arduino is done using the following line of code: SPT/PEF)BOEMFOI

Note that /PEFIBOEMF should be declared before the TFUVQ  function, which will give a global scope to the /PEF)BOEMF instance called OI. The initialization of this node is done inside the TFUVQ  function: OIJOJU/PEF 

The Arduino TFUVQ  function will execute only once when the device starts. Note that we can only create one node from a serial device. Inside the MPPQ  function, we have to use the following line of code to execute the ROS callback once: OITQJO0ODF 

We can create the 4VCTDSJCFS and 1VCMJTIFS objects in Arduino, like the other ROS client libraries. The following are the procedures for defining the subscriber and the publisher. Here is how we define a subscriber object in Arduino: SPT4VCTDSJCFSTUE@NTHT4USJOH TVC UBMLFSDBMMCBDL

[ 265 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

Here we define a subscriber which is subscribing a 4USJOH message, where the callback is the callback function executing when a 4USJOH message arrives on the talker topic. Given next is an example callback for handling the string data: TUE@NTHT4USJOHTUS@NTH SPT1VCMJTIFSDIBUUFS DIBUUFSBNQTUS@NTH WPJEDBMMCBDL DPOTUTUE@NTHT4USJOHBNQNTH\ TUS@NTHEBUBNTHEBUB DIBUUFSQVCMJTI BNQTUS@NTH ^

Note that the DBMMCBDL , 4VCTDSJCFS, and 1VCMJTIFS definitions will be above the TFUVQ  function for getting the global scope. Here we are receiving 4USJOH data, using DPOTUTUE@NTHT4USJOHBNQNTH. The following code shows how to define a publisher object in Arduino: SPT1VCMJTIFSDIBUUFS DIBUUFSBNQTUS@NTH

This next code shows how we publish the string message: DIBUUFSQVCMJTI BNQTUS@NTH

After defining the publisher and the subscriber, we have to initiate this inside the TFUVQ  function, using the following lines of code: OIBEWFSUJTF DIBUUFS OITVCTDSJCF TVC

There are ROS APIs for logging from Arduino. The following are the different logging APIs supported: OIMPHEFCVH %FCVH4UBUFNFOU OIMPHJOGP 1SPHSBNJOGP OIMPHXBSO 8BSOJOHT OIMPHFSSPS &SSPST OIMPHGBUBM 'BUBMJUJFT

[ 266 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

We can retrieve the current ROS time in Arduino using ROS built-in functions, such as time and duration. Current ROS time: ros::Time begin = nh.now();

Converting ROS time in seconds: double secs = nh.now().toSec();

Creating a duration in seconds: ros::Duration ten_seconds(10, 0);

ROS - Arduino Publisher and Subscriber example The first example using the Arduino and ROS interface is a chatter and talker interface. Users can send a 4USJOH message to the UBMLFS topic and Arduino will publish the same message in a DIBUUFS topic. The following ROS node is implemented for Arduino, and we will discuss this example in detail: JODMVEFSPTI JODMVEFTUE@NTHT4USJOHI $SFBUJOH/PEFIBOEMF SPT/PEF)BOEMFOI %FDMBSJOH4USJOHWBSJBCMF TUE@NTHT4USJOHTUS@NTH %FGJOJOH1VCMJTIFS SPT1VCMJTIFSDIBUUFS DIBUUFSBNQTUS@NTH %FGJOJOHDBMMCBDL WPJEDBMMCBDL DPOTUTUE@NTHT4USJOHBNQNTH\ TUS@NTHEBUBNTHEBUB DIBUUFSQVCMJTI BNQTUS@NTH ^ %FGJOJOH4VCTDSJCFS SPT4VCTDSJCFSTUE@NTHT4USJOH TVC UBMLFSDBMMCBDL

WPJETFUVQ 

[ 267 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

\ *OJUJBMJ[JOHOPEF OIJOJU/PEF  4UBSUBEWFSUJTJOHBOETVCTDSJCJOH OIBEWFSUJTF DIBUUFS OITVCTDSJCF TVC ^ WPJEMPPQ  \ OITQJO0ODF  EFMBZ  ^

We can compile the preceding code and upload to the Arduino board. After uploading the code, select the desired Arduino board that we are using for this example and the device serial port of the Arduino IDE. Go to Tools | Boards to select the board and Tools | Port to select the device port name of the board. We are using Arduino Mega for these examples. After compiling and uploading the code, we can start the ROS bridge nodes in the PC that connects Arduino and the PC, using the following command. Ensure that Arduino is already connected to the PC before executing this command: $ rosrun rosserial_python serial_node.py /dev/ttyACM0

In this case, we are running the TFSJBM@OPEFQZ on the port EFWUUZ"$.. We can search for the port name listing the contents of the EFW directory. Note that, to use this port, root permissions are needed. In this case, we could change the permissions using the following command in order to read and write data on the desired port: $ sudo chmod 666 /dev/ttyACM0

We are using the SPTTFSJBM@QZUIPO node here as the ROS bridging node. We have to mention the device name and baud-rate as arguments. The default baud-rate of this communication is . We can change the baud-rate according to our application and the usage of TFSJBM@OPEFQZ inside the SPTTFSJBM@QZUIPO package is given at IUUQXJLJSPTPSHSPTTFSJBM@QZUIPO. If the communication between the ROS node and the Arduino node is correct, we will get the following message:

[ 268 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

(KIWTG4WPPKPIVJGSPTTFSJBM@QZUIPOPQFG

When TFSJBM@OPEFQZ starts running from the PC, it will send some serial data packets called query packets to get the number of topics, the topic names, and the types of topics which are received from the Arduino node. We have already seen the structure of serial packets that are being used for Arduino ROS communication. Given next is the structure of a query packet which is sent from TFSJBM@OPEFQZ to Arduino:

(KIWTG5VTWEVWTGQHVJGSWGT[RCEMGV

[ 269 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

The query topic contains fields such as Sync Flag, ROS version, length of the message, MD5 sum, Topic ID, and so on. When the query packet is received on the Arduino, it will reply with a topic info message that contains the topic name, type, length, topic data, and so on. The following is a typical response packet from Arduino:

(KIWTG5VTWEVWTGQHTGURQPUGRCEMGV

If there is no response for the query packet, it will send it again. The synchronization in communication is based on ROS time. From Figure 6, we can see that when we run TFSJBM@OPEFQZ, the buffer size allocated for publish and subscribe is 512 bytes. The buffer allocation is dependent on the amount of RAM available on each microcontroller that we are working with. The following is a table showing the buffer allocation of each Arduino controller. We can override these settings by changing the #6''&3@4*;& macro inside SPTI. AVR model

Buffer size Publishers/Subscribers

ATMEGA 168

150 bytes

6/6

ATMEGA 328P 280 bytes

25/25

All others

25/25

512 bytes

[ 270 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

There are also some limitations in the float64 data type of ROS in Arduino. It will truncate to 32-bit. Also, when we use string data types, use the unsigned char pointer for saving memory. After running TFSJBM@OPEFQZ, we will get the list of topics, using the following command: $ rostopic list

We can see that topics such as DIBUUFS and UBMLFS are being generated. We can simply publish a message to the UBMLFS topic, using the following command: $ rostopic pub -r 5 talker std_msgs/String "Hello World"

It will publish the )FMMP8PSME message with a rate of . We can echo the DIBUUFS topic, and we will get the same message as we published: $ rostopic echo /chatter

Arduino-ROS, example - blink LED and push button In this example, we can interface the LED and push button to Arduino and control using ROS. When the push button is pressed, the Arduino node sends a 5SVF value to a topic called pushed, and at the same time, it switches on the LED which is on the Arduino board.

[ 271 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

The following shows the circuit for doing this example:

(KIWTG+PVGTHCEKPIVJGRWUJDWVVQPVQ#TFWKPQ   #VUUPO&YBNQMFGPS3PTTFSJBM   JODMVEFSPTI JODMVEFTUE@NTHT#PPMI /PEFIBOEMF SPT/PEF)BOEMFOI #PPMFBONFTTBHFGPS1VTICVUUPO TUE@NTHT#PPMQVTIFE@NTH %FGJOJOH1VCMJTIFSJOBUPQJDDBMMFEQVTIFE SPT1VCMJTIFSQVC@CVUUPO QVTIFEBNQQVTIFE@NTH

[ 272 ]

Chapter 9

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

-&%BOE1VTICVUUPOQJOEFGJOJUJPOT DPOTUJOUCVUUPO@QJO DPOTUJOUMFE@QJO 7BSJBCMFTUPIBOEMFEFCPVODJOH IUUQTXXXBSEVJOPDDFO5VUPSJBM%FCPVODF CPPMMBTU@SFBEJOH MPOHMBTU@EFCPVODF@UJNF MPOHEFCPVODF@EFMBZ CPPMQVCMJTIFEUSVF WPJETFUVQ  \ OIJOJU/PEF  OIBEWFSUJTF QVC@CVUUPO JOJUJBMJ[FBO-&%PVUQVUQJO BOEBJOQVUQJOGPSPVSQVTICVUUPO QJO.PEF MFE@QJO065165 QJO.PEF CVUUPO@QJO*/165 &OBCMFUIFQVMMVQSFTJTUPSPOUIFCVUUPO EJHJUBM8SJUF CVUUPO@QJO)*() 5IFCVUUPOJTBOPSNBMMZCVUUPO MBTU@SFBEJOHEJHJUBM3FBE CVUUPO@QJO ^ WPJEMPPQ  \ CPPMSFBEJOHEJHJUBM3FBE CVUUPO@QJO JG MBTU@SFBEJOHSFBEJOH\ MBTU@EFCPVODF@UJNFNJMMJT  QVCMJTIFEGBMTF ^ JGUIFCVUUPOWBMVFIBTOPUDIBOHFEGPSUIFEFCPVODFEFMBZXFLOPXJUT TUBCMF JG QVCMJTIFEBNQBNQ NJMMJT MBTU@EFCPVODF@UJNF EFCPVODF@EFMBZ\ EJHJUBM8SJUF MFE@QJOSFBEJOH QVTIFE@NTHEBUBSFBEJOH QVC@CVUUPOQVCMJTI BNQQVTIFE@NTH QVCMJTIFEUSVF ^ MBTU@SFBEJOHSFBEJOH OITQJO0ODF  ^

[ 273 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

The preceding code handles the key debouncing and changes the button state only after the button release. The preceding code can upload to Arduino and can interface to ROS, using the following commands: Start SPTDPSF: $ roscore

Start TFSJBM@OPEFQZ: $ rosrun roserial_python serial_node.py /dev/ttyACM0

We can see the button press event by echoing the topic pushed: $ rostopic echo pushed

We will get following values when a button is pressed:

(KIWTG1WVRWVQH#TFWKPQRWUJKPIDWVVQP

Arduino-ROS, example - Accelerometer ADXL 335 In this example, we are interfacing Accelerometer ADXL 335 to Arduino Mega through ADC pins and plotting the values using the ROS tool called SRU@QMPU. The following image shows the circuit of the connection between ADLX 335 and Arduino:

[ 274 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

(KIWTG+PVGTHCEKPI#TFWKPQ#&:.

ADLX 335 is an analog accelerometer. We can simply connect to the ADC port and read the digital value. The following is the embedded code to interface ADLX 335 via Arduino ADC: JG "3%6*/0  JODMVEF"SEVJOPI FMTF JODMVEF81SPHSBNI FOEJG JODMVEFSPTI JODMVEFSPTTFSJBM@BSEVJOP"EDI DPOTUJOUYQJO"YBYJTPGUIFBDDFMFSPNFUFS DPOTUJOUZQJO"ZBYJT DPOTUJOU[QJO"[BYJT POMZPOBYJTNPEFMT

[ 275 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

SPT/PEF)BOEMFOI $SFBUJOHBOBEDNFTTBHF SPTTFSJBM@BSEVJOP"EDBED@NTH SPT1VCMJTIFSQVC BEDBNQBED@NTH WPJETFUVQ  \ OIJOJU/PEF  OIBEWFSUJTF QVC ^ 8FBWFSBHFUIFBOBMPHSFBEJOHUPFMJNJOBUFTPNFPGUIFOPJTF JOUBWFSBHF"OBMPH JOUQJO\ JOUW GPS JOUJJJ

W BOBMPH3FBE QJO SFUVSOW ^

WPJEMPPQ  \ *OTFSUJOH"%$WBMVFTUP"%$NFTTBHF BED@NTHBEDBWFSBHF"OBMPH YQJO BED@NTHBEDBWFSBHF"OBMPH ZQJO BED@NTHBEDBWFSBHF"OBMPH [QJO QVCQVCMJTI BNQBED@NTH OITQJO0ODF  EFMBZ  ^

The preceding code will publish the ADC values of X, Y, and Z axes in a topic called BED. The code uses the SPTTFSJBM@BSEVJOP"ED message to handle the ADC value. We can plot the values using the SRU@QMPU tool.

[ 276 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

The following is the command to plot the three axes values in a single plot: $ rqt_plot adc/adc0 adc/adc1 adc/adc2

Next is a screenshot of the plot of the three channels of ADC:

(KIWTG2NQVVKPI#&:.XCNWGUWUKPITSVARNQV

[ 277 ]

Chapter 9

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

Arduino-ROS, example - ultrasonic distance sensor One of the useful sensors in robots are the range sensors. One of the cheapest range sensors is the ultrasonic distance sensor. The ultrasonic sensor has two pins for handling input and output, called &DIP and 5SJHHFS. We are using the HC-SR04 ultrasonic distance sensor, which is shown in the following image:

(KIWTG2NQVVKPI#&:.XCNWGUWUKPITSVARNQV

[ 278 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

The ultrasonic sound sensor contains two sections: one is the transmitter and the other is the receiver. The ultrasonic distance sensor works like this: when a trigger pulse of a short duration is applied to the trigger pin of the ultrasonic sensors, the ultrasonic transmitter sends the sound signals to the robot environment. The sound signal sent from the transmitter hits on some obstacles and is reflected to the sensor. The reflected sound waves are collected by the ultrasonic receiver, generating an output signal which has a relation to the time required to receive the reflected sound signals.

Equations to find distance using the ultrasonic range sensor The following are the equations used to compute the distance from an ultrasonic range sensor to an obstacle: Distance = Speed * Time/2 Speed of sound at sea level = 343 m/s or 34,300 cm/s Thus, Distance = 17,150 * Time (unit cm) We can compute the distance to the obstacle using the pulse duration of the output. The following is the code to work with the ultrasonic sound sensor and send a value through the ultrasound topic using the range message definition in ROS: JODMVEFSPTI JODMVEFSPTUJNFI JODMVEFTFOTPS@NTHT3BOHFI SPT/PEF)BOEMFOI EFGJOFFDIP1JO&DIP1JO EFGJOFUSJH1JO5SJHHFS1JO JOUNBYJNVN3BOHF.BYJNVNSBOHFOFFEFE JOUNJOJNVN3BOHF.JOJNVNSBOHFOFFEFE MPOHEVSBUJPOEJTUBODF%VSBUJPOVTFEUPDBMDVMBUFEJTUBODF TFOTPS@NTHT3BOHFSBOHF@NTH SPT1VCMJTIFSQVC@SBOHF VMUSBTPVOEBNQSBOHF@NTH

DIBSGSBNFJEVMUSBTPVOE

[ 279 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS WPJETFUVQ \ OIJOJU/PEF  OIBEWFSUJTF QVC@SBOHF SBOHF@NTHSBEJBUJPO@UZQFTFOTPS@NTHT3BOHF6-53"406/% SBOHF@NTHIFBEFSGSBNF@JEGSBNFJE SBOHF@NTHGJFME@PG@WJFXGBLF SBOHF@NTHNJO@SBOHF SBOHF@NTHNBY@SBOHF QJO.PEF USJH1JO065165 QJO.PEF FDIP1JO*/165 ^ GMPBUHFU3BOHF@6MUSBTPVOE \ JOUWBM GPS JOUJJJ

\ EJHJUBM8SJUF USJH1JO-08 EFMBZ.JDSPTFDPOET  EJHJUBM8SJUF USJH1JO)*() EFMBZ.JDSPTFDPOET  EJHJUBM8SJUF USJH1JO-08 EVSBUJPOQVMTF*O FDIP1JO)*() $BMDVMBUFUIFEJTUBODF JODNCBTFEPOUIFTQFFEPGTPVOE WBM EVSBUJPO ^ SFUVSOWBM ^ MPOHSBOHF@UJNF WPJEMPPQ \  5IFGPMMPXJOHUSJH1JOFDIP1JODZDMFJTVTFEUPEFUFSNJOFUIF EJTUBODFPGUIFOFBSFTUPCKFDUCZCPVODJOHTPVOEXBWFTPGGPGJU  JG NJMMJT  SBOHF@UJNF\ JOUS SBOHF@NTHSBOHFHFU3BOHF@6MUSBTPVOE  SBOHF@NTHIFBEFSTUBNQOIOPX  QVC@SBOHFQVCMJTI BNQSBOHF@NTH SBOHF@UJNFNJMMJT   ^ OITQJO0ODF  EFMBZ  ^

[ 280 ]

Chapter 9

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

We can plot the distance value, using the following commands: Start SPTDPSF: $ roscore

Start TFSJBM@OPEFQZ: $ rosrun rosserial_python serial_node.py /dev/ttyACM0

Plot values using SRU@QMPU: $ rqt_plot /ultrasound

As seen in the screenshot below, the center line indicates the current distance (SBOHF) from the sensor. The upper line is the NBY@SBOHF and line below is the NJO@SBOHF.

(KIWTG2NQVVKPIWNVTCUQPKEUQWPFUGPUQTFKUVCPEGXCNWG

[ 281 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

Arduino-ROS example - Odometry Publisher In this example, we will see how to send an PEPN message from an Arduino node to a PC. This example can be used in a robot for computing PEPN and sending to the ROS Navigation stack as the input. The motor encoders can be used for computing PEPN and can transmit to a PC. In this example, we will see how to send PEPN for a robot which is moving in a circle, without taking the motor encoder values:   SPTTFSJBM1MBOBS0EPNFUSZ&YBNQMF   JODMVEFSPTI JODMVEFSPTUJNFI JODMVEFUGUGI JODMVEFUGUSBOTGPSN@CSPBEDBTUFSI SPT/PEF)BOEMFOI 5SBOTGPSNCSPBEDBTUFSPCKFDU HFPNFUSZ@NTHT5SBOTGPSN4UBNQFEU UG5SBOTGPSN#SPBEDBTUFSCSPBEDBTUFS EPVCMFY EPVCMFZ EPVCMFUIFUB DIBSCBTF@MJOLCBTF@MJOL DIBSPEPNPEPN WPJETFUVQ  \ OIJOJU/PEF  CSPBEDBTUFSJOJU OI ^ WPJEMPPQ  \ ESJWFJOBDJSDMF EPVCMFEY EPVCMFEUIFUB Y DPT UIFUB EY  Z TJO UIFUB EY  UIFUB EUIFUB  JG UIFUB 

[ 282 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

UIFUB UGPEPN CBTF@MJOL UIFBEFSGSBNF@JEPEPN UDIJME@GSBNF@JECBTF@MJOL UUSBOTGPSNUSBOTMBUJPOYY UUSBOTGPSNUSBOTMBUJPOZZ UUSBOTGPSNSPUBUJPOUGDSFBUF2VBUFSOJPO'SPN:BX UIFUB UIFBEFSTUBNQOIOPX  CSPBEDBTUFSTFOE5SBOTGPSN U OITQJO0ODF  EFMBZ  ^

After uploading the code, run SPTDPSF and SPTTFSJBM@OPEFQZ. We can view UG and PEPN in RViz. Open RViz and view UG , as shown next. We will see the PEPN pointer moving in a circle on RViz, as follows:

(KIWTG8KUWCNK\KPIQFQOFCVCHTQO#TFWKPQ

[ 283 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

Interfacing non-Arduino boards to ROS Arduino boards are commonly used boards in robots, but what happens if we want a board that is more powerful than Arduino? In such a case, we may want to write our own driver for the board, which can convert the serial messages into topics. We will look at the interfacing of a non-Arduino board called Tiva C Launchpad to ROS, using a Python driver node, in Chapter 11: Building and Interfacing Differential Drive Mobile Robot Hardware in ROS. This chapter is about interfacing a real mobile robot to ROS, and the robot using the Tiva C Launchpad board for its operation.

Setting ROS on Odroid-XU4 and Raspberry Pi 2 Odroid-XU4 and Raspberry Pi2 are single board computers which have a low form factor the size of a credit card. These single board computers can be installed in robots and we can install ROS on them. The main specifications comparison of Odroid-XU4 and Raspberry Pi2 is shown next: Device

Odroid-XU4

Raspberry Pi 2

CPU

2.0 GHz Quad core ARM Cortex-A15 CPU from Samsung

900 MHz quad core ARM Cortex A7 CPU from Broadcom

GPU

Mali-T628 MP6 GPU

VideoCore IV

Memory

2 GB

1 GB

Storage

SD card slot or eMMC module

SD card slot

Connectivity

2 x USB 3.0, 1 x USB 2.0, micro HDMI, 4 x USB, HDMI, Ethernet, 3.5 mm Gigabit Ethernet audio jack

OS

Android, Ubuntu/Linux

Connectors

GPIO, SPI, I2C, RTC (Real Time Clock) Camera interface (CSI), GPIO, SPI, backup battery connector I2C, JTAG

Price

$59

Raspbian, Ubuntu/Linux, Windows 10

$35

[ 284 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

The following is an image of the Odroid-XU4 board:

(KIWTG1FTQKF:7DQCTF

The Odroid board is manufactured by a company called Hard Kernel. The official web page of the Odroid-XU4 board is at IUUQXXXIBSELFSOFMDPNNBJOQSPEVDUTQSEU@JOGP QIQ
H@DPEF(. Odroid-XU4 is the most powerful board of the Odroid family. There are cheaper and lower performance boards as well, such as Odroid-C1+ and C2. All these boards support ROS. One of the popular single board computers is Raspberry Pi. The Raspberry Pi boards are manufactured by the Raspberry Pi Foundation, which is based in the UK (visit IUUQTXXXSBTQCFSSZQJPSH).

[ 285 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

The following is an image of the Raspberry Pi 2 board:

(KIWTG6JG4CURDGTT[2KDQCTF

We can install Ubuntu and Android on Odroid. There are also unofficial distributions of Linux, such as Debian mini, Kali Linux, Arch Linux, and Fedora, and support libraries, such as ROS, OpenCV, PCL, and so on. For getting ROS on Odroid, we can either install a fresh Ubuntu and install ROS manually like a standard desktop PC, or directly download the unofficial Ubuntu distribution for Odroid with ROS already installed. The image for Ubuntu 16.04 for Odroid boards can be downloaded from IUUQEFFVPESPJEJOVCVOUV@MUT . You can download the desired kernel version for the Odroid-XU4 board (for example, ubuntu-16.04-mate-odroidxu4-20170731.img.xz). This file contains pre-installed images of Ubuntu.

[ 286 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

The list of the other operating systems supported on Odroid-XU4 is given on the wiki page at IUUQPESPJEDPNEPLVXJLJEPLVQIQ
JEFOPESPJEYV, while the Raspberry Pi 2 official OS images are given at IUUQTXXXSBTQCFSSZQJPSHEPXOMPBET. The official OSes supported by the Raspberry Pi Foundation are Raspbian and Ubuntu. There are unofficial images based on these OSes which have ROS pre-installed on them. In this book, we are using the Raspbian Jessie images (IUUQTXXXSBTQCFSSZQJPSHEPXOMPBET) with ROS installed, following the ROS wiki page for the installation: IUUQXJLJSPTPSH304CFSSZ1J *OTUBMMJOH304,JOFUJDPOUIF3BTQCFSSZ1J.

How to install an OS image to Odroid-XU4 and Raspberry Pi 2 We can download the Ubuntu image for Odroid and Raspbian Jessie image for Raspberry Pi 2 and can install to a micro SD card, preferably 16 GB. Format the micro SD card in the FAT32 filesystem, or we can use the SD card adapter or the USB-memory card reader for connecting to a PC. We can either install the OS in Windows or in Linux. The procedure for installing the OS on these boards follows.

*OTUBMMBUJPOJO8JOEPXT In Windows, there is a tool called Win32diskimage which is designed specifically for Odroid. You can download the tool from IUUQEOPESPJEDPN%JTL*NBHFS@0%30*%8JO%JTL*NBHFSPESPJEW[JQ.

[ 287 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

Run Win32 Disk Imager with the Administrator privilege. Select the downloaded image, select the memory card drive, and write the image to the drive.

(KIWTG9KP&KUM+OCIGTHQT1FTQKF

After completing this wizard, we can put the micro SD card in Odroid and boot up the OS with ROS support. The same tool can be used for Raspbian installation in Raspberry Pi 2. We can use the actual version of Win32 Disk Imager for writing Raspbian to a micro SD card from the following link: IUUQTPVSDFGPSHFOFUQSPKFDUTXJOEJTLJNBHFS.

[ 288 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

*OTUBMMBUJPOJO-JOVY In Linux, there is a tool called disk dump (dd). This tool helps to copy the content of the image to the SD card. dd is a command line tool which is available in all the Ubuntu/Linuxbased OSes. Insert the micro SD card, format to the FAT32 filesystem, and use the command mentioned later to write an image to the micro SD card. In the dd tool, there is no progress bar to indicate the copy progress. To get the progress bar, we can install a pipe viewer tool called pv: $ sudo apt-get install pv

After installing pv, we can use the following command to install the image file to the micro SD card. Note that you should have the OS image in the same path of the Terminal, and note the micro SD card device name; for example, NNDCML, TEC, TEE, and so on. You will get the device name using the ENFTH command: $ dd bs=4M if=image_name.img | pv | sudo dd of=/dev/mmcblk0

JNBHF@OBNFJNH is the image name and the device name is EFWNNDCML. CT. indicates the block size. If the block size is ., dd will read 4 megabytes from the image and

write 4 megabytes to the device. After completing the operation, we can send it to Odroid and Raspberry Pi and boot the OS.

Connecting to Odroid-XU4 and Raspberry Pi 2 from a PC We can work with Odroid-XU4 and Raspberry Pi 2 by connecting to the HDMI display port and connecting the keyboard and mouse to the USB like a normal PC. This is the simplest way of working with Odroid and Raspberry Pi. In most of the projects, the boards will be placed on the robot, so we can't connect the display and the keyboards to it. There are several methods for connecting these boards to the PC. It will be good if we can connect the internet to these boards too. The following methods can connect the internet to these boards, and, at the same time, we can remotely connect via the SSH protocol: Remote connection using Wi-Fi router and Wi-Fi dongle through SSH: In this method, we need a Wi-Fi router with internet connectivity and a Wi-Fi dongle in the board for getting the Wi-Fi support. Both the PC and board will connect to the same network, so each will have an IP address and can communicate using that address.

[ 289 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

Direct connection using an Ethernet hotspot: We can share the internet connection and communicate using SSH via %OTNBTR, a free software DNS forwarder and DHCP server using low system resources. Using this tool, we can tether the Wi-Fi internet connection of the laptop to the Ethernet and we can connect the board to the Ethernet port of the PC. This kind of communication can be used for robots which are static in operation. The first method is very easy to configure; it's like connecting two PCs on the same network. The second method is a direct connection of the board to the laptop through the Ethernet. This method can be used when the robot is not moving. In this method, the board and the laptop can communicate via SSH at the same time and can share Internet access too. We are using this method in this chapter for working with ROS.

Configuring an Ethernet hotspot for Odroid-XU4 and Raspberry Pi 2 The procedure for creating an Ethernet hotspot in Ubuntu and sharing Wi-Fi internet through this connection follows. Go to Edit Connections... from the network settings and click on Add to add a new connection, as shown next:

(KIWTG%QPaIWTKPICPGVYQTMEQPPGEVKQPKP7DWPVW

[ 290 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

Create an Ethernet connection and in the IPv4 setting, change the method to Shared to Other computers , and give the connection name as Share, as shown next:

(KIWTG%TGCVKPICPGYEQPPGEVKQPHQTUJCTKPIVJTQWIJVJG'VJGTPGV 1MVHJOUIFNJDSP4%DBSEQPXFSVQUIF0ESPJEPS3BTQCFSSZ1JBOEDPOOFDUUIF&UIFSOFUQPSUGSPNUIFCPBSE UPUIF1$8IFOUIFCPBSECPPUTVQXFXJMMTFFUIBUUIFTIBSFEOFUXPSLJTBVUPNBUJDBMMZDPOOFDUFEUPUIF CPBSE 8FDBODPNNVOJDBUFXJUIUIFCPBSEVTJOHUIFGPMMPXJOHDPNNBOET

In Odroid: $ ssh odroid@ip_address password is odroid

In Raspberry Pi 2: $ ssh pi@ip_adress password is raspberry

After doing SSH into the board, we can launch SPTDPSF and most of the ROS commands on the board like our PC. We will look at two examples using these boards. One is for blinking an LED, and the other is for handling a push button. The library we are using for handling the GPIO pins of Odroid and Raspberry is called Wiring Pi.

[ 291 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

Odroid and Raspberry Pi have the same pin layout and most of the Raspberry Pi GPIO libraries are ported to Odroid, which will make the programming easier. One of the libraries we are using in this chapter for GPIO programming is Wiring Pi. Wiring Pi is based on C++ APIs, which can access the board GPIO using C++ APIs. In the following sections, we will look at the instructions for installing Wiring Pi on Odroid and Raspberry 2.

*OTUBMMJOH8JSJOH1JPO0ESPJE96 The following procedure can be used to install Wiring Pi on Odroid-XU4. This is a customized version of Wiring Pi, which is used in Raspberry Pi 2: $ git clone https://github.com/hardkernel/wiringPi.git $ cd wiringPi $ sudo ./build

Odroid-XU4 has 42 pins placed on two different connectors, CON10 and CON11 respectively, as show in the following image:

(KIWTG6JG%10CPF%10JGCFGTUQP1FTQKF:7

[ 292 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

The Wiring Pi pin out of Odroid-XU4 (CON10) for bot connectors is given next:

(KIWTG2KPQWVQH1FTQKF:7%10

The Wiring Pi pin out of Odroid-XU4 (CON11) for bot connectors is given next:

(KIWTG2KPQWVQH1FTQKF:7%10

[ 293 ]

Chapter 9

Interfacing I/O Boards, Sensors, and Actuators to ROS

*OTUBMMJOH8JSJOH1JPO3BTQCFSSZ1J The following procedure can be used to install Wiring Pi on Raspberry Pi 2: $ git clone git clone git://git.drogon.net/wiringPi $ cd wiringPi $ sudo ./build

The pin out of Raspberry Pi 2 and Wiring Pi is shown next:

(KIWTG2KPQWVQH4CURDGTT[2K

The following are the ROS examples for Raspberry Pi 2.

[ 294 ]

Chapter 9

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

Blinking LED using ROS on Raspberry Pi 2 This is a basic LED example which can blink the LED connected to the first pin of Wiring Pi, that is the 12th pin on the board. The LED cathode is connected to the GND pin and 12th pin as an anode. The following image shows the circuit of Raspberry Pi with an LED:

(KIWTG$NKPMKPICP.'&WUKPI4CURDGTT[2K

We can create the example ROS package, using the following command: $ catkin_create_pkg ros_wiring_example roscpp std_msgs

[ 295 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

You will get the existing package from the SPT@XJSJOH@FYBNQMFT folder. Create a TSD folder and create the following code called CMJOLDQQ inside the TSD folder: JODMVEFSPTSPTI JODMVEFTUE@NTHT#PPMI JODMVEFJPTUSFBN 8JSJOH1JIFBEFS JODMVEFXJSJOH1JI 8JSJOH1*GJSTUQJO EFGJOF-&% $BMMCBDLUPCMJOLUIF-&%BDDPSEJOHUPUIFUPQJDWBMVF WPJECMJOL@DBMMCBDL DPOTUTUE@NTHT#PPM$POTU1USBNQNTH \ JG NTH EBUB\ EJHJUBM8SJUF -&%)*() 304@*/'0 -&%0/ ^ JG NTH EBUB\ EJHJUBM8SJUF -&%-08 304@*/'0 -&%0'' ^ ^ JOUNBJO JOUBSHDDIBS BSHW \ SPTJOJU BSHDBSHWCMJOL@MFE 304@*/'0 4UBSUFE3BTQCFSSZ#MJOL/PEF 4FUUJOH8JSJOH1J XJSJOH1J4FUVQ 4FUUJOH-&%QJOBTPVUQVU QJO.PEF -&%065165 SPT/PEF)BOEMFO SPT4VCTDSJCFSTVCOTVCTDSJCF MFE@CMJOLCMJOL@DBMMCBDL SPTTQJO  ^

This code will subscribe a topic called MFE@CMJOL, which is a #PPMFBO type. If we publish 1 to this topic, it will switch on the LED. If we publish 0, the LED will turn off.

[ 296 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

Push button + blink LED using ROS on Raspberry Pi 2 The next example is handling input from a button. When we press the button, the code will publish to the MFE@CMJOL topic and blink the LED. When the switch is off, the LED will also be 0''. The LED is connected to the 12th pin and GND, and the button is connected to the 11th pin and GND. The following image shows the circuit of this example. The circuit is also the same for Odroid:

(KIWTG.'&DWVVQPKP4CURDGTT[2K

[ 297 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

The code for interfacing the LED and button is given next. The code can be saved with the name CVUUPODQQ inside the TSD folder: JODMVEFSPTSPTI JODMVEFTUE@NTHT#PPMI JODMVEFJPTUSFBN JODMVEFXJSJOH1JI 8JSJOH1* EFGJOF#6550/ EFGJOF-&%

WPJECMJOL@DBMMCBDL DPOTUTUE@NTHT#PPM$POTU1USBNQNTH \ JG NTH EBUB\ EJHJUBM8SJUF -&%)*() 304@*/'0 -&%0/ ^ JG NTH EBUB\ EJHJUBM8SJUF -&%-08 304@*/'0 -&%0'' ^ ^ JOUNBJO JOUBSHDDIBS BSHW \ SPTJOJU BSHDBSHWCVUUPO@MFE 304@*/'0 4UBSUFE3BTQCFSSZ#VUUPO#MJOL/PEF

XJSJOH1J4FUVQ  QJO.PEF -&%065165 QJO.PEF #6550/*/165 QVMM6Q%O$POUSPM #6550/16%@61&OBCMFQVMMVQSFTJTUPSPOCVUUPO

SPT/PEF)BOEMFO SPT3BUFMPPQ@SBUF  SPT4VCTDSJCFSTVCOTVCTDSJCF MFE@CMJOLCMJOL@DBMMCBDL

[ 298 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

SPT1VCMJTIFSDIBUUFS@QVCOBEWFSUJTFTUE@NTHT#PPM MFE@CMJOL 

TUE@NTHT#PPMCVUUPO@QSFTT CVUUPO@QSFTTEBUB TUE@NTHT#PPMCVUUPO@SFMFBTF CVUUPO@SFMFBTFEBUB XIJMF SPTPL  \ JG EJHJUBM3FBE #6550/3FUVSO5SVFJGCVUUPOQSFTTFE \ 304@*/'0 #VUUPO1SFTTFE DIBUUFS@QVCQVCMJTI CVUUPO@QSFTT ^ FMTF \ 304@*/'0 #VUUPO3FMFBTFE DIBUUFS@QVCQVCMJTI CVUUPO@SFMFBTF ^ SPTTQJO0ODF  MPPQ@SBUFTMFFQ  ^ ^

$.BLF-JTUTUYU for building these two examples is given next. The Wiring Pi code needs to link with the Wiring Pi library. We have added this in the $.BLF-JTUTUYU file: DNBLF@NJOJNVN@SFRVJSFE 7&34*0/ QSPKFDU SPT@XJSJOH@FYBNQMFT GJOE@QBDLBHF DBULJO3&26*3&%$0.10/&/54 SPTDQQ TUE@NTHT  GJOE@QBDLBHF #PPTU3&26*3&%$0.10/&/54TZTUFN *ODMVEFEJSFDUPSZPGXJSJOH1J

[ 299 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

TFU XJSJOH1J@JODMVEFVTSMPDBMJODMVEF

JODMVEF@EJSFDUPSJFT

\DBULJO@*/$-6%&@%*34^ \XJSJOH1J@JODMVEF^  -JOLEJSFDUPSZPGXJSJOH1J -*/,@%*3&$503*&4 VTSMPDBMMJC

BEE@FYFDVUBCMF CMJOL@MFETSDCMJOLDQQ BEE@FYFDVUBCMF CVUUPO@MFETSDCVUUPODQQ UBSHFU@MJOL@MJCSBSJFT CMJOL@MFE \DBULJO@-*#3"3*&4^XJSJOH1J  UBSHFU@MJOL@MJCSBSJFT CVUUPO@MFE \DBULJO@-*#3"3*&4^XJSJOH1J 

Build the project using DBULJO@NBLF and we can run each example. For executing the Wiring Pi based code, we need a root permission.

Running examples in Raspberry Pi 2 Now that we have built the project, before running the examples, we should do the following setup in Raspberry Pi. You can do this setup by logging in to Raspberry Pi through SSH. We need to add the following lines to the CBTISD file of the root user. Take the CBTISD file of the root user: $ sudo -i $ nano .bashrc

[ 300 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

Add the following lines to the end of this file: TPVSDFPQUSPTJOEJHPTFUVQTI TPVSDFIPNFQJDBULJO@XTEFWFMTFUVQCBTI FYQPSU304@."45&3@63*IUUQMPDBMIPTU

We can now log in with a different Terminal in our Raspberry Pi 2, and run the following commands to execute the blink_demo program: Start roscore in one Terminal: $ roscore

Run the executable as the root in another Terminal: $ sudo -s # cd /home/odroid/catkin_ws/build/ros_wiring_examples #./blink_led

After starting the CMJOL@MFE node, publish 1 to the MFE@CMJOL topic in another Terminal: For setting the LED to the ON state: $ rostopic pub /led_blink std_msgs/Bool 1

For setting the LED to the OFF state: $ rostopic pub /led_blink std_msgs/Bool 0

Run the button LED node in another Terminal: $ sudo -s # cd /home/odroid/catkin_ws/build/ros_wiring_examples #./button_led

Press the button and we can see the LED blinking. We can also check the button state by echoing the topic MFE@CMJOL: $ rostopic echo /led_blink

[ 301 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

Interfacing DYNAMIXEL actuators to ROS One of the latest smart actuators available on the market is DYNAMIXEL, which is manufactured by a company called Robotis. The DYNAMIXEL servos are available in various versions, some of which are shown in the following image:

(KIWTG&KbGTGPVV[RGUQH&;0#/+:'.UGTXQU

These smart actuators have complete support in ROS, and clear documentation is also available for them. The official ROS wiki page of DYNAMIXEL is at IUUQXJLJSPTPSHEZOBNJYFM@DPOUSPMMFST5VUPSJBMT.

Questions What are the different rosserial packages? What is the main function of SPTTFSJBM@BSEVJOP? How does rosserial protocol work? What are the main differences between Odroid and Raspberry Pi boards?

[ 302 ]

Interfacing I/O Boards, Sensors, and Actuators to ROS

Chapter 9

Summary This chapter was about interfacing I/O boards to ROS and adding sensors on it. We have discussed the interfacing of the popular I/O board called Arduino to ROS, and interface's basic components, such as LEDs, buttons, accelerometers, ultrasonic sound sensors, and so on. After looking at the interfacing of Arduino, we discussed how to set up ROS on Raspberry Pi 2 and Odroid-XU4. We also presented a few basic examples in Odroid and Raspberry Pi based on ROS and Wiring Pi. Finally, we looked at the interfacing of smart actuators called DYNAMIXEL in ROS.

[ 303 ]

10

Programming Vision Sensors Using ROS, Open CV, and PCL In the last chapter, we discussed the interfacing of sensors and actuators using I/O boards in ROS. In this chapter, we are going to discuss how to interface various vision sensors in ROS and program them using libraries such as Open Source Computer Vision (OpenCV) and Point Cloud Library (PCL). The vision of a robot is an important aspect of the robot for manipulating objects and navigation. There are lots of 2D/3D vision sensors available on the market, and most of the sensors have an interface driver package in ROS. We will discuss the interfacing of new vision sensors to ROS and programming them using OpenCV and PCL. Finally, we will discuss the use of fiducial marker libraries to develop vision-based robotic applications. We will cover the following topics in this chapter: Integrating ROS, PCL, and OpenCV Using a USB webcam in ROS Learning how to calibrate a camera Using RGB-D sensors in ROS Using laser scanners in ROS Working with augmented-reality markers in ROS

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

Understanding ROS ` OpenCV interfacing packages OpenCV is one of the popular open source real-time computer vision libraries, which is mainly written in C/C++. OpenCV comes with a BSD license, and is free both for academic and commercial applications. OpenCV can be programmed using C/C++, Python, and Java, and it has multi-platform support, such as Windows, Linux, macOS X, Android, and iOS. OpenCV has tons of computer vision APIs that can be used for implementing computer vision applications. The web page of OpenCV library can be found at IUUQPQFODWPSH. The OpenCV library is interfaced to ROS via a ROS stack, called WJTJPO@PQFODW. WJTJPO@PQFODW consists of two important packages for interfacing OpenCV to ROS. They are: DW@CSJEHF: The DW@CSJEHF package contains a library that provides APIs for converting the OpenCV image data type, DW.BU, to the ROS image message called TFOTPS@NTHT*NBHF and vice versa. In short, it can act as a bridge

between OpenCV and ROS. We can use OpenCV APIs to process the image and convert to ROS image messages whenever we want to send to another node. We will discuss how to do this conversion in the upcoming sections. JNBHF@HFPNFUSZ: One of the first processes that we should do before working with cameras is calibration. The JNBHF@HFPNFUSZ package contains libraries written in C++ and Python, which helps to correct the geometry of the image using calibration parameters. The package uses a message type called TFOTPS@NTHT$BNFSB*OGP for handling the calibration parameters and feeding to the OpenCV image rectification function.

Understanding ROS - PCL interfacing packages The point cloud data can be defined as a group of data points in some coordinate system. In 3D, it has x, y, and z coordinates. The PCL library is an open source project for handling 2D/3D images and point cloud processing. Like OpenCV, it is under BSD license, and free for academic and commercial purposes. It is also a cross-platform packages that has support in Linux, Windows, macOS, and Android/iOS.

[ 305 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

The library consists of standard algorithms for filtering, segmentation, feature estimation, and so on, which are required to implement different point cloud applications. The main web page of the point cloud library can be found at IUUQQPJOUDMPVETPSH. The point cloud data can be acquired by sensors such as Kinect, Asus Xtion Pro, Intel Real Sense, and others. We can use this data for robotic applications, such as robot object manipulation and grasping. PCL is tightly integrated into ROS for handling point cloud data from various sensors. The QFSDFQUJPO@QDM stack is the ROS interface for the PCL library. It consists of packages for pumping the point cloud data from ROS to PCL data types and vice versa. QFSDFQUJPO@QDM consists of the following packages: QDM@DPOWFSTJPOT: This package provides APIs to convert PCL data types to

ROS messages and vice versa. QDM@NTHT: This package contains the definition of PCL-related messages in ROS. The PCL messages are: .PEFM$PFGGJDJFOUT 1PJOU*OEJDFT 1PMZHPO.FTI 7FSUJDFT QDM@SPT: This is the PCL bridge of ROS. This package contains tools and nodes

to bridge ROS messages to PCL data types and vice versa. QPJOUDMPVE@UP@MBTFSTDBO: The main function of this package is to convert the 3D point cloud into a 2D laser scan. This package is useful for converting an inexpensive 3D vision sensor, such as Kinect and Asus Xtion Pro, to a laser scanner. The laser scanner data is mainly used for 2D-SLAM, for the purpose of robot navigation.

Installing ROS perception We are going to install a single package called QFSDFQUJPO, which is a metapackage of ROS containing all the perception-related packages, such as OpenCV, PCL, and so on: $ sudo apt-get install ros-kinetic-perception

[ 306 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

The ROS perception stack contains the following ROS packages: JNBHF@DPNNPO: This metapackage contains common functionalities to handle an

image in ROS. The metapackage consists of the following list of packages (IUUQXJLJSPTPSHJNBHF@DPNNPO): JNBHF@USBOTQPSU: This package helps to compress the image during publishing and subscribes the images to save the band width (IUUQXJLJSPTPSHJNBHF@USBOTQPSU). The various compression methods are JPEG/PNG compression and Theora for streaming videos. We can also add custom compression methods to JNBHF@USBOTQPSU. DBNFSB@DBMJCSBUJPO@QBSTFST: This package contains a routine to read/write camera calibration parameters from an XML file. This package is mainly used by camera drivers for accessing calibration parameters. DBNFSB@JOGP@NBOBHFS: This package consists of a routine to save, restore, and load the calibration information. This is mainly used by camera drivers. QPMMFE@DBNFSB: This package contains the interface for requesting images from a polling camera driver (for example, QSPTJMJDB@DBNFSB). *NBHF@QJQFMJOF: This metapackage contains packages to

process the raw image from the camera driver. The various processing done by this metapackage include calibration, distortion removal, stereo vision processing, depth-image processing, and so on. The following packages are present in this metapackage for this processing (IUUQXJLJSPTPSHJNBHF@QJQFMJOF). DBNFSB@DBMJCSBUJPO: One of the important tools for relating the 3D world to

the 2D camera image is calibration. This package provides tools for doing monocular and stereo image calibration in ROS. JNBHF@QSPD: The nodes in this package act between the camera driver and the vision processing nodes. It can handle the calibration parameters, correct image distortion from the raw image, and convert to color images. EFQUI@JNBHF@QSPD: This package contains nodes and OPEFMFUT for handling depth images from Kinect and 3D vision sensors. The depth image can be processed by these OPEFMFUT to produce point cloud data.

[ 307 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

TUFSFP@JNBHF@QSPD: This package has nodes to perform distortion removal for a pair of cameras. It is the same as the JNBHF@QSPD package, except that it

handles two cameras for stereo vision and for developing point cloud and disparity images. JNBHF@SPUBUF: This package contains nodes to rotate the input image. JNBHF@WJFX This is a simple ROS tool for viewing ROS message topics. It can also view stereo and disparity images. *NBHF@USBOTQPSU@QMVHJOT: These are the plugins of ROS image transport for publishing and subscribing the ROS images in different compression levels or different video codec to reduce the bandwidth and latency. -BTFS@QJQFMJOF: This is a set of packages that can process laser data, such as filtering and converting into 3D Cartesian points and assembling points to form a cloud. The MBTFSQJQFMJOF stack contains the following packages: MBTFS@GJMUFST: This package contains nodes to filter the noise in the raw laser data, remove the laser points inside the robot footprint, and remove spurious values inside the laser data. MBTFS@HFPNFUSZ: After filtering the laser data, we have to transform the laser ranges and angles into 3D Cartesian coordinates efficiently by taking into account the tilt and skew angle of laser scanner MBTFS@BTTFNCMFS: This package can assemble the laser scan into a 3D point cloud or 2.5D scan. QFSDFQUJPO@QDM: This is the stack of the PCL-ROS interface. WJTJPO@PQFODW: This is the stack of the OpenCV-ROS interface.

Interfacing USB webcams in ROS We can start interfacing with an ordinary webcam or a laptop cam in ROS. Overall, there are no ROS-specific packages to install and use web cameras. If the camera is working in Ubuntu/Linux, it may be supported by the ROS driver too. After plugging in the camera, check whether a EFWWJEFP9 device file has been created, or check with some application such as Cheese, VLC, or similar others. The guide to check whether the webcam is supported on Ubuntu is available at IUUQTIFMQVCVOUVDPNDPNNVOJUZ8FCDBN.

[ 308 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

We can find the video devices present on the system using the following command: $ ls /dev/ | grep video

If you get an output of WJEFP, you can confirm a USB cam is available for use. After ensuring the webcam support in Ubuntu, we can install a ROS webcam driver called VTC@DBN using the following command: $ sudo apt-get install ros-kinetic-usb-cam

We can install the latest package of VTC@DBN from the source code. The driver is available on GitHub, at IUUQTHJUIVCDPNCPTDISPTQLHVTC@DBN The VTC@DBN package contains a node called VTC@DBN@OPEF, which is the driver of USB cams. There are some parameters that need to be set before running this node. We can run the ROS node along with its parameters. The VTC@DBNUFTUMBVODI file can launch the USB cam driver with the necessary parameters: MBVODI OPEFOBNFVTC@DBNQLHVTC@DBNUZQFVTC@DBN@OPEFPVUQVUTDSFFO QBSBNOBNFWJEFP@EFWJDFWBMVFEFWWJEFP QBSBNOBNFJNBHF@XJEUIWBMVF QBSBNOBNFJNBHF@IFJHIUWBMVF QBSBNOBNFQJYFM@GPSNBUWBMVFZVZW QBSBNOBNFDBNFSB@GSBNF@JEWBMVFVTC@DBN QBSBNOBNFJP@NFUIPEWBMVFNNBQ OPEF OPEFOBNFJNBHF@WJFXQLHJNBHF@WJFXUZQFJNBHF@WJFX SFTQBXOGBMTFPVUQVUTDSFFO SFNBQGSPNJNBHFUPVTC@DBNJNBHF@SBX QBSBNOBNFBVUPTJ[FWBMVFUSVF OPEF MBVODI

This launch file will start VTC@DBN@OPEF with the video device EFWWJEFP, with a resolution of 640 x 480. The pixel format here is YUV (IUUQTFOXJLJQFEJBPSHXJLJ:67). After initiating VTC@DBN@OPEF, it will start an JNBHF@WJFX node for displaying the raw image from the driver. We can launch the previous file by using the following command: $ roslaunch usb_cam usb_cam-test.launch

[ 309 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

We will get the following message with an image view, as shown next:

(KIWTG75$ECOGTCXKGYWUKPIVJGKOCIGXKGYVQQN

[ 310 ]

Chapter 10

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

The topics generated by the driver are shown next. There are raw, compressed, and Theora codec topics generated by the driver:

(KIWTG.KUVQHVQRKEUIGPGTCVGFD[VJG75$ECOGTCFTKXGT

We can visualize the image in another window by using the following command: $ rosrun image_view image_view image:=/usb_cam/image_raw

As you can see from the topic list, due to the installation of the JNBHF@USBTQPSU package, images are published in multiple ways, compressed and uncompressed. The latter format is useful to send images to other ROS nodes over the network or store video data of the topic into CBHGJMFT, occupying little space on the hard disk. In order to use the compressed image from a CBHGJMFT on a remote machine or from a CBHGJMF , we need to republish it in an uncompressed format, using the republish node of the JNBHF@USBOTQPSU package: $ rosrun image_transport republish [input format] in:= [output format] out:=

For example: $ rosrun image_transport republish compressed in:=/usb_cam/image_raw [output format] out:=/usb_cam/image_raw/republished

Note that in the previous example, we have used the topic base name as input (VTC@DBNJNH@SBX), and not its compressed version (VTC@DBNJNBHF@SBXDPNQSFTTFE). Now that we have learned how to acquire images from the camera, we have to work with the camera calibration.

[ 311 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

Working with ROS camera calibration Like all sensors, cameras also need calibration for correcting the distortions in the camera images due to the camera's internal parameters, and for finding the world coordinates from the camera coordinates. The primary parameters that cause image distortions are radial distortions and tangential distortions. Using the camera calibration algorithm, we can model these parameters and also calculate the real-world coordinates from the camera coordinates by computing the camera calibration matrix, which contains the focal distance and the principle points. Camera calibration can be done using a classic black-white chessboard, symmetrical circle pattern, or an asymmetrical circle pattern. According to each different pattern, we use different equations to get the calibration parameters. Using the calibration tools, we detect the patterns, and each detected pattern is taken as a new equation. When the calibration tool gets enough detected patterns it can compute the final parameters for the camera. ROS provides a package named DBNFSB@DBMJCSBUJPO (IUUQXJLJSPTPSHDBNFSB@DBMJCSBUJPO5VUPSJBMT.POPDVMBS$BMJCSBUJPO) to do camera calibration, which is a part of the image pipeline stack. We can calibrate monocular, stereo, and even 3D sensors, such as Kinect and Asus Xtion pro. The first thing we have to do before calibration is to download the chessboard pattern mentioned in the ROS Wiki page, and print it and paste it onto a card board. This is the pattern we are going to use for calibration. This check board has 8x6 with 108 mm squares. Run the VTC@DBN launch file to start the camera driver. We are going to run the camera calibration node of ROS using the raw image from the VTC@DBNJNBHF@SBX topic. The following command will run the calibration node with the necessary parameters: $ rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.108 image:=/usb_cam/image_raw camera:=/usb_cam

[ 312 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

A calibration window will pop up, and when we show the calibration pattern to the camera, and the detection is made, we will see the following screenshot:

(KIWTG415ECOGTCECNKDTCVKQP

Move the calibration pattern in the X direction and Y direction. If the calibrator node gets a sufficient amount of samples, a CALIBRATE button will become active on the window. When we press the CALIBRATE button, it will compute the camera parameters using these samples. It will take some time for the calculation. After computation, two buttons, SAVE and COMMIT, will become active inside the window, as shown in the following image. If we press the SAVE button, it will save the calibration parameters to a file in the UNQ folder. If we press the COMMIT button, it will save them to SPTDBNFSB@JOGPIFBE@DBNFSBZBNM:

[ 313 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

(KIWTG)GPGTCVKPIVJGECOGTCECNKDTCVKQPaNG

Restart the camera driver, and we will see the YAML calibration file loaded along with the driver. The calibration file that we generated will look as follows: JNBHF@XJEUI JNBHF@IFJHIU DBNFSB@OBNFIFBE@DBNFSB DBNFSB@NBUSJY SPXT DPMT EBUB EJTUPSUJPO@NPEFMQMVNC@CPC EJTUPSUJPO@DPFGGJDJFOUT SPXT DPMT EBUB SFDUJGJDBUJPO@NBUSJY SPXT DPMT EBUB QSPKFDUJPO@NBUSJY

[ 314 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

SPXT DPMT EBUB

Converting images between ROS and OpenCV using cv_bridge In this section, we will see how to convert between the ROS image message (TFOTPS@NTHT*NBHF) and the OpenCV image data type (DW.BU). The main ROS package used for this conversion is DW@CSJEHF, which is part of the WJTJPO@PQFODW stack. The ROS library inside DW@CSJEHF, called $W#SJEHF, helps to perform this conversion. We can use the $W#SJEHF library inside our code and perform the conversion. The following figure shows how the conversion is performed between ROS and OpenCV:

(KIWTG%QPXGTVKPIKOCIGUWUKPI%X$TKFIG

Here, the $W#SJEHF library acts as a bridge for converting the ROS messages to OpenCV images and vice versa. We will see how the conversion between ROS and OpenCV is performed in the following example.

[ 315 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

Image processing using ROS and OpenCV In this section, we will see an example of using DW@CSJEHF for acquiring images from a camera driver, and converting and processing the images using OpenCV APIs. The following is how the example works: 1. Subscribe the images from the camera driver from the topic VTC@DBNJNBHF@SBX (TFOTPS@NTHT*NBHF) 2. Convert the ROS images to the OpenCV image type using $W#SJEHF 3. Process the OpenCV image using its APIs and find the edges on the image 4. Convert the OpenCV image type of the edge detection to the ROS image messages and publish into the topic FEHF@EFUFDUPSQSPDFTTFE@JNBHF The step-by-step procedure to build this example follows:

Step 1 ` Creating a ROS package for the experiment You can get the existing package DW@CSJEHF@UVUPSJBM@QLH provided with this book, or you can create a new package, using the following command: $ catkin_create_pkg cv_bridge_tutorial_pkg cv_bridge image_transport roscpp sensor_msgs std_msgs

This package is mainly dependent on DW@CSJEHF, JNBHF@USBOTQPSU, and TFOTPS@NTHT.

Step 2 ` Creating source files You can get the source code of the example TBNQMF@DW@CSJEHF@OPEFDQQ from the DW@CSJEHF@UVUPSJBM@QLHTSD folder.

Step 3 ` Explanation of the code The following is the explanation of the complete code: JODMVEFJNBHF@USBOTQPSUJNBHF@USBOTQPSUI

[ 316 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

We are using the JNBHF@USBOTQPSU package in this code for publishing and subscribing to an image in ROS: JODMVEFDW@CSJEHFDW@CSJEHFI JODMVEFTFOTPS@NTHTJNBHF@FODPEJOHTI

This header includes the $W#SJEHF class and image encoding related functions in the code: JODMVEFPQFODWJNHQSPDJNHQSPDIQQ JODMVEFPQFODWIJHIHVJIJHIHVJIQQ

These are main OpenCV image processing modules and GUI modules which provide image processing and GUI APIs in our code: JNBHF@USBOTQPSU*NBHF5SBOTQPSUJU@ QVCMJD &EHF@%FUFDUPS  JU@ OI@ \ 4VCTDSJCFUPJOQVUWJEFPGFFEBOEQVCMJTIPVUQVUWJEFPGFFE JNBHF@TVC@JU@TVCTDSJCF VTC@DBNJNBHF@SBX *NBHF$POWFSUFSJNBHF$CUIJT JNBHF@QVC@JU@BEWFSUJTF FEHF@EFUFDUPSQSPDFTTFE@JNBHF

We will look in more detail at the line JNBHF@USBOTQPSU*NBHF5SBOTQPSUJU@. This line creates an instance of *NBHF5SBOTQPSU, which is used to publish and subscribe the ROS image messages. More information about the *NBHF5SBOTQPSU API is given next.

1VCMJTIJOHBOETVCTDSJCJOHJNBHFTVTJOHJNBHF@USBOTQPSU ROS image transport is very similar to ROS publishers and subscribers, and it is used to publish or subscribe the images along with the camera information. We can publish the image data using SPT1VCMJTIFS, but image transport is a more efficient way of sending the image data. The image transport APIs are provided by the JNBHF@USBOTQPSU package. Using these APIs, we can transport an image in different compression formats; for example, we can transport it as an uncompressed image, JPEG/PNG compression, or Theora compression in separate topics. We can also add different transport formats by adding plugins. By default, we can see the compressed and Theora transports: JNBHF@USBOTQPSU*NBHF5SBOTQPSUJU@

[ 317 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

In the following line, we are creating an instance of the *NBHF5SBOTQPSU class: JNBHF@USBOTQPSU4VCTDSJCFSJNBHF@TVC@ JNBHF@USBOTQPSU1VCMJTIFSJNBHF@QVC@

After that, we declare the subscriber and publisher objects for subscribing and publishing the images, using the JNBHF@USBOTQPSU object: JNBHF@TVC@JU@TVCTDSJCF VTC@DBNJNBHF@SBX *NBHF$POWFSUFSJNBHF$CUIJT JNBHF@QVC@JU@BEWFSUJTF FEHF@EFUFDUPSQSPDFTTFE@JNBHF

The following is how we subscribe and publish an image: DWOBNFE8JOEPX 01&/$7@8*/%08 ^ _&EHF@%FUFDUPS  \ DWEFTUSPZ8JOEPX 01&/$7@8*/%08 ^

This is how we subscribe and publish an JNBHFDWOBNFE8JOEPX  , which is an OpenCV function to create a GUI for displaying an image. The argument inside this function is the window name. Inside the class destructor, we are destroying the named window.

$POWFSUJOH0QFO$7UP304JNBHFTVTJOHDW@CSJEHF This is an image callback function, and it basically converts the ROS image messages into the OpenCV DW.BU type using the $W#SJEHF APIs. The following is how we can convert ROS to OpenCV, and vice versa: WPJEJNBHF$C DPOTUTFOTPS@NTHT*NBHF$POTU1USNTH \ DW@CSJEHF$W*NBHF1USDW@QUS OBNFTQBDFFODTFOTPS@NTHTJNBHF@FODPEJOHT USZ \ DW@QUSDW@CSJEHFUP$W$PQZ NTH TFOTPS@NTHTJNBHF@FODPEJOHT#(3 ^ DBUDI DW@CSJEHF&YDFQUJPOF \ 304@&3303 DW@CSJEHFFYDFQUJPOTFXIBU 

[ 318 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

SFUVSO ^

To start with $W#SJEHF, we should start with creating an instance of a $W*NBHF. Given next is the creation of the $W*NBHF pointer: DW@CSJEHF$W*NBHF1USDW@QUS

The $W*NBHF type is a class provided by DW@CSJEHF, which consists of information such as an OpenCV image, its encoding, ROS header, and so on. Using this type, we can easily convert an ROS image to OpenCV, and vice versa: DW@QUSDW@CSJEHFUP$W$PQZ NTHTFOTPS@NTHTJNBHF@FODPEJOHT#(3

We can handle the ROS image message in two ways: either we can make a copy of the image or we can share the image data. When we copy the image, we can process the image, but if we use a shared pointer, we can't modify the data. We use UP$W$PQZ  for creating a copy of the ROS image, and the UP$W4IBSF  function is used to get the pointer of the image. Inside these functions, we should mention the ROS message and the type of encoding: JG DW@QUS JNBHFSPXT DW@QUS JNBHFDPMT \ EFUFDU@FEHFT DW@QUS JNBHF JNBHF@QVC@QVCMJTI DW@QUS UP*NBHF.TH  ^

In this section, we are extracting the image and its properties from the $W*NBHF instance, and accessing the DW.BU object from this instance. This code simply checks whether the rows and columns of the image are in a particular range, and, if it is true, it will call another method called EFUFDU@FEHFT DW.BU, which will process the image given as an argument and display the edge-detected-image: JNBHF@QVC@QVCMJTI DW@QUS UP*NBHF.TH 

The preceding line will publish the edge-detected-image after converting to the ROS image message. Here we are using the UP*NBHF.TH  function for converting the $W*NBHF instance to a ROS image message.

[ 319 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

'JOEJOHFEHFTPOUIFJNBHF After converting the ROS images to OpenCV type, the EFUFDU@FEHFT DW.BU function will be called for finding the edges on the image, using the following inbuilt OpenCV functions: DWDWU$PMPS JNHTSD@HSBZ$7@#(3(3": DWCMVS TSD@HSBZEFUFDUFE@FEHFTDW4J[F  DW$BOOZ EFUFDUFE@FEHFTEFUFDUFE@FEHFTMPX5ISFTIPME MPX5ISFTIPME SBUJPLFSOFM@TJ[F

Here, the DWU$PMPS  function will convert an RGB image to a gray color space and DWCMVS  will add blurring to the image. After that, using the Canny edge detector, we extract the edges of the image.

7JTVBMJ[JOHSBXBOEFEHFEFUFDUFEJNBHFT Here we are displaying the image data using the OpenCV function called JNTIPX , which consists of the window name and the image name: DWJNTIPX 01&/$7@8*/%08JNH DWJNTIPX 01&/$7@8*/%08@ETU DWXBJU,FZ 

Step 4 ` Editing the CMakeLists.txt file The definition of the $.BLF-JTUTUYU file is given next. In this example, we need OpenCV support, so we should include the OpenCV header path and also link the source code against the OpenCV library path: JODMVEF@EJSFDUPSJFT

\DBULJO@*/$-6%&@%*34^ \0QFO$7@*/$-6%&@%*34^  BEE@FYFDVUBCMF TBNQMF@DW@CSJEHF@OPEFTSDTBNQMF@DW@CSJEHF@OPEFDQQ 4QFDJGZMJCSBSJFTUPMJOLBMJCSBSZPSFYFDVUBCMFUBSHFUBHBJOTU UBSHFU@MJOL@MJCSBSJFT TBNQMF@DW@CSJEHF@OPEF \DBULJO@-*#3"3*&4^ \0QFO$7@-*#3"3*&4^ 

[ 320 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

Step 5 ` Building and running an example After building the package using DBULJO@NBLF, we can run the node using the following command: 1. Launch the webcam driver: $ roslaunch usb_cam usb_cam-test.launch

2. Run the DW@CSJEHF sample node: $ rosrun cv_bridge_tutorial_pkg sample_cv_bridge_node

3. If everything works fine, we will get two windows, as shown in the following image. The first window shows the raw image and the second is the processed edge-detected-image:

(KIWTG4CYKOCIGCPFGFIGFGVGEVGFKOCIG

[ 321 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

Interfacing Kinect and Asus Xtion Pro in ROS The webcams that we have worked with until now can only provide 2D visual information of the surroundings. For getting 3D information about the surroundings, we have to use 3D vision sensors or range finders, such as laser finders. Some of the 3D vision sensors that we are discussing in this chapter are Kinect, Asus Xtion Pro, Intel Real sense, and Hokuyo laser scanner:

(KIWTG6QR-KPGEVDQVVQO#UWU:VKQP2TQ

The first two sensors we are going to discuss are Kinect and Asus Xtion Pro. Both of these devices need the OpenNI (Open source Natural Interaction) driver library for operating in the Linux system. OpenNI acts as a middleware between the 3D vision devices and the application software. The OpenNI driver is integrated to ROS and we can install these drivers by using the following commands. These packages help to interface the OpenNI supported devices, such as Kinect and Asus Xtion Pro: $ sudo apt-get install ros-kinetic-openni-launch ros-kinetic-openni2-launch

The preceding command will install OpenNI drivers and launch files for starting the RGB/depth streams. After successful installation of these packages, we can launch the driver by using the following command: $ roslaunch openni2_launch openni2.launch

[ 322 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

This launch file will convert the raw data from the devices into useful data, such as 3D point clouds, disparity images, and depth, and the RGB images using ROS nodelets. Other than the OpenNI drivers, there is another driver available called MJCGSFFOFDU. The common launch files of the drivers are organized into a package called SHCE@MBVODI. This package consists of common launch files that are used for the freenect and openni drivers. We can visualize the point cloud generated by the OpenNI ROS driver by using RViz. Run RViz, using the following command: $ rosrun rviz rviz

Set the fixed frame to DBNFSB@EFQUI@PQUJDBM@GSBNF, add a PointCloud2 display, and set the topic as DBNFSBEFQUIQPJOUT. This is the unregistered point cloud from the IR camera, that is, it may have a complete match with the RGB camera and it only uses the depth camera for generating the point cloud:

(KIWTG7PTGIKUVGTGFRQKPVENQWFXKGYKP48K\

[ 323 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

We can enable the registered point cloud by using the Dynamic Reconfigure GUI, by using the following command: $ rosrun rqt_reconfigure rqt_reconfigure

(KIWTG&[PCOKE4GEQPaIWTG)7+

[ 324 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

Click on camera | driver and tick EFQUI@SFHJTUSBUJPO. Change the point cloud to DBNFSBEFQUI@SFHJTUFSFEQPJOUT and Color Transformer to 3(# in RViz. We will see the registered point cloud in RViz as it appears in the following image. The registered point cloud takes information from the depth and the RGB camera to generate the point cloud:

(KIWTG6JGTGIKUVGTGFRQKPVENQWF

Interfacing Intel Real Sense camera with ROS One of the new 3D depth sensors from Intel is Real Sense. Until now, different versions of this sensor have been released (F200, R200, SR30, ...). To interface Real Sense sensors with ROS, we first have to install the MJCSFBMTFOTF library.

[ 325 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

Download the Real Sense library from the following link: IUUQTHJUIVCDPN *OUFM3FBM4FOTFMJCSFBMTFOTF, using the following code: $ git clone https://github.com/IntelRealSense/librealsense.git

Then follow these steps: 1. Install MJCVEFWEFW, QLHDPOGJH, and MJCHUL: $ sudo apt-get install libudev-dev pkg-config libgtk-3-dev

2. Install HMGX: $ sudo apt-get install libglfw3-dev

3. Navigate to the MJCSFBMTFOTF root directory and run: $ $ $ $

mkdir build && cd build cmake .. make sudo make install

After installing the Real Sense library, we can install the ROS package to start sensor data streaming. We can install from the Ubuntu/Debian package manager by using the following command: $ sudo make install ros-kinetic-realsense

Or we can directly clone the package from the Git repository and compile the workspace: $ git clone https://github.com/intel-ros/realsense.git

[ 326 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

Now we can start the sensor using the example launch file and open Rviz to visualize the color and depth data streamed by SFBMTFOTF: roslaunch realsense_camera sr300_nodelet_rgbd.launch

(KIWTG+PVGN4GCN5GPUGXKGYKP48K\

The following are the topics generated by the Real Sense driver: TFOTPS@NTHT1PJOU$MPVE DBNFSBEFQUIQPJOUTQPJOUDMPVEXJUIPVU3(# DBNFSBEFQUI@SFHJTUFSFEQPJOUTQPJOUDMPVEXJUI3(# TFOTPS@NTHT*NBHF DBNFSBJNBHFSHC@SBXSBXJNBHFGPS3(#TFOTPS DBNFSBJNBHFEFQUI@SBXSBXJNBHFGPSEFQUITFOTPS DBNFSBJNBHFJS@SBXSBXJNBHFGPSJOGSBSFETFOTPS

[ 327 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

Working with a point cloud to a laser scan package One of the important applications of 3D vision sensors is mimicking the functionalities of a laser scanner. We need the laser scanner data for working with autonomous navigation algorithms, such as SLAM. We can make a fake laser scanner using a 3D vision sensor. We can take a slice of the point cloud data/depth image and convert it to laser range data. In ROS, we have a set of packages to convert the point cloud to laser scans: EFQUIJNBHF@UP@MBTFSTDBO: This package contains nodes that take the depth

image from the vision sensor and generate a 2D laser scan based on the provided parameters. The inputs of the node are depth image and camera info parameters, which include calibration parameters. After conversion to the laser scan data, it will publish laser scanner data in the TDBO topic. The node parameters are TDBO@IFJHIUTDBO@UJNFSBOHF@NJOSBOHF@NBY, and the output frame ID. The official ROS Wiki page of this package can be found at IUUQXJLJSPT PSHEFQUIJNBHF@UP@MBTFSTDBO. QPJOUDMPVE@UP@MBTFSTDBO: This package converts the real point cloud data into 2D laser scan, instead of taking a depth image as in the previous package. The official Wiki page of this package can be found at IUUQXJLJSPTPSHQPJOUDMPVE@UP@MBTFSTDBO. The first package is suitable for normal applications; however, if the sensor is placed on an angle, it is better to use the second package. Also, the first package takes less processing than the second one. Here we are using the EFQUIJNBHF@UP@MBTFSTDBO package to convert a laser scan. We can install EFQUIJNBHF@UP@MBTFSTDBO and QPJOUDMPVE@UP@MBTFSTDBO by using the following command: $ sudo apt-get install ros-kinetic-depthimage-to-laserscan ros- kineticpointcloud-to-laserscan

We can start converting from the depth image of the OpenNI device to the 2D laser scanner by using the following package.

[ 328 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

Create a package for performing the conversion: $ catkin_create_pkg fake_laser_pkg depthimage_to_laserscan nodelet roscpp

Create a folder called MBVODI, and inside this folder create the following launch file called TUBSU@MBTFSMBVODI. You will get this package and file from the GBLF@MBTFS@QLHMBVODI folder: MBVODI DBNFSBTIPVMEVOJRVFMZJEFOUJGZUIFEFWJDF"MMUPQJDTBSF QVTIFEEPXO JOUPUIFDBNFSBOBNFTQBDFBOEJUJTQSFQFOEFEUPUGGSBNF JET BSHOBNFDBNFSBEFGBVMUDBNFSB BSHOBNFQVCMJTI@UGEFGBVMUUSVF   HSPVQJG BSHTDBO@QSPDFTTJOH OPEFQLHOPEFMFUUZQFOPEFMFUOBNFEFQUIJNBHF@UP@MBTFSTDBO BSHTMPBEEFQUIJNBHF@UP@MBTFSTDBO%FQUI*NBHF5P-BTFS4DBO/PEFMFU BSH DBNFSB BSHDBNFSB@OPEFMFU@NBOBHFS

QBSBNOBNFTDBO@IFJHIUWBMVF QBSBNOBNFPVUQVU@GSBNF@JEWBMVF BSH DBNFSB@EFQUI@GSBNF QBSBNOBNFSBOHF@NJOWBMVF SFNBQGSPNJNBHFUP BSHDBNFSB BSHEFQUIJNBHF@SBX SFNBQGSPNTDBOUP BSHTDBO@UPQJD   MBVODI

The following code snippet will launch the nodelet for converting the depth image to laser scanner: OPEFQLHOPEFMFUUZQFOPEFMFUOBNFEFQUIJNBHF@UP@MBTFSTDBO BSHTMPBEEFQUIJNBHF@UP@MBTFSTDBO%FQUI*NBHF5P-BTFS4DBO/PEFMFU BSH DBNFSB BSHDBNFSB@OPEFMFU@NBOBHFS

[ 329 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

Launch this file and we can view the laser scanner in RViz. Launch this file using the following command: $ roslaunch fake_laser_pkg start_laser.launch

We will see the data in RViz, as shown in the following image:

(KIWTG.CUGTUECPKP48K\

Set Fixed Frame as DBNFSB@EFQUI@GSBNF and Add LaserScan in the topic TDBO. We can see the laser data in the view port.

[ 330 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

Interfacing Hokuyo Laser in ROS We can interface different ranges of laser scanners in ROS. One of the popular laser scanners available in the market is Hokuyo Laser scanner (IUUQXXXSPCPUTIPQDPNFOIPLVZPVUNMYMBTFSTDBOOJOHSBOHFGJOEFSIUNM):

(KIWTG&KbGTGPVUGTKGUQH*QMW[QNCUGTUECPPGTU

One of the commonly used Hokuyo laser scanner models is 65.-9. This sensor is fast and accurate and is suitable for robotic applications. The device has a USB 2.0 interface for communication, and has up to a 30 meters range with a millimeter resolution. The arc range of the scan is about 270 degrees:

[ 331 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

(KIWTG*QMW[Q76/.:

There is already a driver available in ROS for interfacing these scanners. One of the interfaces is called VSH@OPEF (IUUQXJLJSPTPSHVSH@OPEF). We can install this package by using the following command: $ sudo apt-get install ros-kinetic-urg-node

When the device connects to the Ubuntu system, it will create a device called UUZ"$.Y. Check the device name by entering the ENFTH command in the Terminal. Change the USB device permission by using the following command: $ sudo chmod a+rw /dev/ttyACMx

Start the laser scan device, using the following launch file called IPLVZP@TUBSUMBVODI: MBVODI OPEFOBNFVSH@OPEFQLHVSH@OPEFUZQFVSH@OPEFPVUQVUTDSFFO QBSBNOBNFTFSJBM@QPSUWBMVFEFWUUZ"$. QBSBNOBNFGSBNF@JEWBMVFMBTFS QBSBNOBNFBOHMF@NJOWBMVF QBSBNOBNFBOHMF@NBYWBMVF OPEF OBNFSWJ[QLHSWJ[UZQFSWJ[SFTQBXOGBMTFPVUQVUTDSFFO BSHTE GJOEIPLVZP@OPEFIPLVZP@UFTUWDH MBVODI

[ 332 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

This launch file starts the node to get the laser data from the device EFWUUZ"$.. The laser data can be viewed inside the RViz window, as shown in the following image:

(KIWTG*QMW[Q.CUGTUECPFCVCKP48K\

Working with point cloud data We can handle the point cloud data from Kinect or the other 3D sensors for performing a wide variety of tasks, such as 3D object detection and recognition, obstacle avoidance, 3D modeling, and so on. In this section, we will see some basic functionalities: using the PCL library and its ROS interface. We will discuss the following examples: How to publish a point cloud in ROS How to subscribe and process a point cloud How to write point cloud data to a PCD file How to read and publish a point cloud from a PCD file

[ 333 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

How to publish a point cloud In this example, we will see how to publish a point cloud data using the TFOTPS@NTHT1PJOU$MPVE message. The code will use PCL APIs for handling and creating the point cloud, and converting the PCL cloud data to the PointCloud2 message type. You will get the example code QDM@QVCMJTIFSDQQ from the QDM@SPT@UVUPSJBMTSD folder: JODMVEFSPTSPTI QPJOUDMPVEIFBEFST JODMVEFQDMQPJOU@DMPVEI )FBEFSXIJDIDPOUBJO1$-UP304BOE304UP1$-DPOWFSTJPOGVODUJPOT JODMVEFQDM@DPOWFSTJPOTQDM@DPOWFSTJPOTI TFOTPS@NTHTIFBEFSGPSQPJOUDMPVE JODMVEFTFOTPS@NTHT1PJOU$MPVEI NBJO JOUBSHDDIBS BSHW \ SPTJOJU BSHDBSHWQDM@DSFBUF

304@*/'0 4UBSUFE1$-QVCMJTIJOHOPEF SPT/PEF)BOEMFOI $SFBUJOHQVCMJTIFSPCKFDUGPSQPJOUDMPVE SPT1VCMJTIFSQDM@QVCOIBEWFSUJTFTFOTPS@NTHT1PJOU$MPVE

QDM@PVUQVU $SFBUJOHBDMPVEPCKFDU QDM1PJOU$MPVEQDM1PJOU9:; DMPVE $SFBUJOHBTFOTPS@NTHPGQPJOUDMPVE TFOTPS@NTHT1PJOU$MPVEPVUQVU *OTFSUDMPVEEBUB DMPVEXJEUI DMPVEIFJHIU DMPVEQPJOUTSFTJ[F DMPVEXJEUI DMPVEIFJHIU

[ 334 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

*OTFSUSBOEPNQPJOUTPOUIFDMPVET GPS TJ[F@UJJDMPVEQPJOUTTJ[F 

J \ DMPVEQPJOUTY SBOE  3"/%@."9 G DMPVEQPJOUTZ SBOE  3"/%@."9 G DMPVEQPJOUT[ SBOE  3"/%@."9 G ^ $POWFSUUIFDMPVEUP304NFTTBHF QDMUP304.TH DMPVEPVUQVU PVUQVUIFBEFSGSBNF@JEQPJOU@DMPVE SPT3BUFMPPQ@SBUF  XIJMF SPTPL  \ QVCMJTIJOHQPJOUDMPVEEBUB QDM@QVCQVCMJTI PVUQVU SPTTQJO0ODF  MPPQ@SBUFTMFFQ  ^ SFUVSO ^

The creation of the PCL cloud is done as follows: $SFBUJOHBDMPVEPCKFDU QDM1PJOU$MPVEQDM1PJOU9:; DMPVE

After creating this cloud, we insert random points to the clouds. We convert the PCL cloud to a ROS message by using the following function: $POWFSUUIFDMPVEUP304NFTTBHF QDMUP304.TH DMPVEPVUQVU

After converting to ROS messages, we can simply publish the data on the topic QDM@PVUQVU.

[ 335 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

How to subscribe and process the point cloud In this example, we will see how to subscribe the generated point cloud on the topic QDM@PVUQVU. After subscribing the point cloud, we apply a filter called the 7PYFM(SJE class in PCL to down-sample the input cloud by keeping the same centroid of the input cloud. You will get the example code QDM@GJMUFSDQQ from the TSD folder of the package: JODMVEFSPTSPTI JODMVEFQDMQPJOU@DMPVEI JODMVEFQDM@DPOWFSTJPOTQDM@DPOWFSTJPOTI JODMVEFTFOTPS@NTHT1PJOU$MPVEI 7PSUFYGJMUFSIFBEFS JODMVEFQDMGJMUFSTWPYFM@HSJEI $SFBUJOHBDMBTTGPSIBOEMJOHDMPVEEBUB DMBTTDMPVE)BOEMFS \ QVCMJD DMPVE)BOEMFS  \ 4VCTDSJCJOHQDM@PVUQVUUPQJDTGSPNUIFQVCMJTIFS 5IJTUPQJDDBODIBOHFBDDPSEJOHUPUIFTPVSDFPGQPJOUDMPVE QDM@TVCOITVCTDSJCF QDM@PVUQVUDMPVE)BOEMFSDMPVE$#UIJT $SFBUJOHQVCMJTIFSGPSGJMUFSFEDMPVEEBUB QDM@QVCOIBEWFSUJTFTFOTPS@NTHT1PJOU$MPVE QDM@GJMUFSFE  ^ $SFBUJOHDMPVEDBMMCBDL WPJEDMPVE$# DPOTUTFOTPS@NTHT1PJOU$MPVEJOQVU \ QDM1PJOU$MPVEQDM1PJOU9:; DMPVE QDM1PJOU$MPVEQDM1PJOU9:; DMPVE@GJMUFSFE TFOTPS@NTHT1PJOU$MPVEPVUQVU QDMGSPN304.TH JOQVUDMPVE $SFBUJOH7PYFM(SJEPCKFDU QDM7PYFM(SJEQDM1PJOU9:; WPY@PCK 4FUJOQVUUPWPYFMPCKFDU WPY@PCKTFU*OQVU$MPVE DMPVENBLF4IBSFE  4FUUJOHQBSBNFUFSTPGGJMUFSTVDIBTMFBGTJ[F WPY@PCKTFU-FBG4J[F GGG 1FSGPSNJOHGJMUFSJOHBOEDPQZUPDMPVE@GJMUFSFEWBSJBCMF WPY@PCKGJMUFS DMPVE@GJMUFSFE QDMUP304.TH DMPVE@GJMUFSFEPVUQVU PVUQVUIFBEFSGSBNF@JEQPJOU@DMPVE

[ 336 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

QDM@QVCQVCMJTI PVUQVU ^ QSPUFDUFE SPT/PEF)BOEMFOI SPT4VCTDSJCFSQDM@TVC SPT1VCMJTIFSQDM@QVC ^ NBJO JOUBSHDDIBS BSHW \ SPTJOJU BSHDBSHWQDM@GJMUFS 304@*/'0 4UBSUFE'JMUFS/PEF DMPVE)BOEMFSIBOEMFS SPTTQJO  SFUVSO ^

This code subscribes the point cloud topic called QDM@PVUQVU, filters, using 7PYFM(SJE, and publishes the filtered cloud through the DMPVE@GJMUFSFE topic.

Writing data to a Point Cloud Data (PCD) file We can save the point cloud to a PCD file by using the following code. The filename is QDM@XSJUFDQQ inside the TSD folder: JODMVEFSPTSPTI JODMVEFQDMQPJOU@DMPVEI JODMVEFQDM@DPOWFSTJPOTQDM@DPOWFSTJPOTI JODMVEFTFOTPS@NTHT1PJOU$MPVEI )FBEFSGJMFGPSXSJUJOH1$%GJMF JODMVEFQDMJPQDE@JPI WPJEDMPVE$# DPOTUTFOTPS@NTHT1PJOU$MPVEJOQVU \ QDM1PJOU$MPVEQDM1PJOU9:; DMPVE QDMGSPN304.TH JOQVUDMPVE 4BWFEBUBBTUFTUQDEGJMF QDMJPTBWF1$%'JMF"4$** UFTUQDEDMPVE ^ NBJO JOUBSHDDIBS BSHW \ SPTJOJU BSHDBSHWQDM@XSJUF

[ 337 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

304@*/'0 4UBSUFE1$-XSJUFOPEF SPT/PEF)BOEMFOI SPT4VCTDSJCFSCBU@TVCOITVCTDSJCF QDM@PVUQVUDMPVE$# SPTTQJO  SFUVSO ^

Reading and publishing a point cloud from a PCD file This code can read a PCD file and publish the point cloud in the QDM@PVUQVU topic. The code QDM@SFBEDQQ is available in the TSD folder: JODMVEFSPTSPTI JODMVEFQDMQPJOU@DMPVEI JODMVEFQDM@DPOWFSTJPOTQDM@DPOWFSTJPOTI JODMVEFTFOTPS@NTHT1PJOU$MPVEI JODMVEFQDMJPQDE@JPI NBJO JOUBSHDDIBS BSHW \ SPTJOJU BSHDBSHWQDM@SFBE 304@*/'0 4UBSUFE1$-SFBEOPEF SPT/PEF)BOEMFOI SPT1VCMJTIFSQDM@QVCOIBEWFSUJTFTFOTPS@NTHT1PJOU$MPVE

QDM@PVUQVU TFOTPS@NTHT1PJOU$MPVEPVUQVU QDM1PJOU$MPVEQDM1PJOU9:; DMPVE -PBEUFTUQDEGJMF QDMJPMPBE1$%'JMF UFTUQDEDMPVE QDMUP304.TH DMPVEPVUQVU PVUQVUIFBEFSGSBNF@JEQPJOU@DMPVE SPT3BUFMPPQ@SBUF  XIJMF SPTPL  \ 1VCMJTIJOHUIFDMPVEJOTJEFQDEGJMF

[ 338 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

QDM@QVCQVCMJTI PVUQVU SPTTQJO0ODF  MPPQ@SBUFTMFFQ  ^ SFUVSO ^

We can create a ROS package called QDM@SPT@UVUPSJBM for compiling these examples: $ catkin_create_pkg pcl_ros_tutorial pcl pcl_ros roscpp sensor_msgs

Otherwise, we can use the existing package. Create the preceding examples inside TSD as QDM@QVCMJTIFSDQQ, QDM@GJMUFSDQQ, QDM@XSJUFDQQ, and QDM@SFBEDQQ. Create $.BLF-JTUTUYU for compiling all the sources: %FDMBSFBDQQFYFDVUBCMF BEE@FYFDVUBCMF QDM@QVCMJTIFS@OPEFTSDQDM@QVCMJTIFSDQQ BEE@FYFDVUBCMF QDM@GJMUFSTSDQDM@GJMUFSDQQ BEE@FYFDVUBCMF QDM@XSJUFTSDQDM@XSJUFDQQ BEE@FYFDVUBCMF QDM@SFBETSDQDM@SFBEDQQ UBSHFU@MJOL@MJCSBSJFT QDM@QVCMJTIFS@OPEF \DBULJO@-*#3"3*&4^  UBSHFU@MJOL@MJCSBSJFT QDM@GJMUFS \DBULJO@-*#3"3*&4^  UBSHFU@MJOL@MJCSBSJFT QDM@XSJUF \DBULJO@-*#3"3*&4^  UBSHFU@MJOL@MJCSBSJFT QDM@SFBE \DBULJO@-*#3"3*&4^ 

[ 339 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

Build this package using DBULJO@NBLF, and we can run QDM@QVCMJTIFS@OPEF and the view point cloud inside RViz by using the following command: $ rosrun rviz rviz -f point_cloud

A screenshot of the point cloud from QDM@PVUQVU is shown in the following image:

(KIWTG2%.ENQWFKP48K\

[ 340 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

We can run the QDM@GJMUFS node to subscribe this same cloud and do voxel grid filtering. The following screenshot shows the output from the QDM@GJMUFSFE topic, which is the resultant down-sampled cloud:

(KIWTG(KNVGTGF2%.ENQWFKP48K\

We can write the QDM@PVUQVU cloud by using the QDM@XSJUF node and read or publish by using the QDM@SFBE nodes.

Working with AR Marker detection for object pose estimation In this section, we will see how to use fiducial markers in order to enable a robot to easily interact with its environment. To interact with arbitrary objects, a robot should be able to recognize and localize them by relying on its vision sensors. Estimating the pose of an object represents an important feature of all robotic and computer-vision applications. However, efficient algorithms to perform object recognition and pose estimation working in realworld environments are difficult to implement, and in many cases one camera is not enough to retrieve the three-dimensional pose of an object.

[ 341 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

More precisely, with the use of only one fixed camera, it is not possible to get spatial information about the depth of a framed scene. For this reason, object pose estimation is often simplified by exploiting AR markers. An AR marker is typically represented by a synthetic square image composed by a wide black border and an inner binary matrix which determines its unique identifier, as shown in Figure 19. The presence of a black border facilitates its fast detection in the image, and the binary codification allows its identification and the application of error detection and correction techniques:

(KIWTG#WIOGPVGFTGCNKV[OCTMGTU

The main benefit of these markers is that every fiducial image can be uniquely detected by our program, starting from a dictionary of markers, allowing the robot to identify and interact with a high number of objects. In addition, by configuring the size of the markers to detect, it is possible to estimate the distance of a given object with respect to the visual sensor. These markers are commonly called Augmented Reality markers because they have been widely used in augmented reality applications to display artificial information or artificial objects upon video frames. Different programs working with AR markers have been developed and many of these have been interfaced with ROS, such as, for example, ARToolKit (IUUQTHJUIVCDPNBSUPPMTBS@UPPMT) or ArUco: a minimal library for Augmented Reality applications (IUUQTXXXVDPFTJOWFTUJHBHSVQPTBWBOPEF). In the following section, we will discuss how to use the AprilTag visual fiducial system to detect and localize markers, interfacing it with ROS, and estimating the three-dimensional position and orientation of different objects.

Using AprilTag with ROS AprilTag (IUUQTBQSJMFFDTVNJDIFEVTPGUXBSFBQSJMUBHIUNM) is a fiducial system particularly designed for robotics applications, thanks to its high level of precision computing the pose of AR markers, AprtilTag is particularly designed for robotics applications, you should clone the following repository: $ git clone https://github.com/RIVeR-Lab/apriltags_ros.git

[ 342 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

Now you can compile the ROS workspace in order to build the BQSJMUBHT package and its ROS porting BQSJMUBHT@SPT. To use AprilTag, the following things are needed: Video stream: The video data received via TFOTPS@NTHT*NBHF is elaborated by searching for a list. Camera calibration: The calibration data received via TFOTPS@NTHT$BNFSB*OGP, as shown in the previous sections. Tag description: The configuration of the marker to detect. In particular its ID, the size of the markers, and the frames associated with its pose must be specified. Tags: A printed copy of the markers to detect. ApriTag already provides a complete set of QOH ready-to-print individual markers of five different encodings: 16h5, 25h7, 25h9, 36h9, or 36h11. These markers can be found in the BQSJMUBHTUBHT directory of BQSJMUBHT package. After it has been configured and launched, BQSJMUBHT@SPT will publish the pose of all the markers detected in the framed scene and configured in the launch file. In particular, the following topics will be published by the BQSJMUBHT node:

(KIWTG415VQRKEURWDNKUJGFD[#RTKN6CI

[ 343 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

The image elaboration process and the detected markers can be graphically seen displaying the image published on UBH@EFUFDUJPOT@JNBHF, where each fiducial marker is enlightened and labelled with its ID:

(KIWTG)TCRJKECNQWVRWVQH415#RTKN6CI

[ 344 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

Pose information about detected markers is published on UBH@EFUFDUJPOT of the BQSJMUBHT@SPT"QSJM5BH%FUFDUJPO"SSBZ type. In this topic, the IDs and the poses of all detected markers are published. The content of this topic, associated to the frame of the previous figure, is shown following:

(KIWTG2QUKVKQPCPFQTKGPVCVKQPQHOCTMGTUFGVGEVGFD[415#RTKN6CI

Let's now discuss how to use BQSJMUBHT to get the position of three objects. After calibrating our vision system, as shown in previous sections, we could configure and launch the BQSJMUBHT@SPT node. However, you could run this demo using the files of the BQSJMUBHT@SPT@EFNP ROS packages provided with the code of this book, or directly downloaded from the following Git repository: $ git clone https://github.com/jocacace/apriltags_ros_demo

This package contains useful files to launch BQSJMUBHT properly configured. In particular, you can find two main files: CBHT: This contains the PCKFDUCBH bagfile, in which a video data and camera

calibration information are streamed via ROS topics MBVODI: This contains the launch file BQSJMUBHT@SPT@PCKFDUTMBVODI that plays the PCKFDUTCBH bagfile, and launches the BQSJMUBHT@SPT node configured to recognize three different markers

[ 345 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

To configure the BQSJMUBHT@SPT node, we have to modify the launch file in the following way: OPEFQLHBQSJMUBHT@SPTUZQFBQSJMUBH@EFUFDUPS@OPEF OBNFBQSJMUBH@EFUFDUPSPVUQVUTDSFFO SFNBQGSPNJNBHF@SFDUUPDBNFSBSHCJNBHF@SBX SFNBQGSPNDBNFSB@JOGPUPDBNFSBSHCDBNFSB@JOGP QBSBNOBNFJNBHF@USBOTQPSUUZQFTUSWBMVFDPNQSFTTFE QBSBNOBNFUBH@GBNJMZUZQFTUSWBMVFI SPTQBSBNQBSBNUBH@EFTDSJQUJPOT < \JETJ[FGSBNF@JENBTUFSJOH@SPT^ \JETJ[FGSBNF@JEMBQUPQ^ \JETJ[FGSBNF@JETNBSUQIPOF^ > SPTQBSBN OPEF

In this launch file, we could set the topic used by the camera to stream the video stream and its image, using the SFNBQ command: SFNBQGSPNJNBHF@SFDUUPDBNFSBSHCJNBHF@SBX SFNBQGSPNDBNFSB@JOGPUPDBNFSBSHCDBNFSB@JOGP

In addition, we must inform BQSJMUBHT@SPT about which markers must be detected. First, we should specify the family name: QBSBNOBNFUBH@GBNJMZUZQFTUSWBMVFI

Finally, the ID of the markers, their size expressed in meters, and the frame attached to each tag, should be specified in the following way: SPTQBSBNQBSBNUBH@EFTDSJQUJPOT < \JETJ[FGSBNF@JENBTUFSJOH@SPT^ \JETJ[FGSBNF@JEMBQUPQ^ \JETJ[FGSBNF@JETNBSUQIPOF^ > SPTQBSBN

Note that the size of the marker is represented by the length of one of the sides of its black border. This information allows AR marker detectors to estimate the distance of the tag from the camera. For this reason, you should pay attention when providing this measure to get a precise pose estimation.

[ 346 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

In this case, we want to detect three different markers, with IDs 6, 1, and 7 respectively, and with a size of 3.5 centimetres. Each marker is linked to a different frame, which will be even displayed on Rviz using UG. The complete launch file can be found in the BQSJMUBHT@SPT@EFNPMBVODI directory. We can launch this example directly using the provided launch file, which firstly reproduces a CBHGJMF containing the video of a scene composed by three objects or markers and the calibration of the camera: $ roslaunch apriltag_ros_demo apriltags_ros_objects.launch

Now you can read the poses of these objects via the BQSJMUBHT@SPT topic output, or visualize them using Rviz, as show in the next image:

(KIWTG6TCEMKPIOWNVKRNGQDLGEVUWUKPI#4OCTMGTUCPF415

[ 347 ]

Programming Vision Sensors Using ROS, Open CV, and PCL

Chapter 10

Questions What are the packages in the WJTJPO@PQFODW stack? What are the packages in the QFSDFQUJPO@QDM stack? What are the functions of DW@CSJEHF? How do we convert a PCL cloud to a ROS message? How do we do distributive computing using ROS? What is the main benefit of the AR markers?

Summary This chapter was about vision sensors and their programming in ROS. We saw the interfacing packages used to interface the cameras and 3D vision sensors, such as WJTJPO@PQFODW and QFSDFQUJPO@QDM. We looked at each package and its functions on these stacks. We looked at the interfacing of a basic webcam and processing image, using ROS DW@CSJEHF. After discussing DW@CSJEHF, we looked at the interfacing of various 3D vision sensors and laser scanners with ROS. After interfacing, we learned how to process the data from these sensors, using the PCL library and ROS. Finally, in the last part of the chapter, we showed how to use fiducial markers to easily perform object pose estimation and localization. In the next chapter, we will look at the interfacing of robotic hardware in ROS.

[ 348 ]

11

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS In the previous chapter, we discussed robotic vision using ROS. In this chapter, we will see how to build autonomous mobile robot hardware with a differential drive configuration and how to interface it into ROS. We will see how to configure the ROS Navigation stack for this robot and perform SLAM and AMCL to move the robot autonomously. This chapter aims to give you an idea about building a custom mobile robot and interfacing it on ROS. We will cover the following topics in this chapter: Introduction to Chefbot a DIY autonomous mobile robot Developing base controller and odometry nodes for Chefbot in ROS Configuring the Navigation stack for Chefbot and understanding AMCL Working with the Chefbot simulation Interacting with the Navigation stack with a ROS node

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

The first topic we are going to discuss in this chapter is how to build a DIY (Do It Yourself) autonomous mobile robot, develop its firmware, and interface it to the ROS Navigation stack. The robot, called Chefbot, was built as part of a book called Learning Robotics using Python by Josep Lentin and published by Packt Publishing (IUUQMFBSOSPCPUJDTDPN). The step-by-step procedure to build this robot is discussed in that book. In this chapter, we will cover abstract information about this robot hardware, and we will learn more about configuring the ROS Navigation stack and its fine tuning for performing autonomous navigation using SLAM and AMCL. We have already discussed the ROS Navigation stack in $IBQUFS, Using the ROS MoveIt! and Navigation Stack, and we have simulated a differential robot using Gazebo and performed SLAM and AMCL. In the first part of the chapter, the Chefbot hardware is required to follow the tutorials. However, the concepts discussed in the first part of the chapter are then applied to a simulated robot in the second part of the chapter.

Introducing to Chefbot ` a DIY mobile robot and its hardware configuration In $IBQUFS, Using the ROS MoveIt! and Navigation Stack, we have discussed some mandatory requirements for interfacing a mobile robot with the ROS Navigation package. These requirements are recalled in the following: Odometry source: The robot should publish its odometry/position data with respect to the starting position. The necessary hardware components that provide odometry information are wheel encoders, IMU, or 2D/3D cameras (visual odometry). Sensor source: There should be a laser scanner, or a vision sensor that can act as a laser scanner. The laser scanner data is essential for the map-building process using SLAM. Sensor transform using tf: The robot should publish the transform of the sensors and other robot components using ROS transform. Base controller: The base controller is an ROS node, which can convert a twist message from the Navigation stack to corresponding motor velocities:

[ 350 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

(KIWTG%JGHDQVRTQVQV[RG

We can check the components present in the robot and determine whether they satisfy the Navigation stack requirements. The following components are present in the robot: Pololu DC Gear motor with Quadrature encoder (IUUQTXXXQPMPMVDPN QSPEVDU): The motor is operated in 12V, 80 RPM, and an 18 kg-cm torque. It takes a current of 300 mA in free run and a 5 A in-stall condition. The motor shaft is attached to a quadrature encoder, which can deliver a maximum count of 8,400 counts per revolution of the gearbox's output shaft. Motor encoders are one source of odometry of robot. Pololu motor drivers (IUUQTXXXQPMPMVDPNQSPEVDU): These are dual motor controllers for Pololu motors that can support up to 30 A and motor voltage from 5.5V to 16V.

[ 351 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

Tiva C Launchpad Controller (IUUQXXXUJDPNUPPMFLUNDHYM): This robot has a Tiva C LaunchPad controller for interfacing motors, encoders, sensors, and so on. Also, it can receive control commands from the PC and can send appropriate signals to the motors according to the command. This board can act as an embedded controller board of the robot. Tiva C LaunchPad board runs on 80 MHz. MPU 6050 IMU: The IMU used in this robot is MPU 6050, which is a combination of accelerometer, gyroscope, and Digital Motion Processer (DMP). This motion processor can run sensor fusion algorithms onboard and can provide accurate results of roll, pitch, and yaw. The IMU values can be taken to calculate the odometry along with the wheel encoders. Xbox Kinect/Asus Xtion Pro: These are 3D vision sensors and we can use these sensors to mock a laser scanner. The point cloud generated from these sensors can be converted into laser scan data and used in the Navigation stack. Intel NUC PC: This is a mini PC from Intel, and we have to load this with Ubuntu and ROS. The PC is connected to Kinect and LaunchPad to retrieve the sensor values and the odometry details. The program running on the PC can compute TF of the robot and run the Navigation stack and associated packages, such as SLAM and AMCL. This PC is placed within the robot itself. We can check from the aforementioned hardware list that all the requirements needed by the ROS Navigation package are satisfied. The following figure shows the block diagram of this robot:

(KIWTG$NQEMFKCITCOQH%JGHDQV

[ 352 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

In this robot, the embedded controller board is the Tiva C LaunchPad. All the sensors and actuators are connected to the controller board, and it is connected to the Intel NUC PC for receiving higher-level commands. The board and the PC communicate in the UART protocol, IMU, and the board communicates using I2C. Kinect is interfaced to the PC via USB, and all the other sensors are interfaced through GPIO pins. A detailed connection diagram of the robot components follows:

(KIWTG%QPPGEVKQPFKCITCOQH%JGHDQV

Flashing Chefbot firmware using Energia IDE After developing the preceding connections, we can program the Launchpad using the Energia IDE (IUUQFOFSHJBOV). After setting Energia IDE on the PC (Ubuntu is preferred), we can flash the robot firmware to the board. We will get the firmware code and the ROS interface package by using the following command: $ git clone https://github.com/qboticslabs/Chefbot_ROS_pkg

[ 353 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

The folder contains a folder called UJWB@D@FOFSHJB@DPEF, which has the firmware code that flashes to the board after compilation in Energia IDE. The firmware can read the encoder, ultrasonic sensor, and IMU values, and can receive values of the motor velocity command. The important section of the firmware is discussed here. The programming language in the LaunchPad is the same as Arduino. Here we are using Energia IDE to program the controller, which is built using the Arduino IDE. The following code snippet is the TFUVQ  function definition of the code. This function starts serial communication with a baud rate of 115,200. It also configures the pins of the motor encoder, motor driver pins, ultrasonic distance sensor, and the IMU. Also, through this code, we are configuring a pin to reset the LaunchPad: WPJETFUVQ  \ *OJU4FSJBMQPSUXJUICBVESBUF 4FSJBMCFHJO  4FUVQ&ODPEFST 4FUVQ&ODPEFST  4FUVQ.PUPST 4FUVQ.PUPST  4FUVQ6MUSBTPOJD 4FUVQ6MUSBTPOJD  4FUVQ.16 [email protected]  4FUVQ3FTFUQJOT 4FUVQ3FTFU  4FUVQ.FTTFOHFS .FTTFOHFS@)BOEMFSBUUBDI 0O.TTBHF$PNQMFUFE ^

In the MPPQ  function, the sensor values are continuously polled and the data is sent through a serial port and incoming serial data is continuously polled for getting the robot commands. The following convention protocols are used to send each sensor value from the LaunchPad to the PC using serial communication (UART):

[ 354 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

Serial data sending protocol from LaunchPad to PC For the encoder, the protocol will be as follows: FTQBDF MFGU@FODPEFS@UJDLT TQBDF SJHIU@FODPEFS@UJDLT

For the ultrasonic sensor, the protocol will be as follows: VTQBDF EJTUBODF@JO@DFOUJNFUFS

For IMU, the protocol will be as follows: JTQBDF WBMVF@PG@Y@RVBUFSOJPO TQBDF WBMVF@PG@Z@RVBUFSOJPO TQBDF WBMVF@PG@[@RVBUFSOJPO TQBDF WBMVF@PG@X@RVBUFSOJPO

Serial data sending protocol from PC to Launchpad For the motor, the protocol will be as follows: TTQBDF QXN@WBMVF@PG@NPUPS@ TQBDF QXN@WBMVF@PG@NPUPS@

For resetting the device, the protocol will be as follows: STQBDF

We can check the serial values from the LaunchPad using a command-line tool called NJOJUFSNQZ. This tool can view the serial data coming from a device. This script is already installed with the QZUIPOTFSJBM package, which is installed along with the SPTTFSJBMQZUIPO Debian package. The following command will display the serial values from the robot controller: $ miniterm.py /dev/ttyACM0 115200

[ 355 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

We will get values such as these shown in the following screenshot:

NJOJUFSNQZ

(KIWTG%JGEMKPIUGTKCNFCVCWUKPI

Discussing Chefbot interface packages on ROS After confirming the serial values from the board, we can install the Chefbot ROS package. The Chefbot package contains the following files and folders: DIFGCPU@CSJOHVQ: This package contains Python scripts, C++ nodes, and launch files to start publishing robot odometry and UG, and performing gmapping and

AMCL. It contains the Python/C++ nodes to read/write values from the LaunchPad, convert the encoder ticks to UG, and twist messages to motor commands. It also has the PID node for handling velocity commands from the motor commands. DIFGCPU@EFTDSJQUJPO: This package contains the Chefbot URDF model.

[ 356 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

DIFGCPU@TJNVMBUPS: This package contains launch files to simulate the robot in

Gazebo. DIFGCPU@OBWJH@DQQ: This package contains the C++ implementation of a few nodes that are already implemented in DIFGCPU@CSJOHVQ as the Python node. After cloning the Chefbot main directory from the previous GitHub repository, we could compile the workspace. To successfully compile the packages of the repository, some dependencies must be installed using the following command: $ sudo apt-get install ros-kinetic-depthimage-to-laserscan ros-kinetickobuki-gazebo-plugins ros-kinetic-robot-pose-ekf ros-kinetic-yocs-cmd-velmux ros-kinetic-move-base-msgs ros-kinetic-openni-launch ros-kinetickobuki-description ros-kinetic-gmapping ros-kinetic-amcl ros-kinetic-mapserver

The following launch file will start the robot odometry and the UG publishing nodes: $ roslaunch chefbot_bringup robot_standalone.launch

In the next figure, the nodes started by this launch file and how they are interconnected are shown:

(KIWTG+PVGTEQPPGEVKQPQHGCEJPQFGKP%JGHDQV

[ 357 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

The nodes run by this launch file and their workings are described next: MBVODIQBE@OPEFQZ: We know that this robot uses the Tiva C LaunchPad board

as its controller. This node acts as a bridge between the robot controller and the ROS. The basic functionality of this node is to receive serial values from the LaunchPad and convert each sensor data into ROS topics. This acts as the ROS driver for the LaunchPad board. UXJTU@UP@NPUPSTQZ: This node converts the HFPNFUSZ@NTHT5XJTU message to motor velocity targets. It subscribes the command velocity, which is either from a teleop node or from an ROS Navigation stack, and publishes MXIFFM@WUBSHFU and SXIFFM@WUBSHFU. QJE@WFMPDJUZQZ: This node subscribes XIFFM@WUBSHFU from the UXJTU@UP@NPUPST node and the wheel topic, which is the encoder tick from the MBVODIQBE@OPEF. We have to start two PID nodes for each wheel of the robot, as shown in the previous figure. This node finally generates the motor speed commands for each motor. EJGG@UGQZ: This node subscribes the encoder ticks from the two motors, and computes odometry, and publishes UG for the Navigation stack. The list of topics generated after running SPCPU@TUBOEBMPOFMBVODI are shown in the following image:

(KIWTG.KUVQHVQRKEUIGPGTCVGFYJGPGZGEWVKPI

SPCPU@TUBOEBMPOFMBVODI

[ 358 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

The following is the content of the SPCPU@TUBOEBMPOFMBVODI file: MBVODI BSHOBNFTJNVMBUJPOEFGBVMU PQUFOW5635-@4*.6-"5*0/GBMTF QBSBNOBNFVTF@TJN@UJNFWBMVF BSHTJNVMBUJPO 63%'SPCPUNPEFM BSHOBNFVSEG@GJMFEFGBVMU GJOEYBDSPYBDSPQZ  GJOE DIFGCPU@EFTDSJQUJPOVSEGDIFGCPU@CBTFYBDSP  QBSBNOBNFSPCPU@EFTDSJQUJPODPNNBOE BSHVSEG@GJMF JNQPSUBOUHFOFSBMMZCVUTQFDJGJDBMMZVUJMJTFECZUIFDVSSFOUBQQ NBOBHFS QBSBNOBNFSPCPUOBNFWBMVF PQUFOW30#05UVSUMFCPU QBSBNOBNFSPCPUUZQFWBMVFUVSUMFCPU 4UBSUJOHSPCPUTUBUFQVCMJTIFS OPEFQLHSPCPU@TUBUF@QVCMJTIFSUZQFSPCPU@TUBUF@QVCMJTIFS OBNFSPCPU@TUBUF@QVCMJTIFS QBSBNOBNFQVCMJTI@GSFRVFODZUZQFEPVCMFWBMVF OPEF 3PCPUQBSBNFUFST SPTQBSBNQBSBNCBTF@XJEUI SPTQBSBN SPTQBSBNQBSBNUJDLT@NFUFS SPTQBSBN 4UBSUJOHMBVODIQBE@OPEF OPEFOBNFMBVODIQBE@OPEFQLHDIFGCPU@CSJOHVQ UZQFMBVODIQBE@OPEFQZ SPTQBSBNGJMF GJOEDIFGCPU@CSJOHVQQBSBNTFSJBMZBNM DPNNBOEMPBE OPEF 1*%OPEFGPSMFGUNPUPSTFUUJOH1*%QBSBNFUFST OPEFOBNFMQJE@WFMPDJUZQLHDIFGCPU@CSJOHVQUZQFQJE@WFMPDJUZQZ PVUQVUTDSFFO SFNBQGSPNXIFFMUPMXIFFM SFNBQGSPNNPUPS@DNEUPMFGU@XIFFM@TQFFE SFNBQGSPNXIFFM@WUBSHFUUPMXIFFM@WUBSHFU SFNBQGSPNXIFFM@WFMUPMXIFFM@WFM SPTQBSBNQBSBN,Q SPTQBSBN SPTQBSBNQBSBN,J SPTQBSBN SPTQBSBNQBSBN,E SPTQBSBN SPTQBSBNQBSBNPVU@NJO SPTQBSBN SPTQBSBNQBSBNPVU@NBY SPTQBSBN SPTQBSBNQBSBNSBUF SPTQBSBN SPTQBSBNQBSBNUJNFPVU@UJDLT SPTQBSBN

[ 359 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

SPTQBSBNQBSBNSPMMJOH@QUT SPTQBSBN OPEF 1*%OPEFGPSSJHIUNPUPSTFUUJOH1*%QBSBNFUFST OPEFOBNFSQJE@WFMPDJUZQLHDIFGCPU@CSJOHVQUZQFQJE@WFMPDJUZQZ PVUQVUTDSFFO SFNBQGSPNXIFFMUPSXIFFM SFNBQGSPNNPUPS@DNEUPSJHIU@XIFFM@TQFFE SFNBQGSPNXIFFM@WUBSHFUUPSXIFFM@WUBSHFU SFNBQGSPNXIFFM@WFMUPSXIFFM@WFM SPTQBSBNQBSBN,Q SPTQBSBN SPTQBSBNQBSBN,J SPTQBSBN SPTQBSBNQBSBN,E SPTQBSBN SPTQBSBNQBSBNPVU@NJO SPTQBSBN SPTQBSBNQBSBNPVU@NBY SPTQBSBN SPTQBSBNQBSBNSBUF SPTQBSBN SPTQBSBNQBSBNUJNFPVU@UJDLT SPTQBSBN SPTQBSBNQBSBNSPMMJOH@QUT SPTQBSBN OPEF 4UBSUJOHUXJTUUPNPUPSBOEEJGG@UGOPEFT OPEFQLHDIFGCPU@CSJOHVQUZQFUXJTU@UP@NPUPSTQZ OBNFUXJTU@UP@NPUPSTPVUQVUTDSFFO OPEFQLHDIFGCPU@CSJOHVQUZQFEJGG@UGQZOBNFEJGG@UG PVUQVUTDSFFO MBVODI

After running SPCPU@TUBOEBMPOFMBVODI, we can visualize the robot in RViz, using the following command: $ roslaunch chefbot_bringup view_robot.launch

[ 360 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

We will see the robot model, as shown in this next screenshot:

(KIWTG8KUWCNK\CVKQPQHTQDQVOQFGNWUKPITGCNTQDQVXCNWGU

Launch the keyboard UFMFPQ node and we can start moving the robot: $ roslaunch chefbot_bringup keyboard_teleop.launch

[ 361 ]

Chapter 11

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

Move the robot using the keys and we will see that the robot is moving around. If we enable UG of the robot in RViz, we can view the odometry, as shown in the following screenshot:

(KIWTG8KUWCNK\KPITQDQVQFQOGVT[

The graph of the connections between each node can be viewed, using the SRU@HSBQI tool: $ rqt_graph

Until now we have discussed the Chefbot interfacing on ROS. The coding of Chefbot is completely done in Python. There are some nodes implemented in C++ for computing odometry from the encoder ticks and generating motor speed commands from the twist messages.

[ 362 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

Computing odometry from encoder ticks In this section, we will see the C++ interpretation of the EJGG@UGQZ node, which subscribes the encoder data and computes the odometry, and publishes the odometry and UG of the robot. We can see the C++ interpretation of this node, called EJGG@UGDQQ, which can be found in the TSD folder of a package named DIFGCPU@OBWJH@DQQ. Discussed next are the important code snippets of this code and their explanations. The following code snippet is the constructor of the 0EPNFUSZ@DBMDclass. This class contains the definition of computing odometry. The following code declares the subscriber for the left and right wheel encoders along with the publisher for the PEPN value: 0EPNFUSZ@DBMD0EPNFUSZ@DBMD \ *OJUJBMJ[FWBSJBCMFTVTFEJOUIFOPEF JOJU@WBSJBCMFT  304@*/'0 4UBSUFEPEPNFUSZDPNQVUJOHOPEF 4VCTDSJCJOHMFGUBOESJHIUXIFFMFODPEFSWBMVFT M@XIFFM@TVCOTVCTDSJCF MXIFFM0EPNFUSZ@DBMDMFGUFODPEFS$C UIJT S@XIFFM@TVCOTVCTDSJCF SXIFFM0EPNFUSZ@DBMDSJHIUFODPEFS$C UIJT $SFBUJOHBQVCMJTIFSGPSPEPN PEPN@QVCOBEWFSUJTFOBW@NTHT0EPNFUSZ PEPN 3FUSJFWJOHQBSBNFUFSTPGUIJTOPEF HFU@OPEF@QBSBNT  ^

The following code is the update loop of computing odometry. It computes the delta distance moved and the angle rotated by the robot using the encoder values, base width of the robot, and ticks per meter of the encoder. After calculating the delta distance and the delta theta, we can compute the final Y, Z, and theta using the standard differential drive robot equations: JG OPX U@OFYU\ FMBQTFEOPXUP4FD UIFOUP4FD  JG FOD@MFGU\ E@MFGU E@SJHIU

[ 363 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

^ FMTF\ E@MFGU MFGUFOD@MFGU UJDLT@NFUFS E@SJHIU SJHIUFOD@SJHIU UJDLT@NFUFS ^ FOD@MFGUMFGU FOD@SJHIUSJHIU E E@MFGU E@SJHIU UI E@SJHIUE@MFGUCBTF@XJEUI EYEFMBQTFE ESUIFMBQTFE JG E\ YDPT UI E ZTJO UI E DBMDVMBUFUIFGJOBMQPTJUJPOPGUIFSPCPU Y@GJOBMY@GJOBM  DPT UIFUB@GJOBM YTJO

UIFUB@GJOBM Z Z@GJOBMZ@GJOBM  TJO UIFUB@GJOBM Y DPT

UIFUB@GJOBM Z ^ JG UI UIFUB@GJOBMUIFUB@GJOBM UI

After computing the robot position and the orientation from the preceding code snippet, we can feed the PEPN values to the PEPN message header and the UG header, which will publish the topics in PEPN and UG: HFPNFUSZ@NTHT2VBUFSOJPOPEPN@RVBU PEPN@RVBUY PEPN@RVBUZ PEPN@RVBU[ PEPN@RVBU[TJO UIFUB@GJOBM PEPN@RVBUXDPT UIFUB@GJOBM GJSTUXF MMQVCMJTIUIFUSBOTGPSNPWFSUG HFPNFUSZ@NTHT5SBOTGPSN4UBNQFEPEPN@USBOT PEPN@USBOTIFBEFSTUBNQOPX PEPN@USBOTIFBEFSGSBNF@JEPEPN

[ 364 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

PEPN@USBOTDIJME@GSBNF@JECBTF@GPPUQSJOU PEPN@USBOTUSBOTGPSNUSBOTMBUJPOYY@GJOBM PEPN@USBOTUSBOTGPSNUSBOTMBUJPOZZ@GJOBM PEPN@USBOTUSBOTGPSNUSBOTMBUJPO[ PEPN@USBOTUSBOTGPSNSPUBUJPOPEPN@RVBU TFOEUIFUSBOTGPSN PEPN@CSPBEDBTUFSTFOE5SBOTGPSN PEPN@USBOT OFYUXF MMQVCMJTIUIFPEPNFUSZNFTTBHFPWFS304 OBW@NTHT0EPNFUSZPEPN PEPNIFBEFSTUBNQOPX PEPNIFBEFSGSBNF@JEPEPN TFUUIFQPTJUJPO PEPNQPTFQPTFQPTJUJPOYY@GJOBM PEPNQPTFQPTFQPTJUJPOZZ@GJOBM PEPNQPTFQPTFQPTJUJPO[ PEPNQPTFQPTFPSJFOUBUJPOPEPN@RVBU TFUUIFWFMPDJUZ PEPNDIJME@GSBNF@JECBTF@GPPUQSJOU PEPNUXJTUUXJTUMJOFBSYEY PEPNUXJTUUXJTUMJOFBSZ PEPNUXJTUUXJTUBOHVMBS[ES QVCMJTIUIFNFTTBHF PEPN@QVCQVCMJTI PEPN

Computing motor velocities from ROS twist message The C++ implementation of UXJTU@UP@NPUPSQZ is discussed in this section. This node will convert the twist message (HFPNFUSZ@NTHT5XJTU) to motor target velocities. The topic subscribed by this node is the twist message from the UFMFPQ node or Navigation stack and it publishes the target velocities for the two motors. The target velocities are fed into the PID nodes, which will send appropriate commands to each motor. The CPP filename is UXJTU@UP@NPUPSDQQ, and you can get it from the DIFGCPU@OBWJH@DQQTSD folder: 5XJTU5P.PUPST5XJTU5P.PUPST  \ JOJU@WBSJBCMFT  HFU@QBSBNFUFST 

[ 365 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

304@*/'0 4UBSUFE5XJTUUP.PUPSOPEF DNE@WFM@TVCOTVCTDSJCF DNE@WFM@NVYJOQVUUFMFPQ 5XJTU5P.PUPSTUXJTU$BMMCBDLUIJT QVC@MNPUPSOBEWFSUJTFTUE@NTHT'MPBU MXIFFM@WUBSHFU QVC@SNPUPSOBEWFSUJTFTUE@NTHT'MPBU SXIFFM@WUBSHFU ^

The following code snippet is the callback function of the twist message. The linear velocity 9 is assigned as EY, : as EZ, and angular velocity ; as ES: WPJE5XJTU5P.PUPSTUXJTU$BMMCBDL DPOTUHFPNFUSZ@NTHT5XJTUNTH \ UJDLT@TJODF@UBSHFU EYNTHMJOFBSY EZNTHMJOFBSZ ESNTHBOHVMBS[ ^

After getting EY, EZ, and ES, we can compute the motor velocities using the following equations: EY M S ES SMX

Here, r and l are the right and left wheel velocities, and w is the base width. The preceding equations are implemented in the following code snippet. After computing the wheel velocities, they are published to the MXIFFM@WUBSHFU and SXIFFM@WUBSHFU topics: SJHIU  EY  ES X MFGU  EY ES X TUE@NTHT'MPBUMFGU@ TUE@NTHT'MPBUSJHIU@ MFGU@EBUBMFGU SJHIU@EBUBSJHIU

QVC@MNPUPSQVCMJTI MFGU@ QVC@SNPUPSQVCMJTI SJHIU@ UJDLT@TJODF@UBSHFU  SPTTQJO0ODF 

[ 366 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

Running the robot standalone launch file using C++ nodes The following command can launch SPCPU@TUBOE@BMPOFMBVODI, which uses the C++ nodes: $ roslaunch chefbot_navig_cpp robot_standalone.launch

Configuring the Navigation stack for Chefbot After setting the odometry nodes, the base controller node, and the PID nodes, we need to configure the Navigation stack to perform SLAM and Adaptive Monte Carlo Localization (AMCL) for building the map, localizing the robot, and performing autonomous navigation. In $IBQUFS, Using the ROS MoveIt! and Navigation Stack, we have seen the basic packages in the Navigation stack. To build the map of the environment, we need to configure mainly two nodes: the HNBQQJOH node for performing SLAM and the NPWF@CBTF node. We also need to configure the global planner, the local planner, the global cost map, and the local cost map inside the Navigation stack. Let's look at the configuration of the HNBQQJOH node first.

Configuring the gmapping node The gmapping node is the package to perform SLAM (IUUQXJLJSPTPSHHNBQQJOH). The HNBQQJOH node inside this package mainly subscribes and publishes the following topics. The following are the subscribed topics: UG (UGUG.FTTBHF): The robot transform that relates to Kinect, robot base, and

odometry TDBO (TFOTPS@NTHT-BTFS4DBO): The laser scan data that is required to create the map The following are the published topics: NBQ (OBW@NTHT0DDVQBODZ(SJE): Publishes the occupancy grid map data NBQ@NFUBEBUB (OBW@NTHT.BQ.FUB%BUB): Basic information about the

occupancy grid

[ 367 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

The HNBQQJOH node is highly configurable, using various parameters. The HNBQQJOH node parameters are defined inside the DIFGCPU@CSJOHVQMBVODIJODMVEFHNBQQJOHMBVODIYNM file. The following is a code snippet of this file and its uses: MBVODI BSHOBNFTDBO@UPQJDEFGBVMUTDBO 4UBSUJOHHNBQQJOHOPEF OPEFQLHHNBQQJOHUZQFTMBN@HNBQQJOHOBNFTMBN@HNBQQJOH PVUQVUTDSFFO 'SBNFPGNPCJMFCBTF QBSBNOBNFCBTF@GSBNFWBMVFCBTF@GPPUQSJOU QBSBNOBNFPEPN@GSBNFWBMVFPEPN 5IFJOUFSWBMPGNBQVQEBUJPOSFEVDJOHUIJTWBMVFXJMMTQFFEPGNBQ HFOFSBUJPOCVUJODSFBTFDPNQVUBUJPOMPBE QBSBNOBNFNBQ@VQEBUF@JOUFSWBMWBMVF .BYJNVNVTBCMFSBOHFPGMBTFSLJOFDU QBSBNOBNFNBY6SBOHFWBMVF .BYJNVNSBOHFPGTFOTPSNBYSBOHFTIPVMECF NBY6SBOHF QBSBNOBNFNBY3BOHFWBMVF QBSBNOBNFTJHNBWBMVF QBSBNOBNFLFSOFM4J[FWBMVF OPEF MBVODI

By fine tuning these parameters, we improve the accuracy of the gmapping node. The main HNBQQJOH launch file is given next. It is placed in DIFGCPU@CSJOHVQMBVODIJODMVEFTHNBQQJOH@EFNPMBVODI. This launch file launches the PQFOOJ@MBVODI file and the EFQUI@UP@MBTFSTDBO node to convert the depth image to the laser scan. After launching the Kinect nodes, it launches the HNBQQJOH node and the NPWF@CBTF configurations: MBVODI -BVODIFT%TFOTPSOPEFT JODMVEFGJMF GJOEDIFGCPU@CSJOHVQMBVODIETFOTPSMBVODI BSHOBNFSHC@QSPDFTTJOHWBMVFGBMTF BSHOBNFEFQUI@SFHJTUSBUJPOWBMVFGBMTF BSHOBNFEFQUI@QSPDFTTJOHWBMVFGBMTF BSHOBNFTDBO@UPQJDWBMVFTDBO JODMVEF 4UBSUHNBQQJOHOPEFTBOEJUTDPOGJHVSBUJPOT JODMVEFGJMF GJOE

[ 368 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

DIFGCPU@CSJOHVQMBVODIJODMVEFTHNBQQJOHMBVODIYNM 4UBSUNPWF@CBTFOPEFBOEJUTDPOGJHVSBUJPO JODMVEFGJMF GJOE DIFGCPU@CSJOHVQMBVODIJODMVEFTNPWF@CBTFMBVODIYNM MBVODI

Configuring the Navigation stack packages The next node we need to configure is NPWF@CBTF. Along with the NPWF@CBTF node, we need to configure the global and the local planners, and also the global and the local cost maps. We will first look at the launch file to load all these configuration files. The following launch file, DIFGCPU@CSJOHVQMBVODIJODMVEFTNPWF@CBTFMBVODIYNM, will load all the parameters of NPWF@CBTF, QMBOOFS, and DPTUNBQ: MBVODI BSHOBNFPEPN@UPQJDEFGBVMUPEPN 4UBSUJOHNPWF@CBTFOPEF OPEFQLHNPWF@CBTFUZQFNPWF@CBTFSFTQBXOGBMTFOBNFNPWF@CBTF PVUQVUTDSFFO DPNNPOQBSBNFUFSTPGHMPCBMDPTUNBQ SPTQBSBNGJMF GJOE DIFGCPU@CSJOHVQQBSBNDPTUNBQ@DPNNPO@QBSBNTZBNMDPNNBOEMPBE OTHMPCBM@DPTUNBQ DPNNPOQBSBNFUFSTPGMPDBMDPTUNBQ SPTQBSBNGJMF GJOE DIFGCPU@CSJOHVQQBSBNDPTUNBQ@DPNNPO@QBSBNTZBNMDPNNBOEMPBE OTMPDBM@DPTUNBQ MPDBMDPTUNBQQBSBNFUFST SPTQBSBNGJMF GJOE DIFGCPU@CSJOHVQQBSBNMPDBM@DPTUNBQ@QBSBNTZBNMDPNNBOEMPBE HMPCBMDPTUNBQQBSBNFUFST SPTQBSBNGJMF GJOE DIFGCPU@CSJOHVQQBSBNHMPCBM@DPTUNBQ@QBSBNTZBNMDPNNBOEMPBE CBTFMPDBMQMBOOFSQBSBNFUFST SPTQBSBNGJMF GJOE DIFGCPU@CSJOHVQQBSBNCBTF@MPDBM@QMBOOFS@QBSBNTZBNMDPNNBOEMPBE EXBMPDBMQMBOOFSQBSBNFUFST SPTQBSBNGJMF GJOE

[ 369 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

DIFGCPU@CSJOHVQQBSBNEXB@MPDBM@QMBOOFS@QBSBNTZBNMDPNNBOEMPBE NPWF@CBTFOPEFQBSBNFUFST SPTQBSBNGJMF GJOEDIFGCPU@CSJOHVQQBSBNNPWF@CBTF@QBSBNTZBNM DPNNBOEMPBE SFNBQGSPNDNE@WFMUPDNE@WFM@NVYJOQVUOBWJ SFNBQGSPNPEPNUP BSHPEPN@UPQJD OPEF MBVODI

We will now take a look at each configuration file and its parameters.

Common configuration local_costmap and global_costmap The common parameters of the local and the global DPTUNBQ are discussed in this section. The DPTUNBQ is created using the obstacles present around the robot. Fine tuning the parameters of the DPTUNBQ can increase the accuracy of map generation. The customized file DPTUNBQ@DPNNPO@QBSBNTZBNM of Chefbot follows. This configuration file contains the common parameters of both the global and the local cost maps. It is present in the DIFGCPU@CSJOHVQQBSBN folder. For more about DPTUNBQ common parameters, check IUUQXJLJSPTPSHDPTUNBQ@EGMBU: 5IFNBYJNVNWBMVFPGIFJHIUXIJDIIBTUPCFUBLFOBTBOPCTUBDMF NBY@PCTUBDMF@IFJHIU 5IJTQBSBNFUFSTTFUUIFNBYJNVNPCTUBDMFSBOHF*OUIJTDBTFUIFSPCPU XJMMPOMZMPPLBUPCTUBDMFTXJUIJONFUFSTJOGSPOUPGSPCPU PCTUBDMF@SBOHF 5IJTQBSBNFUFSIFMQTSPCPUUPDMFBSPVUTQBDFJOGSPOUPGJUVQUP NFUFSTBXBZHJWFOBTFOTPSSFBEJOH SBZUSBDF@SBOHF *GUIFSPCPUJTDJSDVMBSXFDBOEFGJOFUIFSPCPUSBEJVTPUIFSXJTFXF OFFEUPNFOUJPOUIFSPCPU GPPUQSJOU SPCPU@SBEJVT GPPUQSJOU 5IJTQBSBNFUFSXJMMBDUVBMMZJOGMBUFUIFPCTUBDMFVQUPUIJTEJTUBODFGSPN UIFBDUVBMPCTUBDMF5IJTDBOCFUBLFOBTBUPMFSBODFWBMVFPGPCTUBDMF 5IFDPTUPGNBQXJMMCFTBNFBTUIFBDUVBMPCTUBDMFVQUPUIFJOGMBUFE WBMVF JOGMBUJPO@SBEJVT 5IJTGBDUPSJTVTFEGPSDPNQVUJOHDPTUEVSJOHJOGMBUJPO DPTU@TDBMJOH@GBDUPS

[ 370 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

8FDBOFJUIFSDIPPTFNBQUZQFBTWPYFMXIJDIXJMMHJWFB%WJFXPGUIF XPSMEPSUIFPUIFSUZQFDPTUNBQXIJDIJTB%WJFXPGUIFNBQ)FSFXF BSFPQUJOHWPYFM NBQ@UZQFWPYFM 5IJTJTUIF[@PSJHJOPGUIFNBQJGJUWPYFM PSJHJO@[ [SFTPMVUJPOPGNBQJONFUFST [@SFTPMVUJPO /PPGWPYFMJOBWFSUJDBMDPMVNO [@WPYFMT 5IJTGMBHTFUXIFUIFSXFOFFENBQGPSWJTVBMJ[BUJPOQVSQPTF QVCMJTI@WPYFM@NBQGBMTF "MJTUPGPCTFSWBUJPOTPVSDFJOXIJDIXFHFUTDBOEBUBBOEJUTQBSBNFUFST PCTFSWBUJPO@TPVSDFTTDBO 5IFMJTUPGTDBOXIJDINFOUJPOEBUBUZQFPGTDBOBT-BTFS4DBONBSLJOH BOEDMFBSJOHJOEJDBUFXIFUIFSUIFMBTFSEBUBJTVTFEGPSNBSLJOHBOE DMFBSJOHDPTUNBQ TDBO\EBUB@UZQF-BTFS4DBOUPQJDTDBONBSLJOHUSVFDMFBSJOHUSVF NJO@PCTUBDMF@IFJHIUNBY@PCTUBDMF@IFJHIU^

After discussing the common parameters, we will now look at the HMPCBM@DPTUNBQ configuration.

Configuring global costmap parameters The following are the main configurations required for building a global DPTUNBQ The definition of the DPTUNBQ parameters is dumped in DIFGCPU@CSJOHVQQBSBN HMPCBM@DPTUNBQ@QBSBNTZBNM. The following is the definition of this file and its uses: HMPCBM@DPTUNBQ HMPCBM@GSBNFNBQ SPCPU@CBTF@GSBNFCBTF@GPPUQSJOU VQEBUF@GSFRVFODZ QVCMJTI@GSFRVFODZ TUBUJD@NBQUSVF USBOTGPSN@UPMFSBODF

[ 371 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

The HMPCBM@GSBNF here is NBQ, which is the coordinate frame of the DPTUNBQ. The SPCPU@CBTF@GSBNF parameter is CBTF@GPPUQSJOU; it is the coordinate frame in which the DPTUNBQ should reference as the robot base. The VQEBUF@GSFRVFODZ is the frequency at which the DPTUNBQ runs its main update loop. The QVCMJTIJOH@GSFRVFODZ of the costmap is given as QVCMJTI@GSFRVFODZ, which is . If we are using an existing map, we have to set TUBUJD@NBQ as USVF, otherwise we set it as GBMTF. The USBOTGPSN@UPMFSBODF is the rate at which the transform has to perform. The robot would stop if the transforms are not updated at this rate.

Configuring local costmap parameters The following is the local DPTUNBQ configuration of this robot. The configuration of this file is located in DIFGCPU@CSJOHVQQBSBNMPDBM@DPTUNBQ@QBSBNTZBNM: MPDBM@DPTUNBQ HMPCBM@GSBNFPEPN SPCPU@CBTF@GSBNFCBTF@GPPUQSJOU VQEBUF@GSFRVFODZ QVCMJTI@GSFRVFODZ TUBUJD@NBQGBMTF SPMMJOH@XJOEPXUSVF XJEUI IFJHIU SFTPMVUJPO USBOTGPSN@UPMFSBODF

The HMPCBM@GSBNF, SPCPU@CBTF@GSBNF, QVCMJTI@GSFRVFODZ, and TUBUJD@NBQ are the same as the global DPTUNBQ. The SPMMJOH@XJOEPX parameter makes the DPTUNBQ centered around the robot. If we set this parameter to USVF, we will get a DPTUNBQ that is built centered around the robot. The width , height, and resolution parameters are the width, height, and resolution of the DPTUNBQ. The next step will be to configure the base local planner.

[ 372 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

Configuring base local planner parameters The main function of the base local planner is to compute the velocity commands from the goal sent from the ROS nodes. This file mainly contains the configurations related to velocity, acceleration, and so on. The base local planner configuration file of this robot is in DIFGCPU@CSJOHVQQBSBNCBTF@MPDBM@QMBOOFS@QBSBNTZBNM. The definition of this file is as follows: 5SBKFDUPSZ1MBOOFS304 3PCPU$POGJHVSBUJPO1BSBNFUFSTUIFTFBSFUIFWFMPDJUZMJNJUPGUIFSPCPU NBY@WFM@Y NJO@WFM@Y "OHVMBSWFMPDJUZMJNJU NBY@WFM@UIFUB NJO@WFM@UIFUB NJO@JO@QMBDF@WFM@UIFUB 5IFTFBSFUIFBDDFMFSBUJPOMJNJUTPGUIFSPCPU BDD@MJN@Y BDD@MJN@UIFUB (PBM5PMFSBODF1BSBNFUFST5IFUPMFSBODFPGSPCPUXIFOJUSFBDIUIFHPBM QPTJUJPO ZBX@HPBM@UPMFSBODF YZ@HPBM@UPMFSBODF 'PSXBSE4JNVMBUJPO1BSBNFUFST TJN@UJNF WY@TBNQMFT WUIFUB@TBNQMFT 5SBKFDUPSZ4DPSJOH1BSBNFUFST NFUFS@TDPSJOHUSVF QEJTU@TDBMF HEJTU@TDBMF PDDEJTU@TDBMF IFBEJOH@MPPLBIFBE EXBUSVF 0TDJMMBUJPO1SFWFOUJPO1BSBNFUFST PTDJMMBUJPO@SFTFU@EJTU %JGGFSFOUJBMESJWFSPCPUDPOGJHVSBUJPO*GUIFSPCPUJTIPMPOPNJD DPOGJHVSBUJPOTFUUPUSVFPUIFSWJDFTFUUPGBMTF$IFGCPUJTBOPO IPMPOPNJDSPCPU IPMPOPNJD@SPCPUGBMTF NBY@WFM@Z NJO@WFM@Z BDD@MJN@Z WZ@TBNQMFT

[ 373 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

Configuring DWA local planner parameters The DWA planner is another local planner in ROS. Its configuration is almost the same as the base local planner. It is located in DIFGCPU@CSJOHVQQBSBN EXB@MPDBM@QMBOOFS@QBSBNTZBNM. We can either use the base local planner or the DWA local planner for our robot.

Configuring move_base node parameters There are some configurations to the NPWF@CBTF node too. The NPWF@CBTF node configuration is placed in the QBSBN folder. The following is the definition of NPWF@CBTF@QBSBNTZBNM: 5IJTQBSBNFUFSEFUFSNJOFXIFUIFSUIFDPTUNBQOFFEUPTIVUEPXOXIFO NPWF@CBTFJOJOBDUJWFTUBUF TIVUEPXO@DPTUNBQTGBMTF 5IFSBUFBUXIJDINPWFCBTFSVOUIFVQEBUFMPPQBOETFOEUIFWFMPDJUZ DPNNBOET DPOUSPMMFS@GSFRVFODZ $POUSPMMFSXBJUUJNFGPSBWBMJEDPNNBOECFGPSFBTQBDFDMFBSJOH PQFSBUJPOT DPOUSPMMFS@QBUJFODF 5IFSBUFBUXIJDIUIFHMPCBMQMBOOJOHMPPQJTSVOOJOHJGJUJTQMBOOFS POMZQMBOXIFOBOFXHPBMJTSFDFJWFE QMBOOFS@GSFRVFODZ 1MBOOFSXBJUUJNFGPSGJOEJOHBWBMJEQBUICFGJSFUIFTQBDFDMFBSJOH PQFSBUJPOT QMBOOFS@QBUJFODF 5JNFBMMPXFEGPSPTDJMMBUJPOCFGPSFTUBSUJOHSPCPUSFDPWFSZPQFSBUJPOT PTDJMMBUJPO@UJNFPVU %JTUBODFUIBUSPCPUTIPVMENPWFUPCFDPOTJEFSFEXIJDIOPUCFPTDJMMBUJOH .PWJOHBCPWFUIJTEJTUBODFXJMMSFTFUUIFPTDJMMBUJPO@UJNFPVU PTDJMMBUJPO@EJTUBODF MPDBMQMBOOFSEFGBVMUJTUSBKFDUPSZSPMMPVU CBTF@MPDBM@QMBOOFSEXB@MPDBM@QMBOOFS%8"1MBOOFS304

We have discussed most of the parameters used in the Navigation stack, the HNBQQJOH node, and the NPWF@CBTF node. Now we can start running a HNBQQJOH demo for building the map. Start the robot's UG nodes and base controller nodes: $ roslaunch chefbot_bringup robot_standalone.launch

[ 374 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

Start the HNBQQJOH node by using the following command: $ roslaunch chefbot_bringup gmapping_demo.launch

The HNBQQJOH@EFNPMBVODI launches the OpenNI driver and the depth to laser scan nodes in order to stream the 3D sensor data, and launches the HNBQQJOH node and the NPWFCBTF node with the necessary parameters. We can now launch a UFMFPQ node for moving the robot to build the map of the environment. The following command will launch the UFMFPQ node for moving the robot: $ roslaunch chefbot_bringup keyboard_teleop.launch

We can see the map building in RViz, which can be invoked by using the following command: $ roslaunch chefbot_bringup view_navigation.launch

We are testing this robot in a plain room; we can move robot in all areas inside the room. If we move the robot in all the areas, we will get a map, as shown in the following screenshot:

(KIWTG%TGCVKPICOCRWUKPIIOCRRKPIKUUJQYPKP48K\

[ 375 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

After completing the mapping process, we can save the map by using the following command: $ rosrun map_server map_saver -f /home/lentin/room

The NBQ@TFSWFS package in ROS contains the NBQ@TFSWFS node, which provides the current map data as an ROS service. It provides a command utility called NBQ@TBWFS, which helps to save the map. It will save the current map as two files: SPPNQHN and SPPNZBNM. The first one is the map data and the next is its metadata, which contains the map file's name and its parameters. The following screenshot shows map generation using the NBQ@TFSWFS tool, which is saved in the home folder:

(KIWTG6GTOKPCNOGUUCIGUYJKNGUCXKPICOCR

The following is the SPPNZBNM: JNBHFSPPNQHN SFTPMVUJPO PSJHJO OFHBUF PDDVQJFE@UISFTI GSFF@UISFTI

The definition of each parameter follows: JNBHF: The image contains the occupancy data. The data can be absolute or

relative to the origin mentioned in the YAML file. SFTPMVUJPO: This parameter is the resolution of the map, which is meters/pixels. PSJHJO: This is the 2D pose of the lower left pixel in the map (x, y, yaw), where yaw is counter-clockwise (yaw = 0 means no rotation). OFHBUF: This parameter can reverse the semantics of white/black in the map and the free space/occupied space representation.

[ 376 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

PDDVQJFE@UISFTI: This is the threshold deciding whether the pixel is occupied

or not. If the occupancy probability is greater than this threshold, it is considered free space. GSFF@UISFTI: The map pixel with occupancy probability less than this threshold is considered completely occupied. After mapping the environment, we can quit all the terminals and rerun the following commands to start AMCL. Before starting the AMCL nodes, we will look at the configuration and the main application of AMCL.

Understanding AMCL After building a map of the environment, the next thing we need to implement is localization. The robot should localize itself on the generated map. We have worked with AMCL in $IBQUFS, Using the ROS MoveIt! and Navigation Stack. In this section, we will see a detailed study of the BNDM package and the BNDM launch files used in Chefbot. AMCL implements the probabilistic localization technique for robot working in 2D. This algorithm uses particle filters for tracking the pose of the robot with respect to the known map. To know more about this localization technique, you can refer to a book called Probabilistic Robotics by Thrun (IUUQXXXQSPCBCJMJTUJDSPCPUJDTPSH). The AMCL algorithm is implemented in the AMCL ROS package (IUUQXJLJSPTPSHBNDM), which has an AMCL node that subscribes the scan (TFOTPS@NTHT-BTFS4DBO), the tf (UGUG.FTTBHF), the initial pose (HFPNFUSZ@NTHT1PTF8JUI$PWBSJBODF4UBNQFE), and the map (OBW@NTHT0DDVQBODZ(SJE). After processing the sensor data, it publishes BNDM@QPTF

HFPNFUSZ@NTHT1PTF8JUI$PWBSJBODF4UBNQFE, QBSUJDMFDMPVE HFPNFUSZ@NTHT1PTF"SSBZ and UG UG.FTTBHF. BNDM@QPTF is the estimated pose of the robot after processing, where the particle cloud is the set of pose estimates maintained by the filter.

If the initial pose of the robot is not mentioned, the particle will be around the origin. We can set the initial pose of the robot in RViz by using the 2D Pose estimate button. We can see the BNDM launch file used in this robot. The following is the main launch file for starting BNDM, called BNDM@EFNPMBVODI: MBVODI SPTQBSBNDPNNBOEEFMFUFOTNPWF@CBTF JODMVEFGJMF GJOEDIFGCPU@CSJOHVQMBVODIETFOTPSMBVODI BSHOBNFSHC@QSPDFTTJOHWBMVFGBMTF

[ 377 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

BSHOBNFEFQUI@SFHJTUSBUJPOWBMVFGBMTF BSHOBNFEFQUI@QSPDFTTJOHWBMVFGBMTF 8FNVTUTQFDJGZBOBCTPMVUFUPQJDOBNFCFDBVTFJGOPUJUXJMMCF QSFGJYFECZ BSHDBNFSB BSHOBNFTDBO@UPQJDWBMVFTDBO JODMVEF .BQTFSWFS BSHOBNFNBQ@GJMFEFGBVMU GJOE UVSUMFCPU@OBWJHBUJPONBQTXJMMPXZBNM OPEFOBNFNBQ@TFSWFSQLHNBQ@TFSWFSUZQFNBQ@TFSWFSBSHT BSH NBQ@GJMF BSHOBNFJOJUJBM@QPTF@YEFGBVMU 6TFGPSXJMMPX TNBQ JOTJNVMBUJPO BSHOBNFJOJUJBM@QPTF@ZEFGBVMU 6TFGPSXJMMPX TNBQ JOTJNVMBUJPO BSHOBNFJOJUJBM@QPTF@BEFGBVMU JODMVEFGJMF GJOEDIFGCPU@CSJOHVQMBVODIJODMVEFTBNDMMBVODIYNM BSHOBNFJOJUJBM@QPTF@YWBMVF BSHJOJUJBM@QPTF@Y BSHOBNFJOJUJBM@QPTF@ZWBMVF BSHJOJUJBM@QPTF@Z BSHOBNFJOJUJBM@QPTF@BWBMVF BSHJOJUJBM@QPTF@B JODMVEF JODMVEFGJMF GJOE DIFGCPU@CSJOHVQMBVODIJODMVEFTNPWF@CBTFMBVODIYNM MBVODI

The preceding launch file starts the 3D sensor-related nodes, the map server for providing the map data, the BNDM node for performing localization, and the NPWF@CBTF node to move the robot from the commands received from the higher level. The complete BNDM launch parameters are mentioned inside another sub-file called BNDMMBVODIYNM. It is placed in DIFGCPU@CSJOHVQMBVODIJODMVEF. The following is the definition of this file: MBVODI BSHOBNFVTF@NBQ@UPQJDEFGBVMUGBMTF BSHOBNFTDBO@UPQJDEFGBVMUTDBO BSHOBNFJOJUJBM@QPTF@YEFGBVMU BSHOBNFJOJUJBM@QPTF@ZEFGBVMU BSHOBNFJOJUJBM@QPTF@BEFGBVMU OPEFQLHBNDMUZQFBNDMOBNFBNDM QBSBNOBNFVTF@NBQ@UPQJDWBMVF BSHVTF@NBQ@UPQJD

[ 378 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

 

*ODSFBTFUPMFSBODFCFDBVTFUIFDPNQVUFSDBOHFURVJUFCVTZ QBSBNOBNFUSBOTGPSN@UPMFSBODFWBMVF QBSBNOBNFSFDPWFSZ@BMQIB@TMPXWBMVF QBSBNOBNFSFDPWFSZ@BMQIB@GBTUWBMVF QBSBNOBNFJOJUJBM@QPTF@YWBMVF BSHJOJUJBM@QPTF@Y QBSBNOBNFJOJUJBM@QPTF@ZWBMVF BSHJOJUJBM@QPTF@Z QBSBNOBNFJOJUJBM@QPTF@BWBMVF BSHJOJUJBM@QPTF@B SFNBQGSPNTDBOUP BSHTDBO@UPQJD OPEF MBVODI

We can refer to the ROS BNDM package wiki for getting more details about each parameter. We will see how to localize and path plan the robot using the existing map. Launch the robot hardware nodes by using the following command: $ roslaunch chefbot_bringup robot_standalone.launch

Run the BNDM launch file by using the following command: $ roslaunch chefbot_bringup amcl_demo.launch map_file:=/home/lentin/room.yaml

We can launch RViz for commanding the robot to move to a particular pose on the map by using the following command: $ roslaunch chefbot_bringup view_navigation.launch

[ 379 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

The following is the screenshot of RViz:

(KIWTG4QDQVCWVQPQOQWUPCXKICVKQPWUKPI#/%.

We will see more about each option in RViz and how to command the robot in the map in the following section.

Understanding RViz for working with the Navigation stack We will explore various GUI options inside RViz to visualize each parameter in the Navigation stack.

2D Pose Estimate button The first step in RViz is to set the initial position of the robot on the map. If the robot is able to localize on the map by itself, there is no need to set the initial position. Otherwise, we have to set the initial position using the 2D Pose Estimate button in RViz, as shown in the following screenshot:

[ 380 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

(KIWTG48K\&2QUG'UVKOCVGDWVVQP

[ 381 ]

Chapter 11

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

Press the 2D Pose Estimate button and select a pose of the robot by using the left mouse button, as shown in the preceding figure. Check if the actual pose of the robot and the robot model in RViz are the same. After setting the pose, we can start the path plan to the robot. The green color cloud around the robot is the particle cloud of BNDM. If the particle amount is high, it means the uncertainty in the robot position is high, and if the cloud is less, it means that the uncertainty is low and the robot is almost sure about its position. The topic handling the robot's initial pose is: Topic name: JOJUJBMQPTF Topic type: HFPNFUSZ@NTHT1PTF8JUI$PWBSJBODF4UBNQFE

Visualizing the particle cloud The particle cloud around the robot can be enabled using the 1PTF"SSBZ display topic. Here the 1PTF"SSBZ topic is QBSUJDMFDMPVE displayed in RViz. The 1PTF"SSBZ type is renamed as "NDM particles: Topic: QBSUJDMFDMPVE Type: HFPNFUSZ@NTHT1PTF"SSBZ

(KIWTG8KUWCNK\KPI#/%.RCTVKENGU

[ 382 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

The 2D Nav Goal button The 2D Nav Goal button is used to give a goal position to the NPWF@CBTF node in the ROS Navigation stack through RViz. We can select this button from the top panel of RViz and can give the goal position inside the map by left clicking the map using the mouse. The goal position will send to the NPWF@CBTF node for moving the robot to that location: Topic: NPWF@CBTF@TJNQMFHPBM Topic type: HFPNFUSZ@NTHT1PTF4UBNQFE

(KIWTG5GVVKPIVJGTQDQV UIQCNRQUKVKQPKP48K\WUKPI&0CX)QCN

[ 383 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

Displaying the static map The static map is the map that we feed into the NBQ@TFSWFS node. The NBQ@TFSWFS node serves the static map in the NBQ topic: Topic: NBQ Type: OBW@NTHT(FU.BQ The following is the static map in RViz:

(KIWTG8KUWCNK\KPIUVCVKEOCRKP48K\

[ 384 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

Displaying the robot footprint We have defined the robot footprint in the configuration file called DPTUNBQ@DPNNPO@QBSBNTZBNM. This robot has a circular shape, and we have given the radius as 0.45 meters. It can visualize using the Polygon display type in RViz. The following is the circular footprint of the robot around the robot model and its topics: Topic: NPWF@CBTFHMPCBM@DPTUNBQPCTUBDMF@MBZFS@GPPUQSJOUGPPUQSJOU@TU BNQFE

Topic: NPWF@CBTFMPDBM@DPTUNBQPCTUBDMF@MBZFS@GPPUQSJOUGPPUQSJOU@TUB NQFE Type: HFPNFUSZ@NTHT1PMZHPO

(KIWTG)NQDCNCPFNQECNTQDQVHQQVRTKPVKP48K\

[ 385 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

Displaying the global and local cost map The following RViz screenshot shows the local cost map, the global cost map, the real obstacles, and the inflated obstacles. The display type of each of these maps is the map itself: Local cost map topic: NPWF@CBTFMPDBM@DPTUNBQDPTUNBQ Local cost map topic type: OBW@NTHT0DDVQBODZ(SJE Global cost map topic: NPWF@CBTFHMPCBM@DPTUNBQDPTUNBQ Global cost map topic type: OBW@NTHT0DDVQBODZ(SJE

(KIWTG8KUWCNK\KPIVJGINQDCNCPFNQECNOCRCPFTGCNCPFKPcCVGFQDUVCENGKP48K\

To avoid collision with the real obstacles, it is inflated to some distance from the real obstacles, called inflated obstacles, as per the values in the configuration files. The robot only plans a path beyond the inflated obstacle; inflation is a technique to avoid collision with the real obstacles.

[ 386 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

Displaying the global plan, the local plan, and the planner plan In the next figure, the global, local, and planner plans are shown. In this context, the planner and the global plans represent the complete plan to reach the goal. The local plan represents a short-term plan to follow the global planning. The global plan and the planner plan can be changed if there are any obstacles. The plans can be displayed using the Path display type in RViz: Global plan topic: NPWF@CBTF%8"1MBOOFS304HMPCBM@QMBO Global plan topic type: OBW@NTHT1BUI Local plan topic: NPWF@CBTF%8"1MBOOFS304MPDBM@QMBO Local plan topic type: OBW@NTHT1BUI Planner plan topic: NPWF@CBTF/BWGO304QMBO Planner plan topic type: OBW@NTHT1BUI

(KIWTG8KUWCNK\KPIVJGINQDCNNQECNCPFRNCPPGTRNCPKP48K\

[ 387 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

The current goal The current goal is the commanded position of the robot using the 2D Nav Goal button or using the ROS client nodes. The red arrow indicates the current goal of the robot: Topic: NPWF@CBTFDVSSFOU@HPBM Topic type: HFPNFUSZ@NTHT1PTF4UBNQFE

(KIWTG8KUWCNK\KPITQDQVIQCNRQUKVKQP

Obstacle avoidance using the Navigation stack The Navigation stack can be used to allow the robot to avoid random obstacles during its motion. The following is a scenario where we have placed a dynamic obstacle in the planned path of the robot. In particular, the first figure shows a path planning without any obstacle on the path. When we place a dynamic obstacle on the robot path, we can see it planning a path by avoiding the obstacle:

[ 388 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

(KIWTG8KUWCNK\KPIQDUVCENGCXQKFCPEGECRCDKNKVKGUKP48K\

[ 389 ]

Chapter 11

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

Working with Chefbot simulation The DIFGCPU@HB[FCP simulator package is available along with the DIFGCPU@CSJOHVQ package, and we can simulate the robot in Gazebo. We will see how to build a room similar to the room we tested with the hardware. First, we will check how to build a virtual room in Gazebo.

Building a room in Gazebo We will start building the room in Gazebo, save into Semantic Description Format (SDF), and insert in the Gazebo environment. Launch Gazebo with the Chefbot robot in an empty world: $ roslaunch chefbot_gazebo chefbot_empty_world.launch

It will open the Chefbot model in an empty world on Gazebo. We can build the room using walls, windows, doors, and stairs. There is a Building Editor in Gazebo. We can take this editor from the menu Edit | Building Editor. We will get an editor in the Gazebo viewport:

(KIWTG$WKNFKPIYCNNUKP)C\GDQ

[ 390 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

We can add walls by clicking the Add Wall option on the left-side pane of Gazebo. In the Building Editor, we can draw the walls by clicking the left mouse button. We can see that adding walls in the editor will build real 3D walls in Gazebo. We are building a similar layout of the room that we tested for the real robot. Save the room through the Save As option, or press the Done button; a box will pop up to save the file. The file will get saved in the TEG format. We can save this example as GJOBM@SPPN. After saving the room file, we can add the model of this room to the HB[FCPNPEFM folder, so that we can access the model in any simulation.

Adding model files to the Gazebo model folder The following is the procedure to add a model to the Gazebo folder. Locate the default model folder of Gazebo, which is located in the _HB[FCPNPEFMT folder. Create a folder called GJOBM@SPPN and copy GJOBM@SPPNTEG inside this folder. Also, create a file called NPEFMDPOGJH, which contains the details of the model file. The definition of this file follows: 
YNMWFSTJPO
NPEFM /BNFPGNPEFMXIJDIJTEJTQMBZJOHJO(B[FCP OBNF 5FTU3PPNOBNF WFSTJPO WFSTJPO .PEFMGJMFOBNF TEGWFSTJPO GJOBM@SPPNTEGTEG BVUIPS OBNF -FOUJO+PTFQIOBNF FNBJM RCPUJDTMBCT!HNBJMDPNFNBJM BVUIPS EFTDSJQUJPO "UFTUSPPNGPSQFSGPSNJOH4-". EFTDSJQUJPO NPEFM

[ 391 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

After adding this model to the model folder, restart Gazebo, and we can see the model named 5FTU3PPN in the entry in the Insert tab, as shown in the next screenshot. We have named this model as 5FTU3PPN in the NPEFMDPOGJH file; that name will show on this list. We can select this file and add it to the viewport, as shown next:

(KIWTG+PUGTVKPIVJGYCNNUKP%JGHDQVUKOWNCVKQP

After adding to the viewport, we can save the current world configuration. Select 'JMF from the (B[FCP menu and the 4BWFXPSMEBT option. Save the file as UFTU@SPPNTEG in the XPSMET folder of the DIFGCPU@HB[FCP ROS package, then add it to the DIFGCPU@FNQUZ@XPSMEMBVODI file, and save this launch file as the DIFGCPU@SPPN@XPSMEMBVODI file, which is shown next: JODMVEFGJMF GJOEHB[FCP@SPTMBVODIFNQUZ@XPSMEMBVODI BSHOBNFVTF@TJN@UJNFWBMVFUSVF BSHOBNFEFCVHWBMVFGBMTF "EEJOHXPSMEUFTU@SPPNTEGBTBSHVNFOU BSHOBNFXPSME@OBNFWBMVF GJOE DIFGCPU@HB[FCPXPSMETUFTU@SPPNTEG JODMVEF

[ 392 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

After saving this launch file, we can start the launch file DIFGCPU@SPPN@XPSMEMBVODI for simulating the same environment as the hardware robot. We can add obstacles in Gazebo, using the primitive shapes available in it. Instead of launching the SPCPU@TUBOEBMPOFMBVODI file from DIFGCPU@CSJOHVQ for the hardware, we can start DIFGCPU@IPUFM@XPSMEMBVODI for getting a more complex environment of the robot, and the PEPN and tf data in simulation: $ roslaunch chefbot_gazebo chefbot_hotel_world.launch

Other operations, such as SLAM and AMCL, have the same procedure that we followed for the hardware. The following launch files are used to perform SLAM and AMCL in the simulation. To use HNBQQJOH and the AMCL program to perform localization and mapping, we should install it by using the following commands: Run SLAM in the simulation: $ roslaunch chefbot_gazebo gmapping_demo.launch

Run the Teleop node: $ roslaunch chefbot_bringup keyboard_teleop.launch

Run AMCL in the simulation: $ roslaunch chefbot_gazebo amcl_demo.launch

After moving the robot into its environment, we can save the generated map as already seen: $ rosrun map_server map_saver -f /tmp/hotel

Sending a goal to the Navigation stack from a ROS node We have seen how to send a goal position to a robot for moving it from point A to B using the RViz %/BW(PBM button. Now we will see how to command the robot using the actionlib client and the ROS C++ APIs. The following is a sample package and node for communicating with the Navigation stack NPWF@CBTF node. The NPWF@CBTF node is 4JNQMF"DUJPO4FSWFS. We can send and cancel the goals to the robot if the task takes a lot of time to complete.

[ 393 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

The following code is 4JNQMF"DUJPO$MJFOU for the NPWF@CBTF node, which can send the Y, Z, and theta from the command-line arguments. The following code is in the DIFGCPU@CSJOHVQTSD folder with the name of TFOE@SPCPU@HPBMDQQ: JODMVEFSPTSPTI JODMVEFNPWF@CBTF@NTHT.PWF#BTF"DUJPOI JODMVEFBDUJPOMJCDMJFOUTJNQMF@BDUJPO@DMJFOUI JODMVEFUGUSBOTGPSN@CSPBEDBTUFSI JODMVEFTTUSFBN JODMVEFJPTUSFBN %FDMBSJOHBOFX4JNQMF"DUJPO$MJFOUXJUIBDUJPOPG NPWF@CBTF@NTHT.PWF#BTF"DUJPO UZQFEFG BDUJPOMJC4JNQMF"DUJPO$MJFOUNPWF@CBTF@NTHT.PWF#BTF"DUJPO .PWF#BTF$MJFOU JOUNBJO JOUBSHDDIBS BSHW\ SPTJOJU BSHDBSHWOBWJHBUJPO@HPBMT *OJUJBUJOHNPWF@CBTFDMJFOU .PWF#BTF$MJFOUBD NPWF@CBTFUSVF 8BJUJOHGPSTFSWFSUPTUBSU XIJMF BDXBJU'PS4FSWFS SPT%VSBUJPO \ 304@*/'0 8BJUJOHGPSUIFNPWF@CBTFBDUJPOTFSWFS ^ %FDMBSJOHNPWFCBTFHPBM NPWF@CBTF@NTHT.PWF#BTF(PBMHPBM 4FUUJOHUBSHFUGSBNFJEBOEUJNFJOUIFHPBMBDUJPO HPBMUBSHFU@QPTFIFBEFSGSBNF@JENBQ HPBMUBSHFU@QPTFIFBEFSTUBNQSPT5JNFOPX  3FUSJFWJOHQPTFGSPNDPNNBOEMJOFPUIFSWJDFFYFDVUFBEFGBVMUWBMVF USZ\ HPBMUBSHFU@QPTFQPTFQPTJUJPOYBUPG BSHW HPBMUBSHFU@QPTFQPTFQPTJUJPOZBUPG BSHW HPBMUBSHFU@QPTFQPTFPSJFOUBUJPOXBUPG BSHW ^ DBUDI JOUF\ HPBMUBSHFU@QPTFQPTFQPTJUJPOY HPBMUBSHFU@QPTFQPTFQPTJUJPOZ HPBMUBSHFU@QPTFQPTFPSJFOUBUJPOX ^ 304@*/'0 4FOEJOHNPWFCBTFHPBM 4FOEJOHHPBM BDTFOE(PBM HPBM

[ 394 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

BDXBJU'PS3FTVMU  JG BDHFU4UBUF BDUJPOMJC4JNQMF$MJFOU(PBM4UBUF46$$&&%&% 304@*/'0 3PCPUIBTBSSJWFEUPUIFHPBMQPTJUJPO FMTF\ 304@*/'0 5IFCBTFGBJMFEGPSTPNFSFBTPO ^ SFUVSO ^

The following lines are added to $.BLF-JTUTUYU for building this node: BEE@FYFDVUBCMF TFOE@HPBMTSDTFOE@SPCPU@HPBMDQQ UBSHFU@MJOL@MJCSBSJFT TFOE@HPBM\DBULJO@-*#3"3*&4^

Build the package using DBULJO@NBLF and test the working of the client using the following set of commands using Gazebo. Start the Gazebo simulation in a room: $ roslaunch chefbot_gazebo chefbot_room_world.launch

Start the BNDM node with the generated map: $ roslaunch chefbot_gazebo amcl_demo.launch map_file:=/tmp/hotel.yaml

Start RViz for the navigation: $ roslaunch chefbot_bringup view_navigation.launch

Run the send HPBM node for sending the move base goal: $ rosrun chefbot_bringup send_goal 1 0 1

We will get the desired pose of the robot in the map by using the RViz 2D Nav goal button. Simply echoing the topic NPWF@CBTFHPBM will print the pose that we commanded through RViz. We can use these values as command-line arguments in the TFOE@HPBM node.

[ 395 ]

Building and Interfacing Differential Drive Mobile Robot Hardware in ROS

Chapter 11

Questions What are the basic requirements for working with the ROS Navigation stack? What are the main configuration files for working with the ROS Navigation stack? How does the AMCL package in ROS work? What are the methods to send a goal pose to the Navigation stack?

Summary In this chapter, we mainly covered interfacing a DIY autonomous mobile robot to ROS and the Navigation package. We looked at an introduction to this robot and the necessary components and connection diagrams. We looked at the robot firmware and saw how to flash it into the real robot. After flashing the firmware, we learned how to interface it to the ROS and saw the Python nodes for interfacing the LaunchPad controller in the robot and the nodes for converting twist messages to motor velocities and encoder ticks to odom and tf. After discussing the interconnection of the Chefbot nodes, we covered the C++ port of some important nodes for odometry calculation and the base controller node. After discussing these nodes, we looked at detailed configurations of the ROS Navigation stack. We also did HNBQQJOH and AMCL and looked at a detailed description of each option in RViz for working with the Navigation stack. We also covered obstacle avoidance using the Navigation stack and worked with the Chefbot simulation. We set up a similar environment in Gazebo to the environment of the real robot and went through the steps to perform SLAM and AMCL. At the end of this chapter, we saw how we can send a goal pose to the Navigation stack using BDUJPOMJC.

[ 396 ]

12

Exploring the Advanced Capabilities of ROS-MoveIt! In the previous chapter, we covered ROS navigation and interfacing with mobile robotic hardware. Similarly, in this chapter, we are going to cover the advanced capabilities of ROS-MoveIt!, such as collision avoidance, perception with 3D sensors, grasping, picking, and placing. After this, we will see how to interface robotic manipulator hardware with MoveIt!. The following are the main topics discussed in this chapter: Motion planning collision checking of a robotic arm, using MoveIt! C++ APIs Working with perception in MoveIt! and Gazebo Performing object manipulation with MoveIt! Understanding DYNAMIXEL ROS servo controllers for robot hardware interfacing Interfacing 7-DOF DYNAMIXEL-based robotic arm to ROS MoveIt! In $IBQUFS, Simulating Robots Using ROS and Gazebo, and $IBQUFS, Using the ROS MoveIt! and Navigation Stack, we discussed MoveIt! and how to simulate an arm in Gazebo, and motion plan using MoveIt!. In this chapter, we are going to see some of the advanced capabilities of MoveIt! and how to interface a real robotic manipulator to ROS MoveIt!. The first topic that we are going to discuss is how to motion plan our robot using MoveIt! C++ APIs.

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

Motion planning using the move_group C++ interface In $IBQUFS, Using the ROS MoveIt! and Navigation Stack, we discussed how to interact with a robot arm, and how to plan its path using the MoveIt! RViz motion planning plugin. In this section, we will see how to program the robot motion using the NPWF@HSPVQ C++ APIs. Motion planning using RViz can also be done programmatically through the NPWF@HSPVQ C++ APIs. The first step to start working with C++ APIs is to create another ROS package that has the MoveIt! packages as dependencies. You can get an existing TFWFO@EPG@BSN@UFTU package from the code provided with this book, or you can download it from the following repository: $ git clone https://github.com/jocacace/seven_dof_arm_test.git

We can create this same package using the following command: $ catkin_create_pkg seven_dof_arm_test catkin cmake_modules interactive_markers moveit_core moveit_ros_perception moveit_ros_planning_interface pluginlib roscpp std_msgs

Motion planning a random path using MoveIt! C++ APIs The first example that we are going to see is a random motion plan using MoveIt! C++ APIs. You will get the code named UFTU@SBOEPNDQQ from the TSD folder. The code and the description of each line follows. When we execute this node, it will plan a random path and execute it: JODMVEFNPWFJUNPWF@HSPVQ@JOUFSGBDFNPWF@HSPVQ@JOUFSGBDFI JOUNBJO JOUBSHDDIBS BSHW \ SPTJOJU BSHDBSHWNPWF@HSPVQ@JOUFSGBDF@EFNP SPTJOJU@PQUJPOT"OPOZNPVT/BNF TUBSUB304TQJOOJOHUISFBE SPT"TZOD4QJOOFSTQJOOFS  TQJOOFSTUBSU  UIJTDPOOFDUTUPBSVOOJOHJOTUBODFPGUIFNPWF@HSPVQOPEF NPWF@HSPVQ@JOUFSGBDF.PWF(SPVQHSPVQ BSN NPWFJUQMBOOJOH@JOUFSGBDF.PWF(SPVQ*OUFSGBDFHSPVQ BSN

[ 398 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

TQFDJGZUIBUPVSUBSHFUXJMMCFBSBOEPNPOF HSPVQTFU3BOEPN5BSHFU  QMBOUIFNPUJPOBOEUIFONPWFUIFHSPVQUPUIFTBNQMFEUBSHFU HSPVQNPWF  SPTXBJU'PS4IVUEPXO  ^

To build the source code, we should add the following lines of code to $.BLF-JTUTUYU. You will get the complete $.BLF-JTUTUYU file from the existing package itself: BEE@FYFDVUBCMF UFTU@SBOEPN@OPEFTSDUFTU@SBOEPNDQQ BEE@EFQFOEFODJFT UFTU@SBOEPN@OPEFTFWFO@EPG@BSN@UFTU@HFOFSBUF@NFTTBHFT@DQQ UBSHFU@MJOL@MJCSBSJFT UFTU@SBOEPN@OPEF \DBULJO@-*#3"3*&4^

We can build the package using the DBULJO@NBLF command. First, check whether UFTU@SBOEPNDQQ is built properly or not. If the code is built properly, we can start testing the code. The following command will start the RViz with 7-DOF arm with the motion planning plugin: $ roslaunch seven_dof_arm_config demo.launch

Move the end effector to check whether everything is working properly in RViz. Run the C++ node for planning to a random position using the following command: $ rosrun seven_dof_arm_test test_random_node

[ 399 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

The output of the RViz is shown next. The arm will select a random position that has a valid IK and motion plan from the current position:

(KIWTG4CPFQOOQVKQPRNCPPKPIWUKPIOQXGAITQWR#2+U

[ 400 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

Motion planning a custom path using MoveIt! C++ APIs We saw random motion planning in the preceding example. In this section, we will check how to command the robot end effector to move to a custom goal position. The following example, UFTU@DVTUPNDQQ, will do that job: JODMVEFNPWFJUNPWF@HSPVQ@JOUFSGBDFNPWF@HSPVQ@JOUFSGBDFI JODMVEFNPWFJUQMBOOJOH@TDFOF@JOUFSGBDFQMBOOJOH@TDFOF@JOUFSGBDFI JODMVEFNPWFJUNPWF@HSPVQ@JOUFSGBDFNPWF@HSPVQ@JOUFSGBDFI JODMVEFNPWFJU@NTHT%JTQMBZ3PCPU4UBUFI JODMVEFNPWFJU@NTHT%JTQMBZ5SBKFDUPSZI JODMVEFNPWFJU@NTHT"UUBDIFE$PMMJTJPO0CKFDUI JODMVEFNPWFJU@NTHT$PMMJTJPO0CKFDUI JOUNBJO JOUBSHDDIBS BSHW \ 304JOJUJBMJ[BUJPO SPTJOJU BSHDBSHWNPWF@HSPVQ@JOUFSGBDF@UVUPSJBM SPT/PEF)BOEMFOPEF@IBOEMF SPT"TZOD4QJOOFSTQJOOFS  TQJOOFSTUBSU  TMFFQ  .PWFHSPVQTFUVQ NPWFJUQMBOOJOH@JOUFSGBDF.PWF(SPVQ*OUFSGBDFHSPVQ BSN NPWFJUQMBOOJOH@JOUFSGBDF1MBOOJOH4DFOF*OUFSGBDF QMBOOJOH@TDFOF@JOUFSGBDF SPT1VCMJTIFSEJTQMBZ@QVCMJTIFS OPEF@IBOEMFBEWFSUJTFNPWFJU@NTHT%JTQMBZ5SBKFDUPSZ NPWF@HSPVQEJTQMBZ@ QMBOOFE@QBUIUSVF NPWFJU@NTHT%JTQMBZ5SBKFDUPSZEJTQMBZ@USBKFDUPSZ 304@*/'0 3FGFSFODFGSBNFTHSPVQHFU&OE&GGFDUPS-JOL D@TUS  5BSHFUQPTFTFUVQ HFPNFUSZ@NTHT1PTFUBSHFU@QPTF UBSHFU@QPTFPSJFOUBUJPOX UBSHFU@QPTFPSJFOUBUJPOYF UBSHFU@QPTFPSJFOUBUJPOZ UBSHFU@QPTFPSJFOUBUJPO[F UBSHFU@QPTFQPTJUJPOY UBSHFU@QPTFQPTJUJPOZF UBSHFU@QPTFQPTJUJPO[ HSPVQTFU1PTF5BSHFU UBSHFU@QPTF .PUJPOQMBOOJOH NPWFJUQMBOOJOH@JOUFSGBDF.PWF(SPVQ*OUFSGBDF1MBONZ@QMBO NPWFJUQMBOOJOH@JOUFSGBDF.PWF*U&SSPS$PEFTVDDFTT HSPVQQMBO NZ@QMBO

[ 401 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

304@*/'0 7JTVBMJ[JOHQMBO QPTFHPBMTTVDDFTTWBM
'"*-&% 4MFFQUPHJWF3WJ[UJNFUPWJTVBMJ[FUIFQMBO TMFFQ  SPTTIVUEPXO  ^

The following are the extra lines of code added on for building the source code: BEE@FYFDVUBCMF UFTU@DVTUPN@OPEFTSDUFTU@DVTUPNDQQ BEE@EFQFOEFODJFT UFTU@DVTUPN@OPEFTFWFO@EPG@BSN@UFTU@HFOFSBUF@NFTTBHFT@DQQ UBSHFU@MJOL@MJCSBSJFT UFTU@DVTUPN@OPEF \DBULJO@-*#3"3*&4^

Following is the command to execute the custom node: $ rosrun seven_dof_arm_test test_custom_node

The following screenshot shows the result of the UFTU@DVTUPN@OPEF:

(KIWTG%WUVQOOQVKQPRNCPPKPIWUKPI/QXG+V%#2+U

[ 402 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

Collision checking with a robot arm using MoveIt! Along with motion planning and the IK solving algorithm, one of the most important tasks that is done in parallel in MoveIt! is collision checking, and its avoidance. MoveIt! can handle both self-collisions and environmental collisions exploiting the built-in Flexible Collision Library (FCL) (IUUQHBNNBDTVODFEV'$-GDM@EPDTXFCQBHFHFOFSBUFE JOEFYIUNM), an open source project that implements various collision-detection andavoidance algorithms. MoveIt! takes the power of FCL and handles the collision inside the planning scene using a DPMMJTJPO@EFUFDUJPO$PMMJTJPO8PSME class. The MoveIt! collision checking includes objects, such as meshes; primitive shapes, such as boxes and cylinders, and OctoMap. The OctoMap (IUUQPDUPNBQHJUIVCJP) library implements a 3D occupancy grid, called an octree, which consists of probabilistic information regarding obstacles in the environment. The MoveIt! package can build an OctoMap using 3D point cloud information, and can directly feed the OctoMap to FCL for collision checking. Like motion planning, collision checking is also very computationally intensive. We can fine-tune the collision checking between two bodiescsay, a robot link, or with the environment - using a parameter called Allowed Collision Matrix (ACM). If the value of a collision between two links is set to 1 in ACM, there will not be any collision checks. We may set this for links that are far from each other. We can optimize the collision-checking process by optimizing this matrix.

Adding a collision object to MoveIt! We can add a collision object to the MoveIt! planning scene, and we can see how the motion-planning works. For adding a collision object, we can use mesh files, which can directly be imported from the MoveIt! interface, and can be added by writing a ROS node using MoveIt! APIs.

[ 403 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

We will first discuss how to add a collision object using the ROS node: 1. In the node BEE@DPMMJTJPO@PCKFDUDQQ, which is inside the TFWFO@EPG@BSN@UFTUTSD folder, we are starting a ROS node and creating an object of NPWFJUQMBOOJOH@JOUFSGBDF1MBOOJOH4DFOF*OUFSGBDF, which can access the planning scene of MoveIt! and perform any action on the current scene. We will now add a sleep of 5 seconds to wait for the QMBOOJOH4DFOF*OUFSGBDF object instantiation: NPWFJUQMBOOJOH@JOUFSGBDF1MBOOJOH4DFOF*OUFSGBDF DVSSFOU@TDFOF TMFFQ 

2. In the next step, we need to create an instance of the collision object message NPWFJU@NTHT$PMMJTJPO0CKFDU. This message is going to be sent to the current planning scene. Here, we are making a collision object message for a cylinder shape, and the message is given as TFWFO@EPG@BSN@DZMJOEFS. When we add this object to the planning scene, the name of the object is its ID: NPWFJU@NTHT$PMMJTJPO0CKFDUDZMJOEFS DZMJOEFSJETFWFO@EPG@BSN@DZMJOEFS

3. After making the collision object message, we have to define another message of type TIBQF@NTHT4PMJE1SJNJUJWF, which is used to define what kind of primitive shape we are using and its properties. In this example, we are creating a cylinder object, as shown in the following code. We have to define the type of shape, the resizing factor, the width, and the height of the cylinder: TIBQF@NTHT4PMJE1SJNJUJWFQSJNJUJWF QSJNJUJWFUZQFQSJNJUJWF$:-*/%&3 QSJNJUJWFEJNFOTJPOTSFTJ[F  QSJNJUJWFEJNFOTJPOT QSJNJUJWFEJNFOTJPOT QSJNJUJWFEJNFOTJPOT

[ 404 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

4. After creating the shape message, we have to create a HFPNFUSZ@NTHT1PTF message to define the pose of this object. We define a pose that may be closer to robot. We can change the pose after the creation of the object in the planning scene: HFPNFUSZ@NTHT1PTFQPTF QPTFPSJFOUBUJPOX QPTFQPTJUJPOY QPTFQPTJUJPOZ QPTFQPTJUJPO[

5. After defining the pose of the collision object, we need to add the defined primitive object, and the pose to the cylinder collision object. The operation we need to perform is adding the planning scene: DZMJOEFSQSJNJUJWFTQVTI@CBDL QSJNJUJWF DZMJOEFSQSJNJUJWF@QPTFTQVTI@CBDL QPTF DZMJOEFSPQFSBUJPODZMJOEFS"%%

6. In the next step, we create a vector called DPMMJTJPO@PCKFDUT of type NPWFJU@NTHT$PMMJTJPO0CKFDU, inserting the collision object to this vector: TUEWFDUPSNPWFJU@NTHT$PMMJTJPO0CKFDU DPMMJTJPO@PCKFDUT DPMMJTJPO@PCKFDUTQVTI@CBDL DZMJOEFS

7. We will add the collision objects vector to the current planning scene by using the following line of code. BEE$PMMJTJPO0CKFDUT  inside the 1MBOOJOH4DFOF*OUFSGBDF class is used to add the object to the planning scene: DVSSFOU@TDFOFBEE$PMMJTJPO0CKFDUT DPMMJTJPO@PCKFDUT

8. The following are the compile and build lines of the code in $.BLF-JTUTUYU: BEE@FYFDVUBCMF BEE@DPMMJTJPO@PCKFDU TSDBEE@DPMMJTJPO@PCKFDUDQQ BEE@EFQFOEFODJFT BEE@DPMMJTJPO@PCKFDU TFWFO@EPG@BSN@UFTU@HFOFSBUF@NFTTBHFT@DQQ UBSHFU@MJOL@MJCSBSJFT BEE@DPMMJTJPO@PCKFDU \DBULJO@-*#3"3*&4^

[ 405 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

Let's see how this node works in RViz with the MoveIt! motion-planning Plugin: We will start the EFNPMBVODI inside the TFWFO@EPG@BSN@DPOGJH package for testing this node: $ roslaunch seven_dof_arm_config demo.launch

Then we, add the following collision object: $ rosrun seven_dof_arm_test add_collision_object

When we run the BEE@DPMMJTJPO@PCKFDU node, a green cylinder will pop up, and we can move the collision object as shown in the following screenshot. When the collision object is successfully added to the planning scene, it will list out in the Scene Objects tab. We can click on the object and modify its pose. We can also attach the new model in any links of robots too. There is a Scale option to scale down the collision model:

(KIWTG#FFKPIEQNNKUKQPQDLGEVUVQ48K\WUKPI/QXG+V%#2+U

[ 406 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

The RViz Motion-Planning plugin also gives an option to import a 3D mesh to the planning scene. Click the Import File button for importing the meshes. The following image shows our importing of a cube mesh DAE file, which is imported along with the cylinder in the planning scene. We can scale up the collision object using the Scale slider, and set the desired pose using the Manage Pose option. When we move the arm end effector to any of these collision objects, MoveIt! detects it as a collision. The MoveIt! collision detection can detect environment collision as well as self-collision. The following is a snapshot of a collision with the environment:

(KIWTG8KUWCNK\KPIEQNNKFGFNKPM

The collided link will turn red when the arm touches the object. In self-collision also, the collided link will turn red. We can change the color setting of the collision in the Motion Planning plugin settings.

[ 407 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

Removing a collision object from the planning scene Removing the collision object from the planning scene is easy. We have to create an object of NPWFJUQMBOOJOH@JOUFSGBDF1MBOOJOH4DFOF*OUFSGBDF, as we did in the previous example, along with some delay: NPWFJUQMBOOJOH@JOUFSGBDF1MBOOJOH4DFOF*OUFSGBDFDVSSFOU@TDFOF TMFFQ 

Next, we need to create a vector of the string that contains the collision object IDs. Here, our collision object ID is TFWFO@EPG@BSN@DZMJOEFS. After pushing the string to this vector, we will call SFNPWF$PMMJTJPO0CKFDUT PCKFDU@JET, which will remove the collision objects from the planning scene: TUEWFDUPSTUETUSJOH PCKFDU@JET PCKFDU@JETQVTI@CBDL TFWFO@EPG@BSN@DZMJOEFS DVSSFOU@TDFOFSFNPWF$PMMJTJPO0CKFDUT PCKFDU@JET

This code is placed in TFWFO@EPG@BSN@UFTUTSDSFNPWF@DPMMJTJPO@PCKFDUDQQ.

Attaching a collision object to a robot link After seeing how to insert and remove objects from the planning scene, we are now going to discuss how to attach and detach objects to the robot's body. This important feature of ROS MoveIt! allow us to perform object manipulation. In fact, after attaching an object to the robot's body, the obstacle avoidance is additionally extended to the grasped object. In this way, the robot will be free to move into its workspace, avoiding obstacles and carrying the object to manipulate. The code we are going to discuss in place is the TFWFO@EPG@BSN@UFTUTSDBUUBDI@EFUBDI@PCKTDQQ source code. After creating a NPWFJUQMBOOJOH@JOUFSGBDF1MBOOJOH4DFOF*OUFSGBDF, as shown in the previous examples, we must initialize a NPWFJU@NTHT"UUBDIFE$PMMJTJPO0CKFDU, filling information about which scene object will be attached to a specific link of the robot's body: NPWFJU@NTHT"UUBDIFE$PMMJTJPO0CKFDUBUUBDDIFE@PCKFDU BUUBDDIFE@PCKFDUMJOL@OBNFHSBTQJOH@GSBNF BUUBDDIFE@PCKFDUPCKFDUHSBTQJOH@PCKFDU DVSSFOU@TDFOFBQQMZ"UUBDIFE$PMMJTJPO0CKFDU BUUBDDIFE@PCKFDU

[ 408 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

In this example, the HSBTQJOH@PCKFDU attached to the robot link is the one already used in the BEE@DPMMJTJPO@PCKFDUDQQ example. When an object is successfully attached to a robot, its color in the Moveit! visualization will change from green to purple, and will move along with the robot motion. To detach an object from the robot's body, we should invoke the BQQMZ"UUBDIFE$PMMJTJPO0CKFDU function on the desired object to detach modifying its operation from "%% to 3&.07&: HSBTQJOH@PCKFDUPQFSBUJPOHSBTQJOH@PCKFDU3&.07& BUUBDDIFE@PCKFDUMJOL@OBNFHSBTQJOH@GSBNF BUUBDDIFE@PCKFDUPCKFDUHSBTQJOH@PCKFDU

Checking self-collisions using MoveIt! APIs We have seen how to detect collision in RViz, but what do we have to do if we want to get collision information in our ROS node? In this section, we will discuss how to get the collision information of our robot in a ROS code. This example can check self collisions and environment collisions, and also tell which links were collided. The example called 9 is placed in the TFWFO@EPG@BSN@UFTUTSD folder. This code is a modified version of the collision-checking example of PR2 MoveIt! robot tutorials. In this code, the following snippet loads the kinematic model of the robot to the planning scene: SPCPU@NPEFM@MPBEFS3PCPU.PEFM-PBEFS SPCPU@NPEFM@MPBEFS SPCPU@EFTDSJQUJPO SPCPU@NPEFM3PCPU.PEFM1USLJOFNBUJD@NPEFMSPCPU@NPEFM@MPBEFSHFU.PEFM  QMBOOJOH@TDFOF1MBOOJOH4DFOFQMBOOJOH@TDFOF LJOFNBUJD@NPEFM

To test self-collision in the robot's current state, we can create two instances of class DPMMJTJPO@EFUFDUJPO$PMMJTJPO3FRVFTU and DPMMJTJPO@EFUFDUJPO$PMMJTJPO3FTVMU, which have the name of DPMMJTJPO@SFRVFTUBOEDPMMJTJPO@SFTVMU. After creating these objects, pass it MoveIt! collision-checking function, QMBOOJOH@TDFOFDIFDL4FMG$PMMJTJPO , which can give the collision result in the DPMMJTJPO@SFTVMU object, and we can print the details, which are shown in the following code: QMBOOJOH@TDFOFDIFDL4FMG$PMMJTJPO DPMMJTJPO@SFRVFTUDPMMJTJPO@SFTVMU 304@*/'0@453&". 4FMGDPMMJTJPO5FTU DPMMJTJPO@SFTVMUDPMMJTJPO
JOOPUJO TFMGDPMMJTJPO

[ 409 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

If we want to test collision in a particular group, we can do that by NFOUJPOJOH HSPVQ@OBNF, as shown in the following code. Here, the HSPVQ@OBNF is BSN: DPMMJTJPO@SFRVFTUHSPVQ@OBNFBSN DVSSFOU@TUBUFTFU5P3BOEPN1PTJUJPOT  1SFWJPVTSFTVMUTTIPVMECFDMFBSFE DPMMJTJPO@SFTVMUDMFBS  QMBOOJOH@TDFOFDIFDL4FMG$PMMJTJPO DPMMJTJPO@SFRVFTUDPMMJTJPO@SFTVMU 304@*/'0@453&". 4FMGDPMMJTJPO5FTU *OBHSPVQ

DPMMJTJPO@SFTVMUDPMMJTJPO
JOOPUJO

For performing a full collision check, we have to use the following function, called QMBOOJOH@TDFOFDIFDL$PMMJTJPO . We need to mention the current robot state and the ACM matrix in this function. The following is the code snippet to perform full collision checking using this function: DPMMJTJPO@EFUFDUJPO"MMPXFE$PMMJTJPO.BUSJYBDN QMBOOJOH@TDFOFHFU"MMPXFE$PMMJTJPO.BUSJY  SPCPU@TUBUF3PCPU4UBUFDPQJFE@TUBUFQMBOOJOH@TDFOFHFU$VSSFOU4UBUF  QMBOOJOH@TDFOFDIFDL$PMMJTJPO DPMMJTJPO@SFRVFTUDPMMJTJPO@SFTVMU DPQJFE@TUBUFBDN 304@*/'0@453&". 'VMMDPMMJTJPO5FTU DPMMJTJPO@SFTVMUDPMMJTJPO
JOOPUJO DPMMJTJPO

We can launch the demo of motion planning and run this node using the following command: $ roslaunch seven_dof_arm_config demo.launch

Run the collision-checking node: $ rosrun seven_dof_arm_test check_collision

[ 410 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

You will get a report, such as the one shown in the following image. The robot is now not in collision; if it is in collision, it will send a report of it:

(KIWTG%QNNKUKQPEJGEMKPIKPHQTOCVKQPOGUUCIGU

Working with perception using MoveIt! and Gazebo Until now, in MoveIt!, we have worked with an arm only. In this section, we will see how to interface a 3D vision sensor data to MoveIt!. The sensor can be either simulated using Gazebo, or you can directly interface an RGB-D sensor, such as Kinect or Xtion Pro, using the PQFOOJ@MBVODI package. Here, we will work using Gazebo simulation. We will add sensors to MoveIt! for vision-assisted pick-and-place. We will create a grasp table and a grasp object in Gazebo for the pick-and-place operation. We will add two custom models called (SBTQ@0CKFDU and (SBTQ@5BCMF. The sample models are placed into the TFWFO@EPG@BSN@UFTU package in the model directory, and should be copied to the _HB[FCPNPEFMT folder for accessing the models from Gazebo. The following command will launch the robot arm and the Asus Xtion pro simulation in Gazebo: $ roslaunch seven_dof_arm_gazebo seven_dof_arm_bringup_grasping.launch

[ 411 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

This command will open up Gazebo with arm joint controllers and the Gazebo plugin for 3D vision sensor. We can add a grasp table and grasp objects to the simulation, as shown in the following image, by simply clicking and dragging them to the workspace. We can create any kind of table or object. The objects shown in the following image are only for demonstration purposes. We can edit the model SDF file to change the size and shape of the model:

(KIWTG4QDQVCTOYKVJITCURVCDNGCPFQDLGEVKP)C\GDQ

[ 412 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

Check the topics generated after starting the simulation: $ rostopic list

Make sure that we are getting the RGB-D camera topics, as shown in the following code snippet:

(KIWTG.KUVKPI4)$&UGPUQTVQRKEU

We can view the point cloud in RViz using the following command: $ rosrun rviz rviz -f base_link

[ 413 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

The following is the output generated:

(KIWTG8KUWCNK\KPIRQKPVENQWFFCVCKP48K\

After confirming the point cloud data from the Gazebo plugins, we have to add some files to the MoveIt! configuration package of this arm, that is, the TFWFO@EPG@BSN@DPOGJH, for bringing the point cloud data from Gazebo to the MoveIt! planning scene.

[ 414 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

The robot environment is mapped as octree representation (IUUQTFOXJLJQFEJBPSHXJLJ0DUSFF), which can be built using a library called OctoMap, which we have already seen in the previous section. The OctoMap is incorporated as a plugin in MoveIt!, called the Occupany Map Updator plugin, which can update octree from different kinds of sensor inputs, such as point cloud and depth images from 3D vision sensors. Currently, there are the following plugins for handling 3D data: 1PJOU$MPVE0DDVQBODZNBQ6QEBUFS: This plugin can take input in the form of point clouds (TFOTPS@NTHT1PJOU$MPVE) %FQUI*NBHF0DDVQBODZNBQ6QEBUFS: This plugin can take input in the form of input depth images (TFOTPS@NTHT*NBHF)

The first step is to write a configuration file for these plugins. This file contains information about which plugin we are using in this robot, and what its properties are. The file exploiting the first plugin is found in the TFWFO@EPG@BSN@DPOGJHDPOGJH folder, called TFOTPS@LJOFDU@QPJOUDMPVEZBNM. The definition of this file is as follows: TFOTPST TFOTPS@QMVHJOPDDVQBODZ@NBQ@NPOJUPS1PJOU$MPVE0DUPNBQ6QEBUFS QPJOU@DMPVE@UPQJDSHCE@DBNFSBEFQUIQPJOUT NBY@SBOHF QBEEJOH@PGGTFU QBEEJOH@TDBMF QPJOU@TVCTBNQMF GJMUFSFE@DMPVE@UPQJDPVUQVU@DMPVE

The explanation of a general parameter is: TFOTPS@QMVHJO: This parameter specifies the name of the plugin we are using in

the robot The following are the parameters of the given TFOTPS@QMVHJO: QPJOU@DMPVE@UPQJD: The plugin will listen to this topic for point cloud data NBY@SBOHF: This is the distance limit in meters in which points above the range

will not be used for processing QBEEJOH@PGGTFU: This value will be taken into account for robot links and attached objects when filtering clouds containing the robot links (self-filtering)

[ 415 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

QBEEJOH@TDBMF: This value will also be taken into account while self-filtering QPJOU@TVCTBNQMF: If the update process is slow, points can be subsampled. If

we make this value greater than 1, the points will be skipped instead of processed GJMUFSFE@DMPVE@UPQJD: This is the final filtered cloud topic. We will get the processed point cloud through this topic. It can be used mainly for debugging If we are using the %FQUI*NBHF0DUPNBQ6QEBUFS plugin, we can have a different configuration file. We are not using this plugin in our example robot, but we can see its usage and properties: TFOTPST TFOTPS@QMVHJOPDDVQBODZ@NBQ@NPOJUPS%FQUI*NBHF0DUPNBQ6QEBUFS JNBHF@UPQJDIFBE@NPVOU@LJOFDUEFQUI@SFHJTUFSFEJNBHF@SBX RVFVF@TJ[F OFBS@DMJQQJOH@QMBOF@EJTUBODF GBS@DMJQQJOH@QMBOF@EJTUBODF TLJQ@WFSUJDBM@QJYFMT TLJQ@IPSJ[POUBM@QJYFMT TIBEPX@UISFTIPME QBEEJOH@TDBMF QBEEJOH@PGGTFU GJMUFSFE@DMPVE@UPQJDPVUQVU@DMPVE

The explanation of a general parameter is: TFOTPS@QMVHJO: This parameter specifies the name of the plugin we are using in

the robot. The following are the parameters of the given TFOTPS@QMVHJO: JNBHF@UPQJD: The topic that streams the image. RVFVF@TJ[F: This is the queue size for the depth image transport subscriber. OFBS@DMJQQJOH@QMBOF@EJTUBODF: This is the minimum valid distance from the

sensor. GBS@DMJQQJOH@QMBOF@EJTUBODF: This is the maximum valid distance from the sensor. TLJQ@WFSUJDBM@QJYFMT: This is the number of pixels we have to skip from the top and bottom of the image. If we give a value of 5, it will skip five columns from first and last of the image.

[ 416 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

TLJQ@IPSJ[POUBM@QJYFMT: Skipping pixels in horizontal direction. TIBEPX@UISFTIPME: In some situations, points can appear below the robot links. This happens because of padding. TIBEPX@UISFTIPME removes the points with a distance that is greater than the TIBEPX@UISFTIPME.

After discussing the OctoMap update plugin and its properties, we can switch to the launch files necessary to initiate this plugin and its parameters. The first file we need to create is inside the TFWFO@EPG@BSN@DPOGJHMBVODI folder with the name TFWFO@EPG@BSN@NPWFJU@TFOTPS@NBOBHFSMBVODI. The following is the definition of this file. This launch file basically loads the plugin parameters: MBVODI SPTQBSBNDPNNBOEMPBEGJMF GJOE TFWFO@EPG@BSN@DPOGJHDPOGJHTFOTPST@LJOFDU@QPJOUDMPVEZBNM MBVODI

The next file that we need to edit is the TFOTPS@NBOBHFSMBVODI, which is located inside the MBVODI folder. The definition of this file is as follows: MBVODI 5IJTGJMFNBLFTJUFBTZUPJODMVEFUIFTFUUJOHTGPSTFOTPSNBOBHFST  1BSBNTGPSUIFPDUPNBQNPOJUPS QBSBNOBNFPDUPNBQ@GSBNFUZQFTUSJOHWBMVFTPNFGSBNFJO XIJDIUIFSPCPUNPWFT  QBSBNOBNFPDUPNBQ@SFTPMVUJPOUZQFEPVCMFWBMVF QBSBNOBNFNBY@SBOHFUZQFEPVCMFWBMVF -PBEUIFSPCPUTQFDJGJDTFOTPSNBOBHFSUIJTTFUTUIF NPWFJU@TFOTPS@NBOBHFS304QBSBNFUFS BSHOBNFNPWFJU@TFOTPS@NBOBHFSEFGBVMUTFWFO@EPG@BSN JODMVEFGJMF GJOETFWFO@EPG@BSN@DPOGJHMBVODI BSH NPWFJU@TFOTPS@NBOBHFS@NPWFJU@TFOTPS@NBOBHFSMBVODIYNM MBVODI

[ 417 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

The following line is commented because it can be used if the robot is mobile. In our case, our robot is static. If it is fixed on a mobile robot, we can give the frame value as PEPN, or PEPN@DPNCJOFE of the robot: QBSBNOBNFPDUPNBQ@GSBNFUZQFTUSJOHWBMVFTPNFGSBNFJOXIJDIUIF SPCPUNPWFT

The following parameter is the resolution of OctoMap, which is visualizing in RViz measured in meters. The rays beyond the NBY@SBOHF value will be truncated: QBSBNOBNFPDUPNBQ@SFTPMVUJPOUZQFEPVCMFWBMVF QBSBNOBNFNBY@SBOHFUZQFEPVCMFWBMVF

The interfacing is now complete. We can test the MoveIt! interface using the following command. Launch Gazebo for perception using the following command, and add the desired grasp table and grasp object model: $ roslaunch seven_dof_arm_gazebo seven_dof_arm_bringup_grasping.launch

Start the MoveIt! planner with sensor support: $ roslaunch seven_dof_arm_config moveit_planning_execution.launch

Now RViz has sensor support. We can see the OctoMap in front of the robot in the following screenshot:

[ 418 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

(KIWTG8KUWCNK\KPIQEVQOCRKP48K\

[ 419 ]

Chapter 12

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

Manipulating objects with MoveIt! Manipulating objects is one of main usages of robotic arms. The capacity to pick up objects and place them in a different location of the robot's workspace is extremely useful both in industry and research applications. The picking process is also known as grasping, and represents a complex task because a lot of constraints are required to pick an object up in a proper way. Humans handle grasping operations using their intelligence, but robots need rules for it. One of the constraints in grasping is the approaching force; the end effector should adjust the grasping force for picking the object, but not make any deformation on the object while grasping. In addition, a grasping pose is needed to pick an object in the best way, and should be calculated considering its shape and its pose. MoveIt! doesn't provide any built-in functionality to find the best approaching or grasping pose to pick objects. For this reason, in this section, we will first discuss how to pick-and-place an object placed into the planning scene considering a known grasping pose. Later, we will present the Grasping Pose Detector (GPD) package, a ROS package able to detect 6-DOF grasp poses based on the point clouds.

Working with a robot pick-and-place task using MoveIt! We can do pick and place in various ways. One is by using pre-defined sequences of joint values; in this case, we put the object in a known position, and move the robot into towards that by providing direct joint values or forward kinematics. Another method of pick and place is by using inverse kinematics without any visual feedback. In this case, we command the robot to move in a cartesian position with respect to the robot base frame, and by solving IK. In this way, the robot can reach that position and pick up that object. One more method is to use external sensors, such as the vision ones, to calculate the pick-and-place positions; in this case, a vision sensor is used to identify the object's location, and the arm goes to that position by solving to pick the object. Of course, the use of vision sensors requires the development of robust algorithms to perform object recognition and tracking, and are able to calculate the best grasping pose to pick that object. But in this section, we want to demonstrate a pick and place sequence, by defining the approaching and grasping position to pick the object and place it on another location of its workspace. We can work with this example along with Gazebo, or simply use the MoveIt! demo interface. The complete source code of this example is reported in the TFWFO@EPS@BSN@UFTUTSDQJDL@QMBDFDQQ file. As we have already seen, we first should initialize the planning scene: SPTJOJU BSHDBSHWTFWFO@EPG@BSN@QMBOOFS

[ 420 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

SPT/PEF)BOEMFOPEF@IBOEMF SPT"TZOD4QJOOFSTQJOOFS  TQJOOFSTUBSU  NPWFJUQMBOOJOH@JOUFSGBDF.PWF(SPVQ*OUFSGBDFHSPVQ BSN NPWFJUQMBOOJOH@JOUFSGBDF1MBOOJOH4DFOF*OUFSGBDF QMBOOJOH@TDFOF@JOUFSGBDF TMFFQ  NPWFJUQMBOOJOH@JOUFSGBDF.PWF(SPVQ*OUFSGBDF1MBONZ@QMBO DPOTUSPCPU@TUBUF+PJOU.PEFM(SPVQ KPJOU@NPEFM@HSPVQ HSPVQHFU$VSSFOU4UBUF  HFU+PJOU.PEFM(SPVQ BSN

Then, we must create the working environment of the robot, placing the grasping table and the grasping object into the scene: NPWFJUQMBOOJOH@JOUFSGBDF1MBOOJOH4DFOF*OUFSGBDFDVSSFOU@TDFOF HFPNFUSZ@NTHT1PTFQPTF TIBQF@NTHT4PMJE1SJNJUJWFQSJNJUJWF QSJNJUJWFUZQFQSJNJUJWF#09 QSJNJUJWFEJNFOTJPOTSFTJ[F  QSJNJUJWFEJNFOTJPOT QSJNJUJWFEJNFOTJPOT QSJNJUJWFEJNFOTJPOT NPWFJU@NTHT$PMMJTJPO0CKFDUHSBTQJOH@PCKFDU HSBTQJOH@PCKFDUJEHSBTQJOH@PCKFDU QPTFPSJFOUBUJPOX QPTFQPTJUJPOZ QPTFQPTJUJPOY QPTFQPTJUJPO[ HSBTQJOH@PCKFDUQSJNJUJWFTQVTI@CBDL QSJNJUJWF HSBTQJOH@PCKFDUQSJNJUJWF@QPTFTQVTI@CBDL QPTF HSBTQJOH@PCKFDUPQFSBUJPOHSBTQJOH@PCKFDU"%% HSBTQJOH@PCKFDUIFBEFSGSBNF@JECBTF@MJOL QSJNJUJWFEJNFOTJPOT QSJNJUJWFEJNFOTJPOT QSJNJUJWFEJNFOTJPOT NPWFJU@NTHT$PMMJTJPO0CKFDUHSBTQJOH@UBCMF HSBTQJOH@UBCMFJEHSBTQJOH@UBCMF QPTFPSJFOUBUJPOX QPTFQPTJUJPOZ QPTFQPTJUJPOY QPTFQPTJUJPO[ HSBTQJOH@UBCMFQSJNJUJWFTQVTI@CBDL QSJNJUJWF HSBTQJOH@UBCMFQSJNJUJWF@QPTFTQVTI@CBDL QPTF HSBTQJOH@UBCMFPQFSBUJPOHSBTQJOH@PCKFDU"%% HSBTQJOH@UBCMFIFBEFSGSBNF@JECBTF@MJOL TUEWFDUPSNPWFJU@NTHT$PMMJTJPO0CKFDU DPMMJTJPO@PCKFDUT DPMMJTJPO@PCKFDUTQVTI@CBDL HSBTQJOH@PCKFDU DPMMJTJPO@PCKFDUTQVTI@CBDL HSBTQJOH@UBCMF

[ 421 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

QVCMJTIPCKFDUTJOUPUIFTDFOF DVSSFOU@TDFOFBEE$PMMJTJPO0CKFDUT DPMMJTJPO@PCKFDUT

Now that the planning scene is properly configured, we can request the motion of the robot towards a pre-configured position of the workspace to bring its end effector close to the object and pick it up: BQQSPBDIJOH HFPNFUSZ@NTHT1PTFUBSHFU@QPTF UBSHFU@QPTFPSJFOUBUJPOY UBSHFU@QPTFPSJFOUBUJPOZ UBSHFU@QPTFPSJFOUBUJPO[ UBSHFU@QPTFPSJFOUBUJPOX UBSHFU@QPTFQPTJUJPOZ UBSHFU@QPTFQPTJUJPOY UBSHFU@QPTFQPTJUJPO[ HSPVQTFU1PTF5BSHFU UBSHFU@QPTF HSPVQNPWF  HSBTQJOH UBSHFU@QPTFQPTJUJPOZ UBSHFU@QPTFQPTJUJPOY UBSHFU@QPTFQPTJUJPO[ HSPVQTFU1PTF5BSHFU UBSHFU@QPTF HSPVQNPWF 

If the grasping succeeded, we can attach the object to the end effector of the robot, in order to place it in another location of the workspace: BUUBDIPCKFDUUPUIFSPCPU NPWFJU@NTHT"UUBDIFE$PMMJTJPO0CKFDUBUUBDDIFE@PCKFDU BUUBDDIFE@PCKFDUMJOL@OBNFHSBTQJOH@GSBNF BUUBDDIFE@PCKFDUPCKFDUHSBTQJOH@PCKFDU DVSSFOU@TDFOFBQQMZ"UUBDIFE$PMMJTJPO0CKFDU BUUBDDIFE@PCKFDU QMBDF UBSHFU@QPTFQPTJUJPOZ UBSHFU@QPTFQPTJUJPOY UBSHFU@QPTFQPTJUJPO[ HSPVQTFU1PTF5BSHFU UBSHFU@QPTF HSPVQNPWF   UBSHFU@QPTFPSJFOUBUJPOY UBSHFU@QPTFPSJFOUBUJPOZ UBSHFU@QPTFPSJFOUBUJPO[ UBSHFU@QPTFPSJFOUBUJPOX UBSHFU@QPTFQPTJUJPOZ UBSHFU@QPTFQPTJUJPOY UBSHFU@QPTFQPTJUJPO[

[ 422 ]

Exploring the Advanced Capabilities of ROS-MoveIt! HSPVQTFU1PTF5BSHFU UBSHFU@QPTF HSPVQNPWF 

Finally, we must remove the object from the robot's gripper: HSBTQJOH@PCKFDUPQFSBUJPOHSBTQJOH@PCKFDU3&.07& BUUBDDIFE@PCKFDUMJOL@OBNFHSBTQJOH@GSBNF BUUBDDIFE@PCKFDUPCKFDUHSBTQJOH@PCKFDU DVSSFOU@TDFOFBQQMZ"UUBDIFE$PMMJTJPO0CKFDU BUUBDDIFE@PCKFDU

To run this example, launch the MoveIt! demo: $ roslaunch seven_dof_arm_config demo.launch

Run the pick and place program: $ rosrun seven_dof_arm_test pick_place

The following is the screenshot of the grasping process:

(KIWTG2KEMCPFRNCEGUGSWGPEGUWUKPI/QXG+V

[ 423 ]

Chapter 12

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

The various steps in the grasping process are explained next: In the first step, we can see a green block, which is the object that is going to be grasped by the robot gripper. We have created this object inside the planning scene using the QJDL@BOE@QMBDF node. In the first part of the node, the end effector of the robot is approaching the object. After approaching the object, a valid trajectory to grasp the object is generated. After the grasping is completed, the green block will be attached to the robot's gripper, and will change its color to purple. After picking the block, the robot will transport it to another place of the workspace, before placing it on the working table. If there is a valid IK in the place pose, the gripper holds the object in the planned trajectory. Finally, the object is placed on the table and detached from from the robot's gripper. Another way to perform the pick and place tasks is by using actions provided by MoveIt!. After launching MoveIt!, two action servers start: pickup: This action accepts a NPWFJU@NTHT1JDLVQ(PBM message in which we mainly must specify the target object to grasp, and a list of possible grasping configurations. These configurations are filled in a NPWFJU@NTHT(SBTQ in which we have to set the complete position of the joints of the robot during the approaching and grasping actions, and the position of the end effector during the picking . place: This action is used to place an object on to a surface. It accepts a NPWFJU@NTHT1MBDF(PBM message to specify a list of possible objects, positioning configuration. Using MoveIt! actions assure the success of safe and complete pick and place tasks, but a lot of pre-planned information is required, making them difficult to use in advanced complex and dynamic robotic applications.

[ 424 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

Calculating grasp poses with GPD In this section, we will present Grasp Pose Detector (IUUQTHJUIVCDPNBUFOQBTHQE ), a ROS package able to detect 6-DOF grasp poses for two-finger grippers, such as the one for our TFWFO@EPG@BSN manipulator. Grasping poses are detected using 3D point clouds, so we can use the depth sensor of the robot to find object grasping poses. This package exploits deep learning and GPU calculation to detect different grasping poses for all the objects detected into the scene. To download this package, just clone the following code repository: $ git clone https://github.com/atenpas/gpd.git

Owing to the use of GPU parallel calculation, to use this package an Nvidia video card is needed. To compile the GPD, the following libraries must be installed: $ sudo apt-get install libgflags-dev libprotobuf-dev liblmdb-dev libleveldb-dev libsnappy-dev libatlas3-base

In addition, GPD uses the Caffe framework (IUUQTHJUIVCDPN#7-$DBGGF) to implement deep learning functionalities. To install this software, follow these lines of code: $ $ $ $

git clone https://github.com/BVLC/caffe.git cd caffe && mkdir build && cd build cmake -DCMAKE_INSTALL_PREFIX:PATH=/usr .. make && sudo make install

If everything goes well, we can build an ROS workspace to compile the GPD package. After completion, we we can test the Grasping Pose Detector on an example dataset, using the following command: $ roslaunch gpd tutorial0.launch

[ 425 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

Within the GUI that appears, press r to center the view, and q to quit the GUI and load the next visualization. The output should show several grasping poses for a point cloud test set, as shown in the next diagram:

(KIWTG)TCURKPIRQUGUIGPGTCVGFQPCRQKPVENQWFVGUVUGV

To test the GPD package on our robotic system, we can use Gazebo with a simulation scene containing an object to grasp: $ roslaunch seven_dof_arm_gazebo seven_dof_arm_bringup_grasping_gpd.launch

[ 426 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

After we've spawned the simulation scene, we can lunch the GPD software with the following command: $ roslaunch seven_dof_arm_test gpd.launch

We can use Rviz to visualize the possible grasping poses generated by GPD:

(KIWTG)TCURKPIRQUGUHQTCPQDLGEVKPVJGRNCPPKPIUEGPG

In the following code, we will see some entries of the Grasping pose Detector launch file: -PBEIBOEHFPNFUSZQBSBNFUFST JODMVEFGJMF GJOEHQEMBVODIIBOE@HFPNFUSZMBVODI BSHOBNFOPEFWBMVFEFUFDU@HSBTQT JODMVEF

[ 427 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

Firstly, we must inform the system of the geometry of the end effector. In the IBOE@HFPNFUSZ launch file, a list of parameters are included to define the width of the gripper fingers, and its depth: QBSBNOBNFDMPVE@UPQJDWBMVFSHCE@DBNFSBEFQUIQPJOUT QBSBNOBNFOVN@TBNQMFTWBMVF QBSBNOBNFOVN@UISFBETWBMVF QBSBNOBNFOVN@TFMFDUFEWBMVF

Here, we define the topic name of the point clouds and the number of samples used to detect the grasping pose; of course, a higher number of samples requires more computation time. Finally, the OVN@TFMFDUFE parameter specifies the number of grasping poses to provide as output. These poses are scored, and the one with the highest score is at the top of the list. After selecting a grasp, you can get information about where to place the robot hand from the EFUFDU@HSBTQTDMVTUFSFE@HSBTQT topic. In this message, the list of grasping configurations is published, defining the position and the orientation of the end effector during the picking action. In particular, this message contains the following field: CPUUPN: A 3D point specifying the centred base of the robotic hand UPQ: A 3D point specifying the centered top of the robotic hand TVSGBDF: A 3D point specifying the centred position on the object surface BQQSPBDI: A 3D vector indicating the grasp approaching direction CJOPSNBM: A 3D vector indicating the grasping surface orientation BYJT: A 3D vector perpendicular to the approaching direction vector XJEUI: The opening width of the gripper needed to grasp the object TDPSF: The grasping pose score

Sadly, right now no direct integration between GPD and MoveIt! are provided.

Pick and place action in Gazebo and real robot The grasping sequence executed in the MoveIt! demo uses fake controllers. We can send the trajectory to the actual robot or Gazebo. In Gazebo, we can launch the grasping world to perform this action. In the real hardware, the only difference is that we need to create joint trajectory controllers for the arm. One of the commonly used hardware controllers is the DYNAMIXEL controller. We will learn more about the DYNAMIXEL controllers in the next section.

[ 428 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

Understanding DYNAMIXEL ROS Servo controllers for robot hardware interfacing Till now, we have learned about MoveIt! interfacing using Gazebo simulation. In this section, we will see how to replace Gazebo and put a real robot interface to MoveIt!. Let's discuss the DYNAMIXEL servos and the ROS controllers.

The DYNAMIXEL servos The DYNAMIXEL servos are smart, high-performance networked actuators for high-end robotics applications. These servos are manufactured by a Korean company called ROBOTIS (IUUQFOSPCPUJTDPN). These servos are very popular among robotics enthusiasts because they can provide excellent position and torque control, and also provide a variety of feedback, such as position, speed, temperature, voltage, and so on. One of their useful features is that they can be networked in a daisy chain manner. This feature is very useful in multi-joint systems, such as a robotic arm, humanoid robots, robotic snakes, and others. The servos can be directly connected to PCs using a USB to DYNAMIXEL controller, which is provided from ROBOTIS. This controller has a USB interface, and when it is plugged into the PC, it acts as a virtual COM port. We can send data to this port, and internally it will convert the RS 232 protocol to Transistor-Transistor Logic (TTL), and in RS 485 standards. The DYNAMIXEL can be powered and then connected to the USB to DYNAMIXEL controller to start working with it. DYNAMIXEL servos support both TTL and RS 485 level standards. The following figure shows the DYNAMIXEL servos, called MX-106, and USB To DYNAMIXEL controller:

(KIWTG&;0#/+:'.5GTXQCPF75$VQ&;0#/+:'.EQPVTQNNGT

[ 429 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

There are different series of DYNAMIXEL available in the market. Some of the series are MX - 28, 64 and 106, RX - 28,64, 106, and so on. The following is the connection diagram of DYNAMIXEL, USB to DYNAMIXEL to PC:

(KIWTG&;0#/+:'.5GTXQUEQPPGEVGFVQ2%WUKPI75$6Q&;0#/+:'.EQPVTQNNGT

Multiple DYNAMIXEL devices can be connected together in sequence (or a daisy chain), as shown in the preceding figure. Each DYNAMIXEL has a firmware setting inside its controller. We can assign the ID of servo, the joint limits, the position limits, the position commands, the PID values, the voltage limits, and so on, inside the controller. There are ROS drivers and controllers for DYNAMIXEL, which are available at: IUUQXJLJSPTPSHEZOBNJYFM@NPUPS.

DYNAMIXEL-ROS interface The ROS stack for interfacing the DYNAMIXEL motor is EZOBNJYFM@NPUPS. This stack contains an interface for DYNAMIXEL motors, such as MX-28, MX64, MX-106, RX-28, RX64, EX106, AX-12, and AX-18. The stack consists of the following packages: EZOBNJYFM@ESJWFS: This package is the driver package of DYNAMIXEL, which

can do low-level IO communication with DYNAMIXEL from the PC. This driver has hardware interface for the previously mentioned series of servos, and can do the read/write operation for DYNAMIXEL through this package. This package is used by high-level packages, such as EZOBNJYFM@DPOUSPMMFST. There are only a few cases when the user directly interacts with this package. EZOBNJYFM@DPOUSPMMFST: This is a higher-level package that works using the EZOBNJYFM@NPUPS package. Using this package, we can create a ROS controller for each DYNAMIXEL joint of the robot. The package contains a configurable node, services, and spawner script to start, stop, and restart one or more controller plugins. In each controller, we can set the speed and the torque. Each DYNAMIXEL controller can be configured using the ROS parameters, or can be loaded by a YAML file. The EZOBNJYFM@DPOUSPMMFST package supports position, torque, and trajectory controllers.

[ 430 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

EZOBNJYFM@NTHT: These are the message definitions that are used inside the EZOBNJYFM@NPUPS stack.

Interfacing 7 DOF DYNAMIXEL-based robotic arm to ROS MoveIt! In this section, we will discuss a 7 DOF robot manipulator called COOL arm-5000, which is manufactured by a company called ASIMOV Robotics (IUUQBTJNPWSPCPUJDTDPN), and is shown in the following figure. The robot is built using DYNAMIXEL servos. We will see how to interface a Dynamixel-based robotic arm to ROS using EZOBNJYFM@DPOUSPMMFST:

(KIWTG%11.TQDQVKE#TO

[ 431 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

COOL arm robots are fully compatible with ROS and MoveIt!, and are mainly used in education and research. Following are the details of the arms: Degree of Freedom: 7 DOF Types of Actuators: DYNAMIXEL MX-64 and MX-28 List of Joints: Shoulder Roll, Shoulder Pitch, Elbow Roll, Elbow Pitch, Wrist Yaw, Wrist Pitch, and Wrist Roll Payload: 5 Kg Reach: 1 meter Work Volume: 2.09 m3 Repeatability: +/- 0.05 mm Gripper with 3 fingers

Creating a controller package for a COOL arm robot The first step is to create a controller package for a COOL arm for interfacing to ROS. The COOL arm controller package is available for download along with the book codes. Before we create the package, we should install the EZOBNJYFM@DPOUSPMMFST package: $ sudo apt-get install ros-kinetic-dynamixel-controllers

The following command will create the controller package with the necessary dependencies. The important dependency of this package is the EZOBNJYFM@DPOUSPMMFST package: $ catkin_create_pkg cool5000_controller roscpp rospy dynamixel_controller std_msgs sensor_msgs

The next step is to create a configuration file for each joint. The configuration file is called DPPMZBNM, which contains a definition of each controller's name, its type, and its parameters. We can see this file in the DPPM@DPOUSPMMFSDPOGJH folder. We have to create parameters for the seven joints in this arm. The following is a snippet of this config file: KPJOU@DPOUSPMMFS DPOUSPMMFS QBDLBHFEZOBNJYFM@DPOUSPMMFST NPEVMFKPJOU@QPTJUJPO@DPOUSPMMFS UZQF+PJOU1PTJUJPO$POUSPMMFS

[ 432 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

KPJOU@OBNFKPJOU KPJOU@TQFFE NPUPS JE JOJU NJO NBY KPJOU@DPOUSPMMFS DPOUSPMMFS QBDLBHFEZOBNJYFM@DPOUSPMMFST NPEVMFKPJOU@QPTJUJPO@DPOUSPMMFS UZQF+PJOU1PTJUJPO$POUSPMMFS KPJOU@OBNFKPJOU KPJOU@TQFFE NPUPS JE JOJU NJO NBY

The controller configuration file mentions the joint name, package of the controller, controller type, joint speed, motor ID, initial position, and minimum and maximum limits of the joint. We can connect as many motors as we want and can create controller parameters by including them in the configuration file. The next configuration file to create is a KPJOU@SBKFDUPSZDPOUSPMMFS configuration. MoveIt! can only interface if the robot has the 'PMMPX+PJOU5SBKFDUPSZ action server. The file called DPPM@USBKFDUPSZ@DPOUSPMMFSZBNM is put in the DPPM@DPOUSPMMFSDPOGJH folder, and its definition is given in the following code: DPPM@USBKFDUPSZ@DPOUSPMMFS DPOUSPMMFS QBDLBHFEZOBNJYFM@DPOUSPMMFST NPEVMFKPJOU@USBKFDUPSZ@BDUJPO@DPOUSPMMFS UZQF+PJOU5SBKFDUPSZ"DUJPO$POUSPMMFS KPJOU@USBKFDUPSZ@BDUJPO@OPEF NJO@WFMPDJUZ DPOTUSBJOUT HPBM@UJNF

[ 433 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

After creating the +PJOU5SBKFDUPSZ controller, we need to create a KPJOU@TUBUF@BHHSFHBUPS node for combining and publishing the joint states of the robotic arm. You can find this node from the DPPM@DPOUSPMMFSTSD folder named KPJOU@TUBUF@BHHSFHBUPSDQQ. The function of this node is to subscribe controller states of each controller with the message type of EZOBNJYFM+PJOU4UBUF, and combine each message of the controller into the TFOTPS@NTHT+PJOU4UBUF messages and publish in the KPJOU@TUBUFT topic. This message will be the aggregate of the joint states of all the DYNAMIXEL controllers. The definition of KPJOU@TUBUF@BHHSFHBUPSMBVODI, which runs the KPJOU@TUBUF@BHHSFHBUPS node with its parameters, follows. It is placed in the DPPM@DPOUSPMMFSMBVODI folder: MBVODI OPEFOBNFKPJOU@TUBUF@BHHSFHBUPSQLHDPPM@DPOUSPMMFS UZQFKPJOU@TUBUF@BHHSFHBUPSPVUQVUTDSFFO SPTQBSBN SBUF DPOUSPMMFST KPJOU@DPOUSPMMFS KPJOU@DPOUSPMMFS KPJOU@DPOUSPMMFS KPJOU@DPOUSPMMFS KPJOU@DPOUSPMMFS KPJOU@DPOUSPMMFS KPJOU@DPOUSPMMFS HSJQQFS@DPOUSPMMFS SPTQBSBN OPEF MBVODI

We can launch the entire controller using the following launch file, called DPPM@DPOUSPMMFSMBVODI, which is inside the MBVODI folder. The code inside this launch file will start communication between the PC and the DYNAMIXEL servos, and will also start the controller manager. The controller manager parameters are serial port, baud rate, servo ID range, and update rate: MBVODI 4UBSUUIF%ZOBNJYFMNPUPSNBOBHFSUPDPOUSPMBMMDPPMTFSWPT  OPEFOBNFEZOBNJYFM@NBOBHFSQLHEZOBNJYFM@DPOUSPMMFST UZQFDPOUSPMMFS@NBOBHFSQZSFRVJSFEUSVFPVUQVUTDSFFO SPTQBSBN OBNFTQBDFEYM@NBOBHFS TFSJBM@QPSUT

[ 434 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

EZOBNJYFM@QPSU QPSU@OBNFEFWUUZ64# CBVE@SBUF NJO@NPUPS@JE NBY@NPUPS@JE VQEBUF@SBUF SPTQBSBN OPEF

In the next step, it should launch the controller spawner by reading the controller configuration file: -PBEKPJOUDPOUSPMMFSDPOGJHVSBUJPOGSPN:".-GJMFUPQBSBNFUFS TFSWFS SPTQBSBNGJMF GJOEDPPM@DPOUSPMMFSDPOGJHDPPMZBNM DPNNBOEMPBE 4UBSUBMM$PPM"SNKPJOUDPOUSPMMFST OPEFOBNFDPOUSPMMFS@TQBXOFSQLHEZOBNJYFM@DPOUSPMMFST UZQFDPOUSPMMFS@TQBXOFSQZ BSHTNBOBHFSEYM@NBOBHFS QPSUEZOBNJYFM@QPSU KPJOU@DPOUSPMMFS KPJOU@DPOUSPMMFS KPJOU@DPOUSPMMFS KPJOU@DPOUSPMMFS KPJOU@DPOUSPMMFS KPJOU@DPOUSPMMFS KPJOU@DPOUSPMMFS HSJQQFS@DPOUSPMMFS PVUQVUTDSFFO

In the next section of the code, it will launch the +PJOU5SBKFDUPSZ controller from the controller configuration file: 4UBSUUIFDPPMBSNUSBKFDUPSZDPOUSPMMFS SPTQBSBNGJMF GJOE DPPM@DPOUSPMMFSDPOGJHDPPM@USBKFDUPSZ@DPOUSPMMFSZBNM DPNNBOEMPBE OPEFOBNFDPOUSPMMFS@TQBXOFS@NFUBQLHEZOBNJYFM@DPOUSPMMFST UZQFDPOUSPMMFS@TQBXOFSQZ BSHTNBOBHFSEYM@NBOBHFS UZQFNFUB DPPM@USBKFDUPSZ@DPOUSPMMFS KPJOU@DPOUSPMMFS KPJOU@DPOUSPMMFS KPJOU@DPOUSPMMFS

[ 435 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

KPJOU@DPOUSPMMFS KPJOU@DPOUSPMMFS KPJOU@DPOUSPMMFS PVUQVUTDSFFO

The following section will launch the KPJOU@TUBUF@BHHSFHBUPS node and the robot description from the DPPM@EFTDSJQUJPO package: 1VCMJTIDPNCJOFEKPJOUJOGP JODMVEFGJMF GJOE DPPM@DPOUSPMMFSMBVODIKPJOU@TUBUF@BHHSFHBUPSMBVODI QBSBNOBNFSPCPU@EFTDSJQUJPODPNNBOE GJOEYBDSPYBDSPQZ  GJOE DPPM@EFTDSJQUJPOSPCPUTDPPMYBDSP  OPEFOBNFKPJOU@TUBUF@QVCMJTIFSQLHKPJOU@TUBUF@QVCMJTIFS UZQFKPJOU@TUBUF@QVCMJTIFSPVUQVUTDSFFO SPTQBSBNQBSBNTPVSDF@MJTU SPTQBSBN SPTQBSBNQBSBNVTF@HVJ '"-4&SPTQBSBN OPEF

This is all about the COOL arm controller package. Next, we need to set up the controller configuration inside the MoveIt! configuration package of the COOL arm, called DPPM@NPWFJU@DPOGJH.

MoveIt! configuration of the COOL Arm The first step is to configure the DPOUSPMMFSTZBNM, which is inside the DPPM@NPWFJU@DPOGJHDPOGJH folder. The definition of this file follows. For now, we are only focusing on moving the arm, and not on handling the gripper control. So the configuration only contains the arm group joints: DPOUSPMMFS@MJTU OBNFDPPM@USBKFDUPSZ@DPOUSPMMFS BDUJPO@OTGPMMPX@KPJOU@USBKFDUPSZ UZQF'PMMPX+PJOU5SBKFDUPSZ EFGBVMUUSVF KPJOUT KPJOU KPJOU KPJOU KPJOU KPJOU KPJOU KPJOU

[ 436 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

The following is the definition of the DPPM@EFTDSJQUJPO@NPWFJU@DPOUSPMMFS@NBOBHFSMBVODIYNM inside DPPM@NPWFJU@DPOGJHMBVODI: MBVODI  4FUUIFQBSBNUIBUUSBKFDUPSZ@FYFDVUJPO@NBOBHFSOFFETUPGJOEUIF DPOUSPMMFSQMVHJO  BSHOBNFNPWFJU@DPOUSPMMFS@NBOBHFS EFGBVMU.PWF*U@TJNQMF@DPOUSPMMFS@NBOBHFS.PWF*U4JNQMF$POUSPMMFS.BOBHFS QBSBNOBNF.PWF*U@DPOUSPMMFS@NBOBHFSWBMVF BSH .PWF*U@DPOUSPMMFS@NBOBHFS MPBEDPOUSPMMFS@MJTU SPTQBSBNGJMF GJOEDPPM@NPWFJU@DPOGJHDPOGJHDPOUSPMMFSTZBNM MBVODI

After configuring MoveIt!, we can start working on the arm. Apply a proper power supply the arm and connect it to connect your PC to the USB of the DYNAMIXEL. We will see a serial device generate; it may be either EFWUUZ64#, or EFWUUZ"$.. According to the device, change the port name inside the controller launch file. Start the DPPM arm controller using the following command: $ roslaunch cool5000_controller cool5000_controller.launch

Start the RViz demo, and start path planning. If we press the Execute button, the trajectory will execute on the hardware arm: $ roslaunch cool5000_moveit_config 5k.launch

[ 437 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

A random pose (which is shown in RViz) and the COOL arm is shown in the following image:

(KIWTG%11.#TORTQVQV[RGYKVJ/QXG+VXKUWCNK\CVKQP

Questions What is the role of the FCL library in MoveIt!? How does MoveIt! build OctoMap of the environment? How could a robot avoid obstacles after grasped an object? What is the main aim of the GPD ROS package? What are the main features of DYNAMIXEL servos?

[ 438 ]

Exploring the Advanced Capabilities of ROS-MoveIt!

Chapter 12

Summary In this chapter, we explored some advanced features of MoveIt!, and how to interface it into a real hardware. The chapter started with a discussion on collision checking using MoveIt!. We saw how to add a collision object using MoveIt! APIs, and also saw the direct importing of mesh to the planning scene. We discussed a ROS node to check collision using MoveIt! APIs. After learning about collisions, we moved to perception using MoveIt!. We connected the simulated point cloud data to MoveIt! and created an OctoMap in MoveIt!. The next topic we discussed was how to perform pick and place actions to manipulate objects in the scene. We presented a ROS package to autonomously generate grasping poses starting from object point clouds. After discussing these things, we switched to hardware interfacing of MoveIt! using dynamixel servos and its ROS controllers. In the end, we saw a real robotic arm called COOL arm and its interfacing to MoveIt!, which was completely built using DYNAMIXEL controllers. In the next chapter, we will discuss how to interface Matlab, a world-renowned numerical computing environment with ROS.

[ 439 ]

13

Using ROS in MATLAB and Simulink In previous chapters, we discussed how to simulate and control robots implementing ROS nodes in C++. In this chapter, we will learn how to create ROS nodes using MATLAB, a powerful piece of software that provides several toolboxes with algorithms and hardware connectivity, for developing autonomous robotic applications for ground vehicles, manipulators, and humanoid robots. In addition, MATLAB integrates Simulink: a block diagram environment for model-based design, allowing the implementation of our control programs through a graphical editor. In this chapter, we will also discuss how to implement robotic applications using Simulink. The first part of this chapter is dedicated to a brief introduction to MATLAB and the Robotic System Toolbox. After we have learned how to exchange data between ROS and MATLAB, we will implement an obstacle avoidance system for the differential drive mobile robot, Turtlebot, showing how simple it is to use components already available in the Robotic System Toolbox and minimize the number of elements to develop in the system. In the second part of the chapter, we will introduce Simulink, showing an initial model as an example, and then discuss a publisher and a subscriber model to demonstrate the Simulink and ROS communication interface. Finally, a control system to regulate the orientation of the Turtlebot robot is developed in Simulink and tested in the Gazebo simulator.

Using ROS in MATLAB and Simulink

Chapter 13

The following are the main topics discussed in this chapter: Getting started with MATLAB and MATLAB-ROS and the Robotic System Toolbox Working with ROS topics in MATLAB Developing a robotic application using MATLAB and Gazebo Working with Simulink and the Simulink-ROS interface Developing a control system in Simulink and Gazebo

Getting started with MATLAB and MATLABROS MATLAB (MATrix LABoratory) is a multi-platform numerical computing environment widely used by industries, universities, and research centers. MATLAB was born as a mathematical software, but now it offers a lot of additional packages for different areas, such as control design, plotting, image processing, robotics, and so on. MATLAB is a proprietary product of MathWorks and it's not a free software. Usually, free licenses are distributed for student and academic institutions. You can use MATLAB on Windows, GNU/Linux, and macOS. After you have launched it, the main window of MATLAB will appear with its default layout, as shown in the following screenshot:

(KIWTG/CKPYKPFQYQH/#6.#$KPKVUFGHCWNVNC[QWV

[ 441 ]

Using ROS in MATLAB and Simulink

Chapter 13

This window includes three main panels: Current Folder: This shows local files Command Window: This is a command line to enter MATLAB commands or run MATLAB scripts Workspace: This shows data created from the Command Window or in the MATLAB scripts Using the Command Window, you can issue mathematical commands and create variables that will be shown in the Workspace. The same window can be used to view MATLAB function documentation. In fact, all the built-in MATLAB functions have supporting documentation, including examples and descriptions of the function inputs, outputs, and calling syntax. You can access the documentation using EPD or IFMQ commands. The first one will open an external window containing the documentation, while the second one will display the documentation in the Command Window. Let's see how to get the documentation about the NFBO function: >> doc mean

You could also use this command: >> help mean

Getting started with the Robotic System Toolbox and ROS-MATLAB interface Beyond the standard functions provided by the default installation of MATLAB, several external toolboxes give you access to other utilities and libraries. To enable the communication between ROS and MATLAB, we need the ROS-MATLAB interface, which is part of the Robotic System Toolbox (IUUQTJUNBUIXPSLTDPNQSPEVDUTSPCPUJDTIUNM). This toolbox provides several algorithms that help us to develop autonomous robot applications, such as path planners, obstacle avoidance methods, state estimations, kinematics, and dynamics algorithms.

[ 442 ]

Using ROS in MATLAB and Simulink

Chapter 13

In addition, this toolbox implements the interface between MATLAB and ROS that enables developers to test and port their applications on real robots and robotic simulators. You can add the Robotic System Toolbox from the packages list during the MATLAB installation, or purchase it from the toolbox website:

(KIWTG4QDQVKE5[UVGO6QQNDQZUGNGEVKQPFWTKPI/#6.#$KPUVCNNCVKQP

Using the Robotic System Toolbox, we can transform MATLAB into a real ROS node able to exchange information with other nodes of the system and directly control simulated or real ROS-enabled robots using topics and services. The following block diagram depicts the connection between MATLAB and ROS. After connecting MATLAB to a ROS master node, it can fetch data to process from the robot or other ROS nodes. MATLAB could itself initialize a ROS master node in order to manage the communication with the nodes of the network, or it could be connected to another remote ROS master, like any other element of the ROS network. In addition, in the final version of the application, we are not forced to run MATLAB on our computer to execute it, but we can deploy the developed application as a typical C++ node:

[ 443 ]

Using ROS in MATLAB and Simulink

Chapter 13

(KIWTG415/#6.#$KPVGTHCEGUEJGOC

With the installation of the Robotic System Toolbox, we will have access to several ROS commands equivalent to the ones used under Linux. To list these commands, you can enter the following line in the Command Window: >> help robotics.ros

[ 444 ]

Using ROS in MATLAB and Simulink

Chapter 13

The output of this command is shown in the following figure:

(KIWTG415/#6.#$KPVGTHCEGEQOOCPFU

To initialize the ROS-MATLAB interface, we can use the SPTJOJU command, while SPTTIVUEPXO is used to stop it. By default, SPTJOJU creates a ROS master node in Matlab, starting a NBUMBC@HMPCBM@OPEF to communicate with the ROS network. We can see the active ROS nodes after initializing SPTDPSF using the SPTOPEF list command:

(KIWTG&GHCWNVKPKVKCNK\CVKQPQHVJG415/#6.#$KPVGTHCEG

[ 445 ]

Using ROS in MATLAB and Simulink

Chapter 13

Using the default configuration of the ROS-MATLAB interface, we must set the 304@."45&3@63* environmental variable on the other node of the ROS network with the IP address of the computer running MATLAB. If you are running MATLAB on Windows, you can easily get the IP address of your computer by using the following command: >> !ipconfig

Or you can use the following command if you are running MATLAB on Linux: >> !ifconfig

The output of this command in Windows is shown in the following screenshot:

(KIWTGKREQPaIEQOOCPFQP/#6.#$TWPPKPIQP9KPFQYU

Otherwise, we can directly connect MATLAB to an active ROS network. In this case, we must inform the ROS-MATLAB interface about the address of the computer/robot where the ROS master is running. This is done with the following command: >> setenv('ROS_MASTER_URI', 'http://192.168.1.131:11311')

(KIWTG+PKVKCNK\KPIVJG415/#6.#$KPVGTHCEGQPCPGZVGTPCN415PGVYQTM

In the next section, we will start to work with topic callbacks, initializing the ROS-MATLAB interface and elaborating data directly from MATLAB scripts.

[ 446 ]

Using ROS in MATLAB and Simulink

Chapter 13

Starting with ROS topics and MATLAB callback functions In this section, we will discuss how to publish and subscribe ROS messages using MATLAB scripts. The first script that we analyze defines a typical template to develop the control loop of our robot. Firstly, we will subscribe to an input topic, and, successively, we will republish its value on an output topic for a certain amount of time. The complete source code is contained in the UBMLFSN, in the code provided with the book, or you can clone the following Git repository: $ git clone https://github.com/jocacace/ros_matlab_test

Let's see the content of the UBMLFSN script: SPT@NBTUFS@JQ IUUQ  NBUMBC@JQ   SPTJOJU SPT@NBTUFS@JQ /PEF)PTU NBUMBC@JQ QBVTF XBJUBCJUUIFSPTDPSFJOJUJBMJ[BUJPO UBMLFS@TVCSPTTVCTDSJCFS  UBMLFS  SPTQVCMJTIFS DIBUUFS  TUE@NTHT4USJOH  SSPTSBUF )[MPPQSBUF GPSJ EBUBUBMLFS@TVC-BUFTU.FTTBHF DIBUUFS@NTH%BUBEBUB%BUB TFOE DIBUUFS@QVCDIBUUFS@NTH XBJUGPS S FOE SPTTIVUEPXO

Let's see how the script works: SPT@NBTUFS@JQ IUUQ  NBUMBC@JQ   SPTJOJU SPT@NBTUFS@JQ /PEF)PTU NBUMBC@JQ

In the preceding code, we initialize the MATLAB-ROS node. In this example, we want to connect MATLAB to an external ROS network and make it able to both read and write data on topics. For this reason, we should export both 304@."45&3@63* and 304@)045/".& environmental variables. Change the IP addresses on the base of your system configuration: UBMLFS@TVCSPTTVCTDSJCFS UBMLFS  SPTQVCMJTIFS DIBUUFS  TUE@NTHT4USJOH 

[ 447 ]

Using ROS in MATLAB and Simulink

Chapter 13

Then, we subscribe to the UBMLFS topic while initializing the advertiser to the DIBUUFS topic of the TUE@NTHT4USJOH type: EBUBUBMLFS@TVC-BUFTU.FTTBHF TFOE DIBUUFS@QVCDIBUUFS@NTH

Finally, we use the -BUFTU.FTTBHF function to get the last message on the input topic, while publishing the message on theDIBUUFS topic. At this point, you can publish the desired message on the UBMLFS topic, using the command line from one of the computers running Linux in the same network of the MATLAB computer, and visualize the message published on the DIBUUFS topic. Before running the MATLAB script, be sure to have correctly exported the 304@)045/".& variable on the computer where you want to publish the message in order to enable MATLAB to receive the published data. Now, you can run the script by typing its name in the Command Window: >> talker

If everything has been correctly set, the output on the Linux machine should appear like in the following screenshot:

(KIWTG%QOOWPKECVKQPDGVYGGP/#6.#$CPF415

[ 448 ]

Using ROS in MATLAB and Simulink

Chapter 13

The previous script defines a typical template to implement the control loop of an autonomous robot. Instead of continuously asking for the last message received on the topics, we can define a callback function that is called every time that a new message is received. In this way, we could write more complex control loops to handle the robot behavior, asynchronously receiving multiple information from ROS topics. In the next example, we will start to connect ROS-MATLAB to Gazebo, simulating the Turtlebot robot and plotting the value of its laser sensor using MATLAB. To run the Gazebo simulation, we will use the UVSUMFCPU@HB[FCP package: $ roslaunch turtlebot_gazebo turtlebot_world.launch

After starting Gazebo, different topics are published, among which isTDBO . In this example, we need the following MATLAB functions: QMPU@MBTFSN: This initializes the ROS-MATLAB interface subscribing to the

desired laser scanner topic and plots the laser data at a desired frame rate HFU@MBTFSN: This receives and stores the value of the laser scanner data Let's look at the code of the QMPU@MBTFS script: GVODUJPOQMPU@MBTFS  HMPCBMMBTFS@NTH SPT@NBTUFS@JQ IUUQ  NBUMBC@JQ   SPTJOJU SPT@NBTUFS@JQ /PEF)PTU NBUMBC@JQ QBVTF  MBTFS@TVCSPTTVCTDSJCFS TDBO !HFU@MBTFS SSPTSBUF )[MPPQSBUF GPSJ QMPU MBTFS@NTH .BYJNVN3BOHF  XBJUGPS S FOE SPTTIVUEPXO DMPTFBMM FOE

[ 449 ]

Using ROS in MATLAB and Simulink

Chapter 13

After setting up the ROS-MATLAB interface, we initialize the subscriber to the laser scan topic: MBTFS@TVCSPTTVCTDSJCFS TDBO !HFU@MBTFS

With this line, we demand, the HFU@MBTFS function handles the data contained in the TDBO topic. To exchange data between different MATLAB scripts, we use a global variable: HMPCBMMBTFS@NTH

Finally, we plot the laser scanner data of the laser data for 25 seconds: QMPU MBTFS@NTH .BYJNVN3BOHF 

Let's now look at the code of the HFU@MBTFS function: GVODUJPOHFU@MBTFS _NFTTBHF HMPCBMMBTFS@NTH MBTFS@NTHNFTTBHF FOE

In this function, we just save the value of the laser scanner data. After launching the Gazebo simulation, we can run the MATLAB script: >> plot_laser

[ 450 ]

Using ROS in MATLAB and Simulink

Chapter 13

The output of the the default placement of the scene objects is shown in the following screenshot:

(KIWTG)C\GDQNCUGTUECPPGTFCVCRNQVVGFKP/#6.#$

[ 451 ]

Using ROS in MATLAB and Simulink

Chapter 13

Implementing an obstacle avoidance system for a Turtlebot robot Until now, we have used MATLAB only to exchange data using ROS topics. In this section, we are going to demonstrate how easy it is to create a robotic application for a mobile robot using MATLAB and the Robotic System Toolbox. We will design an obstacle avoidance system for a differential mobile robot, that allows the Turtlebot robot to navigate a crowded environment without colliding with any obstacle. We will present a MATLAB script that will set the control velocities of the robot to generate a random movement. At the same time, the laser scanner data of the sensor of the robot will be used to avoid obstacles. To implement this behavior, we will rely on the Vector Field Histogram (VFH) algorithm to compute the obstacle-free steering directions of the robot, based on range sensor readings. This algorithm is already provided by the Robotic System Toolbox in the SPCPUJDT7FDUPS'JFME)JTUPHSBN class. Finally, after some navigation time, some log data will be plotted, using MATLAB function. This could help developers to debug our application. The complete source code of the script that we are going to discuss can be found in the WGI@PCTUBDMF@BWPJEBODFN source file: GVODUJPOWGI@PCTUBDMF@BWPJEBODF SPT@NBTUFS@JQNBUMBC@JQ SPTJOJU SPT@NBTUFS@JQ /PEF)PTU NBUMBC@JQ QBVTF  MBTFS4VCSPTTVCTDSJCFS TDBO  PEPN4VCSPTTVCTDSJCFS PEPN  SPTQVCMJTIFS NPCJMF@CBTFDPNNBOETWFMPDJUZ  WGISPCPUJDT7FDUPS'JFME)JTUPHSBN WGI%JTUBODF-JNJUT WGI3PCPU3BEJVT WGI.JO5VSOJOH3BEJVT WGI4BGFUZ%JTUBODF S@NBY S@NJO PC@EJTU PNPE@WFM SBUFSPCPUJDT3BUF  PEPN@WFM@Y PEPN@WFM@[ PEPN@QPT@Y PEPN@QPT@Z XIJMFSBUF5PUBM&MBQTFE5JNF (FUMBTFSTDBOEBUB MBTFS4DBOSFDFJWF MBTFS4VC

[ 452 ]

Using ROS in MATLAB and Simulink

Chapter 13

PEPNSFDFJWF PEPN4VC SBOHFTEPVCMF MBTFS4DBO3BOHFT BOHMFTEPVCMF MBTFS4DBOSFBE4DBO"OHMFT PEPN@WFM@Y PEPN@WFM@[> Simulink

Then, you should choose to create a new blank model. To create a new system, we must import the desired Simulink blocks that will compose it. These blocks can be directly dragged and dropped into the model window from the Library Browser. To open the Library Browser, select View | Library Browser from the model pane toolbar. For our first system, we need four blocks: Sine Wave: This generates a sinusoidal signal that will represent the input of our system Integrator: This integrates an input signal Bus Creator: This combines multiple signals in one signal Scope: This graphically visualizes the input signal

[ 458 ]

Using ROS in MATLAB and Simulink

Chapter 13

After importing these blocks, your model pane should appear like in the following figure:

(KIWTG6JGUKPGYCXGKPVGITCVQTDWUETGCVQTCPFUEQRG5KOWNKPMDNQEMU

Some blocks must be properly configured with some parameters. For example, the Sine Wave block requires the amplitude and the frequency sinusoidal signal to generate. To set these values, we can explore block parameters with a double click on the desired block. To make the system work, we need to properly connect the Simulink blocks, as shown in this model:

(KIWTG5KPWUQKFCNUKIPCNKPVGITCVQT

Now that the model components have been connected, we can simulate the behavior of our system. First, we should configure the duration of the simulation setting the Start and Stop simulation time. Open the Simulation | Model Configuration Parameters window and insert the desired value. In our example, we are considering 4UBSUUJNF and 4UPQ UJNF:

(KIWTG5KOWNCVKQPVKOGHQTQWTU[UVGO

[ 459 ]

Using ROS in MATLAB and Simulink

Chapter 13

Now, we can press the play button in the model pane toolbar, while we check the output by exploring the content of the Scope block, with a double click on it:

(KIWTG5KPWUQKFCNCPFKPVGITCVGFUKIPCN

Note that, even if we inserted 10 seconds of simulation time, Simulink will not work in real time but only simulate the increment of the time steps along the simulation. In this way, the effective elapsed time during the simulation will be very short. The model proposed in this example can be found in the book source code in the SPT@NBUMBC@UFTUTUBSJOH@FYBNQMFNEM model file.

[ 460 ]

Using ROS in MATLAB and Simulink

Chapter 13

Working with ROS messages in Simulink Simulink support for ROS allows us to model systems that can be linked to other nodes of the ROS network. This support includes a library of Simulink blocks for sending and receiving messages via topics. When we start the simulation of the developed model, Simulink will try to connect to a ROS network, which can be running on the same computer where Simulink is or on another remote machine. Once this connection is established, Simulink exchanges messages with the ROS network until the simulation is terminated. As we did in the previous section, we will start by showing how to read and write data, using ROS topics, and then we will discuss how to create a more complex system to control the Turtlebot robot simulated in Gazebo. Let's start to create two different Simulink models. In one model, we are going to develop a message publisher while in the other one we will implement a simple subscriber. These models can be found in the source code directory, SPT@NBUMBC@UFTU, called QVCMJTIFSNEM and TVCTDSJCFSNEM, respectively.

Publishing a ROS message in Simulink To publish a ROS message in Simulink, we mainly need two blocks: Publish: This block sends a message on the ROS network. Using block parameters, we can specify the topic name and the message type. Blank message: This block creates a blank message with the specified message type. Let's see how to connect these blocks to publish a HFPNFUSZ@NTHT5XJTU message on a new topic, called QPTJUJPO. Get started by importing the blank message block from the library browser and configuring the type of message by double-clicking on it. From the block parameters pane, we can press the Select button to select the ROS message type from a list, as shown in the following screenshot:

[ 461 ]

Using ROS in MATLAB and Simulink

Chapter 13

(KIWTG2CTCOGVGTEQPaIWTCVKQPHQTC5KOWNKPM415DNCPMOGUUCIGDNQEM

Now we are ready to import the ROS publish block: drag and drop the block to the model and double click on it to configure the topic name and the message type. Select Specify your own for the topic source field in order to enter a desired topic name. Enter QPTJUJPO in the topic field. As we have already seen, we can select the type of the message to publish:

(KIWTG2CTCOGVGTEQPaIWTCVKQPHQTC5KOWNKPM4152WDNKUJDNQEM

[ 462 ]

Using ROS in MATLAB and Simulink

Chapter 13

Now, we must fill in the fields of the ROS message to publish before sending it into the ROS network. We will use two other Simulink components to do this work. The first is the Sine Wave, the sinusoidal signal generation already used in the first Simulink example. The second one is a signal bus assignment. In fact, a ROS message is represented as a bus signal in the Simulink environment, allowing us to manage its field using the bus signal block. Connect the output port of the blank message block to the bus input port of the BusAssignment block. Connect the output port of the BusAssignment block to the input port of the ROS publish block. Then configure the bus signal parameters: double-click on the BusAssignment block. You should see 9, : , and ; (the signals comprising a HFPNFUSZ@NTHT5XJTt message) listed on the left. Remove the element in the right list and select both the 9 and : signals of the linear part of the message in the left list, click Select >> , and then click OK to close the block mask. In this case, we will assign only the first two components of the linear part of the 5XJTU message:

(KIWTG$WUCUUKIPOGPVHQTC

HFPNFUSZ@NTHT5XJTUOGUUCIG

[ 463 ]

Using ROS in MATLAB and Simulink

Chapter 13

After completing the parameter configuration of the bus assignment module, the shape of the block will change, accepting the value of the selected input signals. Now, we should assign the desired value to publish to these components. We can do this by using the Sine Wave block, as we did in the previous example. Drag and drop two sinusoidal signal generators, linking them to the bus assignment block. The final model will look as follows:

(KIWTG2WDNKUJGT5KOWNKPMOQFGN

[ 464 ]

Using ROS in MATLAB and Simulink

Chapter 13

An additional block has been included in our publisher Simulink model: SPT@SBUF. This block is needed to simulate a real-time behavior during the execution of our model, implementing the ROS rate mechanism. Without this module, in fact, the execution rate of this node will be very high, publishing ROS messages at its maximum frequency. The SPT@SBUF block is a special module called the MATLAB System block and allows us to instantiate and invoke a NBUMBCDMBTT object. After importing this block into the system model, we should select the System object name to invoke or to create a new one:

(KIWTG/#6.#$U[UVGODNQEM

The code of the SPT@SBUF block is in the SPT@SBUFN source file: DMBTTEFGSPT@SBUFNBUMBC4ZTUFN 1VCMJDUVOBCMFQSPQFSUJFT QSPQFSUJFT 3"5& FOE QSPQFSUJFT %JTDSFUF4UBUF FOE 1SFDPNQVUFEDPOTUBOUT QSPQFSUJFT "DDFTTQSJWBUF SBUF0CK FOE NFUIPET "DDFTTQSPUFDUFE GVODUJPOTFUVQ*NQM PCK 1FSGPSNPOFUJNFDBMDVMBUJPOTTVDIBTDPNQVUJOHDPOTUBOUT PCKSBUF0CKSPCPUJDT3BUF PCK3"5& FOE GVODUJPOTUFQ*NQM PCK PCKSBUF0CKXBJUGPS  FOE GVODUJPOSFTFU*NQM PCK

[ 465 ]

Using ROS in MATLAB and Simulink

Chapter 13

FOE FOE FOE

In this code, we defined the SPT@SBUF class, which has two objects: the rate specifying the loop frequency, and SBUF0CK, which implements the SPCPUJDT3BUF mechanism. The most important methods of this class are the TFUVQ*NQM PCK method that is called at the start of the simulation and is used to initialize the class stuff, and the TUFQ*NQM PCK method that is invoked at each step time in order to regulate the execution time of the simulation. Now that our model is complete, we require a never-ending duration for our simulation setting to JOG the 4UPQUJNF of the simulation. In this way, we can terminate the simulation when desired by using the stop button. Now we can play the simulation and read the content published on the QPTJUJPO topic.

Subscribing to a ROS topic in Simulink To subscribe to a ROS topic, we only need the Subscribe block. Even in this case, we must configure the type of the message to read and the topic name. Let's select the QPTJUJPO topic in order to read the data sent to the ROS network by the publisher Simulink model. The Subscriber block has two outputs: IsNew, a Boolean signal that defines if a new message is received, and Msg, which contains the received message:

(KIWTG5KOWNKPM5WDUETKDGTDNQEM

[ 466 ]

Using ROS in MATLAB and Simulink

Chapter 13

In the publisher model, if we have used a bus creator to aggregate multiple data in one message, then we need to split the data of the message. For this, we will use a bus selector block with one input and two outputs: the 9 and : fields of the linear part of the twist message. To create this block, configure it in order to have, as the selected signals, only the -JOFBS9 and -JOFBS: parts of the twist message:

(KIWTG$WU5GNGEVQTDNQEM

[ 467 ]

Using ROS in MATLAB and Simulink

Chapter 13

In our implementation, we include the bus selector into a Subsystem, another type of block that can be enabled/disabled with the use of an enable port. In this way, we can link the IsNew field of the Subscriber block to the Subsystem, and enable its output only if a new message is received. To explore the content of a subsystem, it is enough to double click on it, like any other block. Finally, we can add two scope blocks to plot the output of the subsystem. The final linked model is shown in the following figure:

(KIWTG5WDUETKDGTU[UVGOOQFGN

We can now run both the publisher and subscriber systems and check the output on the scope blocks.

[ 468 ]

Using ROS in MATLAB and Simulink

Chapter 13

Developing a simple control system in Simulink Now that we have learned how to interface Simulink and ROS, we can try to implement a more complex system that is able to control a real or simulated robot. We will continue to work with the Turtlebot robot simulated in Gazebo, and we will see how to control its orientation in order to bring it to a desired value. In other words, we will implement a control system that will measure the orientation of the robot using its odometry, comparing this value with the desired orientation and obtaining the orientation error. We will use a PID controller to calculate the velocity to actuate the robot to reach the final desired orientation, setting the orientation error to zero. This controller is already available in Simulink, so we don't need to implement it by ourselves. Let's start to discuss all the elements of our model:

(KIWTG6WTVNGDQVQTKGPVCVKQPEQPVTQNOQFGNKP5KOWNKPM

The input of the system is represented by the PEPN message, which contains information about the actual pose of the robot and its velocity, and the constant block, which specifies the desired orientation of the Turtlebot.

[ 469 ]

Using ROS in MATLAB and Simulink

Chapter 13

The first thing that our model does is to estimate the orientation from the PEPN message. The orientation is estimated by considering the angular velocity of the robot, integrating it at each time step. We use a MATLAB function block to threshold the velocity value of the PEPN message to discard noise measurements. To integrate the velocity data, we use the Integrator block provided by Simulink. Again, we include this part in a subsystem:

(KIWTG/#6.#$HWPEVKQPDNQEM

The MATLAB function block allows developers to translate their own MATLAB functions into Simulink blocks. In this case, the code function is as follows: GVODUJPOZGDO V Z JGBCT V  ZV FOE FOE

We extract the "OHVMBS; value from the received twist message that specifies the angular velocity with respect to the ; axes, representing which direction is rotating the robot. We consider as noise the values below 0.01 rad/s. Now that we know how to rotate the robot, we can calculate the orientation error by considering the desired orientation (that is constant) using the Simulink sum block. To change the desired orientation, we can double click on the constant block and configure its parameters.

[ 470 ]

Using ROS in MATLAB and Simulink

Chapter 13

Finally, we can implement our robot controller. For this scope, we will use a PID controller, one of the most commonly used control loop mechanisms with feedback. This kind of controller is widely used both in industry and university settings for a variety of applications. It continuously tries to minimize the input error, applying a control output based on proportional, integral, and derivative terms, which give the controller its name. After dragging and dropping this controller in the model, its response to the input data will depends on P, I, and D terms (called gains) that can be properly tuned from the block proprieties. Finally, we must publish the data generated by the PID controller on the CBTFDPNNBOETWFMPDJUZ topic to actuate the robot in the Gazebo simulation. As usual, we can check on the scope block how the orientation error decreases after starting the simulation. Before applying the calculated velocity, we use another MATLAB function block to set the sign of the velocity. In fact, considering the sign of the velocity, the robot will rotate in two different directions: a negative velocity will make the robot rotate in a clockwise direction, while a positive velocity will make the robot rotate in a counter-clockwise direction. In our case, we want to choose the direction that will bring the robot more quickly to its direction: GVODUJPOB@WFMGDO WDNENJT B@WFM JG NJTDNE B@WFMBCT W FMTFJG NJT DNE B@WFMBCT W FOE FOE

This function block receives as input the calculated velocity, the commanded, and the actual orientation of the robot. When the measured orientation is lower than the commanded one, the robot must rotate in a clockwise direction, or otherwise in a counter-clockwise direction.

Configuring the Simulink model Now that our model is fully connected, we only need to configure and simulate it. Firstly, we need to import the SPT@SBUF module to synchronize the Simulink simulation. In this case, a higher frame rate assures a better behavior, so you can double click on the SPT@SBUF block and set the rate to 100 Hz. Then, open the Model Configuration Parameters by clicking on Simulation | Model Configuration Parameters from the main menu bar of the model window or just type Ctrl + E. A suggested configuration is to use a Fixed-step size solver, specifying the desired step size (we can use  seconds):

[ 471 ]

Using ROS in MATLAB and Simulink

Chapter 13

(KIWTG/QFGNEQPaIWTCVKQPRCTCOGVGTU

Now that the model has been configured, we can simulate it. As in the last example, launch the Turtlebot simulation: $ roslaunch turtlebot_gazebo turtlebot_world.launch

[ 472 ]

Using ROS in MATLAB and Simulink

Chapter 13

Then push the play button to start the Simulink simulation. On Gazebo, you should see the robot that tries to reach the desired orientation, while on Simulink, you can use the scope panels to monitor the orientation error and the generated velocity command:

(KIWTG1TKGPVCVKQPGTTQTGXQNWVKQP

[ 473 ]

Using ROS in MATLAB and Simulink

Chapter 13

Questions What is MATLAB and the Robotic System Toolbox? How can we connect MATLAB with the ROS network? Why is MATLAB useful to develop robotic applications? What is Simulink? What is a PID controller and how can we implement it using Simulink?

Summary In this chapter, we learned how to use MATLAB to develop simple or complex robotic applications and how to connect MATLAB with the other ROS nodes running on the same computer or in other nodes of the ROS network. We discussed how to handle topics in MATLAB and how to develop a simple obstacle avoidance system for a differential driver robot, reusing functions already available in the MATLAB toolboxes. Then, we introduced Simulink, a graphically-based program editor that allow developers to implement, simulate, and validate their dynamic system models. We learned how to get and set data into the ROS network and how do develop a simple control system that controls the orientation of the Turtlebot robot. In the next chapter, we will present ROS-Industrial, a ROS package to interface industrial robot manipulators to ROS, and how to control it using the power of ROS, such as MoveIt!, Gazebo, RViz, and so on.

[ 474 ]

14

ROS for Industrial Robots Until now, we have been mainly discussing interfacing personal and research robots with ROS, but one of the main areas where robots are extensively used is in industry. Does ROS support industrial robots? Are there any companies that use ROS for handling manufacturing processes? The ROS-Industrial package comes with a solution to interface and control industrial robot manipulators to ROS, using its powerful tools, such as MoveIt!, Gazebo, and RViz. In this chapter, we will discuss the following topics: Understanding and getting started with ROS-Industrial Creating a URDF for an industrial robot and interfacing it with MoveIt! Working with MoveIt! configuration of a Universal Robots arm and ABB robots Understanding ROS-Industrial robot support packages Understanding ROS-Industrial robot client and driver packages Working with IKfast algorithms and the MoveIt! IKFast plugin Let's start with a brief overview of ROS-Industrial.

ROS for Industrial Robots

Chapter 14

Understanding ROS-Industrial packages ROS-Industrial basically extends the advanced capabilities of ROS software to industrial robots employed in manufacturing processes. ROS-Industrial consists of many software packages, which help us to control industrial robots. These packages are BSD (legacy)/Apache 2.0 (preferred) licensed programs, which contain libraries, drivers, and tools with a standard solution for industrial hardware. ROS-Industrial is now guided by the ROS-Industrial Consortium. The official website of ROS-Industrial (ROS-I) can be found at IUUQSPTJOEVTUSJBMPSH:

(KIWTG.QIQQH415+PFWUVTKCN

Goals of ROS-Industrial The main goals behind ROS-Industrial development are as follows: Combining the strength of ROS with the existing industrial technologies to explore the advanced capabilities of ROS in the manufacturing process Developing a reliable and robust software for industrial robot applications Providing an easy way to do research and development in industrial robotics Creating a wide community supported by researchers and professionals for industrial robotics Providing industrial-grade ROS applications and becoming a one-stop location of industry-related applications

[ 476 ]

ROS for Industrial Robots

Chapter 14

ROS-Industrial ` a brief history In 2012, the ROS-Industrial open source project started as the collaboration of Yaskawa Motoman Robotics (IUUQXXXNPUPNBODPN), Willow Garage (IUUQTXXXXJMMPXHBSBHFDPN), and the Southwest Research Institute (SwRI) (IUUQXXXTXSJPSH), for using ROS in industrial manufacturing. The ROS-I was founded by Shaun Edwards in January 2012. In March 2013, the ROS-I Consortium Americas was launched, led by SwRI, and ROS-I Consortium Europe was launched, led by Fraunhofer IPA in Germany.

Benefits of ROS-Industrial Let's look at the benefits ROS-I provides to the community: Explores the features in ROS: The ROS-Industrial packages are tied to the ROS framework so that we can use all the ROS features in industrial robots too. Using ROS, we can create custom IK solvers for each robot and implement object manipulation, using 2D/3D perception. Out-of-the-box applications: The ROS interface enables advanced perception in robots for working with picking and placing complex objects. Simplifies robotic programming: ROS-I eliminates teaching and planning paths of robots and, instead, automatically calculates a collision-free optimal path for the given points. Open source: ROS-I is open source software that allows commercial use without any restrictions.

Installing ROS-Industrial packages Installing ROS-I packages can be done by using package managers or building from the source code. If we have installed the SPTLJOFUJDEFTLUPQGVMM installation, we can use the following command to install ROS-Industrial packages on Ubuntu, working with ROS Kinetic: $ sudo apt-get install ros-kinetic-industrial-core

[ 477 ]

ROS for Industrial Robots

Chapter 14

The preceding command will install the core packages of ROS-Industrial packages. The JOEVTUSJBMDPSF stack includes the following set of ROS packages: JOEVTUSJBMDPSF: This stack contains packages and libraries for supporting

industrial robotic systems. The package consists of nodes for communicating with industrial robot controllers and industrial robot simulators, and also provides ROS controllers for industrial robots. JOEVTUSJBM@EFQSFDBUFE: This package contains nodes, launch files, and so on that are going to be deprecated. The files inside this package could be deleted from the repository in the next ROS versions, so we should look for the replacement of these files before the content is going to be deleted. JOEVTUSJBM@NTHT: This package contains message definitions, which are specific to the ROS-Industrial packages. TJNQMF@NFTTBHF: This is a part of ROS-Industrial packages that is a standard message protocol containing a simple messaging framework for communicating with industrial robot controllers. JOEVTUSJBM@SPCPU@DMJFOU: This package contains a generic robot client for connecting to industrial robot controllers, which is running an industrial robot server and can communicate using a simple message protocol. JOEVTUSJBM@SPCPU@TJNVMBUPS: This package simulates the industrial robot controller, which follows the ROS-Industrial driver standard. Using this simulator, we can simulate and visualize the industrial robot. JOEVTUSJBM@USBKFDUPSZ@GJMUFST: This package contains libraries and plugins for filtering the trajectories, which are sent to the robot controller.

Block diagram of ROS-Industrial packages The following diagram is a simple block diagram representation of ROS-I packages, which are organized on top of ROS. We can see the ROS-I layer on top of the ROS layers. We can see a brief description of each of the layers for better understanding. The following diagram is taken from the ROS-I wiki page (IUUQXJLJSPTPSH*OEVTUSJBM): The ROS GUI: This layer includes the ROS plugin-based GUI tools layer, which consists of tools such as RViz, SRU@HVJ, and so on The ROS-I GUI: These GUIs are standard industrial UIs for working with industrial robots that may be implemented in the future The ROS Layer: This is the base layer in which all communications are taking place

[ 478 ]

ROS for Industrial Robots

Chapter 14

The MoveIt! Layer: The MoveIt! layer provides a direct solution to industrial manipulators in planning, kinematics, and pick and place The ROS-I Application Layer: This layer consists of an industrial process planner, which is used to plan what is to be manufactured, how it will be manufactured, and what resources are needed for the manufacturing process The ROS-I Interface Layer: This layer consists of the industrial robot client, which can be connected to the industrial robot controller using the simple message protocol The ROS-I Simple Message Layer: This is the communication layer of the industrial robot, which is a standard set of protocols that will send data from the robot client to the controller and vice versa The ROS-I Controller Layer: This layer consists of vendor-specific industrial robot controllers

(KIWTG6JGDNQEMFKCITCOQH415+PFWUVTKCN

[ 479 ]

ROS for Industrial Robots

Chapter 14

After discussing the basic concepts, we will start to interface an industrial robot to ROS using ROS-Industrial. Firstly, we will show how to create a URDF model of an industrial robot and how to create a proper MoveIt! configuration for it. Then, we will discuss how to control real and simulated Universal Robots and Abb industrial manipulators, analyzing all the necessary elements of a ROS-I package. Finally, we will work with the Ikfast algorithm and plugin to speed up kinematic calculation with MoveIt!.

Creating a URDF for an industrial robot Creating the URDF file for an ordinary robot and an industrial robot are the same, but industrial robots require some standards that should be strictly followed during their URDF modeling, which are as follows: Simplify the URDF design: The URDF file should be simple and readable and only need the important tags Develop a common design: Develop a common design formula for all industrial robots by various vendors Modularize the URDF: The URDF needs to be modularized using XACRO macros and it can be included in a large URDF file without much hassle The following points are the main differences in the URDF design followed by ROS-I. Collision-aware: The industrial robot IK planners are collision-aware, so the URDF should contain an accurate collision 3D mesh for each link. Every link in the robot should export to STL or DAE with a proper coordinate system. The coordinate system that ROS-I is following is X-axes pointing forward and Z-axes pointing up when each joint is in the zero position. It is also to be noted that, if the joint's origin coincides with the base of the robot, the transformation will be simpler. It will be good if we are putting robot-based joints in the zero position (origin), which can simplify the robot design. In ROS-I, the mesh file used for visual purposes is highly detailed, but the mesh file used for collision will not be detailed, because it takes more time to perform collision checking. In order to remove the mesh details, we can use tools such as MeshLab (IUUQNFTIMBCTPVSDFGPSHFOFU), using its option ('JMUFST 3FNFTIJOH4JNQMJGJDBUJPOBOE3FDPOTUSVDUJPO $POWFY)VMM).

[ 480 ]

ROS for Industrial Robots

Chapter 14

URDF joint conventions: The orientation value of each robot joint is limited to single rotation, that is, out of the two orientation (SPMM, QJUDI, and ZBX) values, only one value will be there. Xacro macros: In ROS-I, the entire manipulator section is written as a macro using YBDSP. We can add an instance of this macro in another macro file, which can be used for generating a URDF file. We can also include additional endeffector definitions on this same file. Standards frames: In ROS-I, the CBTF@MJOL frame should be the first link and UPPM (tool-zero) should be the end-effector link. Also, the CBTF frame should match with the CBTF of the robot controller. In most cases, the transform from CBTF to CBTF@MJOL is treated as fixed. After building the YBDSP file for the industrial robot, we can convert to URDF and verify it using the following command: $ rosrun xacro xacro -inorder -o $ check_urdf

Next, we can discuss the differences in creating the MoveIt! configuration for an industrial robot.

Creating the MoveIt! configuration for an industrial robot The procedure for creating the MoveIt! interface for industrial robots is the same as the other ordinary robot manipulators, except in some standard conventions. The following procedure gives a clear idea about these standard conventions: 1. Launch the MoveIt! setup assistant by using the following command: $ roslaunch moveit_setup_assistant setup_assistant.launch

2. Load the URDF from the robot description folder or convert xacro to the URDF and load to the setup assistant.

[ 481 ]

ROS for Industrial Robots

Chapter 14

3. Create a Self-Collisiona matrix with a Sampling Density of about ~ 80,000. This value can increase the collision checking in the arm. 4. Add a Virtual Joints matrix, as shown in the following screenshot. Here the virtual and parent frame names are arbitrary:

(KIWTG#FFKPI/QXG+VXKTVWCNLQKPVU

5. In the next step, we are adding Planning Groups for manipulator and End Effector. Here, also, the group names are arbitrary. The default plugin is KDL; we can change it even after creating the MoveIt! configuration:

[ 482 ]

ROS for Industrial Robots

Chapter 14

(KIWTG%TGCVKPI2NCPPKPI)TQWRUKP/QXG+V

[ 483 ]

ROS for Industrial Robots

Chapter 14

6. The Planning Groups, that is, the manipulator plus the endeffector configuration, will be shown like this:

(KIWTG2NCPPKPIITQWRUQHOCPKRWNCVQTUCPFGPFGbGEVQTUKP/QXG+V

7. We can assign Robot Poses, such as home position, up position, and so on. This setting is an optional one. 8. We can assign End Effectors, as shown in the following screenshot; this is also an optional setting:

(KIWTG5GVVKPIGPFGbGEVQTUKP/QXG+V5GVWR#UUKUVCPV

[ 484 ]

ROS for Industrial Robots

Chapter 14

9. After setting the end effector, we can directly generate the configuration files. It should be noted that the NPWFJUDPOGJH package should be named as SPCPU@OBNF @NPWFJU@DPOGJH, where SPCPU@OBNF is the name of the URDF file. Also, if we want to move this generated DPOGJH package to another PC, we need to edit the TFUVQ@BTTJTUBOU file, which is inside the NPWFJU package. We should change the absolute path to the relative path. Here is an example of the BCC@JSC robot. We should mention the relative path of URDF and SRDF in this file, as follows: NPWFJU@TFUVQ@BTTJTUBOU@DPOGJH 63%' QBDLBHFBCC@JSC@TVQQPSU SFMBUJWF@QBUIVSEGJSCVSEG 43%' SFMBUJWF@QBUIDPOGJHBCC@JSCTSEG $0/'*( HFOFSBUFE@UJNFTUBNQ

Updating the MoveIt! configuration files After creating the MoveIt! configuration, we should update the DPOUSPMMFSTZBNM file inside the DPOGJH folder of the MoveIt! package. Here is an example of DPOUSPMMFSTZBNM: DPOUSPMMFS@MJTU OBNF BDUJPO@OTGPMMPX@KPJOU@USBKFDUPSZ UZQF'PMMPX+PJOU5SBKFDUPSZ KPJOUT TIPVMEFS@QBO@KPJOU TIPVMEFS@MJGU@KPJOU FMCPX@KPJOU XSJTU@@KPJOU XSJTU@@KPJOU XSJTU@@KPJOU

[ 485 ]

ROS for Industrial Robots

Chapter 14

We should also update KPJOU@MJNJUTZBNM about the joint information. Here is a code snippet of KPJOU@MJNJUTZBNM: KPJOU@MJNJUT TIPVMEFS@QBO@KPJOU IBT@WFMPDJUZ@MJNJUTUSVF NBY@WFMPDJUZ IBT@BDDFMFSBUJPO@MJNJUTUSVF NBY@BDDFMFSBUJPO

We can also change the Kinematic solver plugin by editing the LJOFNBUJDTZBNM file. After editing all the configuration files, we need to edit the DPOUSPMMFSNBOBHFSMBVODI file (SPCPU @NPWFJU@DPOGJHMBVODISPCPU @NPWFJU@DPOUSPMMFS@NBOBHFSMBVODI). Here is an example of the DPOUSPMMFSNBOBHFSMBVODI file: MBVODI SPTQBSBNGJMF GJOEVS@NPWFJU@DPOGJHDPOGJHDPOUSPMMFSTZBNM QBSBNOBNFVTF@DPOUSPMMFS@NBOBHFSWBMVFGBMTF QBSBNOBNFUSBKFDUPSZ@FYFDVUJPOFYFDVUJPO@EVSBUJPO@NPOJUPSJOH WBMVFGBMTF QBSBNOBNFNPWFJU@DPOUSPMMFS@NBOBHFSWBMVF NPWFJU@TJNQMF@DPOUSPMMFS@NBOBHFS.PWF*U4JNQMF$POUSPMMFS.BOBHFS MBVODI

After creating the controller manger, we need to create the SPCPU @NPWFJU@QMBOOJOH@FYFDVUJPOMBVODI file. Here is an example of this file: MBVODI BSHOBNFTJNEFGBVMUGBMTF BSHOBNFMJNJUFEEFGBVMUGBMTF BSHOBNFEFCVHEFGBVMUGBMTF 3FNBQGPMMPX@KPJOU@USBKFDUPSZ SFNBQJG BSHTJNGSPNGPMMPX@KPJOU@USBKFDUPSZ UPBSN@DPOUSPMMFSGPMMPX@KPJOU@USBKFDUPSZ -BVODINPWFJU JODMVEFGJMF GJOEVS@NPWFJU@DPOGJHMBVODINPWF@HSPVQMBVODI BSHOBNFMJNJUFEEFGBVMU BSHMJNJUFE BSHOBNFEFCVHEFGBVMU BSHEFCVH JODMVEF MBVODI

[ 486 ]

ROS for Industrial Robots

Chapter 14

Testing the MoveIt! configuration After editing the configuration and launch files in the MoveIt! configuration, we can start running the robot simulation and can check whether the MoveIt! configuration is working well or not. Ensure that the SPTJOEVTUSJBMTJNVMBUPS package is installed properly. Here are the steps to test an industrial robot: 1. Start the robot simulator. 2. Start the MoveIt! planning execution launch file using the following command line: $ roslaunch _moveit_config moveit_planning_execution.launch

3. Open RViz and load the RViz Motion planning plugin, using the Plan and Execute button. We can plan and execute the trajectory on the simulated robot.

Installing ROS-Industrial packages of Universal robotic arms Universal Robots (IUUQXXXVOJWFSTBMSPCPUTDPN) is an industrial robot manufacturer based in Denmark. The company mainly produces three arms: UR3, UR5, and UR10. The robots are shown in the following figure:

(KIWTG7474CPF74TQDQVU

[ 487 ]

ROS for Industrial Robots

Chapter 14

The specifications of these robots are given in the following table: Robot

UR-3

UR-5

UR-10

Working radius 500 mm 850 mm 1,300 mm Payload

3 kg

5 kg

10 kg

Weight

11 kg

18.4 kg

28.9 kg

Footprint

118 mm 149 mm 190 mm

In the next section, we will install the Universal Robots packages and work with the MovetIt! interface to simulate industrial robots in Gazebo.

Installing the ROS interface of Universal Robots We can install the Universal Robots packages by using the Ubuntu/Debian package manager: $ sudo apt-get install ros-kinetic-universal-robot

Or, we can directly download these packages from the following repository: $ git clone https://github.com/ros-industrial/universal_robot.git

The Universal Robot packages are: VS@EFTDSJQUJPO: This package consists of the robot description and Gazebo

description of UR-3, UR-5, and UR-1. VS@ESJWFS: This package contains client nodes, which can communicate with the UR-3, UR-5, and UR-10 robot hardware controllers. VS@CSJOHVQ: This package consists of launch files to start communication with the robot hardware controllers to start working with the real robot. VS@HB[FCP: This package consists of Gazebo simulations of UR-3, UR-5, and UR-10. VS@NTHT: This package contains ROS messages used for communication between various UR nodes.

[ 488 ]

ROS for Industrial Robots

Chapter 14

VS99@NPWFJU@DPOGJH: These are the NPWFJU config files of Universal Robot

manipulators. One different package exists for each type of arm (VS@NPWFJU@DPOGJHVS@NPWFJU@DPOGJH and VS@NPWFJU@DPOGJH). VS@LJOFNBUJDT: This package contains kinematic solver plugins for UR-3, UR-5, and UR-10. We can use this solver plugin in MoveIt! After installing or compiling the Universal Robots packages, we can launch the simulation in Gazebo of the UR-10 robot by using the following command: $ roslaunch ur_gazebo ur10.launch

(KIWTG7PKXGTUCN4QDQV74OQFGNUKOWNCVKQPKP)C\GDQ

We can see the robot controller configuration file for interfacing into the MoveIt! package. The following YAML file defines the +PJOU5SBKFDUPSZ controller. It is placed in the VS@HB[FCPDPOUSPMMFS folder with the name BSN@DPOUSPMMFS@VSZBNM: BSN@DPOUSPMMFS UZQFQPTJUJPO@DPOUSPMMFST+PJOU5SBKFDUPSZ$POUSPMMFS KPJOUT TIPVMEFS@QBO@KPJOU TIPVMEFS@MJGU@KPJOU FMCPX@KPJOU XSJTU@@KPJOU XSJTU@@KPJOU XSJTU@@KPJOU DPOTUSBJOUT

[ 489 ]

ROS for Industrial Robots

Chapter 14

HPBM@UJNF TUPQQFE@WFMPDJUZ@UPMFSBODF TIPVMEFS@QBO@KPJOU\USBKFDUPSZHPBM^ TIPVMEFS@MJGU@KPJOU\USBKFDUPSZHPBM^ FMCPX@KPJOU\USBKFDUPSZHPBM^ XSJTU@@KPJOU\USBKFDUPSZHPBM^ XSJTU@@KPJOU\USBKFDUPSZHPBM^ XSJTU@@KPJOU\USBKFDUPSZHPBM^ TUPQ@USBKFDUPSZ@EVSBUJPO TUBUF@QVCMJTI@SBUF BDUJPO@NPOJUPS@SBUF

Understanding the Moveit! configuration of a Universal Robot arm The MoveIt! configuration for Universal Robot arms is in the DPOGJH directory of each NPWFJU@DPOGJH package (VS@NPWFJU@DPOGJH for the UR-10 configuration). Here is the definition of the DPOUSPMMFSZBNM of UR-10: DPOUSPMMFS@MJTU OBNF BDUJPO@OTGPMMPX@KPJOU@USBKFDUPSZ UZQF'PMMPX+PJOU5SBKFDUPSZ KPJOUT TIPVMEFS@QBO@KPJOU TIPVMEFS@MJGU@KPJOU FMCPX@KPJOU XSJTU@@KPJOU XSJTU@@KPJOU XSJTU@@KPJOU

[ 490 ]

ROS for Industrial Robots

Chapter 14

In the same directory, we can find the kinematic configuration: LJOFNBUJDTZBNM. This file specifies the IK solvers used for the robotic arm. For the UR-10 robot, the content of the kinematic configuration file is shown here: NBOJQVMBUPS LJOFNBUJDT@TPMWFSVS@LJOFNBUJDT63,JOFNBUJDT1MVHJO LJOFNBUJDT@TPMWFS@TFBSDI@SFTPMVUJPO LJOFNBUJDT@TPMWFS@UJNFPVU LJOFNBUJDT@TPMWFS@BUUFNQUT NBOJQVMBUPS LJOFNBUJDT@TPMWFSLEM@LJOFNBUJDT@QMVHJO,%-,JOFNBUJDT1MVHJO LJOFNBUJDT@TPMWFS@TFBSDI@SFTPMVUJPO LJOFNBUJDT@TPMWFS@UJNFPVU LJOFNBUJDT@TPMWFS@BUUFNQUT

The definition of VS@NPWFJU@DPOUSPMMFS@NBOBHFSMBVODI inside the MBVODI folder is given as follows. This launch file loads the trajectory controller configuration and starts the trajectory controller manager: MBVODI SPTQBSBNGJMF GJOEVS@NPWFJU@DPOGJHDPOGJHDPOUSPMMFSTZBNM QBSBNOBNFVTF@DPOUSPMMFS@NBOBHFSWBMVFGBMTF QBSBNOBNFUSBKFDUPSZ@FYFDVUJPOFYFDVUJPO@EVSBUJPO@NPOJUPSJOH WBMVFGBMTF QBSBNOBNFNPWFJU@DPOUSPMMFS@NBOBHFS WBMVFNPWFJU@TJNQMF@DPOUSPMMFS@NBOBHFS.PWF*U4JNQMF$POUSPMMFS.BOBHFS MBVODI

After discussing these files, let's see how to plan a motion using MoveIt! and simulate it using Gazebo: 1. Start the simulation of UR-10 with joint trajectory controllers: $ roslaunch ur_gazebo ur10.launch

2. Start the MoveIt! nodes for motion planning. We can set TJNUSVF, to test MoveIt! only in simulation: $ roslaunch ur10_moveit_config ur10_moveit_planning_execution.launch sim:=true

[ 491 ]

ROS for Industrial Robots

Chapter 14

3. Launch RViz with the MoveIt! visualization plugin: $ roslaunch ur10_moveit_config moveit_rviz.launch config:=true

(KIWTG/QVKQPRNCPPKPIKP74OQFGNKP48K\

We can move the end-effector position of the robot and plan the path by using the Plan button. When we press the Execute button or the Plan and Execute button, the trajectory should send to the simulated robot, performing the motion in the Gazebo environment.

Getting started with real Universal Robots hardware and ROS-I After testing our control algorithms in simulation using Gazebo, we can start to perform manipulation tasks with a real Universal Robots arm. The main difference between performing a trajectory simulating the robot and using real hardware, is that we need to start the driver that will contact the arm controller to set the desired joint positions.

[ 492 ]

ROS for Industrial Robots

Chapter 14

The default driver of Universal Robot arms is released with the VS@ESJWFS package of ROS-I. This driver has been successfully tested with system versions ranging from W to W. The last version of Universal Robot controllers is W, so the default version of the ROS-I driver could be not fully compatible. For the newer versions of these systems (WY and up), it is recommended to use the unofficial VS@NPEFSO@ESJWFS package. To download VS@NPEFSO@ESJWFS, use the following Git repository: $ git clone ur10_moveit_config ur10_moveit_planning_execution.launch sim:=true

After downloading this package, we need to compile the workspace to be able to use the driver. The next step is to configure Universal Robots hardware to control it from our computer. Firstly, we must enable the networking capabilities of the Universal Robot, using its teachpendant. Navigate into the Robot -> Setup Network Menu in order to select a proper configuration compatible with our network. If you prefer to have a fixed internet address for the robot, you must select the Static Address option and manually input the desired address information. Anyway, select the DHCP option, and then apply the configuration. After setting the IP address, it could be useful to check the connection status by pinging the robot controller: $ ping IP_OF_THE_ROBOT

If the controller replies to the ping command, the connection is successfully established, and we can start to control the manipulator. If your Universal Robots system has a version lower than WY, we can bring it up by running the following command: $ roslaunch ur_bringup ur10_bringup.launch robot_ip:=IP_OF_THE_ROBOT [reverse_port:=REVERSE_PORT]

Replace the *1@0'@5)&@30#05 with the IP address assigned to the robot controller. Then, we can test the motion of the robot by using the following script: $ rosrun ur_driver IP_OF_THE_ROBOT [reverse_port:=REVERSE_PORT]

[ 493 ]

ROS for Industrial Robots

Chapter 14

To operate with systems greater than WY, we can use launch files provided by the VS@NPEFSO@ESJWFS package: $ roslaunch ur_modern_driver ur10_bringup.launch robot_ip:=IP_OF_THE_ROBOT [reverse_port:=REVERSE_PORT]

The next step is to use MoveIt! to control the robot: $ roslaunch ur10_moveit_config ur5_moveit_planning_execution.launch $ roslaunch ur10_moveit_config moveit_rviz.launch config:=true

Note that for some desired robot configurations, MoveIt! could have difficulties with finding plans with full joint limits. There is another version with lower restrictions of the joint limits. This operative mode can be started simply by using the argument MJNJUFE in the launch command: $ roslaunch ur10_moveit_config ur5_moveit_planning_execution.launch limited:=true

We have seen how to simulate and control a Universal Robot. In the next section, we will work with ABB robots.

Working with MoveIt! configuration of ABB robots We will work with two of the most popular ABB industrial robot models: IRB 2400 and IRB 6640. The following are the images of these two robots and their specifications:

[ 494 ]

ROS for Industrial Robots

Chapter 14

(KIWTG#$$+4$CPF+4$

The specification of these robotic arms is given in the following table: Robot

IRB 2400-10 IRB 6640-130

Working radius 1.55 m

3.2 m

Payload

12 kg

130 kg

Weight

380 kg

1,310-1,405 kg

Footprint

723x600 mm 1,107 x 720 mm

To work with ABB packages, clone the ROS packages of the robot into the DBULJO workspace. We can use the following command to do this task: $ git clone https://github.com/ros-industrial/abb

[ 495 ]

ROS for Industrial Robots

Chapter 14

Then, build the source packages using DBULJO@NBLF. Alternatively, we can also install packages using the Ubuntu/Debian package management system. However, to follow the reminder tutorial of this chapter, it is suggested to clone the ABB repository in your ROS workspace. The following package will install the complete set of ABB robot packages: $ sudo apt-get install ros-kinetic-abb

To launch the ABB IRB 6640 in RViz for motion planning, use the following command: $ roslaunch abb_irb6640_moveit_config demo.launch

The RViz window will be open and we can start motion planning the robot in RViz:

(KIWTG/QVKQPRNCPPKPIQH#$$+4$

[ 496 ]

ROS for Industrial Robots

Chapter 14

One of the other popular ABB robot models is the IRB 2400. We can launch the robot in RViz by using the following command: $ roslaunch abb_irb2400_moveit_config demo.launch

(KIWTG/QVKQPRNCPPKPIQH#$$+4$

[ 497 ]

ROS for Industrial Robots

Chapter 14

Understanding the ROS-Industrial robot support packages The ROS-I robot support packages are a new convention followed for industrial robots. The aim of these support packages is to standardize the ways of maintaining ROS packages for a wide variety of industrial robot types of different vendors. Because of a standardized way of keeping files inside support packages, we don't have any confusion in accessing the files inside them. We can demonstrate a support package of an ABB robot and can see the folders and files and their uses. We have already cloned the ABB robot packages, and inside this folder we can see three support packages that support three varieties of ABB robots. Here we are taking the ABB IRB 2400 model TVQQPSU package: BCC@JSC@TVQQPSU. This is the support package of the ABB industrial robot model called IRB 2400. The following list shows the folders and files inside this package: DPOGJH: As the name of the folder, this contains the configuration files of joint

names, RViz configuration, and robot model specific configuration. KPJOU@OBNFT@JSC: Inside the DPOGJH folder, there is a configuration file, which contains the joint names of the robot that is used by the ROS controller. MBVODI: This folder contains the launch file definitions of this robot. These files are following a common convention in all industrial robots. MPBE@JSCMBVODI: This file simply loads SPCPU@EFTDSJQUJPO on the parameter server. According to the complexity of the robot, the number of xacro files can be increased. This file loads all xacro files in a single launch file. Instead of writing separate code for adding SPCPU@EFTDSJQUJPO in other launch files, we can simply include this launch file. UFTU@JSCMBVODI: This launch file can visualize the loaded URDF. We can inspect and verify the URDF in RViz. This launch file includes the preceding launch files and starts theKPJOU@TUBUF@QVCMJTIFS and SPCPU@TUBUF@QVCMJTIFS nodes, which helps to interact with the user on RViz. This will work without the need for real hardware. SPCPU@TUBUF@WJTVBMJ[F@JSCMBVODI: This launch file visualizes the current state of the real robot by running nodes from the ROS-Industrial driver package with appropriate parameters. The current state of the robot is visualized by running RViz and the SPCPU@TUBUF@QVCMJTIFS node. This launch file needs a real robot or simulation interface. One of the main arguments provided along with this launch file is the IP address of the industrial controller. Also note that the controller should run a ROS-Industrial server node.

[ 498 ]

ROS for Industrial Robots

Chapter 14

SPCPU@JOUFSGBDF@EPXOMPBE@JSCMBVODI: This launch file starts bi-

directional communication with the industrial robot controller to ROS and vice versa. There are industrial robot client nodes for reporting the state of the robot (SPCPU@TUBUFOPEF) and subscribing the joint command topic and issuing the joint position to the controller (KPJOU@USBKFDUPSZOPEF). This launch file also requires access to the simulation or real robot controller and needs to mention the IP address of the industrial controllers. The controller should run the ROSIndustrial server programs too. VSEG: This folder contains the set of standardized YBDSP files of the robot model. JSC@NBDSPYBDSP: This is the xacro definition of a specific robot. It is not a

complete URDF, but it's a macro definition of the manipulator section. We can include this file inside another file and create an instance of this macro. JSCYBDSP: This is the top level YBDSP file, which creates an instance of the macro, which was discussed in the preceding section. This file doesn't include any other files other than the macro of the robot. This YBDSP file will be loading inside the MPBE@JSCMBVODI file that we have already discussed. JSCVSEG: This is the URDF generated from the preceding YBDSP file, using the YBDSP tool. This file is used when the tools or packages can't load YBDSP directly. This is the top-level URDF for this robot. NFTIFT: This contains meshes for visualization and collision checking JSC: This folder contains mesh files for a specific robot WJTVBM: This folder contains STL files used for visualization DPMMJTJPO: This folder contains STL files used for collision checking UFTUT: This folder contains the test launch file to test all the preceding launch files SPTMBVODI@UFTUYNM: This launch file tests all the launch files

Visualizing the ABB robot model in RViz After creating the robot model, we can test it using the UFTU@JSCMBVODI file. The following command will launch the test interface of the ABB IRB 2400 robot: $ roslaunch abb_irb2400_support test_irb2400.launch

[ 499 ]

ROS for Industrial Robots

Chapter 14

It will show the robot model in RViz with a joint state publisher node, as shown in the following screenshot:

(KIWTG#$$+4$YKVJLQKPVUVCVGRWDNKUJGTQP48K\

We can adjust the robot joints by adjusting the joint state publisher slider values. Using this testing interface, we can confirm whether the URDF design is correct or not.

ROS-Industrial robot client package The industrial robot client nodes are responsible for sending robot position/trajectory data from ROS MoveIt! to the industrial robot controller. The industrial robot client converts the trajectory data to TJNQMF@NFTTBHF and communicates to the robot controller using the TJNQMF@NFTTBHF protocol. The industrial robot controller running a server and industrial robot client nodes are connecting to this server and start communicating with this server.

[ 500 ]

ROS for Industrial Robots

Chapter 14

Designing industrial robot client nodes The JOEVTUSJBM@SPCPU@DMJFOU package contains various classes to implement industrial robot client nodes. The main functionalities that a client should have include updating the robot current state from the robot controller, and also sending joint position messages to the controller. There are two main nodes that are responsible for getting the robot state and sending joint position values: The SPCPU@TUBUF node: This node is responsible for publishing the robot's current position, status, and so on The KPJOU@USBKFDUPSZ node: This node subscribes the robot's command topic and sends the joint position commands to the robot controller via the simple message protocol The following figure gives the list of APIs provided by the industrial robot client:

(KIWTG#NKUVQHVJGKPFWUVTKCNTQDQVENKGPV#2+U

We can briefly go through these APIs and their functionalities, as follows: 3PCPU4UBUF*OUFSGBDF: This class contains methods to publish the current robot

position and status at regular intervals after receiving the position data from the robot controller. +PJOU3FMBZ)BOEMFS: The 3PCPU4UBUF*OUFSGBDF class is a wrapper around a class called .FTTBHF.BOBHFS. What it does is it listens to the TJNQMF@NFTTBHF robot connection and processes each message handling, using .FTTBHFIBOEMFST. The +PJOU3FMBZ)BOEMFS functionality is a .FTTBHF)BOEMFS, and its function is to publish the joint position in the KPJOU@TUBUFT topic.

[ 501 ]

ROS for Industrial Robots

Chapter 14

3PCPU4UBUVT3FMBZ)BOEMFS: This is another .FTTBHF)BOEMFS, that can publish the current robot status info in the SPCPU@TUBUVT topic. +PJOU5SBKFDUPSZ*OUFSGBDF: This class contains methods to send the robot's

joint position to the controller when it receives a ROS trajectory command. +PJOU5SBKFDUPSZ%PXOMPBEFS: This class is derived from the +PJOU5SBKFDUPSZ*OUFSGBDF class, and it implements a method called TFOE@UP@SPCPU . This method sends an entire trajectory as a sequence of messages to the robot controller. The robot controller will execute the trajectory in the robot only after getting all sequences sent from the client. +PJOU5SBKFDUPSZ4USFBNFS: This class is the same as the preceding class except in the implementation of the TFOE@UP@SPCPU  method. This method sends independent joint values to the controller in separate threads. Each position command is sent only after the execution of the existing command. In the robot side, there will be a small buffer for receiving the position to make the motion smoother. The list of nodes inside the industrial robot client are as follows: SPCPU@TUBUF: This node is running based on 3PCPU4UBUF*OUFSGBDF, which

can publish the current robot states NPUJPO@EPXOMPBE@JOUFSGBDF: This node runs +PJOU5SBKFDUPSZ%PXOMPBEFS, which will download the trajectory in sequence to the controller NPUJPO@TUSFBNJOH@JOUFSGBDF: This node runs +PJOU5SBKFDUPSZ4USFBNFS, which will send the joint position in parallel using threading KPJOU@USBKFDUPSZ@BDUJPO: This node provides a basic BDUJPOMJC interface

ROS-Industrial robot driver package In this section, we will discuss the industrial robot driver package. If we take the ABB robot as an example, it has a package called BCC@ESJWFS. This package is responsible for communicating with the industrial robot controller. This package contains industrial robot clients and launches the file to start communicating with the controller. We can check what's inside the BCC@ESJWFSMBVODI folder. The following is a definition of a launch file called SPCPU@JOUFSGBDFMBVODI: MBVODI SPCPU@JQ*1BEESFTTPGUIFSPCPU TTPDLFUNFTTBHJOHTFSWFS

[ 502 ]

ROS for Industrial Robots

Chapter 14

BSHOBNFSPCPU@JQ +@DPVQMFETFU536&UPBQQMZDPSSFDUJPOGPS++QBSBMMFMMJOLBHF  BSHOBNF+@DPVQMFEEFGBVMUGBMTF DPQZUIFTQFDJGJFEBSHVNFOUTUPUIF1BSBNFUFS4FSWFSGPSVTFCZ OPEFTCFMPX QBSBNOBNFSPCPU@JQ@BEESFTTUZQFTUSWBMVF BSHSPCPU@JQ QBSBNOBNF+@DPVQMFEUZQFCPPMWBMVF BSH+@DPVQMFE SPCPU@TUBUFQVCMJTIFTKPJOUQPTJUJPOTBOESPCPUTUBUFEBUB  GSPNTPDLFUDPOOFDUJPOUPSPCPU OPEFQLHBCC@ESJWFSUZQFSPCPU@TUBUFOBNFSPCPU@TUBUF NPUJPO@EPXOMPBE@JOUFSGBDFTFOETSPCPUNPUJPODPNNBOETCZ %08/-0"%*/(QBUIUPSPCPU  VTJOHTPDLFUDPOOFDUJPOUPSPCPU OPEFQLHBCC@ESJWFSUZQFNPUJPO@EPXOMPBE@JOUFSGBDF OBNFNPUJPO@EPXOMPBE@JOUFSGBDF KPJOU@USBKFDUPSZ@BDUJPOQSPWJEFTBDUJPOMJCJOUFSGBDFGPSIJHIMFWFM SPCPUDPOUSPM OPEFQLHJOEVTUSJBM@SPCPU@DMJFOUUZQFKPJOU@USBKFDUPSZ@BDUJPO OBNFKPJOU@USBKFDUPSZ@BDUJPO MBVODI

This launch file provides a socket-based connection to ABB robots using the standard ROSIndustrial TJNQMF@NFTTBHF protocol. Several nodes are started to supply both low-level robot communication and high-level BDUJPOMJC support: SPCPU@TUBUF: This publishes the current joint positions and robot state data NPUJPO@EPXOMPBE@JOUFSGBDF: This commands the robot motion by sending

motion points to the robot KPJOU@USBKFDUPSZ@BDUJPO: This is the BDUJPOMJC interface to control the robot motion Their usage is as follows: $ robot_interface.launch robot_ip:=IP_OF_THE_ROBOT [J23_coupled:=false]

[ 503 ]

ROS for Industrial Robots

Chapter 14

We can see the BCC@JSC@TVQQPSUMBVODI SPCPU@JOUFSGBDF@EPXOMPBE@JSCMBVODI file, and this is the driver for the ABB IRB 6640 model. This definition of launch is given in the following code. The preceding driver launch file is included in this launch file. In other support packages of other ABB models, use the same driver with different joint configuration parameter files: MBVODI BSHOBNFSPCPU@JQ BSHOBNF+@DPVQMFEEFGBVMUUSVF SPTQBSBNDPNNBOEMPBEGJMF GJOE BCC@JSC@TVQQPSUDPOGJHKPJOU@OBNFT@JSCZBNM JODMVEFGJMF GJOEBCC@ESJWFSMBVODISPCPU@JOUFSGBDFMBVODI BSHOBNFSPCPU@JQWBMVF BSHSPCPU@JQ BSHOBNF+@DPVQMFEWBMVF BSH+@DPVQMFE JODMVEF MBVODI

The preceding file is the manipulator-specific version of SPCPU@JOUFSGBDFMBVODI (of BCC@ESJWFS): Defaults provided for IRB 2400: +@DPVQMFEUSVF Usage: SPCPU@JOUFSGBDF@EPXOMPBE@JSCMBVODISPCPU@JQWBMVF We should run the driver launch file to start communicating with the real robot controller. For the ABB robot IRB 2400, we can use the following command to start bi-directional communication with the robot controller and the ROS client: $ roslaunch abb_irb2400_support robot_interface_download_irb2400.launch robot_ip:=IP_OF_THE_ROBOT

After launching the driver, we can start planning by using the MoveIt! interface. It should also be noted that the ABB robot should be configured and the IP of the robot controller should be found before starting the robot driver.

[ 504 ]

ROS for Industrial Robots

Chapter 14

Understanding the MoveIt! IKFast plugin One of the default numerical IK solvers in ROS is KDL. KDL is mainly using DOF > 6. In robots DOF