High-Level PID Approach
DiffBot Base Package - High Level Approach
This package contains the so called hardware interface of DiffBot which represents the real hardware in software to work with ROS Control.
In the simpleste case all that is needed in this package is to write a class that inherits from
hardware_interface::RobotHW and provide a launch
file. The launch file will
- Load the robot description from
diffbot_descriptionto the paramter server
- Run the hardware interface of this package
- Load the controller configuration yaml from the
diffbot_controlpackage to the parameter server
- Load the controllers with the controller manager
- Load the value of the encoder resolution to the parameter server
diffbot_base package is created with
1 2 3 4 5 6 7
To work with this package the specified dependencies must be installed either using the available Ubuntu/Debian packages for ROS Noetic or they have to be built from source first. The following table lists the dependencies that we have to install because they are not already part of the ROS Noetic desktop full installation. Refer to the section ROS Noetic Setup for how this was done.
To install a package from source clone (using git) or download the source files from where they are located (commonly hosted on GitHub) into the
src folder of a ros catkin workspace and execute the
catkin build command. Also make sure to source the workspace after building new packages with
1 2 3 4
Make sure to clone/download the source files suitable for the ROS distribtion you are using. If the sources are not available for the distribution you are working with, it is worth to try building anyway. Chances are that the package you want to use is suitable for multiple ROS distros. For example if a package states in its docs, that it is only available for kinetic it is possible that it will work with a ROS noetic install.
src folders of this package and the details on the hardware interface implementation.
For more details on the hardware interface also refer to the section ROS Integration: Control, it gives more details and also this overview article about ROS Control.
The hardware interface provides an interface between the real robot hardware and the controllers provided by ROS Control (or even custom controllers).
DiffBot works with the
diff_drive_controller that is configured in the
diffbot_control package, which is also relevant for the simulation in Gazebo.
Remember that the simulation uses the
gazebo_ros_control package to communicate with the
diff_drive_controller. For the real robot hardware,
ROS Control uses an instance of type
hardware_interface::RobotHW that is passed to the
controller_manager to handle the resources, meaning that the actuated robot joints are not in use by multiple controllers that might be loaded.
The skeleton of DiffBot's hardware interface looks like following, where the constructor is used to read loaded configuration values from the robot's description from the ROS parameter server:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163
The functions above are designed to give the controller manager (and the controllers inside the controller manager) access to the joint state of custom robot,
and to command it. When the controller manager runs, the controllers will read from the
eff variables of the custom robot hardware interface, and the controller will write the desired command into the
cmd variable. It's mandatory to make sure the
eff variables always have the latest joint state available, and to make sure that whatever is written into the
cmd variable gets executed by the robot. This can be done by implementing
hardware_interface::RobotHW::read() and a
write() method also contains the output interface to the motor driver. In this case it is publishing
/diffbot/motor_right topics, which are subscribed by the grove_i2c motor_driver python node that is running on the SBC.
The main node that will be executed uses the
controller_manager to operate the so called control loop.
In the case of DiffBot a simple example looks like the following,
refer to the
diffbot_base.cpp for the complete implementation:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51
As we can see, the basic steps are to initialize the node, instantiate the hardware interface, pass it to a new controller manager and run the control loop that does the following:
- Read joint states from the real robot hardware
- Update the
diff_drive_controllerwith read values and compute the joint velocities using the target
- Write the computed values
You may be wondering why the read values aren't returned from the
diffbot.read() method and nothing is passed to the
This is because the
RobotHW::init() method, shown in the first code snippet, is used to register the actuated joint names (described in the
diffbot_description) to the
joint_effort member variables of the custom robot hardware interface.
The class that registers the variables of the controller with the hardware interface member variables and thereby gives read access to all joint values
without conflicting with other controllers, is the
ROS Control uses the
hardware_interface::VelocityJointInterface (part of the
that registers the command member variable of the controller with the hardware interface to provide it the command that should be written to the actuators.
When the controller manager runs, the controllers will read from the
joint_effort variables of the custom robot hardware interface, and the controller will write the desired command into the
joint_velocity_command variable. It's mandatory to make sure the position, velocity and effort (effort is not needed in the case of the
diff_drive_controller) variables always have the latest joint state available, and to make sure that whatever is written into the
joint_velocity_command variable gets executed by the robot. As mentioned this can be done by implementing
hardware_interface::RobotHW::read() and a
In the control loop the overriden
hardware_interface::RobotHW::read() method of DiffBot is used to read the joint states. The
diff_drive_controller works with a VelocityInterface which is why the
joint_position, defined in rad, and
joint_velocity, defined in rad/s, are calculated from the encoder ticks.
Note the two PID controllers inside the hardware interface, where each PID is passed the error between velocity measured by the encoders
and the target velocity computed by the
diff_drive_controller for a specific wheel joint.
diff_drive_controller doesn't have a PID controller integrated, and doesn't take care if the wheels of the robot are actually turning.
As mentioned above, ROS Control expects that the commands sent by the controller are actually implemented on the real robot hardware and that the
joint states are always up to date. This means that the
diff_drive_controller just uses the
twist_msg on the
cmd_vel topic for example from the
rqt_robot_steering and converts it to a velocity command for the motors. It doesn't take the actual velocity of the motors into account.
See the code of
diff_drive_controller where the
joint_command_velocity is calculated.
This is why a PID controller is needed to avoid situations like the following where the robot moves not straigth although it is commanded to do so:
The PID used here inherits from the ROS Control
control_toolbox::Pid that provides Dynamic Reconfigure out of the box to tune the proportional, integral and derivative gains. The behaviour when using only the P, I and D gains is that the output can overshoot and even change between positive and negative motor percent values because of a P gain that is too high. To avoid this, a feed forward gain can help to reach the setpoint faster.
To add this feed forward gain to the dynamic reconfigure parameters it is necessary to add a new parameter configuration file in this package inside a
For more details on ROS dynamic reconfigure see the official tutorials.
With the use of the PID controller the robot is able to drive straight:
In case of using inexpensive motors like the DG01D-E of DiffBot, you have to take inaccurate driving behaviour into account. The straight driving behaviour can be improved with motors that start spinning at the same voltage levels. To find suitable motors do a voltage sweep test by slightly increasing the voltage and note the voltage level where each motor starts to rotate. Such a test was done on DiffBot's motors.
Using six DG01D-E motors the following values were recorded (sorted by increasing voltage):
|02||2.8 - 3.0|
In the videos above, motors numbered 01 and 03 were used coincidencely and I wasn't aware of the remarkable differences in voltage levels. Using the motors 04 and 05 improved the driving behaviour significantly.
To deal with significant differences in the motors it would also help to tune the two PIDs individually, which is not shown in the video above.
Make also sure that the motor driver outputs the same voltage level on both channels when the robot is commanded to move straight. The used Grove i2c motor driver was tested to do this. Another problem of not driving straight can be weight distribution or the orientation of the caster wheel.
A good test to check the accuracy is to fix two meters of adhesive tape on the floor in a straight line. Then, place the robot on one end oriented in the direction to the other end. Now command it to move straight along the line and stop it when it reaches the end of the tape. Record the lateral displacement from the tape. Measuring a value below 10 cm is considered precise for these motors.
diffbot_hw_interface target library of the
diffbot_base package depends on the custom
To have them built first, add the following to the
CMakeLists.txt of the
This makes sure message headers of this package are generated before being used. If you use messages from other packages inside your catkin workspace, you need to add dependencies to their respective generation targets as well, because catkin builds all projects in parallel.1
To run a single controller_manager, the one from the
diffbot_base package defined inside
difbot_base.cpp use the
launch file from
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21
This will load the DiffBot robot description onto the parameter server which is required for the hardware interface that gets created inside the next
diffbot_base. It creates the hardware interface and instantiates a new controller manager in the
spawner from the
controller_manager package is used to initialize and start the controllers defined in the
diffbot_control/config/diffbot_control.yaml. This step of the launch file is required to get the controllers initialized and started.
Another way would be to use
controller_manager::ControllerManager::loadControllers() inside the
Additionally the launch file loads additional parameters, stored in the
diffbot_base/config/base.yaml on the parameter server.
These parameters are hardware related and used to tune the driving behaviour:
1 2 3 4 5 6 7 8 9 10 11 12
After launching this launch file on DiffBot's single board computer (e.g. Raspberry Pi or Jetson Nano) with
the following parameters are stored on the parameter server:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20
Because the hardware interface subscribes to the encoders, that are connected to the Teensy MCU, and publishes to the motors via the motr driver node,
another launch will be required to run these additional nodes. See the
diffbot_bringup package for this setup.
To have a simulation showing DiffBot, the second step is to use the
diffbot_gazebo/launch/diffbot_base.launch on the work pc:
This will launch the gazebo simulation, which can make use of the running controllers inside the controller manager too:
After launching the Gazebo simulation the controllers got uninitialized.
(It is assumed that the
gazebo_ros_control plugin that gets launched). Because of this the controllers have to be initialized and started again. For this the
diffbot_base/launch/controllers.launch should be used.
This launch file is just loading and starting all controllers again. Note that using the
spawner from the
controller_manager package, like in the
diffbot_base/launch/diffbot.launch results in an error. (TODO this needs some more testing).