gazebo_activity.cpp
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2012, Johannes Meyer, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Flight Systems and Automatic Control group,
00013 //       TU Darmstadt, nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #include <gazebo_rtt_plugin/gazebo_activity.h>
00030 #include <gazebo/physics/PhysicsEngine.hh>
00031 
00032 #include <rtt/base/RunnableInterface.hpp>
00033 
00034 #include <ros/console.h>
00035 
00036 namespace gazebo {
00037 
00038 using namespace RTT::base;
00039 using namespace common;
00040 
00041 GazeboActivity::GazeboActivity(const std::string& name, RunnableInterface* run /*= 0*/ )
00042   : ActivityInterface(run), mname(name), running(false), active(false)
00043 {
00044 }
00045 
00046 GazeboActivity::GazeboActivity(const std::string& name, common::Time period, physics::WorldPtr world, RunnableInterface* run /*= 0*/ )
00047   : ActivityInterface(run), mname(name), mworld(world), running(false), active(false)
00048 {
00049   updateTimer.setUpdatePeriod(period);
00050 }
00051 
00052 GazeboActivity::~GazeboActivity()
00053 {
00054   stop();
00055 }
00056 
00057 void GazeboActivity::Load(physics::WorldPtr _world, sdf::ElementPtr _sdf, const std::string& _prefix)
00058 {
00059   mworld = _world;
00060   updateTimer.Load(_world, _sdf, _prefix);
00061 }
00062 
00063 RTT::Seconds GazeboActivity::getPeriod() const
00064 {
00065   double period = updateTimer.getUpdatePeriod().Double();
00066   if (period > 0.0)
00067     return RTT::Seconds(period);
00068   else
00069     return RTT::Seconds(mworld->GetPhysicsEngine()->GetUpdatePeriod());
00070 }
00071 
00072 bool GazeboActivity::setPeriod(RTT::Seconds s) {
00073   updateTimer.setUpdatePeriod(s);
00074   return true;
00075 }
00076 
00077 unsigned GazeboActivity::getCpuAffinity() const
00078 {
00079   return ~0;
00080 }
00081 
00082 bool GazeboActivity::setCpuAffinity(unsigned cpu)
00083 {
00084   return false;
00085 }
00086 
00087 RTT::os::ThreadInterface* GazeboActivity::thread()
00088 {
00089   return 0;
00090 }
00091 
00092 bool GazeboActivity::initialize()
00093 {
00094   return true;
00095 }
00096 
00097 void GazeboActivity::step()
00098 {
00099 }
00100 
00101 void GazeboActivity::loop()
00102 {
00103   this->step();
00104 }
00105 
00106 bool GazeboActivity::breakLoop()
00107 {
00108   return false;
00109 }
00110 
00111 
00112 void GazeboActivity::finalize()
00113 {
00114 }
00115 
00116 bool GazeboActivity::start()
00117 {
00118   if ( active == true )
00119   {
00120     gzerr << "Unable to start slave as it is already started" << std::endl;
00121     return false;
00122   }
00123 
00124   active = true;
00125 
00126   if ( runner ? runner->initialize() : this->initialize() ) {
00127     running = true;
00128   } else {
00129     active = false;
00130   }
00131 
00132   if (active) {
00133     updateConnection = updateTimer.Connect(boost::bind(&GazeboActivity::execute, this));
00134     updateTimer.Reset();
00135   }
00136 
00137   return active;
00138 }
00139 
00140 
00141 bool GazeboActivity::stop()
00142 {
00143   if ( !active )
00144     return false;
00145 
00146   updateTimer.Disconnect(updateConnection);
00147 
00148   running = false;
00149   if (runner)
00150     runner->finalize();
00151   else
00152     this->finalize();
00153   active = false;
00154   return true;
00155 }
00156 
00157 bool GazeboActivity::isRunning() const
00158 {
00159   return running;
00160 }
00161 
00162 bool GazeboActivity::isPeriodic() const
00163 {
00164   return true;
00165 }
00166 
00167 bool GazeboActivity::isActive() const
00168 {
00169   return active;
00170 }
00171 
00172 bool GazeboActivity::trigger()
00173 {
00174   return false;
00175 }
00176 
00177 bool GazeboActivity::execute()
00178 {
00179   if (!running) return false;
00180 
00181   ROS_DEBUG_NAMED("gazebo_rtt_plugin", "Updating RTT plugin %s at t = %f", mname.c_str(), mworld->GetSimTime().Double());
00182   if (runner) runner->step(); else this->step();
00183 
00184   return true;
00185 }
00186 
00187 } // namespace gazebo


gazebo_rtt_plugin
Author(s): Johannes Meyer
autogenerated on Mon Oct 6 2014 00:23:23