gazebo_ros_benchmarks.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: Plugin for testing pr2 simulator performance
00023  * Author: John Hsu
00024  * Date: 1 July 2010
00025  * SVN info: $Id$
00026  */
00027 
00028 
00029 #include <pr2_gazebo_benchmarks/gazebo_ros_benchmarks.h>
00030 
00031 #include <ros/advertise_options.h>
00032 #include <ros/subscribe_options.h>
00033 
00034 #include <algorithm>
00035 #include <assert.h>
00036 
00037 #include <boost/thread.hpp>
00038 #include <boost/bind.hpp>
00039 
00040 using namespace gazebo;
00041 
00042 GZ_REGISTER_PLUGIN("gazebo_ros_benchmarks", GazeboRosBenchmarks);
00043 
00045 // Constructor
00046 GazeboRosBenchmarks::GazeboRosBenchmarks()
00047     : Plugin()
00048 {
00049   this->timing_initialized_                                = false;
00050   this->world_update_ros_connect_count_                    = 0;
00051   this->simulation_cycle_ros_connect_count_                = 0;
00052   this->world_update_start_sim_time_                       = 0;
00053   this->world_update_start_wall_time_                      = 0;
00054   this->world_update_end_sim_time_                         = 0;
00055   this->world_update_end_wall_time_                        = 0;
00056   this->last_world_update_start_sim_time_                  = 0;
00057   this->last_world_update_start_wall_time_                 = 0;
00058   this->last_world_update_end_sim_time_                    = 0;
00059   this->last_world_update_end_wall_time_                   = 0;
00060   std_srvs::Empty emp;
00061   this->ResetTimingStatistics(emp.request,emp.response);
00062 }
00063 
00064 bool GazeboRosBenchmarks::ResetTimingStatistics(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res)
00065 {
00066   this->world_update_pr2_gazebo_benchmarks_.max_duration       = -1.0;
00067   this->world_update_pr2_gazebo_benchmarks_.min_duration       = 1e6;
00068   this->world_update_pr2_gazebo_benchmarks_.avg_duration       = 0.0;
00069   this->world_update_pr2_gazebo_benchmarks_.tot_duration       = 0;
00070   this->world_update_pr2_gazebo_benchmarks_.cur_duration       = 0;
00071   this->world_update_pr2_gazebo_benchmarks_.count              = 0;
00072   this->simulation_cycle_pr2_gazebo_benchmarks_.max_duration   = -1.0;
00073   this->simulation_cycle_pr2_gazebo_benchmarks_.min_duration   = 1e6;
00074   this->simulation_cycle_pr2_gazebo_benchmarks_.avg_duration   = 0.0;
00075   this->simulation_cycle_pr2_gazebo_benchmarks_.tot_duration   = 0;
00076   this->simulation_cycle_pr2_gazebo_benchmarks_.cur_duration   = 0;
00077   this->simulation_cycle_pr2_gazebo_benchmarks_.count          = 0;
00078   return true;
00079 }
00080 
00081 
00083 // Destructor
00084 GazeboRosBenchmarks::~GazeboRosBenchmarks()
00085 {
00086   World::Instance()->DisconnectWorldUpdateStartSignal(boost::bind(&GazeboRosBenchmarks::UpdateCBWorldUpdateStart, this));
00087   World::Instance()->DisconnectWorldUpdateEndSignal(boost::bind(&GazeboRosBenchmarks::UpdateCBWorldUpdateEnd, this));
00088   delete this->rosnode_;
00089 }
00090 
00092 // Load the controller
00093 void GazeboRosBenchmarks::Load()
00094 {
00095   // start ros node if not initialized
00096   if (!ros::isInitialized())
00097   {
00098     int argc = 0;
00099     char** argv = NULL;
00100     ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00101   }
00102   this->rosnode_ = new ros::NodeHandle("");
00103 
00104   // start custom queue for ros
00105   this->ros_callback_queue_thread_ = new boost::thread( boost::bind( &GazeboRosBenchmarks::RosQueueThread,this ) );
00106 
00107   // publish timing info
00108   ros::AdvertiseOptions simulation_cycle_ros_ao = ros::AdvertiseOptions::create<pr2_gazebo_benchmarks::GazeboBenchmarks>(
00109     "simulation_cycle_benchmarks",1,
00110     boost::bind( &GazeboRosBenchmarks::SimulationCycleRosConnect,this),
00111     boost::bind( &GazeboRosBenchmarks::SimulationCycleRosDisconnect,this), ros::VoidPtr(), &this->ros_queue_);
00112   this->simulation_cycle_pub_ = this->rosnode_->advertise(simulation_cycle_ros_ao);
00113 
00114   ros::AdvertiseOptions world_update_ros_ao = ros::AdvertiseOptions::create<pr2_gazebo_benchmarks::GazeboBenchmarks>(
00115     "world_update_benchmarks",1,
00116     boost::bind( &GazeboRosBenchmarks::WorldUpdateRosConnect,this),
00117     boost::bind( &GazeboRosBenchmarks::WorldUpdateRosDisconnect,this), ros::VoidPtr(), &this->ros_queue_);
00118   this->world_update_pub_ = this->rosnode_->advertise(world_update_ros_ao);
00119 
00120 
00121   // Advertise more services on the custom queue
00122   ros::AdvertiseServiceOptions reset_timing_aso = ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
00123       "reset_simulation_timing_statistics",boost::bind(&GazeboRosBenchmarks::ResetTimingStatistics,this,_1,_2),
00124       ros::VoidPtr(), &this->ros_queue_);
00125   this->reset_timing_service_ = this->rosnode_->advertiseService(reset_timing_aso);
00126 
00127 
00128   // Update the controller through callback slots
00129   World::Instance()->ConnectWorldUpdateStartSignal(boost::bind(&GazeboRosBenchmarks::UpdateCBWorldUpdateStart, this));
00130   World::Instance()->ConnectWorldUpdateEndSignal(boost::bind(&GazeboRosBenchmarks::UpdateCBWorldUpdateEnd, this));
00131 
00132 }
00133 
00134 
00136 // Update the controller through callback slots
00137 void GazeboRosBenchmarks::UpdateCBWorldUpdateStart()
00138 {
00139     // get times
00140     this->world_update_start_sim_time_ = Simulator::Instance()->GetSimTime();
00141     this->world_update_start_wall_time_ = Simulator::Instance()->GetWallTime();
00142     //ROS_INFO("real: %f sim: %f",this->world_update_start_wall_time_.Double(),this->world_update_start_sim_time_.Double());
00143 
00144     // compute statistics
00145     if (this->timing_initialized_)
00146     {
00147       // keep track of simulation cycle time
00148       this->simulation_cycle_pr2_gazebo_benchmarks_.cur_duration = this->world_update_start_wall_time_.Double()
00149                                                               -this->last_world_update_start_wall_time_.Double();
00150       this->simulation_cycle_pr2_gazebo_benchmarks_.tot_duration += this->simulation_cycle_pr2_gazebo_benchmarks_.cur_duration;
00151       this->simulation_cycle_pr2_gazebo_benchmarks_.count++;
00152       this->simulation_cycle_pr2_gazebo_benchmarks_.avg_duration = this->simulation_cycle_pr2_gazebo_benchmarks_.tot_duration
00153                                                                /(double)this->simulation_cycle_pr2_gazebo_benchmarks_.count;
00154       if (this->simulation_cycle_pr2_gazebo_benchmarks_.max_duration < this->simulation_cycle_pr2_gazebo_benchmarks_.cur_duration)
00155         this->simulation_cycle_pr2_gazebo_benchmarks_.max_duration = this->simulation_cycle_pr2_gazebo_benchmarks_.cur_duration;
00156       if (this->simulation_cycle_pr2_gazebo_benchmarks_.min_duration > this->simulation_cycle_pr2_gazebo_benchmarks_.cur_duration)
00157         this->simulation_cycle_pr2_gazebo_benchmarks_.min_duration = this->simulation_cycle_pr2_gazebo_benchmarks_.cur_duration;
00158       if (this->simulation_cycle_ros_connect_count_>0)
00159         this->simulation_cycle_pub_.publish(this->simulation_cycle_pr2_gazebo_benchmarks_);
00160     }
00161     this->last_world_update_start_sim_time_  = this->world_update_start_sim_time_ ;
00162     this->last_world_update_start_wall_time_ = this->world_update_start_wall_time_;
00163 }
00164 
00166 // Update the controller through callback slots
00167 void GazeboRosBenchmarks::UpdateCBWorldUpdateEnd()
00168 {
00169     // get times
00170     this->world_update_end_sim_time_ = Simulator::Instance()->GetSimTime();
00171     this->world_update_end_wall_time_ = Simulator::Instance()->GetWallTime();
00172     //ROS_INFO("real: %f sim: %f",this->world_update_end_wall_time_.Double(),this->world_update_end_sim_time_.Double());
00173 
00174     // compute statistics
00175 
00176     // compute statistics
00177     if (this->timing_initialized_)
00178     {
00179       // keep track of world update duration
00180       this->world_update_pr2_gazebo_benchmarks_.cur_duration = this->world_update_start_wall_time_.Double()
00181                                                           -this->world_update_end_wall_time_.Double();
00182       this->world_update_pr2_gazebo_benchmarks_.tot_duration += this->world_update_pr2_gazebo_benchmarks_.cur_duration;
00183       this->world_update_pr2_gazebo_benchmarks_.count++;
00184       this->world_update_pr2_gazebo_benchmarks_.avg_duration = this->world_update_pr2_gazebo_benchmarks_.tot_duration
00185                                                                /(double)this->world_update_pr2_gazebo_benchmarks_.count;
00186       if (this->world_update_pr2_gazebo_benchmarks_.max_duration < this->world_update_pr2_gazebo_benchmarks_.cur_duration)
00187         this->world_update_pr2_gazebo_benchmarks_.max_duration = this->world_update_pr2_gazebo_benchmarks_.cur_duration;
00188       if (this->world_update_pr2_gazebo_benchmarks_.min_duration > this->world_update_pr2_gazebo_benchmarks_.cur_duration)
00189         this->world_update_pr2_gazebo_benchmarks_.min_duration = this->world_update_pr2_gazebo_benchmarks_.cur_duration;
00190       if (this->world_update_ros_connect_count_>0)
00191         this->world_update_pub_.publish(this->world_update_pr2_gazebo_benchmarks_);
00192     }
00193     else
00194     {
00195       // do not update statistics yet this cycle
00196       this->timing_initialized_ = true;
00197     }
00198 
00199     this->last_world_update_end_sim_time_  = this->world_update_end_sim_time_ ;
00200     this->last_world_update_end_wall_time_ = this->world_update_end_wall_time_;
00201 
00202 }
00203 
00205 // ROS Publisher
00206 void GazeboRosBenchmarks::RosQueueThread()
00207 {
00208   static const double timeout = 0.001;
00209 
00210   while (this->rosnode_->ok())
00211   {
00212     this->ros_queue_.callAvailable(ros::WallDuration(timeout));
00213   }
00214 }
00215 
00216 


pr2_gazebo_benchmarks
Author(s): John Hsu
autogenerated on Thu Apr 24 2014 15:49:06