Programming Robots with ROS A PRACTICAL INTRODUCTION TO THE ROBOT OPERATING SYSTEM Morgan Quigley, Brian Gerkey & William D Smart www.it-ebooks.info www.it-ebooks.info Programming Robots with ROS Morgan Quigley, Brian Gerkey, and William D Smart Boston www.it-ebooks.info Programming Robots with ROS by Morgan Quigley, Brian Gerkey, and William D Smart Copyright © 2015 Morgan Quigley, Brian Gerkey, and William D Smart All rights reserved Printed in the United States of America Published by O’Reilly Media, Inc., 1005 Gravenstein Highway North, Sebastopol, CA 95472 O’Reilly books may be purchased for educational, business, or sales promotional use Online editions are also available for most titles (http://safaribooksonline.com) For more information, contact our corporate/ institutional sales department: 800-998-9938 or corporate@oreilly.com Acquisitions Editor: Mike Loukides Editors: Meghan Blanchette and Dawn Schanafelt Production Editor: Matthew Hacker Copyeditor: Rachel Head Proofreader: Amanda Kersey December 2015: Indexer: WordCo Indexing Services, Inc Interior Designer: David Futato Cover Designer: Ellie Volckhausen Illustrator: Rebecca Demarest First Edition Revision History for the First Edition 2015-11-18 First Release See http://oreilly.com/catalog/errata.csp?isbn=9781449323899 for release details The O’Reilly logo is a registered trademark of O’Reilly Media, Inc Programming Robots with ROS, the cover image of a Salim Ali’s fruit bat, and related trade dress are trademarks of O’Reilly Media, Inc While the publisher and the authors have used good faith efforts to ensure that the information and instructions contained in this work are accurate, the publisher and the authors disclaim all responsibility for errors or omissions, including without limitation responsibility for damages resulting from the use of or reliance on this work Use of the information and instructions contained in this work is at your own risk If any code samples or other technology this work contains or describes is subject to open source licenses or the intellectual property rights of others, it is your responsibility to ensure that your use thereof complies with such licenses and/or rights 978-1-4493-2389-9 [LSI] www.it-ebooks.info Table of Contents Preface xi Part I Fundamentals Introduction Brief History Philosophy Installation Summary 4 Preliminaries The ROS Graph roscore catkin, Workspaces, and ROS Packages catkin Workspaces ROS Packages rosrun Names, Namespaces, and Remapping roslaunch The Tab Key tf: Coordinate Transforms Poses, Positions, and Orientations tf Summary 11 13 13 13 14 17 22 23 25 25 26 27 29 Topics 31 Publishing to a Topic Checking That Everything Works as Expected Subscribing to a Topic 32 34 36 iii www.it-ebooks.info Checking That Everything Works as Expected Latched Topics Defining Your Own Message Types Defining a New Message Using Your New Message When Should You Make a New Message Type? Mixing Publishers and Subscribers Summary 37 38 39 41 45 47 47 48 Services 51 Defining a Service Implementing a Service Checking That Everything Works as Expected Other Ways of Returning Values from a Service Using a Service Checking That Everything Works as Expected Other Ways to Call Services Summary 51 55 56 56 57 58 58 59 Actions 61 Defining an Action Implementing a Basic Action Server Checking That Everything Works as Expected Using an Action Checking That Everything Works as Expected Implementing a More Sophisticated Action Server Using the More Sophisticated Action Checking That Everything Works as Expected Summary 62 64 66 67 68 68 71 72 74 Robots and Simulators 77 Subsystems Actuation: Mobile Platform Actuation: Manipulator Arm Sensors Computation Complete Robots PR2 Fetch Robonaut TurtleBot Simulators iv | Table of Contents www.it-ebooks.info 77 77 80 81 87 88 88 89 90 91 92 Stage Gazebo Other Simulators Summary 93 95 96 97 Wander-bot 99 Creating a Package Reading Sensor Data Sensing and Actuation: Wander-bot! Summary Part II 99 103 106 108 Moving Around Using ROS Teleop-bot 111 Development Pattern Keyboard Driver Motion Generator Parameter Server Velocity Ramps Let’s Drive! rviz Summary 112 112 114 119 122 125 126 134 Building Maps of the World 135 Maps in ROS Recording Data with rosbag Building Maps Starting a Map Server and Looking at a Map Summary 135 138 140 147 150 10 Navigating About the World 151 Localizing the Robot in a Map Getting a Good Initial Localization What’s Going on Behind the Scenes Tips for Setting a Better Initial Pose Using the ROS Navigation Stack The ROS Navigation Stack Navigating in rviz Seeing What’s Going On Navigating in Code Summary 151 154 155 156 156 157 157 158 161 163 Table of Contents www.it-ebooks.info | v 11 Chess-bot 165 Joints, Links, and Kinematic Chains Joint Space Inverse Kinematics The Key to Success Installing and Running a Simulated R2 Moving R2 from the Command Line Moving R2 Around a Chessboard Operating the Hand Modeling a Chessboard Playing Back a Famous Chess Game Summary Part III 166 167 169 170 172 175 177 179 182 185 188 Perception and Behavior 12 Follow-bot 193 Acquiring Images Detecting the Line Following the Line Summary 193 200 206 208 13 On Patrol 209 Simple Patrolling State Machines State Machines in ROS Defining State Machines with smach A Slightly More Relevant Example Defining State Machines Procedurally Patrolling with State Machines A Better Way to Patrol Summary 209 211 212 213 216 219 221 222 224 14 Stockroom-bot 225 Stockroom Simulation Driving to Bins Picking Up the Item Summary vi | 225 238 242 256 Table of Contents www.it-ebooks.info Part IV Bringing Your Own Stuff into ROS 15 Your Own Sensors and Actuators 259 Adding Your Own Sensors A (Fake) Sensor Designing the ROS Wrapper Design 1: Periodic Measurements over a Topic Design 2: Streaming Measurements over a Topic Design 3: Streaming Measurements Published at a Fixed Rate Design 4: Sensor Measurements on Demand Adding Your Own Actuators A (Fake) Actuator Designing the ROS Wrapper Design 1: Continuous Actuation Design 2: Infrequent, Instantaneous Actuation Design 3: Infrequent, Extended Actuation Summary 259 259 260 261 263 264 266 267 267 268 270 271 272 274 16 Your Own Mobile Robot 275 TortoiseBot ROS Message Interface Hardware Driver Modeling the Robot: URDF Simulation in Gazebo Summary 275 277 280 281 289 297 17 Your Own Mobile Robot: Part 299 Verifying Transforms Adding a Laser Sensor Configuring the Navigation Stack Using rviz to Localize and Command a Navigating Robot Summary 299 304 308 313 317 18 Your Own Robot Arm 319 CougarBot ROS Message Interface Hardware Driver Modeling the Robot: URDF Simulation in Gazebo Verifying Transforms Configuring MoveIt 319 321 322 323 327 335 339 Table of Contents www.it-ebooks.info | vii Using rviz to Send Goals Summary 343 345 19 Adding a Software Library 347 Make Your Robot Talk: pyttsx Action Interface Parameters Event Loops The Speech Server The Speech Client Checking That Everything Works as Expected Summary Part V 348 349 350 351 351 354 354 355 Tips and Tricks 20 Tools 359 The Master and Friends: roscore Parameters: rosparam Navigating the Filesystem: roscd Starting a Node: rosrun Starting Many Nodes: roslaunch Testing a Many-Node System: rostest Introspection: rosnode, rostopic, rosmsg, rosservice, and rossrv Summary 359 361 362 362 363 366 369 373 21 Debugging Robot Behavior 375 Log Messages: /rosout and rqt_console Generating Log Messages: /rosout Logger Levels Reading Log Messages: rqt_console /rosout Versus /rosout_agg Nodes, Topics, and Connections: rqt_graph and rosnode Visualizing the Graph: rqt_graph Problem: Mismatched Topic Names Problem: Mismatched Topic Types and/or Checksums Problem: Incorrect Network Settings Sensor Fusion: rviz Plotting Data: rqt_plot Data Logging and Analysis: rosbag and rqt_bag Logging and Playing Back Data: rosbag Visualizing Bags: rqt_bag viii | Table of Contents www.it-ebooks.info 375 376 378 380 382 383 383 385 386 389 391 392 394 394 397 bool count(cpp::WordCount::Request &req, cpp::WordCount::Response &res) { l = strlen(req.words); if (l == 0) count = 0; else { count = 1; for(int i = 0; i < l; ++i) if (req.words[i] == ' ') ++count; } res.count = count; return true; } int main(int argc, char **argv) { ros::init(int argc, char **argv, "count_server"); ros::NodeHandle node; ros::ServiceServer service = node.advertiseService("count", count); ros::spin(); return 0; } Define the callback function Create the server Give control over to ROS The main differences here are that the callback function takes two arguments: the request, of type WordCount::Request, and a response, of type WordCount::Response Again, these are provided automatically when you build the service definition The return value is placed in the response argument, and the callback returns true or false, indicating success or failure Once again, we advertise it through the node handle Example 23-6 shows how to use the service Example 23-6 service_client.cpp #include #include Translating from Python to C++ (and Back Again) www.it-ebooks.info | 413 #include int main(int argc, char **argv) { ros::init(argc, char **argv, "count_client"); ros::NodeHandle node; ros::ServiceClient client = node.serviceClient("count"); cpp::WordCount srv; srv.request.words = "one two three four"; if (client.call(srv)) std::cerr