Tutorials/1.3/control robot/mobile base laser


 * 1) Tutorial: Controlling a Mobile Robot with its Range Sensor#


 * Prerequisites:** Mobile Base with Laser Sensor and  Controlling a Mobile Base

In this tutorial, we will control a wheeled mobile base model using data from an attached hokuyo-like laser sensor. This tutorial demonstrates dynamics control of a model using sensor data as well as how to access sensor data through a model plugin programatically.


 * 1) Creating a model plugin

For this exercise, we'll start with the mobile base with laser sensor model.


 * 1) Setup your workspace

Starting with the model plugin we developed in the previous mobile base control tutorial, we'll modify the plugin further to make use of the laser sensor data. First, make a copy of the original plugin:

cd ~/my_plugin

Make a copy of `my_plugin.cc` named `my_plugin_with_sensor.cc`

cp ~/my_plugin/my_plugin.cc ~/my_plugin/my_plugin_with_sensor.cc

Modify the CMake instructions in `~/my_plugin/CMakeLists.txt`

gedit ~/my_plugin/CMakeLists.txt

Next, add the following block:

add_library(my_plugin_with_sensor SHARED my_plugin_with_sensor.cc) target_link_libraries(my_plugin_with_sensor ${GAZEBO_LIBRARIES})


 * 1) Modify model plugin

Modify `~/my_plugin/my_plugin_with_sensor.cc`:

gedit ~/my_plugin/my_plugin_with_sensor.cc

Copy this content into it:


 * 1) include 
 * 2) include 
 * 3) include 
 * 4) include 
 * 5) include 
 * 6) include 

namespace gazebo {    class MobileBasePlugin : public ModelPlugin {   public: void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) {

// Store the pointer to the model this->model = _parent;

// Load parameters for this plugin if (this->LoadParams(_sdf)) {       // testing to see if race condition exists gzerr << this->leftWheelJoint->GetAngle(0) << "\n"; gzerr << this->rightWheelJoint->GetAngle(0) << "\n"; // Listen to the update event. This event is broadcast every // simulation iteration. this->updateConnection = event::Events::ConnectWorldUpdateStart(           boost::bind(&MobileBasePlugin::OnUpdate, this)); }   }

public: bool LoadParams(sdf::ElementPtr _sdf) {

// Find controller gain if (!_sdf->HasElement("gain")) {       gzerr << "param [gain] not found\n"; return false; }     else {       // Get sensor name this->gain = _sdf->GetElement("gain")->GetValueDouble; }

// Find sensor name from plugin param if (!_sdf->HasElement("ray_sensor")) {       gzerr << "param [ray_sensor] not found\n"; return false; }     else {       // Get sensor name std::string sensorName = _sdf->GetElement("ray_sensor")->GetValueString;

// Get pointer to sensor using the SensorMangaer sensors::SensorPtr sensor = sensors::SensorManager::Instance->GetSensor(sensorName);

if (!sensor) {         gzerr << "sensor by name [" << sensorName << "] not found in model\n"; return false; }

this->laser = boost::shared_dynamic_cast (sensor); if (!this->laser) {         gzerr << "laser by name [" << sensorName << "] not found in model\n"; return false; }     }

// Load joints from plugin param if (!this->FindJointByParam(_sdf, this->leftWheelJoint, "left_wheel_hinge") ||         !this->FindJointByParam(_sdf, this->rightWheelJoint, "right_wheel_hinge")) return false;

// success return true; }

public: bool FindJointByParam(sdf::ElementPtr _sdf,                                 physics::JointPtr &_joint,                                  std::string _param) {     if (!_sdf->HasElement(_param)) {       gzerr << "param [" << _param << "] not found\n"; return false; }     else {       _joint = this->model->GetJoint(          _sdf->GetElement(_param)->GetValueString);

if (!_joint) {         gzerr << "joint by name [" << _sdf->GetElement(_param)->GetValueString << "] not found in model\n"; return false; }     }      return true; }

// Called by the world update start event public: void OnUpdate {     unsigned int n = this->laser->GetRangeCount; double min_dist = 1e6; for (unsigned int i = 0; i < n; ++i) {       if (this->laser->GetRange(i) < min_dist) min_dist = this->laser->GetRange(i); }

double target_dist = 2.0;

if (min_dist < this->laser->GetRangeMax) {       double torque = this->gain*( min_dist - target_dist ); this->leftWheelJoint->SetForce(0, torque); this->rightWheelJoint->SetForce(0, torque); }   }

// Pointer to the model private: physics::ModelPtr model;

// Pointer to the update event connection private: event::ConnectionPtr updateConnection;

private: physics::JointPtr leftWheelJoint; private: physics::JointPtr rightWheelJoint; private: sensors::RaySensorPtr laser; private: double gain; };

// Register this plugin with the simulator GZ_REGISTER_MODEL_PLUGIN(MobileBasePlugin) }

and modify the ` ` block in your robot's `~/.gazebo/models/my_robot/model.sdf`

gedit ~/.gazebo/models/my_robot/model.sdf

such that it refers to the new plugin being created here:

 left_wheel_hinge right_wheel_hinge 5.0      laser

Hint: reduce the gain to make the robot stable.


 * 1) Compile the plugin

cd ~/my_plugin/build cmake ..; make


 * 1) Running the Simulation

Add the path of your plugin to gazebo environment variables:

export GAZEBO_PLUGIN_PATH=~/my_plugin/build:$GAZEBO_PLUGIN_PATH

Start Gazebo (or restart if Gazebo is already running) in the same terminal the you ran the `export GAZEBO_PLUGIN_PATH` from, and insert the model (see GUI tutorial for more details on this process),



Use the GUI to spawn a cube and place it within the laser range of the robot,



Then robot will drive itself until the shortest sensor measurement returns 2 meters.


 * 1) Note

This is a tutorial for working with simulated robots; for real robots, it is advisable to create a separate sensor plugin (see example) and a controller node (see  example). The controller node receives information from the sensor plugin, makes decisions and sends commands to a simple motor controller plugin (see example), independent of the simulation engine. This way, the controller node can be used both in simulation and on hardware platform as long as the sensor node and the motor controller node exposes the same interface between simulation and hardware.


 * 1) Extra Credit

1. Adjust the inertia of the mobile robot's chassis so that it doesn't flip when backing up. 1. Using Gazebo's PID class to control the mobile robot.


 * 1) Next

Next: Use PID Class to Control a Simple Slider Joint in Gazebo