gazebo_ros_diffdrive.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: Controller for diffdrive robots in gazebo.
00023  * Author: Tony Pratkanis
00024  * Date: 17 April 2009
00025  * SVN info: $Id$
00026  */
00027 
00028 #include <gazebo/GazeboError.hh>
00029 #include <gazebo/Quatern.hh>
00030 
00031 #include <ros/ros.h>
00032 #include <tf/transform_broadcaster.h>
00033 #include <geometry_msgs/Twist.h>
00034 #include <nav_msgs/Odometry.h>
00035 
00036 #include <boost/bind.hpp>
00037 
00038 class DiffDrive {
00039 public:
00040   libgazebo::PositionIface *posIface;
00041   ros::NodeHandle* rnh_;
00042   ros::Subscriber  sub_;
00043   ros::Publisher   pub_;
00044   
00045   void cmdVelCallBack(const geometry_msgs::Twist::ConstPtr& cmd_msg) {
00046         std::cout << " pos " << this->posIface
00047                   <<    " x " << cmd_msg->linear.x
00048                   <<    " y " << cmd_msg->linear.y
00049                   <<    " z " << cmd_msg->angular.z
00050                   << std::endl;
00051         
00052     if (this->posIface) {
00053       this->posIface->Lock(1);
00054       this->posIface->data->cmdVelocity.pos.x = cmd_msg->linear.x;
00055       this->posIface->data->cmdVelocity.pos.y = cmd_msg->linear.y;
00056       this->posIface->data->cmdVelocity.yaw = cmd_msg->angular.z;
00057       this->posIface->Unlock();
00058     }
00059   }
00060 
00061   DiffDrive() {
00062     libgazebo::Client *client = new libgazebo::Client();
00063     libgazebo::SimulationIface *simIface = new libgazebo::SimulationIface();
00064     this->posIface = new libgazebo::PositionIface();
00065   
00066     int serverId = 0;
00067     
00069     try {
00070       client->ConnectWait(serverId, GZ_CLIENT_ID_USER_FIRST);
00071     } catch (gazebo::GazeboError e) {
00072       std::cout << "Gazebo error: Unable to connect\n" << e << "\n";
00073       return;
00074     }
00075     
00077     try {
00078       simIface->Open(client, "default");
00079     } catch (gazebo::GazeboError e) {
00080       std::cout << "Gazebo error: Unable to connect to the sim interface\n" << e << "\n";
00081       return;
00082     }
00083     
00085     try {
00086       this->posIface->Open(client, "robot_description::position_iface_0");
00087     } catch (std::string e) {
00088       std::cout << "Gazebo error: Unable to connect to the position interface\n" << e << "\n";
00089       return;
00090     }
00091     
00092     // Enable the motor
00093     this->posIface->Lock(1);
00094     this->posIface->data->cmdEnableMotors = 1;
00095     this->posIface->Unlock();
00096 
00097     this->rnh_ = new ros::NodeHandle();
00098     //this->sub_ = rnh_->subscribe<geometry_msgs::Twist>("/cmd_vel", 100, boost::bind(&DiffDrive::cmdVelCallBack,this,_1));
00099     this->sub_ = rnh_->subscribe<geometry_msgs::Twist>("/cmd_vel", 100, &DiffDrive::cmdVelCallBack,this);
00100     this->pub_ = rnh_->advertise<nav_msgs::Odometry>("/erratic_odometry/odom", 1);
00101    
00102     // spawn 2 threads by default, ///@todo: make this a parameter
00103     ros::MultiThreadedSpinner s(2);
00104     boost::thread spinner_thread( boost::bind( &ros::spin, s ) );
00105 
00106     nav_msgs::Odometry odom;
00107 
00108     // setup transform publishers, need to duplicate pr2_odometry controller functionalities
00109     tf::TransformBroadcaster transform_broadcaster_ ;
00110 
00111     ros::Duration d; d.fromSec(0.01);
00112     
00113     while(rnh_->ok()) { 
00114       if (this->posIface) {
00115         this->posIface->Lock(1);
00116         
00117         // duplicate pr2_odometry functionalities, broadcast
00118         // transforms from base_footprint to odom
00119         // and from base_link to base_footprint
00120 
00121         // get current time
00122         ros::Time current_time_ = ros::Time::now();
00123 
00124         // getting data for base_link to odom transform
00125         btQuaternion qt;
00126         qt.setRPY(this->posIface->data->pose.roll, this->posIface->data->pose.pitch, this->posIface->data->pose.yaw);
00127         btVector3 vt(this->posIface->data->pose.pos.x, this->posIface->data->pose.pos.y, this->posIface->data->pose.pos.z);
00128         tf::Transform base_link_to_odom(qt, vt);
00129         transform_broadcaster_.sendTransform(tf::StampedTransform(base_link_to_odom,ros::Time::now(),"odom","base_link"));
00130 
00131 
00132         // publish odom topic
00133         odom.pose.pose.position.x = this->posIface->data->pose.pos.x;
00134         odom.pose.pose.position.y = this->posIface->data->pose.pos.y;
00135 
00136         gazebo::Quatern rot;
00137         rot.SetFromEuler(gazebo::Vector3(this->posIface->data->pose.roll,this->posIface->data->pose.pitch,this->posIface->data->pose.yaw));
00138 
00139         odom.pose.pose.orientation.x = rot.x;
00140         odom.pose.pose.orientation.y = rot.y;
00141         odom.pose.pose.orientation.z = rot.z;
00142         odom.pose.pose.orientation.w = rot.u;
00143 
00144         odom.twist.twist.linear.x = this->posIface->data->velocity.pos.x;
00145         odom.twist.twist.linear.y = this->posIface->data->velocity.pos.y;
00146         odom.twist.twist.angular.z = this->posIface->data->velocity.yaw;
00147         
00148         odom.header.frame_id = "odom"; 
00149         
00150         odom.header.stamp = ros::Time::now();
00151         
00152         this->pub_.publish(odom); 
00153 
00154         this->posIface->Unlock();
00155       }
00156       d.sleep();
00157     }
00158   }
00159   ~DiffDrive() {
00160     delete this->rnh_;
00161   }
00162 };
00163 
00164 
00165 
00166 
00167 int main(int argc, char** argv) {
00168   ros::init(argc,argv,"gazebo_ros_diffdrive",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00169 
00170   DiffDrive d;
00171   return 0;
00172 }
00173 
00174 


gazebo_plugins
Author(s): Sachin Chitta, Stu Glaser, John Hsu
autogenerated on Mon Oct 6 2014 12:15:44