Welcome back to Part 2 of our multi-part (CMR) series. This series implements concepts learned from CMR with and a . Coursera’s Control of Mobile Robots ROS ROSbots robot ROSbots is a . The robot kit is designed to be an accessible and extremely hack-friendly platform for breadboarding, adding other sensors and actuators to implement any new robotics concepts you come across. Our code is . ROS + OpenCV robot kit for Makers based off a Raspberry Pi open source on Github The kit takes about an hour to assemble. To avoid the pains of compiling ROS and OpenCV yourself, we have a you can download and use. Raspbian ROS + OpenCV SD card image In Part 1 of our series, . The ROS firmware we compiled and uploaded onto the UNO subscribes to two ROS topics, and . The ROS messages published down these topics describe the normalized power (max reverse to max forward is between -1.0 and 1.0) to turn each respective wheel via the respective pulse width. we wired up the ROSbots’ L9110 motor driver and implemented the ROS node to have the UNO board drive our robot’s wheels /wheel_power_left /wheel_power_right Float32 We ended Part 1 by manually publishing a ROS message to drive each wheel using the built-in command line tool. Float32 rostopic In this Part 2, we will describe the kinematics and the dynamics of the ROSbots’ differential drive system. We’ll then apply this understanding and implement the ROS code to predictably drive the robot in a remote-control (RC) tele-operational manner. This will roughly follow along with the lessons and the programming assignments from and . CMR week 1 week 2 For those familiar with the CMR course and who perhaps implemented the course’s Matlab programming assignments using the , we will be implementing our ROS version using roughly the same file and architectural structure described in the CMR course. Sim.I.am simulator Unicycle and Differential Drive Models Since a wheeled robot cannot fly, we only care about these 3 states that define it’s pose: x - position on the x-axis (ie in meters) y - position on the y-axis (ie in meters) φ - phi - angle of the unicycle counter clockwise from x-axis (ie in radians) State and inputs for a unicycle model For a unicycle model, there are 2 inputs that affect these 3 states of the robot: v = forward velocity (ie meters per second) w = angular velocity (ie radians per second) While wheeled robot can have any number of wheels and more complicated factors that can affect its pose, we conveniently use a unicycle model to describe the dynamics of most wheeled robot since it is easy and intuitive to understand. Specifically, we intuitively use and to describe how the unicycle will move. v w This is all fine and dandy, but our ROSbots robot is a differential drive robot with 2 inputs for each of its two wheels: v_r = clockwise angular velocity of right wheel (ie radians per second) v_l = counter-clockwise angular velocity of left wheel (ie radians per second) Fortunately for us, we can convert unicycle inputs into differential drive inputs. Before we describe the equations to do the conversion, there are a couple of measurements we need to make with a straight edge. Differential Drive Model L = wheelbase (in meters per radian) R = wheel radius (in meters per radian) Because is a measurement of the radius of a wheel, it makes sense to think of as meters per radian. R R For , it is not as intuitive. In the differential drive kinematics model, you can think of as the radius of the circle drawn by one wheel spinning while holding the other wheel still. So is also in the units of meters per radian where the radius is of that circle’s. L L L L is in meters per radian We won’t go into excruciating details (since this is not a post on kinematics), but in summary, we can use the and to come up with the following equations to convert unicycle and inputs into and differential drive inputs for our ROSbots robot. kinematics for a unicycle model kinematics for a differential drive model v w v_r v_l v_r = ((2 * v) + (w * L)) / (2 * R) v_l = ((2 * v) - (w * L)) / (2 * R) The numerator for both equations are in meters per second. The denominator is in meters per radian. Both v_r and v_l result in radians per second, clock-wise and counter-clock-wise respectively— what we would expect. Unicycle to Differential Drive Conversion in ROS The two equations above are our sweetheart equations which we implement in a ROS node on our ROSbots robot. To take our robot on a test spin, we will drive our robot in a remote-control (RC) tele-operational manner, with and inputs you specify through keyboard commands. The keyboard interface is through a application created by the ROS community. v w teleop_twist_keyboard ROS node SSH into your Raspberry Pi (RPi) on your ROSbots robot. From your RPi run: $ update_rosbots_git_repos That should pick up the latest ROSbots repository source files from Github. We’ll also assume you are picking up right where we left off from the Part 1 post. Specifically, you have the motor_driver firmware compiled and uploaded on the ROSBots’ UNO board via: $ upload_firmware ~/gitspace/rosbots_driver/platformio/rosbots_firmware/examples/motor_driver/ As a sanity check, here are the running ROS nodes if you run : $ rosnode list /rosout/uno_serial_node Here are the ROS topics if you run : $ rostopic list /diagnostics/rosout/rosout_agg/wheel_power_left/wheel_power_right Now run let’s run the unicycle to diff drive model robot code. Again, from your RPi SSH terminal run: $ rosrun rosbots_driver part2_cmr.py If you should see the following outputs, the ROS node is working: [INFO] [1521228417.529282]: /part2_cmr RCTeleop initialized[INFO] [1521228417.595446]: /part2_cmr wheelbase: 0.14 wheel radius: 0.035[INFO] [1521228417.596918]: /part2_cmr v: 0 w: 0[INFO] [1521228417.598303]: /part2_cmr vl: 0.0 vr: 0.0[INFO] [1521228417.605237]: /part2_cmr right power: data: 0.0 left power: data: 0.0[INFO] [1521228418.098403]: /part2_cmr v: 0 w: 0[INFO] [1521228418.101346]: /part2_cmr vl: 0.0 vr: 0.0[INFO] [1521228418.104728]: /part2_cmr right power: data: 0.0 left power: data: 0.0... Now for the RPi on your robot. open another SSH terminal Let’s take a look at what additional ROS topics are published. From the 2nd SSH terminal on your RPi run and you should see the new ROS node listed: $ rosnode list /part2_cmr /part2_cmr/rosout/uno_serial_node Now let’s see what other topics are added to the ROS system from running the ROS node. /part2_cmr From your 2nd SSH RPi terminal, type — you should see an additional topic. $ rostopic list /part2_cmr/cmd_vel Type and you’ll get more info about the topic: $ rostopic info /part2_cmr/cmd_vel /part2_cmr/cmd_vel Type: geometry_msgs/Twist Publishers: None Subscribers:* /part2_cmr ( ) http://192.168.0.22:33183/ This topic transports a type. The Twist message is part of the standard which includes a number of common messages used to describe robot geometry and kinematics. ROS Twist message geometry_msgs ROS package If you look at the definition of the , you’ll notice it actually can encode motion in 6 degrees of freedom (DOF)— 3 as linear motion and 3 as angular. We actually only need 2 DOF’s — linear.x which is our and angular.z which is our . ROS Twist message v w Notice that our ROS node is a subscriber to the topic, but currently there are no publishers! No one is telling the robot what to do. /part2_cmr Let’s add a publisher by running the ROS node. From the 2nd SSH RPi terminal, type: teleop_twist_keyboard $ rosrun teleop_twist_keyboard teleop_twist_keyboard.py /cmd_vel:=/part2_cmr/cmd_vel By default, the ROS node publishes Twist messages to a topic named . The argument tells rosrun to remap the name to so now the published topic name is the same for both the publishing ROS node and the subscribing ROS node. teleop_twist_keyboard /cmd_vel /cmd_vel:=/part2_cmr/cmd_vel /cmd_vel /part2_cmr/cmd_vel teleop_twist_keyboard /part2_cmr Once you have the ROS node running, let’s tune the linear and angular speed factors first. Hit the “x” button until you get to around and hit the “e” key util you get . If you’re a 3 to 4 decimal places off, that’s fine: teleop_twist_keyboard speed 0.174 turn 2.59 ...currently: speed 0.17433922005 turn 2.5937424601 As described by the output from running teleop_twist_keyboard, use the lower case u i o j k l… keys to drive your ROSbots robot. RC Teleoperating ROSbots Aw yeah! ROSbots Implementation Details We roughly paralleled the software design and file structure from the CMR course’s Sim.I.am Matlab simulator. From the via the ./part2/full folder, you’ll see the following file structure: top level of our source code ├── controller│ ├── controller.py│ ├── dynamics│ │ ├── differential_drive.py│ │ ├── __init__.py│ ├── go_to_angle.py│ ├── __init__.py│ ├── rc_teleop.py│ ├── robot.py│ ├── supervisor.py└── part2_cmr.py The main() function for the ROS node is in at the top level. Inside the main(), we create a object and call its execute() function twice a second (2Hz) via: /part2_cmr part2_cmr.py Supervisor When the Supervisor is created, it does 3 things. Creates a object Robot Creates a object. DifferentialDrive Creates all the Controller objects it needs for the defined objectives — in our case, we only need one Controller object, the controller. RCTeleop Controllers dictate what the robot does. For instance, you can create a Go-To-Goal controller to plan a path to go to a specific waypoint. You can create a Go-To-Angle controller to steer to and track at a specific angle orientation phi. Modern day cars have a Go-To-Velocity controller, aka cruise controller to accelerate/decelerate and track a specific velocity. For the purposes of this Part 2 post, our supervisor will have a single RCTeleop controller that does whatever a teleoperational unicycle command dictates. At init time, the subscribes to the topic and listens for unicycle commands. RCTeleop /part2_cmr/cmd_vel Twist rc_teleop.py The callback function gets called when a message comes in, and stores the and velocities away. self.twist_cb(.) Twist v w rc_teleop.py When the supervisor creates the object, the object fetches the dimensions of its wheelbase and wheel radius as well as the min and max velocities of its motors (yes, real robots have real physical limits) from the . Robot Robot ROS parameter server robot.py When the supervisor creates a object, it passes the wheelbase and wheel radius measurements so the object can accurately when asked to do so. DifferentialDrive DifferentialDrive convert unicycle forward and angular velocity inputs to differential drive wheel velocity inputs That’s pretty much for initialization. Each time the gets called, the supervisor does the following: supervisor.execute() Executes the current controller — the controller — to get the unicycle and velocities. RCTeleop v w Calls the to convert the unicycle , velocities to the and wheel velocities. DifferentialDrive v w v_r v_l Passes the and velocities to the who publishes the ROS messages to the and ROS topics — directing our UNO board to turn ROSbots’ respective wheels as described in Part 1 of our series. v_r v_l Robot Float32 /wheel_power_left /wheel_power_right Each time the Supervisor gets executed: supervisor.py The controller’s execute function simply returns back the current and it has stored away. Nothing exciting here. RCTeleop v w rc_teleop.py The function implements the unicycle to differential drive model conversion. This is where our 2 sweetheart functions get implemented. DifferentialDrive’s uni_to_diff(.) The ’s function has a couple of important responsibilities. Robot set_wheel_speed(.) It clamps the wheel velocity to the motor’s physical limitations. . If you recall from Part 1, our motor_driver firmware only supports wheel power normalized between -1.0 and 1.0 — full speed back to full speed forward. The function converts velocity into power using a lookup table of no-load speeds we measured on our ROSbots robot prior. Converts wheel velocity to wheel power set_wheel_speed(.) . There is a minimum pulse width threshold where the motors just won’t turn. If the controller wants a speed lower than this threshold, the motors won’t be able to support it so currently we just set the motor speed to zero to save power. Our motors also has a minimum speed velocity Don’t we love the messiness of real hardware? This brings us to the conclusion of Part 2. We’ve touched upon unicycle models and how to convert unicycle model inputs to differential drive model inputs. We implemented this system using ROS on a ROSbots robot and got to test the system out in an RC tele-operational manner. In the next Part 3 post, we’ll start to implement feedback which is the critical component to effective control systems. As usual, follow @rosbots on Medium for updates. Follow us on and too! Instagram Facebook Feel free to comment or ask questions here. But if you have technical questions, please post them on answers.ros.org (subject “ROSbots question: ”, tag “rosbots”). blahblah Don’t hesitate to reach out with general feedback, if you want to collaborate, or just to say hello. And if you haven’t already done so, to follow along. purchase your own ROSbots robot here Thanks!Jack “the ROSbots Maker”
