gazebo_plugin
Differences
This shows you the differences between two versions of the page.
Both sides previous revisionPrevious revisionNext revision | Previous revision | ||
gazebo_plugin [2015/05/22 12:20] – [hello_world.cc] superstage | gazebo_plugin [2018/05/22 10:33] (current) – external edit 127.0.0.1 | ||
---|---|---|---|
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:// | ||
+ | * [[http:// | ||
+ | * [[http:// | ||
+ | * [[http:// | ||
+ | |||
+ | |||
+ | ===== Study an example ===== | ||
+ | |||
+ | There is a Gazebo plugin code example. | ||
+ | <code c++> | ||
+ | // | ||
+ | |||
+ | #ifndef HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_SONAR_H | ||
+ | #define HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_SONAR_H | ||
+ | |||
+ | #include < | ||
+ | #include < | ||
+ | |||
+ | #include < | ||
+ | #include < | ||
+ | |||
+ | #include < | ||
+ | #include < | ||
+ | #include < | ||
+ | |||
+ | |||
+ | |||
+ | #include < | ||
+ | #include < | ||
+ | |||
+ | 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:: | ||
+ | virtual void Reset(); | ||
+ | virtual void Update(); | ||
+ | |||
+ | bool notSetup(); | ||
+ | void trySetup(); | ||
+ | float useSensor(); | ||
+ | |||
+ | private: | ||
+ | /// \brief The parent World | ||
+ | physics:: | ||
+ | |||
+ | rendering:: | ||
+ | rendering:: | ||
+ | |||
+ | physics:: | ||
+ | physics:: | ||
+ | |||
+ | sensors:: | ||
+ | |||
+ | ros:: | ||
+ | ros:: | ||
+ | |||
+ | sensor_msgs:: | ||
+ | |||
+ | std::string namespace_; | ||
+ | std::string topic_; | ||
+ | std::string model_ref_name; | ||
+ | std::string link_ref_name; | ||
+ | std::string path_to_config; | ||
+ | |||
+ | std:: | ||
+ | |||
+ | // | ||
+ | |||
+ | |||
+ | event:: | ||
+ | |||
+ | |||
+ | }; | ||
+ | |||
+ | } // namespace gazebo | ||
+ | |||
+ | #endif // HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_SONAR_H | ||
+ | </ | ||
+ | |||
+ | <code c++> | ||
+ | // | ||
+ | |||
+ | #include < | ||
+ | |||
+ | #include " | ||
+ | #include < | ||
+ | #include < | ||
+ | #include < | ||
+ | #include < | ||
+ | #include < | ||
+ | #include < | ||
+ | |||
+ | |||
+ | #include < | ||
+ | #include < | ||
+ | #include < | ||
+ | #include < | ||
+ | #include < | ||
+ | #include < | ||
+ | |||
+ | using namespace std; | ||
+ | |||
+ | bool the_bool_tmp = true; | ||
+ | |||
+ | float line:: | ||
+ | { | ||
+ | float a = sqrt(pow(x-x1, | ||
+ | float b = sqrt(pow(x-x0, | ||
+ | float c = sqrt(pow(x1-x0, | ||
+ | |||
+ | 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, | ||
+ | float delta = acos( (pow(c,2) + pow(a,2) - pow(b, | ||
+ | |||
+ | // 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; | ||
+ | } | ||
+ | else | ||
+ | { | ||
+ | //gzdbg << " | ||
+ | return min(a,b); | ||
+ | } | ||
+ | return 0.0; | ||
+ | } | ||
+ | |||
+ | |||
+ | namespace gazebo { | ||
+ | |||
+ | GazeboRosSonar:: | ||
+ | { | ||
+ | } | ||
+ | |||
+ | |||
+ | GazeboRosSonar:: | ||
+ | { | ||
+ | } | ||
+ | |||
+ | bool GazeboRosSonar:: | ||
+ | { | ||
+ | return link_ref == NULL || scene == NULL || !scene-> | ||
+ | } | ||
+ | |||
+ | void GazeboRosSonar:: | ||
+ | { | ||
+ | physics:: | ||
+ | physics:: | ||
+ | physics:: | ||
+ | |||
+ | model_ref = world-> | ||
+ | |||
+ | |||
+ | link_ref = model_ref-> | ||
+ | |||
+ | |||
+ | |||
+ | scene = rendering:: | ||
+ | // Wait until the scene is initialized. | ||
+ | if (!scene || !scene-> | ||
+ | return; | ||
+ | else | ||
+ | { | ||
+ | |||
+ | gzdbg << "Nom scene : " << scene-> | ||
+ | |||
+ | |||
+ | math:: | ||
+ | math:: | ||
+ | math:: | ||
+ | rendering:: | ||
+ | visual = tmp; | ||
+ | |||
+ | scene-> | ||
+ | // | ||
+ | |||
+ | // visual-> | ||
+ | // visual-> | ||
+ | // gzdbg << "name visual : " << visual-> | ||
+ | |||
+ | // rendering:: | ||
+ | // test.MakeStatic(); | ||
+ | // test.Load(); | ||
+ | |||
+ | the_bool_tmp = false; | ||
+ | gzdbg << "Fin de la config" | ||
+ | } | ||
+ | |||
+ | } | ||
+ | |||
+ | float GazeboRosSonar:: | ||
+ | { | ||
+ | math:: | ||
+ | float res = 1000000.0; | ||
+ | |||
+ | for (std:: | ||
+ | { | ||
+ | res = min( (*it).distNorm(my_pose.pos.x, | ||
+ | } | ||
+ | //gzdbg << "pose : " << my_pose << endl; //////////////////////////////////////////// | ||
+ | //gzdbg << "pose : " << wire_pose << endl; //////////////////////////////////////////// | ||
+ | return res; | ||
+ | } | ||
+ | |||
+ | void GazeboRosSonar:: | ||
+ | { | ||
+ | gzdbg << "Debut fonction Load" << endl; ///////////////////////////////////////////////////// | ||
+ | |||
+ | //===== Get the world name. =====// | ||
+ | std:: | ||
+ | world = physics:: | ||
+ | |||
+ | |||
+ | //===== Get the link ref name and model ref name. =====// | ||
+ | string get_parent_name_tmp; | ||
+ | get_parent_name_tmp = _sensor-> | ||
+ | |||
+ | boost:: | ||
+ | boost:: | ||
+ | boost:: | ||
+ | |||
+ | model_ref_name = *p; | ||
+ | link_ref_name = *(++p); | ||
+ | |||
+ | //===== Load Param =====// | ||
+ | if (_sdf-> | ||
+ | { | ||
+ | path_to_config = _sdf-> | ||
+ | //gzdbg << "path to config : " << path_to_config << endl; ////////////////////////// | ||
+ | ifstream fichier_conf(path_to_config.c_str(), | ||
+ | |||
+ | 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:: | ||
+ | // { | ||
+ | // gzdbg << (*it).distNorm(0, | ||
+ | // } | ||
+ | fichier_conf.close(); | ||
+ | |||
+ | } | ||
+ | else | ||
+ | { | ||
+ | path_to_config.clear(); | ||
+ | gzdbg << "No pathToConfig in SDF file" << endl; | ||
+ | exit(1); | ||
+ | } | ||
+ | |||
+ | |||
+ | if (_sdf-> | ||
+ | namespace_ = _sdf-> | ||
+ | else | ||
+ | namespace_.clear(); | ||
+ | |||
+ | if (!_sdf-> | ||
+ | topic_ = " | ||
+ | else | ||
+ | topic_ = _sdf-> | ||
+ | |||
+ | //===== ROS Loading =====// | ||
+ | if (!ros:: | ||
+ | { | ||
+ | int argc = 0; | ||
+ | char** argv = NULL; | ||
+ | ros:: | ||
+ | } | ||
+ | |||
+ | node_handle_ = new ros:: | ||
+ | publisher_ = node_handle_-> | ||
+ | |||
+ | |||
+ | |||
+ | |||
+ | |||
+ | //===== Run update fonction. =====/ | ||
+ | updateConnection = event:: | ||
+ | |||
+ | gzdbg << "Fin fonction Load" << endl; ///////////////////////////////////////////////////// | ||
+ | } | ||
+ | |||
+ | void GazeboRosSonar:: | ||
+ | { | ||
+ | } | ||
+ | |||
+ | |||
+ | void GazeboRosSonar:: | ||
+ | { | ||
+ | if (notSetup()) | ||
+ | { | ||
+ | trySetup(); | ||
+ | } | ||
+ | else | ||
+ | { | ||
+ | std_msgs:: | ||
+ | my_ros_float.data = useSensor(); | ||
+ | publisher_.publish(my_ros_float); | ||
+ | |||
+ | } | ||
+ | } | ||
+ | |||
+ | // Register this plugin with the simulator | ||
+ | GZ_REGISTER_SENSOR_PLUGIN(GazeboRosSonar); | ||
+ | |||
+ | } | ||
+ | |||
+ | </ | ||
+ | |||
+ | |||
+ | 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:: | ||
+ | { | ||
+ | gzdbg << "Debut fonction Load" << endl; ///////////////////////////////////////////////////// | ||
+ | |||
+ | //===== Get the world name. =====// | ||
+ | std:: | ||
+ | world = physics:: | ||
+ | |||
+ | |||
+ | //===== Get the link ref name and model ref name. =====// | ||
+ | string get_parent_name_tmp; | ||
+ | get_parent_name_tmp = _sensor-> | ||
+ | |||
+ | boost:: | ||
+ | boost:: | ||
+ | boost:: | ||
+ | |||
+ | model_ref_name = *p; | ||
+ | link_ref_name = *(++p); | ||
+ | |||
+ | </ | ||
+ | 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-> | ||
+ | { | ||
+ | path_to_config = _sdf-> | ||
+ | //gzdbg << "path to config : " << path_to_config << endl; ////////////////////////// | ||
+ | ifstream fichier_conf(path_to_config.c_str(), | ||
+ | |||
+ | 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:: | ||
+ | // { | ||
+ | // gzdbg << (*it).distNorm(0, | ||
+ | // } | ||
+ | fichier_conf.close(); | ||
+ | |||
+ | } | ||
+ | else | ||
+ | { | ||
+ | path_to_config.clear(); | ||
+ | gzdbg << "No pathToConfig in SDF file" << endl; | ||
+ | exit(1); | ||
+ | } | ||
+ | </ | ||
+ | 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-> | ||
+ | namespace_ = _sdf-> | ||
+ | else | ||
+ | namespace_.clear(); | ||
+ | |||
+ | if (!_sdf-> | ||
+ | topic_ = " | ||
+ | else | ||
+ | topic_ = _sdf-> | ||
+ | </ | ||
+ | We do the same things with robotNamespace, | ||
+ | |||
+ | <code c++> | ||
+ | |||
+ | //===== ROS Loading =====// | ||
+ | if (!ros:: | ||
+ | { | ||
+ | int argc = 0; | ||
+ | char** argv = NULL; | ||
+ | ros:: | ||
+ | } | ||
+ | |||
+ | node_handle_ = new ros:: | ||
+ | publisher_ = node_handle_-> | ||
+ | |||
+ | |||
+ | |||
+ | </ | ||
+ | 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 " | ||
+ | |||
+ | |||
+ | <code c++> | ||
+ | //===== Run update fonction. =====/ | ||
+ | updateConnection = event:: | ||
+ | |||
+ | gzdbg << "Fin fonction Load" << endl; ///////////////////////////////////////////////////// | ||
+ | } | ||
+ | </ | ||
+ | Finally, we need to tell to Gazebo that we want the Update function to spin at each update steps. |