plugin.cpp
Go to the documentation of this file.
00001  /*********************************************************************
00002  *  Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2015 Fetch Robotics Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Fetch Robotics nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 // Author: Michael Ferguson
00036 
00037 #include <ros/ros.h>
00038 #include <sensor_msgs/JointState.h>
00039 #include <robot_controllers_interface/controller_manager.h>
00040 #include <fetch_gazebo/joint_handle.h>
00041 #include <gazebo/common/common.hh>
00042 #include <gazebo/physics/physics.hh>
00043 #include <gazebo/gazebo.hh>
00044 #include <urdf/model.h>
00045 
00046 namespace gazebo
00047 {
00048 
00049 class FetchGazeboPlugin : public ModelPlugin
00050 {
00051 public:
00052   FetchGazeboPlugin();
00053   ~FetchGazeboPlugin();
00054   virtual void Load(physics::ModelPtr parent, sdf::ElementPtr sdf);
00055   virtual void Init();
00056 
00057 private:
00058   void OnUpdate(const common::UpdateInfo& info);
00059 
00060   physics::ModelPtr model_;
00061   event::ConnectionPtr update_;
00062   common::Time prevUpdateTime;
00063 
00064   std::vector<JointHandlePtr> joints_;
00065   robot_controllers::ControllerManager controller_manager_;
00066   ros::Time last_update_time_;
00067 
00068   ros::Publisher joint_state_pub_;
00069 
00070   ros::NodeHandle nh_;
00071   ros::Time last_publish_;
00072 };
00073 
00074 FetchGazeboPlugin::FetchGazeboPlugin()
00075 {
00076 }
00077 
00078 FetchGazeboPlugin::~FetchGazeboPlugin()
00079 {
00080 }
00081 
00082 void FetchGazeboPlugin::Load(
00083   physics::ModelPtr parent,
00084   sdf::ElementPtr sdf)
00085 {
00086   // Need to hang onto model
00087   model_ = parent;
00088 
00089   // Update each simulation iteration
00090   update_ = event::Events::ConnectWorldUpdateBegin(
00091               boost::bind(&FetchGazeboPlugin::OnUpdate, this, _1));
00092 }
00093 
00094 void FetchGazeboPlugin::Init()
00095 {
00096   // Init time stuff
00097   prevUpdateTime = model_->GetWorld()->GetSimTime();
00098   last_publish_ = ros::Time(prevUpdateTime.Double());
00099   urdf::Model urdfmodel;
00100   if (!urdfmodel.initParam("robot_description"))
00101   {
00102     ROS_ERROR("Failed to parse URDF");  
00103   }
00104 
00105   // Init joint handles
00106   gazebo::physics::Joint_V joints = model_->GetJoints();
00107   for (physics::Joint_V::iterator it = joints.begin(); it != joints.end(); ++it)
00108   {
00109     //get effort limit and continuous state from URDF
00110     boost::shared_ptr<const urdf::Joint> urdf_joint = urdfmodel.getJoint((*it)->GetName());
00111 
00112     JointHandlePtr handle(new JointHandle(*it,
00113                                           urdf_joint->limits->velocity,
00114                                           urdf_joint->limits->effort,
00115                                           (urdf_joint->type == urdf::Joint::CONTINUOUS)));
00116     joints_.push_back(handle);
00117     robot_controllers::JointHandlePtr h(handle);
00118     controller_manager_.addJointHandle(h);
00119   }
00120 
00121   // Init controllers
00122   ros::NodeHandle pnh("~");
00123   controller_manager_.init(pnh);
00124 
00125   // Publish joint states only after controllers are fully ready
00126   joint_state_pub_ = nh_.advertise<sensor_msgs::JointState>("joint_states", 10);
00127 
00128   ROS_INFO("Finished initializing FetchGazeboPlugin");
00129 }
00130 
00131 void FetchGazeboPlugin::OnUpdate(
00132   const common::UpdateInfo& info)
00133 {
00134   if (!ros::ok())
00135     return;
00136 
00137   // Get time and timestep for controllers
00138   common::Time currTime = model_->GetWorld()->GetSimTime();
00139   common::Time stepTime = currTime - prevUpdateTime;
00140   prevUpdateTime = currTime;
00141   double dt = stepTime.Double();
00142   ros::Time now = ros::Time(currTime.Double());
00143 
00144   // Update controllers
00145   controller_manager_.update(now, ros::Duration(dt));
00146 
00147   // Update joints back into Gazebo
00148   for (size_t i = 0; i < joints_.size(); ++i)
00149     joints_[i]->update(now, ros::Duration(dt));
00150 
00151   // Limit publish rate
00152   if (now - last_publish_ < ros::Duration(0.01))
00153     return;
00154 
00155   // Publish joint_state message
00156   sensor_msgs::JointState js;
00157   js.header.stamp = ros::Time(currTime.Double());
00158   for (size_t i = 0; i < joints_.size(); ++i)
00159   {
00160     js.name.push_back(joints_[i]->getName());
00161     js.position.push_back(joints_[i]->getPosition());
00162     js.velocity.push_back(joints_[i]->getVelocity());
00163     js.effort.push_back(joints_[i]->getEffort());
00164   }
00165   joint_state_pub_.publish(js);
00166 
00167   last_publish_ = now;
00168 }
00169 
00170 GZ_REGISTER_MODEL_PLUGIN(FetchGazeboPlugin)
00171 
00172 }  // namespace gazebo


fetch_gazebo
Author(s): Michael Ferguson
autogenerated on Wed Apr 3 2019 02:48:45