Service Robot Tod. First steps with ROS



    Good afternoon, Habr. Our team is developing the Tod service robot. We strive to create a multi-functional robot, which in its capabilities will be able to compete with such flagships in mobile robotics as PR2 Willow Garage. We start small, but every day our robot gains new skills, is equipped with new sensors. You can find out what service robots are all about in our previous article., and today we will focus on the implementation of the Tod navigation system. Today we will tell you how to teach a robot to perform the navigation task of determining its own location based on wheel odometry and to receive sensory data from ultrasonic sonars. All this business will be managed under the operating system for robots ROS (Robot Operating System), which has proven itself in various robotic projects. Welcome to cat.

    For the layman, the word "robot" is most likely associated with smart humanoid robots from science fiction films in the spirit of the Terminator. What is the difference between real robots and conventional machines? First of all, robots have autonomy, which is expressed in the ability to make decisions independently, without human intervention.
    An autonomous robot must be able to solve navigation problems to achieve its goal. The basic navigation tasks include environmental perception based on the interpretation of data from various types of sensors (rangefinders, cameras, GPS navigators, special beacons, etc.), route planning and interactive interaction with the environment using executive bodies , wheels and manipulators.
    High-quality navigation algorithms are based on complex mathematics, so many beginner robotics lose their enthusiasm after colliding with the calculations of the Jacobians and quaternions, the construction of a kinematic model and the use of probabilistic algorithms. Fortunately, today there are many robotic frameworks such as ROS, Player and Microsoft Robotics Studio, with which even beginners with the necessary persistence will be able to use complex navigation and AI algorithms in their projects.

    ROS and navigation stack


    It was no coincidence that our team decided to use the open-source Robot Operating System framework for the Tod robot. ROS today finds application in robotic projects of many research groups and companies. This framework provides features comparable to the functions of the whole OS, including hardware abstractions, low-level device management, implementation of basic functions and algorithms, message passing between processes and a package manager. A program executed in ROS is a set of nodes that can exchange messages among themselves by subscribing to a common topic. Such nodes can be independently implemented in C ++ and Python. ROS fully runs under Ubuntu, in particular, we use Tod Tod Ubuntu 12.40 and ROS Groovy. More information about ROS,
    To solve navigation problems, ROS provides a navigation stack. As input data, the stack uses odometry data (the path traveled by the robot wheels) and sensors, and at the output it transfers the speed control commands to the robot. Using the out-of-box navigation stack on the robot becomes possible under certain conditions:
    - The robot must be round or rectangular in shape and its wheels must be non-holonomic, i.e. the movement of the robot should be carried out only along the direction of rotation of the wheels. For example, the wheels of a car or bicycle are nonholonomic.
    - The robot must provide information on all geometric relationships between the kinematic nodes and the sensors of the robot. This information is specified in the URDF model, and the node tf can perform complex geometric transformations from one coordinate system to another using rotation matrices, Euler angles, and quaternions.
    - The robot must send messages to control the movement in the format of linear and angular velocity.
    - A laser rangefinder or a 3-D scanner should be used to solve the problems of location and map building. However, if you cheat a little, then you can use other cheaper analogs instead of expensive sensors: sonars or infrared rangefinders. In this case, the main thing is to observe the correct format of messages that are transmitted to the executable node.



    The diagram shows the general layout of the navigation stack. The text between the arrows indicates the type of message exchanged between the nodes. There are 3 types of nodes in this stack :
    - Nodes placed in the white rectangle are provided by ROS
    - Nodes placed in the gray rectangles are also provided by ROS, but their use on the stack is optional
    - Nodes placed in the turquoise rectangles are device-dependent and their the implementation usually rests with the developer.
    Now that the requirements for using the ROS navigation stack are known, we can begin to adapt it to our Tod robot.

    Base controller and motion control


    Base controller is the navigation stack node responsible for controlling the movement of the robot. ROS does not provide a standard base controller, so for your robot you need to write your own node or take third-party open source solutions as a basis. Robot movement control commands are sent to the base controller in the cmd_vel topic in messages of the geometry_msgs / Twist type.
    geometry_msgs/Vector3 linear
      float64 x
      float64 y
      float64 z
    geometry_msgs/Vector3 angular
      float64 x
      float64 y
      float64 z
    

    The linear vector defines the linear velocity of the robot along the x, y, z axes, and the angular vector defines the angular velocity of the x, y, z axes. Further, these commands are converted into commands for controlling the rotation of the engines, and the robot moves in a given direction.
    The order of setting the velocity vectors depends on the kinematics of the robot. Our Tod robot is equipped with a two-wheel differential drive based on DC motors, which allows it to move forward, backward, in an arc or rotate in place. This means that in the geometry_msgs / Twist message, only the linear velocity along the x axis (corresponds to the forward-backward movement) and the angular velocity z (corresponds to in-place rotation or movement along the arc when specifying a non-zero linear velocity) will be specified.


    3-wheel robot with differential drive. A steering wheel or ball joint provides robotic stability.

    Converting these robot speeds to the corresponding engine speeds is a trivial kinematics task that requires some geometric calculations.
    We assigned the low-level task of controlling the rotation of the engine to the Arduino Uno in conjunction with the Pololu Dual MC33926 Motor Driver Shield, which provides the required power for our 12-volt engines. After implementing the base controller, you can ride the robot using the keyboard and the ROS node turtlebot_teleop, which sends geometry_msgs / Twist base controller messages.



    Odometry


    Odometry is the most common dead reckoning method. The essence of this method is to determine the position of the robot based on the calculation of incremental wheel speed relative to any fixed point on the map. Typically, odometry measurements are made by optical digital encoders mounted on wheels or directly on robot motors. Tod is equipped with digital encoders with a resolution of 64 pulses per revolution of the motor shaft, which corresponds to 8384 pulses per revolution of the wheel.


    Odometry geometry. Given a robot position (x, y, theta) and dbaseline wheelbase width, a new position (x ', y', theta ') needs to be calculated.

    The navigation stack uses messages like nav_msgs / Odometry to obtain odometry data.
    std_msgs/Header header 
      uint32 seq 
      time stamp 
      string frame_id 
    string child_frame_id 
    geometry_msgs/PoseWithCovariance pose 
      geometry_msgs/Pose pose 
        geometry_msgs/Point position 
          float64 x 
          float64 y 
          float64 z 
        geometry_msgs/Quaternion orientation 
          float64 x 
          float64 y 
          float64 z 
          float64 w 
      float64[36] covariance 
    geometry_msgs/TwistWithCovariance twist 
      geometry_msgs/Twist twist 
        geometry_msgs/Vector3 linear 
          float64 x 
          float64 y 
          float64 z 
        geometry_msgs/Vector3 angular 
          float64 x 
          float64 y 
          float64 z 
      float64[36] covariance
    

    The geometry_msgs / Pose message determines the current position of the robot in three-dimensional space and the orientation, which, if the object rotates in three-dimensional space, will be conveniently calculated by quaternions. The geometry_msgs / Twist message we already know defines linear velocity x and angular velocity z.
    Since when we perform calculations, we are dealing with several coordinate systems, then we need the node tf. The tf node, working with the URDF model of the robot, takes care of performing cumbersome calculations of converting the position from the local coordinate system of the robot to the global coordinate system of the map.


    Visualization of the Tod URDF model with odometry data in the Rviz simulator.

    Sonar


    The robot can use various types of sensors to obtain information about the surrounding world. Sensors vary greatly in their characteristics, have their limitations, weaknesses and strengths, so the joint use of several of their types is considered the most beneficial.
    Using ultrasonic sonars, you can measure the distance from the object to the robot. Sonars are powered by TOF (time-of-flight) technology. They emit a sound signal, which is reflected from the nearest object in the path and returns in the form of an echo. The “flight” time of the signal is fixed, and on its basis the distance to the object is calculated.


    The sonar emits a beep and “listens” to the echo.

    Tod uses HC-SR04 sonars, which supports a measuring range of 0.2 to 5 m with a stated accuracy of 0.03 m. The viewing angle of one HC-SR04 is 30 degrees, and if you place several sonars side by side, you can get a larger viewing angle. 3 sonars located on the front of the Tod provide a viewing angle of 90 degrees.



    The ROS navigation stack can use data from various types of sensors to obtain odometry, building a map of a room or avoiding obstacles. Theoretically, it is possible to use sonars to build a room map, because 12 or more sonars give a viewing angle of 360 degrees and are a cheaper replacement for expensive laser rangefinders. Tod uses Kinect to build a map, which surpasses sonar in many sensory characteristics. However, this is not a reason to discard sonars. Kinect is mounted high enough on the robot, which does not allow you to see what is happening right under the wheels. Sonars capture this blind spot, thereby appearing to be useful in solving the problems of planning a path and avoiding obstacles.
    As mentioned earlier, the navigation stack only supports the use of a laser sensor and a 3-D scanner. This limitation can be circumvented by presenting the sonar system as a fake 3-D scanner. The 3-D scanner uses a sensor_msgs / PointCloud message describing a point cloud in three-dimensional space.
    std_msgs/Header header 
      uint32 seq 
      time stamp 
      string frame_id 
    geometry_msgs/Point32[] points 
      float32 x 
      float32 y 
      float32 z 
    sensor_msgs/ChannelFloat32[] channels 
      string name 
      float32[] values
    

    The sonar sensory data can be represented in this format by setting each point of the cloud in the form of x, y coordinates and z coordinate equal to 0. At the same time, several such points can be set for each sonar, which makes it possible to increase the density of the cloud. This is how the visualization of sensory data from Tod sonars looks like.


    Visualization of sonar sensory data in Rviz.

    Thank you for your attention, that's all for today. In the next article, we will continue to talk about the capabilities of the ROS navigation stack using the example of our experimental subject: we’ll connect to Tod Kinect, build an apartment map with it, teach us how to plan a route and avoid obstacles.

    Also popular now: