GetVelPlugin.cpp
Go to the documentation of this file.
00001 #include <iostream>
00002 #include <stdio.h>
00003 
00004 #include <boost/bind.hpp>
00005 #include <gazebo/gazebo.hh>
00006 #include <gazebo/physics/physics.hh>
00007 #include <gazebo/common/common.hh>
00008 #include <gazebo/common/Time.hh>
00009 
00010 #include <ros/ros.h>
00011 #include <ros/callback_queue.h>
00012 #include <ros/advertise_options.h>
00013 #include <ros/subscribe_options.h>
00014 
00015 #include <geometry_msgs/Twist.h>
00016 #include <geometry_msgs/TwistStamped.h>
00017 #include <geometry_msgs/Pose.h>
00018 #include <geometry_msgs/PoseStamped.h>
00019 #include <geometry_msgs/Vector3.h>
00020 #include <geometry_msgs/Wrench.h>
00021 #include <std_msgs/String.h>
00022 
00023 #include "PubQueue.h"
00024 
00025 
00026 namespace gazebo
00027 {
00028   class GetVel : public ModelPlugin
00029   {
00030 
00031   public:
00032     // Initialize
00033     void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
00034     {
00035       // Store the pointer to the model
00036       this->model = _parent;
00037 
00038       // read option args in sdf tags
00039       this->obj_name = "";
00040       if (_sdf->HasElement("objname")) {
00041         this->obj_name = _sdf->Get<std::string>("objname");
00042       }
00043       this->link_name = "root";
00044       if (_sdf->HasElement("linkname")) {
00045         this->link_name = _sdf->Get<std::string>("linkname");
00046       }
00047 
00048       // find root link
00049       this->link = this->model->GetLink(this->link_name);
00050       if(!this->link) {
00051         gzerr << "Root link are not found. (link_name is "<< this->link_name << ")" << std::endl;
00052         return;
00053       }
00054       world = this->model->GetWorld();
00055 
00056       // Make sure the ROS node for Gazebo has already been initialized
00057       if (!ros::isInitialized()) {
00058         gzerr << "A ROS node for Gazebo has not been initialized, unable to load plugin. "
00059               << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)";
00060         return;
00061       }
00062       // ros node
00063       this->rosNode = new ros::NodeHandle("");
00064       // ros callback queue for processing subscription
00065       this->deferredLoadThread = boost::thread(boost::bind(&GetVel::DeferredLoad, this));
00066     }
00067 
00068     void DeferredLoad() {
00069       // publish multi queue
00070       this->pmq.startServiceThread();
00071 
00072       // ros topic publications
00073       this->pubRelVelQueue = this->pmq.addPub<geometry_msgs::TwistStamped>();
00074       this->pubRelVel = this->rosNode->advertise<geometry_msgs::TwistStamped>("/" + this->obj_name + "/GetVelPlugin/RelVel", 100, true);
00075       this->pubAbsVelQueue = this->pmq.addPub<geometry_msgs::TwistStamped>();
00076       this->pubAbsVel = this->rosNode->advertise<geometry_msgs::TwistStamped>("/" + this->obj_name + "/GetVelPlugin/AbsVel", 100, true);
00077       this->pubRelAccelQueue = this->pmq.addPub<geometry_msgs::TwistStamped>();
00078       this->pubRelAccel = this->rosNode->advertise<geometry_msgs::TwistStamped>("/" + this->obj_name + "/GetVelPlugin/RelAccel", 100, true);
00079       this->pubAbsAccelQueue = this->pmq.addPub<geometry_msgs::TwistStamped>();
00080       this->pubAbsAccel = this->rosNode->advertise<geometry_msgs::TwistStamped>("/" + this->obj_name + "/GetVelPlugin/AbsAccel", 100, true);
00081       this->pubPoseQueue = this->pmq.addPub<geometry_msgs::PoseStamped>();
00082       this->pubPose = this->rosNode->advertise<geometry_msgs::PoseStamped>("/" + this->obj_name + "/GetVelPlugin/Pose", 100, true);
00083 
00084       // Listen to the update event.
00085       this->updateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&GetVel::OnUpdate, this, _1));
00086 
00087       gzmsg << "GetVelPlugin was loaded !" << std::endl;
00088     }
00089 
00090     // Called by the world update start event
00091     void OnUpdate(const common::UpdateInfo & /*_info*/)
00092     {
00093       common::Time curTime = this->world->GetSimTime();
00094 
00095       // publish topics
00096       this->PublishVel(curTime);
00097     }
00098 
00099     // Publish function
00100     void PublishVel(const common::Time &_curTime)
00101     {
00102       geometry_msgs::TwistStamped _twist;
00103       geometry_msgs::PoseStamped _pose;
00104       math::Vector3 linear_vel;
00105       math::Vector3 angular_vel;
00106       math::Vector3 linear_accel;
00107       math::Vector3 angular_accel;
00108       math::Pose pose;
00109       _twist.header.stamp = ros::Time(_curTime.sec, _curTime.nsec);
00110       _pose.header.stamp = ros::Time(_curTime.sec, _curTime.nsec);
00111 
00112       // set rel vel
00113       linear_vel = this->model->GetRelativeLinearVel();
00114       _twist.twist.linear.x = linear_vel.x;
00115       _twist.twist.linear.y = linear_vel.y;
00116       _twist.twist.linear.z = linear_vel.z;
00117       angular_vel = this->model->GetRelativeAngularVel();
00118       _twist.twist.angular.x = angular_vel.x;
00119       _twist.twist.angular.y = angular_vel.y;
00120       _twist.twist.angular.z = angular_vel.z;
00121       // publish rel vel
00122       this->pubRelVelQueue->push(_twist, this->pubRelVel);
00123 
00124       // set abs vel
00125       linear_vel = this->model->GetWorldLinearVel();
00126       _twist.twist.linear.x = linear_vel.x;
00127       _twist.twist.linear.y = linear_vel.y;
00128       _twist.twist.linear.z = linear_vel.z;
00129       angular_vel = this->model->GetWorldAngularVel();
00130       _twist.twist.angular.x = angular_vel.x;
00131       _twist.twist.angular.y = angular_vel.y;
00132       _twist.twist.angular.z = angular_vel.z;
00133       // publish abs vel
00134       this->pubAbsVelQueue->push(_twist, this->pubAbsVel);
00135 
00136       // set rel accel
00137       linear_accel = this->model->GetRelativeLinearAccel();
00138       _twist.twist.linear.x = linear_accel.x;
00139       _twist.twist.linear.y = linear_accel.y;
00140       _twist.twist.linear.z = linear_accel.z;
00141       angular_accel = this->model->GetRelativeAngularAccel();
00142       _twist.twist.angular.x = angular_accel.x;
00143       _twist.twist.angular.y = angular_accel.y;
00144       _twist.twist.angular.z = angular_accel.z;
00145       // publish rel accel
00146       this->pubRelAccelQueue->push(_twist, this->pubRelAccel);
00147 
00148       // set abs accel
00149       linear_accel = this->model->GetWorldLinearAccel();
00150       _twist.twist.linear.x = linear_accel.x;
00151       _twist.twist.linear.y = linear_accel.y;
00152       _twist.twist.linear.z = linear_accel.z;
00153       angular_accel = this->model->GetWorldAngularAccel();
00154       _twist.twist.angular.x = angular_accel.x;
00155       _twist.twist.angular.y = angular_accel.y;
00156       _twist.twist.angular.z = angular_accel.z;
00157       // publish abs accel
00158       this->pubAbsAccelQueue->push(_twist, this->pubAbsAccel);
00159 
00160       // set pose
00161       pose = this->link->GetWorldCoGPose();
00162       // pose = this->link->GetWorldPose();
00163       _pose.pose.position.x = pose.pos.x;
00164       _pose.pose.position.y = pose.pos.y;
00165       _pose.pose.position.z = pose.pos.z;
00166       _pose.pose.orientation.x = pose.rot.x;
00167       _pose.pose.orientation.y = pose.rot.y;
00168       _pose.pose.orientation.z = pose.rot.z;
00169       _pose.pose.orientation.w = pose.rot.w;
00170       // publish pose
00171       this->pubPoseQueue->push(_pose, this->pubPose);
00172     }
00173 
00174   private:
00175     physics::ModelPtr model;
00176     physics::WorldPtr world;
00177     std::string obj_name;
00178     std::string link_name;
00179     physics::LinkPtr link;
00180     event::ConnectionPtr updateConnection;
00181 
00182     ros::NodeHandle* rosNode;
00183     PubMultiQueue pmq;
00184     boost::mutex mutex;
00185     boost::thread deferredLoadThread;
00186 
00187     ros::Publisher pubRelVel;
00188     PubQueue<geometry_msgs::TwistStamped>::Ptr pubRelVelQueue;
00189     ros::Publisher pubAbsVel;
00190     PubQueue<geometry_msgs::TwistStamped>::Ptr pubAbsVelQueue;
00191     ros::Publisher pubRelAccel;
00192     PubQueue<geometry_msgs::TwistStamped>::Ptr pubRelAccelQueue;
00193     ros::Publisher pubAbsAccel;
00194     PubQueue<geometry_msgs::TwistStamped>::Ptr pubAbsAccelQueue;
00195     ros::Publisher pubPose;
00196     PubQueue<geometry_msgs::PoseStamped>::Ptr pubPoseQueue;
00197   };
00198 
00199   GZ_REGISTER_MODEL_PLUGIN(GetVel)
00200 }


hrpsys_gazebo_general
Author(s): Yohei Kakiuchi , Kei Okada , Masaki Murooka
autogenerated on Thu Jun 6 2019 20:56:53