User Tools

Site Tools


gazebo_plugin

Differences

This shows you the differences between two versions of the page.

Link to this comparison view

Both sides previous revision Previous revision
Next revision
Previous revision
gazebo_plugin [2015/05/22 12:24]
superstage [hello_world.cc]
gazebo_plugin [2018/05/22 10:33] (current)
Line 1: Line 1:
 +====== GAZEBO PLUGIN OVERVIEW ======
  
 +===== Introduction =====
 +You can use plugin to access a huge amount of GAZEBO parameters. The following resources are available online :
 +  * [[http://​gazebosim.org/​tutorials?​cat=write_plugin|Learn to write a plugin]]
 +  * [[http://​gazebosim.org/​tutorials?​tut=ros_gzplugins&​cat=connect_ros|Gazebo plugin that you can use with ros]]
 +  * [[http://​gazebosim.org/​api.html|Gazebo API]]
 +  * [[http://​wiki.ros.org/​hector_gazebo_plugins|ROS Package that contain other Gazebo plugins]]
 +
 +
 +===== Study an example =====
 +
 +There is a Gazebo plugin code example.
 +<code c++>
 +//​hello_world.hh
 +
 +#ifndef HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_SONAR_H
 +#define HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_SONAR_H
 +
 +#include <​gazebo/​gazebo.hh>​
 +#include <​gazebo/​common/​Plugin.hh>​
 +
 +#include <​ros/​callback_queue.h>​
 +#include <​ros/​ros.h>​
 +
 +#include <​sensor_msgs/​Range.h>​
 +#include <​std_msgs/​Float64.h>​
 +#include <​gazebo/​rendering/​rendering.hh>​
 +
 +
 +
 +#include <​dynamic_reconfigure/​server.h>​
 +#include <​fstream>​
 +
 +struct line
 +{
 +  float x0;
 +  float y0;
 +  float x1;
 +  float y1;
 +
 +  float distNorm(float x,float y);
 +};
 +
 +typedef struct line line;
 +
 +
 +using namespace std;
 +
 +namespace gazebo
 +{
 +
 +  class GazeboRosSonar : public SensorPlugin
 +  {
 +  public:
 +    GazeboRosSonar();​
 +    virtual ~GazeboRosSonar();​
 +
 +  protected:
 +    virtual void Load(sensors::​SensorPtr _sensor, sdf::​ElementPtr _sdf);
 +    virtual void Reset();
 +    virtual void Update();
 +
 +    bool notSetup();
 +    void trySetup();
 +    float useSensor();​
 +
 +  private:
 +  /// \brief The parent World
 +    physics::​WorldPtr world;
 +
 +    rendering::​ScenePtr scene;
 +    rendering::​VisualPtr visual;
 +
 +    physics::​ModelPtr model_ref;
 +    physics::​LinkPtr link_ref;
 +
 +    sensors::​RaySensorPtr sensor_;
 +
 +    ros::​NodeHandle* node_handle_;​
 +    ros::​Publisher publisher_;
 +
 +    sensor_msgs::​Range range_;
 +
 +    std::string namespace_;
 +    std::string topic_;
 +    std::string model_ref_name;​
 +    std::string link_ref_name;​
 +    std::string path_to_config;​
 +
 +    std::​vector<​line>​ lines_wire;
 +
 +    //​SensorModel sensor_model_;​
 +
 +
 +    event::​ConnectionPtr updateConnection;​
 +
 +
 +  };
 +
 +} // namespace gazebo
 +
 +#endif // HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_SONAR_H
 +</​code>​
 +
 +<code c++>
 +//​hello_world.cc
 +
 +#include <​gazebo/​gazebo.hh>​
 +
 +#include "​hello_world.hh"​
 +#include <​gazebo/​common/​Events.hh>​
 +#include <​gazebo/​physics/​physics.hh>​
 +#include <​gazebo/​sensors/​RaySensor.hh>​
 +#include <​gazebo/​common/​CommonTypes.hh>​
 +#include <​gazebo/​math/​gzmath.hh>​
 +#include <​gazebo/​rendering/​rendering.hh>​
 +
 +
 +#include <​limits>​
 +#include <​regex>​
 +#include <​iostream>​
 +#include <​fstream>​
 +#include <​math.h>​
 +#include <​std_msgs/​Float64.h>​
 +
 +using namespace std;
 +
 +bool the_bool_tmp = true;
 +
 +float line::​distNorm(float x,float y)
 +{
 + float a = sqrt(pow(x-x1,​2)+pow(y-y1,​2));​
 + float b = sqrt(pow(x-x0,​2)+pow(y-y0,​2));​
 + float c = sqrt(pow(x1-x0,​2)+pow(y1-y0,​2));​
 +
 + float s =(a + b + c)/2;
 + float A = sqrt(s*(s-a)*(s-b)*(s-c));​
 +
 + float h = (2*A)/c;
 +
 + float alpha = acos( (pow(b,2) + pow(c,2) - pow(a,​2))/​(2*b*c) );
 + float delta = acos( (pow(c,2) + pow(a,2) - pow(b,​2))/​(2*c*a) );
 +
 + // gzdbg << "a: " << a << endl;/////////////////////////////​
 + // gzdbg << "b : " << b << endl;/////////////////////////////​
 + // gzdbg << "c : " << c << endl;/////////////////////////////​
 + // gzdbg << "alpha : " << alpha << endl;/////////////////////////////​
 + // gzdbg << "delta : " << delta << endl;/////////////////////////////​
 + if (alpha < 3.14/2 and delta < 3.14/2)
 + {
 + //gzdbg << "​return h :" << endl;/////////////////////////////​
 + return h;
 + }
 + else
 + {
 + //gzdbg << "​return min(a,b) :" << endl;/////////////////////////////​
 + return min(a,b);
 + }
 + return 0.0;
 +}
 +
 +
 +namespace gazebo {
 +
 + GazeboRosSonar::​GazeboRosSonar() : link_ref(physics::​LinkPtr())
 + {
 + }
 +
 +
 + GazeboRosSonar::​~GazeboRosSonar()
 + {
 + }
 +
 + bool GazeboRosSonar::​notSetup()
 + {
 + return link_ref == NULL || scene == NULL || !scene->​GetInitialized() || the_bool_tmp;​
 + }
 +
 + void GazeboRosSonar::​trySetup()
 + {
 + physics::​Model_V list_models_tmp = world->​GetModels();​
 + physics::​LinkPtr link_tmp;
 + physics::​Link_V list_links_tmp;​
 +
 + model_ref = world->​GetModel(model_ref_name);​
 +
 +
 + link_ref = model_ref->​GetLink(link_ref_name);​
 +
 +
 +
 + scene = rendering::​get_scene();​
 + // Wait until the scene is initialized.
 + if (!scene || !scene->​GetInitialized())
 + return;
 + else
 + {
 +
 + gzdbg << "Nom scene : " << scene->​GetName() << endl; ////////////////​
 +
 +
 + math::​Vector3 min(0,0,0);
 + math::​Vector3 max(2,2,2);
 + math::​Box my_box(min,​max);​
 + rendering::​VisualPtr tmp(new rendering::​Visual("​my_visual",​scene));​
 + visual = tmp;
 +
 + scene->​AddVisual(visual);​
 + //​scene->​DrawLine(math::​Vector3(0,​0,​0),​math::​Vector3(2,​2,​2),"​test"​);​
 +
 + // visual->​SetPosition(math::​Vector3(0,​0,​0));​
 + // visual->​Load();​
 + // gzdbg << "name visual : " << visual->​GetName() << endl;/////////////////​
 +
 + // rendering::​WireBox* my_wire_box = new rendering::​WireBox(visual,​my_box);​
 + // test.MakeStatic();​
 + // test.Load();​
 +
 + the_bool_tmp = false;
 + gzdbg << "Fin de la config"​ << endl; ////////////////​
 + }
 +
 + }
 +
 + float GazeboRosSonar::​useSensor()
 + {
 + math::​Pose my_pose = model_ref->​GetWorldPose();​
 + float res = 1000000.0;
 +
 + for (std::​vector<​line>::​iterator it = lines_wire.begin() ; it < lines_wire.end() ; ++it)
 + {
 + res = min( (*it).distNorm(my_pose.pos.x,​my_pose.pos.y),​ res);
 + }
 + //gzdbg << "pose : " << my_pose << endl; ////////////////////////////////////////////​
 + //gzdbg << "pose : " << wire_pose << endl; ////////////////////////////////////////////​
 + return res;
 + }
 +
 + void GazeboRosSonar::​Load(sensors::​SensorPtr _sensor, sdf::​ElementPtr _sdf)
 + {
 + gzdbg << "Debut fonction Load" << endl; /////////////////////////////////////////////////////​
 +
 + //===== Get the world name. =====//
 + std::​string worldName = _sensor->​GetWorldName();​
 + world = physics::​get_world(worldName);​
 +
 +
 + //===== Get the link ref name and model ref name. =====//
 + string get_parent_name_tmp;​
 + get_parent_name_tmp = _sensor->​GetParentName();​
 +
 + boost::​regex re("::"​);​
 + boost::​sregex_token_iterator p(get_parent_name_tmp.begin(),​ get_parent_name_tmp.end(),​ re, -1);
 + boost::​sregex_token_iterator end;
 +
 + model_ref_name = *p;
 + link_ref_name = *(++p);
 +
 + //===== Load Param =====//
 + if (_sdf->​HasElement("​pathToConfig"​))
 + {
 + path_to_config = _sdf->​GetElement("​pathToConfig"​)->​GetValue()->​GetAsString();​
 + //gzdbg << "path to config : " << path_to_config << endl; //////////////////////////​
 + ifstream fichier_conf(path_to_config.c_str(),​ ios::in);
 +
 + while(fichier_conf.eof() == false)
 + {
 + line my_tmp_line;​
 + fichier_conf >> my_tmp_line.x0;​
 + fichier_conf >> my_tmp_line.y0;​
 + fichier_conf >> my_tmp_line.x1;​
 + fichier_conf >> my_tmp_line.y1;​
 + lines_wire.push_back(my_tmp_line);​
 + }
 +
 + // for (std::​vector<​line>::​iterator it = lines_wire.begin() ; it < lines_wire.end() ; ++it)
 + // {
 + // gzdbg << (*it).distNorm(0,​0) << endl; ///////////////////////////////////​
 + // }
 + fichier_conf.close();​
 +
 + }
 + else
 + {
 + path_to_config.clear();​
 + gzdbg << "No pathToConfig in SDF file" << endl;
 + exit(1);
 + }
 +
 +
 + if (_sdf->​HasElement("​robotNamespace"​))
 + namespace_ = _sdf->​GetElement("​robotNamespace"​)->​GetValue()->​GetAsString();​
 + else
 + namespace_.clear();​
 +
 + if (!_sdf->​HasElement("​topicName"​))
 + topic_ = "​default";​
 + else
 + topic_ = _sdf->​GetElement("​topicName"​)->​Get<​std::​string>​();​
 +
 + //===== ROS Loading =====//
 + if (!ros::​isInitialized())
 + {
 + int argc = 0;
 + char** argv = NULL;
 + ros::​init(argc,​argv,"​gazebo",​ros::​init_options::​NoSigintHandler|ros::​init_options::​AnonymousName);​
 + }
 +
 + node_handle_ = new ros::​NodeHandle(namespace_);​
 + publisher_ = node_handle_->​advertise<​std_msgs::​Float64>​(topic_,​ 1);
 +
 +
 +
 +
 +
 + //===== Run update fonction. =====/
 + updateConnection = event::​Events::​ConnectWorldUpdateBegin(boost::​bind(&​GazeboRosSonar::​Update,​ this));
 +
 + gzdbg << "Fin fonction Load" << endl; /////////////////////////////////////////////////////​
 + }
 +
 + void GazeboRosSonar::​Reset()
 + {
 + }
 +
 +
 + void GazeboRosSonar::​Update()
 + {
 + if (notSetup())
 + {
 + trySetup();​
 + }
 + else
 + {
 + std_msgs::​Float64 my_ros_float = std_msgs::​Float64();​
 + my_ros_float.data = useSensor();​
 + publisher_.publish(my_ros_float);​
 +
 + }
 + }
 +
 + // Register this plugin with the simulator
 + GZ_REGISTER_SENSOR_PLUGIN(GazeboRosSonar);​
 +
 +}
 +
 +</​code>​
 +
 +
 +The above example implement a magnetic ware sensor. We are going to explain the main features.
 +
 +==== hello_world.hh ====
 +There is nothing really important here. We can see in the plugin class that we declare 3 functions, Load, Reset and Update. There are the main class function, the others are specific to this particular application.
 +
 +==== hello_world.cc ====
 +=== Load function ===
 +
 +Let's begin with the Load function.
 +
 +<code c++>
 +void GazeboRosSonar::​Load(sensors::​SensorPtr _sensor, sdf::​ElementPtr _sdf)
 + {
 + gzdbg << "Debut fonction Load" << endl; /////////////////////////////////////////////////////​
 + 
 + //===== Get the world name. =====//
 + std::​string worldName = _sensor->​GetWorldName();​
 + world = physics::​get_world(worldName);​
 + 
 + 
 + //===== Get the link ref name and model ref name. =====//
 + string get_parent_name_tmp;​
 + get_parent_name_tmp = _sensor->​GetParentName();​
 + 
 + boost::​regex re("::"​);​
 + boost::​sregex_token_iterator p(get_parent_name_tmp.begin(),​ get_parent_name_tmp.end(),​ re, -1);
 + boost::​sregex_token_iterator end;
 + 
 + model_ref_name = *p;
 + link_ref_name = *(++p);
 +
 +</​code>​
 +We get pointers and names on several gazebo elements like the World, the model and the first link of the model. With names we can access to their pointers as with get_world method.
 +
 +<code c++> ​
 + //===== Load Param =====//
 + if (_sdf->​HasElement("​pathToConfig"​))
 + {
 + path_to_config = _sdf->​GetElement("​pathToConfig"​)->​GetValue()->​GetAsString();​
 + //gzdbg << "path to config : " << path_to_config << endl; //////////////////////////​
 + ifstream fichier_conf(path_to_config.c_str(),​ ios::in);
 + 
 + while(fichier_conf.eof() == false)
 + {
 + line my_tmp_line;​
 + fichier_conf >> my_tmp_line.x0;​
 + fichier_conf >> my_tmp_line.y0;​
 + fichier_conf >> my_tmp_line.x1;​
 + fichier_conf >> my_tmp_line.y1;​
 + lines_wire.push_back(my_tmp_line);​
 + }
 + 
 + // for (std::​vector<​line>::​iterator it = lines_wire.begin() ; it < lines_wire.end() ; ++it)
 + // {
 + // gzdbg << (*it).distNorm(0,​0) << endl; ///////////////////////////////////​
 + // }
 + fichier_conf.close();​
 + 
 + }
 + else
 + {
 + path_to_config.clear();​
 + gzdbg << "No pathToConfig in SDF file" << endl;
 + exit(1);
 + }
 + </​code>​
 +We know that we can pass arguments to the plugin with sdf elements. So firstly we check whether pathToConfig is in the sdf file. Then we load the config.
 + 
 +<code c++>
 + if (_sdf->​HasElement("​robotNamespace"​))
 + namespace_ = _sdf->​GetElement("​robotNamespace"​)->​GetValue()->​GetAsString();​
 + else
 + namespace_.clear();​
 + 
 + if (!_sdf->​HasElement("​topicName"​))
 + topic_ = "​default";​
 + else
 + topic_ = _sdf->​GetElement("​topicName"​)->​Get<​std::​string>​();​
 +</​code>​
 +We do the same things with robotNamespace,​ and topicName.
 + 
 +<code c++>
 +
 + //===== ROS Loading =====//
 + if (!ros::​isInitialized())
 + {
 + int argc = 0;
 + char** argv = NULL;
 + ros::​init(argc,​argv,"​gazebo",​ros::​init_options::​NoSigintHandler|ros::​init_options::​AnonymousName);​
 + }
 + 
 + node_handle_ = new ros::​NodeHandle(namespace_);​
 + publisher_ = node_handle_->​advertise<​std_msgs::​Float64>​(topic_,​ 1);
 + 
 + 
 + 
 +</​code>​
 +Since our plugin will communicate with ros, it needs to initialize a ROS Node. It's the same API that for a class ROS Cpp software. Their is a little trick, we don't have main function so we create "​fake"​ argc and argv in order for ros::init to work.
 +
 +
 +<code c++> ​
 + //===== Run update fonction. =====/
 + updateConnection = event::​Events::​ConnectWorldUpdateBegin(boost::​bind(&​GazeboRosSonar::​Update,​ this));
 + 
 + gzdbg << "Fin fonction Load" << endl; /////////////////////////////////////////////////////​
 + }
 +</​code>​
 +Finally, we need to tell to Gazebo that we want the Update function to spin at each update steps.
gazebo_plugin.txt ยท Last modified: 2018/05/22 10:33 (external edit)