A base controller is an important element in the navigation stack because it is the only way to effectively control your robot. It communicates directly with the electronics of your robot.
ROS does not provide a standard base controller, so you must write a base controller for your mobile platform.
Your robot has to be controlled with the message type geometry_msgs/Twist. This message was used on the Odometry message that we saw before.
So, your base controller must subscribe to a topic with the name cmd_vel, and must generate the correct commands to move the platform with the correct linear and angular velocities.
We are now going to recall the structure of this message. Type the following command in a shell to see the structure:
$ rosmsg show geometry_msgs/Twist
The output of this command is as follows:
geometry_msgs/Vector3 linear float64 x float64 y float64 z geometry_msgs/Vector3 angular float64 x float64 y float64 z
The vector with the name linear indicates the linear velocity for the axes x, y, and z. The vector with the name angular is for the angular velocity on the axes.
For our robot, we will only use the linear velocity x and the angular velocity z. This is because our robot is on a differential-wheeled platform; it has two motors to move the robot forward and backward and to turn.
We are working with a simulated robot on Gazebo, and the base controller is implemented on the driver used to move/simulate the platform. This means that we will not have to create the base controller for this robot.
Anyway, in this chapter, you will see an example to implement the base controller on your physical robot. Before that, let's execute our robot on Gazebo to see how the base controller works. Run the following commands on different shells:
$ roslaunch chapter8_tutorials gazebo_xacro.launch model:="`rospack find robot1_description`/urdf/robot1_base_04.xacro" $ rosrun teleop_twist_keyboard teleop_twist_keyboard.py
When all the nodes are launched and working, open rxgraph to see the relation between all the nodes:
$ rqt_graph
You can see that Gazebo subscribes automatically to the cmd_vel topic that is generated by the teleoperation node.
Inside the Gazebo simulator, the plugin of our robot is running and is getting the data from the cmd_vel topic. Also, this plugin moves the robot in the virtual world and generates the odometry.
To obtain some insight of how Gazebo does that, we are going to take a sneak peek inside the diffdrive_plugin.cpp file. You can find it at https://github.com/ros-simulation/gazebo_ros_pkgs/blob/hydro-devel/gazebo_plugins/src/gazebo_ros_skid_steer_drive.cpp.
The load function performs the function of registering the subscriber of the topic, and when a cmd_vel topic is received, the cmdVelCallback() function is executed to handle the message:
void GazeboRosSkidSteerDrive::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
…
…
// ROS: Subscribe to the velocity command topic (usually "cmd_vel")
ros::SubscribeOptions so =
ros::SubscribeOptions::create<geometry_msgs::Twist>(command_topic_, 1,
boost::bind(&GazeboRosSkidSteerDrive::cmdVelCallback, this, _1),
ros::VoidPtr(), &queue_);
…
…
}When a message arrives, the linear and angular velocities are stored in the internal variables to run operations later:
void GazeboRosSkidSteerDrive::cmdVelCallback(
const geometry_msgs::Twist::ConstPtr& cmd_msg)
{
boost::mutex::scoped_lock scoped_lock(lock);
x_ = cmd_msg->linear.x;
rot_ = cmd_msg->angular.z;
}The plugin estimates the velocity for each motor, using the formulas from the kinematic model of the robot, in the following manner:
void GazeboRosSkidSteerDrive::getWheelVelocities() {
boost::mutex::scoped_lock scoped_lock(lock);
double vr = x_;
double va = rot_;
wheel_speed_[RIGHT_FRONT] = vr + va * wheel_separation_ / 2.0;
wheel_speed_[RIGHT_REAR] = vr + va * wheel_separation_ / 2.0;
wheel_speed_[LEFT_FRONT] = vr - va * wheel_separation_ / 2.0;
wheel_speed_[LEFT_REAR] = vr - va * wheel_separation_ / 2.0;
}And finally, it estimates the distance traversed by the robot using more formulas from the kinematic motion model of the robot. As you can see in the code, you must know the wheel diameter and the wheel separation of your robot:
// Update the controller
void GazeboRosSkidSteerDrive::UpdateChild()
{
common::Time current_time = this->world->GetSimTime();
double seconds_since_last_update =
(current_time - last_update_time_).Double();
if (seconds_since_last_update > update_period_)
{
publishOdometry(seconds_since_last_update);
// Update robot in case new velocities have been requested
getWheelVelocities();
joints[LEFT_FRONT]->SetVelocity(0, wheel_speed_[LEFT_FRONT] / wheel_diameter_);
joints[RIGHT_FRONT]->SetVelocity(0, wheel_speed_[RIGHT_FRONT] / wheel_diameter_);
joints[LEFT_REAR]->SetVelocity(0, wheel_speed_[LEFT_REAR] / wheel_diameter_);
joints[RIGHT_REAR]->SetVelocity(0, wheel_speed_[RIGHT_REAR] / wheel_diameter_);
last_update_time_+= common::Time(update_period_);
}
}This is the way gazebo_ros_skid_steer_drive controls our simulated robot in Gazebo.
Now, we are going to do something similar, that is, prepare a code to be used with a real robot with two wheels and encoders.
Create a new file in chapter8_tutorials/src with the name base_controller.cpp and put in the following code:
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <iostream>
using namespace std;
double width_robot = 0.1;
double vl = 0.0;
double vr = 0.0;
ros::Time last_time;
double right_enc = 0.0;
double left_enc = 0.0;
double right_enc_old = 0.0;
double left_enc_old = 0.0;
double distance_left = 0.0;
double distance_right = 0.0;
double ticks_per_meter = 100;
double x = 0.0;
double y = 0.0;
double th = 0.0;
geometry_msgs::Quaternion odom_quat;
void cmd_velCallback(const geometry_msgs::Twist &twist_aux)
{
geometry_msgs::Twist twist = twist_aux;
double vel_x = twist_aux.linear.x;
double vel_th = twist_aux.angular.z;
double right_vel = 0.0;
double left_vel = 0.0;
if(vel_x == 0){
// turning
right_vel = vel_th * width_robot / 2.0;
left_vel = (-1) * right_vel;
}else if(vel_th == 0){
// forward / backward
left_vel = right_vel = vel_x;
}else{
// moving doing arcs
left_vel = vel_x - vel_th * width_robot / 2.0;
right_vel = vel_x + vel_th * width_robot / 2.0;
}
vl = left_vel;
vr = right_vel;
}
int main(int argc, char** argv){
ros::init(argc, argv, "base_controller");
ros::NodeHandle n;
ros::Subscriber cmd_vel_sub = n.subscribe("cmd_vel", 10, cmd_velCallback);
ros::Rate loop_rate(10);
while(ros::ok())
{
double dxy = 0.0;
double dth = 0.0;
ros::Time current_time = ros::Time::now();
double dt;
double velxy = dxy / dt;
double velth = dth / dt;
ros::spinOnce();
dt = (current_time - last_time).toSec();;
last_time = current_time;
// calculate odomety
if(right_enc == 0.0){
distance_left = 0.0;
distance_right = 0.0;
}else{
distance_left = (left_enc - left_enc_old) / ticks_per_meter;
distance_right = (right_enc - right_enc_old) / ticks_per_meter;
}
left_enc_old = left_enc;
right_enc_old = right_enc;
dxy = (distance_left + distance_right) / 2.0;
dth = (distance_right - distance_left) / width_robot;
if(dxy != 0){
x += dxy * cosf(dth);
y += dxy * sinf(dth);
}
if(dth != 0){
th += dth;
}
odom_quat = tf::createQuaternionMsgFromRollPitchYaw(0,0,th);
loop_rate.sleep();
}
}Do not forget to insert the following in your CMakeLists.txt file to create the executable of this file:
add_executable(base_controller src/base_controller.cpp)
target_link_libraries(base_controller ${catkin_LIBRARIES})This code is only a common example and must be extended with more code to make it work with a specific robot. It depends on the controller used, the encoders, and so on. We assume that you have the right background to add the necessary code in order to make the example work fine.