Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
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 )
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 )
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 updateTimer.Reset();
00126
00127 if ( runner ? runner->initialize() : this->initialize() ) {
00128 running = this->isPeriodic();
00129 } else {
00130 active = false;
00131 }
00132 return active;
00133 }
00134
00135
00136 bool GazeboActivity::stop()
00137 {
00138 if ( !active )
00139 return false;
00140
00141
00142 if ( this->isPeriodic() == false) {
00143 if ( running && (runner ? (runner->breakLoop() == false): (this->breakLoop() == false) ) )
00144 return false;
00145 }
00146
00147 running = false;
00148 if (runner)
00149 runner->finalize();
00150 else
00151 this->finalize();
00152 active = false;
00153 return true;
00154 }
00155
00156 bool GazeboActivity::isRunning() const
00157 {
00158 return running;
00159 }
00160
00161 bool GazeboActivity::isPeriodic() const
00162 {
00163 return true;
00164 }
00165 bool GazeboActivity::isActive() const
00166 {
00167 return active;
00168 }
00169
00170 bool GazeboActivity::trigger()
00171 {
00172 return false;
00173 }
00174
00175 bool GazeboActivity::execute()
00176 {
00177 if (!running) return false;
00178 if (!updateTimer.update()) return true;
00179
00180 ROS_DEBUG_NAMED("gazebo_rtt_plugin", "Updating RTT plugin %s at t = %f", mname.c_str(), mworld->GetSimTime().Double());
00181 if (runner) runner->step(); else this->step();
00182
00183 return true;
00184 }
00185
00186 }