Tutorials/1.3/intermediate/control model


 * 1) Tutorial: Control Model#

Models that are not static rely on the physics engine. These models can be controlled during the simulation, in a realistic manner. In this tutorial, we will create a box with a ray sensor and change its velocity along the x axis ion proportion to the sensor reading.


 * 1) Setup ##

First create a work directory.

mkdir ~/gazebo_control_model; cd ~/gazebo_control_model


 * 1) World File ##

This is the world we'll use for this tutorial. Notice that we defined our box model as NOT `static`. This tells Gazebo to simulate the model in the physics engine.

Our box model also contains a laser sensor on top of it. Sensors must be defined as a child to a link node, and the link node must be a child of the model node. Information about how to define valid SFD nodes can be found in the 1.3 SDF reference. Notice the true element allows us to "see" the laser scan in blue.

The world also contains another box, named wall_model. You can see the laser bounce against it.

The world file will not run at this point because we have not yet compiled and installed the plugin specified in the world file.

Create a world file:

gedit ~/gazebo_control_model/model_control.world

Copy and past the following xml code into it:

  model://ground_plane model://sun

 0 0 0.5 0 0 0      false 1.0            1.0             0.0             0.0             1.0             0.0             1.0               1 1 1                 0 0              1 1 1          true true 0.0 0.0 0.51 0.0 0.0 0.0                180                 -1.57 1.57 0.0              30.0

 10 0 1 0 0 0      true 1.0            1.0             0.0             0.0             1.0             0.0             1.0               1 20 2               1 20 2


 * 1) Control model Plugin ##

The plugin must be attached to a model (as in the above world file). When it is loaded, the plugin instance's Load method is called by Gazebo. The code looks for "raysensor", a reference to a RaySensor instance and stores the reference into a raysensor member variable, as well as the reference to the model instance.

An connection is then created to subscribe to plugin to the WorldUpdateStart events via the OnUpdate callback method. The list of possible events and the signature of their associated callback methods can be found in the Events api reference.

Create the plugin file:

gedit ~/gazebo_control_model/control_model.cc

Copy and paste the code block below into it:


 * 1) include 
 * 2) include "gazebo/gazebo.hh"
 * 3) include "gazebo/physics/physics.hh"
 * 4) include "gazebo/common/common.hh"
 * 5) include "gazebo/sensors/RaySensor.hh"
 * 6) include "gazebo/sensors/SensorManager.hh"


 * 1) include "math/Vector3.hh"
 * 2) include 

namespace gazebo {

class ModelPush : public ModelPlugin {   public: ModelPush {	printf("ModelPush ctor\n"); }	   public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/) {             printf("Load\n"); // Store the pointer to the model this->model = _parent; sensors::SensorPtr sensor = sensors::SensorManager::Instance->GetSensor("raysensor"); if(!sensor) printf("sensor is NULL\n"); this->raysensor = boost::shared_dynamic_cast(sensor); if(!this->raysensor) printf("raysensor is NULL\n");

// Listen to the update event. This event is broadcast every // simulation iteration. this->updateConnection = event::Events::ConnectWorldUpdateStart(                 boost::bind(&ModelPush::OnUpdate, this)); }

// Called by the world update start event

public: void OnUpdate {       std::vector ranges; double min_range = this->raysensor->GetRangeMax; this->raysensor->GetRanges(ranges); for(std::vector ::const_iterator it = ranges.begin;                it != ranges.end;                  ++it) {           if(*it < min_range) min_range = *it; }             double vel; if(min_range <= 1.0) vel = 0.0; else {               double err = min_range - 1.0; vel = err / 10.0; vel = std::max(vel, 0.05); }             this->model->SetLinearVel(math::Vector3(vel, 0, 0)); // console output static int count =0; count --; if(count<0) {             count = 10000; math::Vector3 p = model->GetWorldPose.pos; printf("min_range: %3.3f, x vel: %3.3f, position: [%3.3f, %3.3f, %3.3f]\n", min_range, vel, p.x, p.y, p.z); }    }

// Pointer to the model private: physics::ModelPtr model; private: sensors::RaySensorPtr raysensor;

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

};

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

Copy the contents of this CMake script into a file called `CMakeLists.txt`
 * 1) Build ##

gedit ~/gazebo_control_model/CMakeLists.txt

and copy and paste the following contents into it:

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

include (FindPkgConfig) if (PKG_CONFIG_FOUND) pkg_check_modules(GAZEBO gazebo) endif include_directories(${GAZEBO_INCLUDE_DIRS}) link_directories(${GAZEBO_LIBRARY_DIRS})

add_library(control_model SHARED control_model.cc) target_link_libraries(control_model ${GAZEBO_libraries})

Create a `build` directory and `cd` into it and compile it:

mkdir ~/gazebo_control_model/build cd ~/gazebo_control_model/build cmake .. make


 * 1) Run Tutorial ##

Add the current directory to the shared library path and run gazebo with the world we just created. In a terminal

cd ~/gazebo_control_model/build export GAZEBO_PLUGIN_PATH=`pwd`:$GAZEBO_PLUGIN_PATH gazebo ~/gazebo_control_model/model_control.world

You should see the box move slowly towards the wall.


 * 1) Optional ##

1. Modify the plugin to stop forward motion at 1.0 meters from the wall. 1. Make the box move laterally along the wall until it is clear of the wall. 1. Once clear of the wall, resume forward motion.

Next: Controlling a Mobile Robot
 * 1) Next ##