SetVelPlugin.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 
00009 #include <ros/ros.h>
00010 #include <ros/callback_queue.h>
00011 #include <ros/advertise_options.h>
00012 #include <ros/subscribe_options.h>
00013 
00014 #include <geometry_msgs/Twist.h>
00015 #include <geometry_msgs/Pose.h>
00016 #include <geometry_msgs/Vector3.h>
00017 #include <geometry_msgs/Wrench.h>
00018 #include <std_msgs/String.h>
00019 
00020 
00021 namespace gazebo
00022 {
00023   class SetVel : public ModelPlugin
00024   {
00025 
00026   public:
00027     // Initialize
00028     void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
00029     {
00030       // Store the pointer to the model
00031       this->model = _parent;
00032 
00033       // read option args in sdf tags
00034       this->obj_name = "";
00035       if (_sdf->HasElement("objname")) {
00036         this->obj_name = _sdf->Get<std::string>("objname");
00037       }
00038       this->link_name = "root";
00039       if (_sdf->HasElement("linkname")) {
00040         this->link_name = _sdf->Get<std::string>("linkname");
00041       }
00042       this->apply_in_gazebo_loop = true;
00043       if (_sdf->HasElement("apply_in_gazebo_loop")) {
00044         if(_sdf->Get<std::string>("apply_in_gazebo_loop") == "false") {
00045           this->apply_in_gazebo_loop = false;
00046         }
00047       }
00048 
00049       // find root link
00050       this->link = this->model->GetLink(this->link_name);
00051       if(!this->link) {
00052         gzerr << "Root link are not found. (link_name is "<< this->link_name << ")" << std::endl;
00053         return;
00054       }
00055 
00056       // initialize flag
00057       this->set_pose_flag = false;
00058       this->set_vel_flag = false;
00059 
00060       // Make sure the ROS node for Gazebo has already been initialized
00061       if (!ros::isInitialized()) {
00062         gzerr << "A ROS node for Gazebo has not been initialized, unable to load plugin. "
00063               << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)";
00064         return;
00065       }
00066       // ros node
00067       this->rosNode = new ros::NodeHandle("");
00068       // ros callback queue for processing subscription
00069       this->deferredLoadThread = boost::thread(boost::bind(&SetVel::DeferredLoad, this));
00070     }
00071 
00072     void DeferredLoad() {
00073       // ros topic subscribtions
00074       ros::SubscribeOptions VelCommandSo =
00075         ros::SubscribeOptions::create<geometry_msgs::Twist>("/" + this->obj_name + "/SetVelPlugin/VelCommand", 100,
00076                                                     boost::bind(&SetVel::SetVelCommand, this, _1),
00077                                                     ros::VoidPtr(), &this->rosQueue);
00078       ros::SubscribeOptions PoseCommandSo =
00079         ros::SubscribeOptions::create<geometry_msgs::Pose>("/" + this->obj_name + "/SetVelPlugin/PoseCommand", 100,
00080                                               boost::bind(&SetVel::SetPoseCommand, this, _1),
00081                                               ros::VoidPtr(), &this->rosQueue);
00082       // Enable TCP_NODELAY because TCP causes bursty communication with high jitter,
00083       VelCommandSo.transport_hints = ros::TransportHints().reliable().tcpNoDelay(true);
00084       this->subVelCommand = this->rosNode->subscribe(VelCommandSo);
00085       PoseCommandSo.transport_hints = ros::TransportHints().reliable().tcpNoDelay(true);
00086       this->subPoseCommand = this->rosNode->subscribe(PoseCommandSo);
00087 
00088       // ros callback queue for processing subscription
00089       this->callbackQueeuThread = boost::thread(boost::bind(&SetVel::RosQueueThread, this));
00090 
00091       // Listen to the update event.
00092       this->updateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&SetVel::OnUpdate, this, _1));
00093 
00094       gzmsg << "SetVelPlugin was loaded !" << std::endl;
00095     }
00096 
00097     void SetVelCommand(const geometry_msgs::Twist::ConstPtr &_msg)
00098     {
00099       this->linear_vel.x = _msg->linear.x;
00100       this->linear_vel.y = _msg->linear.y;
00101       this->linear_vel.z = _msg->linear.z;
00102       this->angular_vel.x = _msg->angular.x;
00103       this->angular_vel.y = _msg->angular.y;
00104       this->angular_vel.z = _msg->angular.z;
00105       this->set_vel_flag = true;
00106       gzmsg << "subscribed SetVelCommand. ( linear: " << this->linear_vel << "  angular: " << this->angular_vel << " )" << std::endl;
00107       if (!this->apply_in_gazebo_loop) {
00108         this->model->SetLinearVel(this->linear_vel);
00109         this->model->SetAngularVel(this->angular_vel);
00110       }
00111     }
00112 
00113     void SetPoseCommand(const geometry_msgs::Pose::ConstPtr &_msg)
00114     {
00115       this->model->SetLinearVel(math::Vector3(0, 0, 0));
00116       this->model->SetAngularVel(math::Vector3(0, 0, 0));
00117       this->pose.Set(math::Vector3(_msg->position.x, _msg->position.y, _msg->position.z),
00118                      math::Quaternion(_msg->orientation.w, _msg->orientation.x, _msg->orientation.y, _msg->orientation.z));
00119       this->set_pose_flag = true;
00120       gzdbg << "subscribed SetPoseCommand. ( position: " << this->pose.pos << "  orientation: " << this->pose.rot << " )" << std::endl;
00121       if (!this->apply_in_gazebo_loop) {
00122         // this->model->SetLinkWorldPose(this->pose, this->link);
00123         this->model->SetWorldPose(this->pose, this->link);
00124       }
00125     }
00126 
00127     // Called by the world update start event
00128     void OnUpdate(const common::UpdateInfo & /*_info*/)
00129     {
00130       if (this->apply_in_gazebo_loop) {
00131         if (this->set_pose_flag) {
00132           this->model->SetWorldPose(this->pose, this->link);
00133         }
00134         if (this->set_vel_flag) {
00135           this->model->SetLinearVel(this->linear_vel);
00136           this->model->SetAngularVel(this->angular_vel);
00137         }
00138       }
00139     }
00140 
00141     // Ros loop thread function
00142     void RosQueueThread() {
00143       static const double timeout = 0.01;
00144 
00145       while (this->rosNode->ok()) {
00146         this->rosQueue.callAvailable(ros::WallDuration(timeout));
00147       }
00148     }
00149 
00150   private:
00151     physics::ModelPtr model;
00152     std::string obj_name;
00153     std::string link_name;
00154     math::Vector3 linear_vel;
00155     math::Vector3 angular_vel;
00156     math::Pose pose;
00157     physics::LinkPtr link;
00158     event::ConnectionPtr updateConnection;
00159     bool set_pose_flag;
00160     bool set_vel_flag;
00161     bool apply_in_gazebo_loop;
00162 
00163     ros::NodeHandle* rosNode;
00164     ros::CallbackQueue rosQueue;
00165     ros::Subscriber subVelCommand;
00166     ros::Subscriber subPoseCommand;
00167     boost::thread callbackQueeuThread;
00168     boost::thread deferredLoadThread;
00169   };
00170 
00171   GZ_REGISTER_MODEL_PLUGIN(SetVel)
00172 }


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