teo_plugin.cpp
Go to the documentation of this file.
00001 /*
00002  *  Gazebo - Outdoor Multi-Robot Simulator
00003  *  Copyright (C) 2003
00004  *     Nate Koenig & Andrew Howard
00005  *
00006  *  This program is free software; you can redistribute it and/or modify
00007  *  it under the terms of the GNU General Public License as published by
00008  *  the Free Software Foundation; either version 2 of the License, or
00009  *  (at your option) any later version.
00010  *
00011  *  This program is distributed in the hope that it will be useful,
00012  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014  *  GNU General Public License for more details.
00015  *
00016  *  You should have received a copy of the GNU General Public License
00017  *  along with this program; if not, write to the Free Software
00018  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019  *
00020  */
00021 /*
00022  * Desc: ROS interface to a Position2d controller for a Differential drive.
00023  * Author: Daniel Hewlett (adapted from Nathan Koenig)
00024  */
00025 
00026 #include <algorithm>
00027 #include <assert.h>
00028 
00029 #include "teo_plugin.h"
00030 
00031 #include <gazebo/gazebo.h>
00032 #include <gazebo/GazeboError.hh>
00033 #include <gazebo/Quatern.hh>
00034 #include <gazebo/Controller.hh>
00035 #include <gazebo/Body.hh>
00036 #include <gazebo/World.hh>
00037 #include <gazebo/Simulator.hh>
00038 #include <gazebo/Global.hh>
00039 #include <gazebo/XMLConfig.hh>
00040 #include <gazebo/ControllerFactory.hh>
00041 #include <gazebo/PhysicsEngine.hh>
00042 
00043 #include <ros/ros.h>
00044 #include <tf/transform_broadcaster.h>
00045 #include <tf/transform_listener.h>
00046 #include <geometry_msgs/Twist.h>
00047 #include <nav_msgs/Odometry.h>
00048 #include <boost/bind.hpp>
00049 
00050 using namespace gazebo;
00051 
00052 GZ_REGISTER_DYNAMIC_CONTROLLER("teo_plugin", TeoPlugin);
00053 
00054 enum
00055 {
00056   FR, FL, BR, BL
00057 };
00058 
00059 // Constructor
00060 TeoPlugin::TeoPlugin(Entity * parent) :
00061     Controller(parent),
00062     vel_x_(0.0),
00063     rot_z_(0.0),
00064     previous_displ_(0.0),
00065     alive_(true)
00066 {
00067   parent_ = dynamic_cast<Model *> (parent);
00068 
00069   if (!parent_)
00070     gzthrow("Differential_Position2d controller requires a Model as its parent");
00071 
00072   prevUpdateTime_ = Simulator::Instance()->GetSimTime();
00073 
00074   Param::Begin(&parameters);
00075   robotNamespaceP = new ParamT<std::string> ("robotNamespace", "/", 0);
00076   topicNameP      = new ParamT<std::string> ("topicName", "", 1);
00077   frNameP         = new ParamT<std::string> ("fr_joint","", 1);
00078   flNameP         = new ParamT<std::string> ("fl_joint","", 1);
00079   brNameP         = new ParamT<std::string> ("br_joint","", 1);
00080   blNameP         = new ParamT<std::string> ("bl_joint","", 1);
00081   Param::End();
00082 }
00083 
00084 // Destructor
00085 TeoPlugin::~TeoPlugin()
00086 {
00087     delete robotNamespaceP;
00088     delete topicNameP;
00089     delete frNameP;
00090     delete flNameP;
00091     delete brNameP;
00092     delete blNameP;
00093     delete callback_queue_thread_;
00094     delete rosnode_;
00095     delete transform_broadcaster_;
00096 }
00097 
00098 // Load the controller
00099 void TeoPlugin::LoadChild(XMLConfigNode *node)
00100 {
00101   pos_iface_ = dynamic_cast<libgazebo::PositionIface*> (GetIface("position"));
00102 
00103   // Initialize the ROS node and subscribe to cmd_vel
00104 
00105   robotNamespaceP->Load(node);
00106   topicNameP->Load(node);
00107   frNameP->Load(node);
00108   flNameP->Load(node);
00109   brNameP->Load(node);
00110   blNameP->Load(node);
00111 
00112   robotNamespace = robotNamespaceP->GetValue();
00113   topicName      = topicNameP->GetValue();
00114 
00115   ROS_INFO("fr: '%s'",frNameP->GetValue().c_str());
00116   ROS_INFO("fl: '%s'",flNameP->GetValue().c_str());
00117   ROS_INFO("br: '%s'",brNameP->GetValue().c_str());
00118   ROS_INFO("bl: '%s'",blNameP->GetValue().c_str());
00119 
00120   joints_[FR] = parent_->GetJoint(** frNameP);
00121   joints_[FL] = parent_->GetJoint(** flNameP);
00122   joints_[BR] = parent_->GetJoint(** brNameP);
00123   joints_[BL] = parent_->GetJoint(** blNameP);
00124 
00125   int argc = 0;
00126   char** argv = NULL;
00127   ros::init(argc, argv, "diff_drive_plugin", 
00128             ros::init_options::NoSigintHandler | ros::init_options::AnonymousName);
00129   rosnode_ = new ros::NodeHandle(robotNamespace);
00130 
00131   tf_prefix_ = tf::getPrefixParam(*rosnode_);
00132   transform_broadcaster_ = new tf::TransformBroadcaster();
00133 
00134   // ROS: Subscribe to the velocity command topic (usually "cmd_vel")
00135   ros::SubscribeOptions so =
00136       ros::SubscribeOptions::create<geometry_msgs::Twist>(topicName, 1,
00137                                                           boost::bind(&TeoPlugin::cmdVelCallback, this, _1),
00138                                                           ros::VoidPtr(), &queue_);
00139   sub_ = rosnode_->subscribe(so);
00140   pub_ = rosnode_->advertise<nav_msgs::Odometry>("odom", 1);
00141 }
00142 
00143 // Initialize the controller
00144 void TeoPlugin::InitChild()
00145 {
00146   // Reset odometric pose
00147   ROS_INFO("init child");
00148   odomPose[0] = 0.0;
00149   odomPose[1] = 0.0;
00150   odomPose[2] = 0.0;
00151 
00152   odomVel[0] = 0.0;
00153   odomVel[1] = 0.0;
00154   odomVel[2] = 0.0;
00155 
00156   callback_queue_thread_ = new boost::thread(boost::bind(&TeoPlugin::QueueThread, this));
00157 }
00158 
00159 // Reset
00160 void TeoPlugin::ResetChild()
00161 {
00162   // Reset odometric pose
00163   ROS_INFO("reset child");
00164   odomPose[0] = 0.0;
00165   odomPose[1] = 0.0;
00166   odomPose[2] = 0.0;
00167 
00168   odomVel[0] = 0.0;
00169   odomVel[1] = 0.0;
00170   odomVel[2] = 0.0;
00171 }
00172 
00173 // Update the controller
00174 void TeoPlugin::UpdateChild()
00175 {
00176   // TODO: Step should be in a parameter of this function
00177   double wd, ws;
00178   double d1, d2;
00179   double dr, da;
00180   Time delta_time;
00181 
00182   //myIface->Lock(1);
00183 
00184   // GetPositionCmd();
00185 
00186   // wd = **(wheelDiamP);
00187   // ws = **(wheelSepP);
00188 
00189   //stepTime = World::Instance()->GetPhysicsEngine()->GetStepTime();
00190   delta_time = Simulator::Instance()->GetSimTime() - prevUpdateTime_;
00191   prevUpdateTime_ = Simulator::Instance()->GetSimTime();
00192 
00193   // Distance travelled by front wheels
00194   //d1 = stepTime.Double() * wd / 2 * joints[LEFT]->GetVelocity(0);
00195   //d2 = stepTime.Double() * wd / 2 * joints[RIGHT]->GetVelocity(0);
00196 
00197   //dr = (d1 + d2) / 2;
00198   // da = (d1 - d2) / ws;
00199 
00200   double displacement_x = delta_time.Double() * vel_x_;
00201 
00202   /* if (vel_x_ > 0.0) {
00203       ROS_INFO("vel_x_: '%f'", vel_x_);
00204       ROS_INFO("time  : '%f'", delta_time.Double());
00205       ROS_INFO("displ : '%f'", displacement_x);
00206   }
00207   */
00208   /* Compute odometric pose
00209   odomPose[0] += dr * cos(odomPose[2]);
00210   odomPose[1] += dr * sin(odomPose[2]);
00211   odomPose[2] += da;
00212 
00213   odomPose[0] += dr * cos(odomPose[2]);
00214   odomPose[1] += dr * sin(odomPose[2]);
00215   odomPose[2] += da; */
00216 
00217 
00218     odomPose[0] += displacement_x;
00219     odomPose[1] += 0;
00220     odomPose[2] += 0;
00221 
00222     odomVel[0] = vel_x_;
00223     odomVel[1] = 0.0;
00224     odomVel[2] = 0.0;
00225 
00226     // ROS_DEBUG("updating odomPose[0]: '%f'", odomPose[0]);
00227 
00228     ROS_DEBUG("odomVel[0]: '%f'", odomVel[0]);
00229     ROS_DEBUG("vel X: '%f'", vel_x_);
00230 
00231     joints_[FR]->SetVelocity(0, vel_x_);
00232     joints_[FL]->SetVelocity(0, vel_x_);
00233     joints_[BR]->SetVelocity(0, vel_x_);
00234     joints_[BL]->SetVelocity(0, vel_x_);
00235 
00236     joints_[FR]->SetMaxForce(0, 5);
00237     joints_[FL]->SetMaxForce(0, 5);
00238     joints_[BR]->SetMaxForce(0, 5);
00239     joints_[BL]->SetMaxForce(0, 5);
00240 
00241     write_position_data();
00242     publish_odometry();
00243 
00244     previous_displ_ += odomPose[0];
00245 
00246   //myIface->Unlock();
00247 }
00248 
00249 // Finalize the controller
00250 void TeoPlugin::FiniChild()
00251 {
00252   //std::cout << "ENTERING FINALIZE\n";
00253   alive_ = false;
00254   // Custom Callback Queue
00255   queue_.clear();
00256   queue_.disable();
00257   rosnode_->shutdown();
00258   callback_queue_thread_->join();
00259   //std::cout << "EXITING FINALIZE\n";
00260 }
00261 
00262 void TeoPlugin::cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg)
00263 {
00264   lock.lock();
00265 
00266   vel_x_  = cmd_msg->linear.x;
00267   rot_z_  = cmd_msg->angular.z;
00268 
00269   ROS_INFO("cmd_vel callback recieved - X: '%f'", vel_x_);
00270 
00271   lock.unlock();
00272 }
00273 
00274 // NEW: custom callback queue thread
00275 void TeoPlugin::QueueThread()
00276 {
00277   static const double timeout = 0.01;
00278 
00279   while (alive_ && rosnode_->ok())
00280   {
00281     //    std::cout << "CALLING STUFF\n";
00282     queue_.callAvailable(ros::WallDuration(timeout));
00283   }
00284 }
00285 
00286 // NEW: Update this to publish odometry topic
00287 void TeoPlugin::publish_odometry()
00288 {
00289   // get current time
00290   ros::Time current_time_((Simulator::Instance()->GetSimTime()).sec, (Simulator::Instance()->GetSimTime()).nsec); 
00291 
00292   // getting data for base_footprint to odom transform
00293   btQuaternion qt;
00294   // TODO: Is there something wrong here? RVIZ has a problem?
00295   qt.setEulerZYX(pos_iface_->data->pose.yaw, pos_iface_->data->pose.pitch, pos_iface_->data->pose.roll);
00296   btVector3 vt(pos_iface_->data->pose.pos.x, pos_iface_->data->pose.pos.y, pos_iface_->data->pose.pos.z);
00297   tf::Transform base_footprint_to_odom(qt, vt);
00298 
00299   transform_broadcaster_->sendTransform(tf::StampedTransform(base_footprint_to_odom,
00300                                                             current_time_,
00301                                                             "odom",
00302                                                             "base_footprint"));
00303 
00304   // publish odom topic
00305   odom_.pose.pose.position.x = pos_iface_->data->pose.pos.x;
00306   odom_.pose.pose.position.y = pos_iface_->data->pose.pos.y;
00307 
00308   gazebo::Quatern rot;
00309   rot.SetFromEuler(gazebo::Vector3(pos_iface_->data->pose.roll, pos_iface_->data->pose.pitch, pos_iface_->data->pose.yaw));
00310 
00311   odom_.pose.pose.orientation.x = rot.x;
00312   odom_.pose.pose.orientation.y = rot.y;
00313   odom_.pose.pose.orientation.z = rot.z;
00314   odom_.pose.pose.orientation.w = rot.u;
00315 
00316   odom_.twist.twist.linear.x = pos_iface_->data->velocity.pos.x;
00317   odom_.twist.twist.linear.y = pos_iface_->data->velocity.pos.y;
00318   odom_.twist.twist.angular.z = pos_iface_->data->velocity.yaw;
00319 
00320   odom_.header.frame_id = tf::resolve(tf_prefix_, "odom");
00321   odom_.child_frame_id = "base_footprint";
00322 
00323   //odom_.header.stamp = current_time_;
00324   odom_.header.stamp.sec = (Simulator::Instance()->GetSimTime()).sec;
00325   odom_.header.stamp.nsec = (Simulator::Instance()->GetSimTime()).nsec;
00326 
00327   ROS_DEBUG("Odometry Position - X: '%f' Y: '%f'",  odom_.pose.pose.position.x,  odom_.pose.pose.position.y);
00328   pub_.publish(odom_);
00329 }
00330 
00331 // Update the data in the interface
00332 void TeoPlugin::write_position_data()
00333 {
00334   // TODO: Data timestamp
00335   pos_iface_->data->head.time = Simulator::Instance()->GetSimTime().Double();
00336   ROS_DEBUG("write_position_data - X: '%f', - Y '%f', - Z '%f'", odomPose[0], odomPose[1], NORMALIZE(odomPose[2]));
00337   ROS_DEBUG("write_vel_data - X: '%f', - Y '%f'", odomVel[0], odomVel[2]);
00338 
00339   pos_iface_->data->pose.pos.x = odomPose[0];
00340   pos_iface_->data->pose.pos.y = odomPose[1];
00341   pos_iface_->data->pose.yaw = NORMALIZE(odomPose[2]);
00342 
00343   pos_iface_->data->velocity.pos.x = odomVel[0];
00344   pos_iface_->data->velocity.yaw = odomVel[2];
00345 
00346   // TODO
00347   pos_iface_->data->stall = 0;
00348 }


teo_gazebo_plugin
Author(s): Jose Luis Rivero
autogenerated on Fri Dec 6 2013 21:34:45