rtt_rosclock_sim_clock_thread.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Johannes Meyer, TU Darmstadt
00005  *  Copyright (c) 2013, Intermodalics BVBA
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of TU Darmstadt and Intermodalics BVBA
00019  *     nor the names of its contributors may be
00020  *     used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  *  Copyright (c) 2014, Jonathan Bohren, The Johns Hopkins University
00037  *  - Generalized for multiple time sources
00038  *  - Integrated with rtt_rosclock package
00039  *********************************************************************/
00040 
00041 #include <rtt_rosclock/rtt_rosclock_sim_clock_thread.h>
00042 #include <rtt_rosclock/rtt_rosclock_sim_clock_activity.h>
00043 #include <rtt_rosclock/rtt_rosclock_sim_clock_activity_manager.h>
00044 
00045 #include <rtt/TaskContext.hpp>
00046 #include <rtt/internal/GlobalService.hpp>
00047 #include <rtt/plugin/Plugin.hpp>
00048 
00049 #include <rtt/os/StartStopManager.hpp>
00050 
00051 #include <ros/time.h>
00052 #include <ros/node_handle.h>
00053 #include <ros/param.h>
00054 #include <ros/subscribe_options.h>
00055 
00056 #include <rtt_rosclock/rtt_rosclock.h>
00057 
00058 using namespace rtt_rosclock;
00059 
00060 boost::shared_ptr<SimClockThread> SimClockThread::singleton;
00061 
00062 boost::shared_ptr<SimClockThread> SimClockThread::GetInstance()
00063 {
00064   return singleton;
00065 }
00066 
00067 boost::shared_ptr<SimClockThread> SimClockThread::Instance()
00068 {
00069   // Create a new singleton, if necessary
00070   boost::shared_ptr<SimClockThread> shared = GetInstance();
00071   if(!shared) {
00072     shared.reset(new SimClockThread());
00073     singleton = shared;
00074   }
00075 
00076   return shared;
00077 }
00078 
00079 void SimClockThread::Release()
00080 {
00081   singleton.reset();
00082 }
00083 
00084 namespace {
00085   RTT::os::CleanupFunction cleanup(&SimClockThread::Release);
00086 }
00087 
00088 SimClockThread::SimClockThread() 
00089   : RTT::os::Thread(ORO_SCHED_OTHER, RTT::os::LowestPriority, 0.0, 0, "rtt_rosclock_SimClockThread")
00090   , time_service_(RTT::os::TimeService::Instance())
00091   , clock_source_(SIM_CLOCK_SOURCE_MANUAL)
00092   , process_callbacks_(false)
00093 {
00094 }
00095 
00096 SimClockThread::~SimClockThread()
00097 {
00098   this->stop();
00099 }
00100 
00101 bool SimClockThread::setClockSource(SimClockSource clock_source) 
00102 {
00103   // Don't allow changing the source while running
00104   if(this->isActive()) {
00105     RTT::log(RTT::Error) << "The SimClockThread clock source cannot be changed while the thread is running." << RTT::endlog();
00106     return false;
00107   }
00108 
00109   // Set the clock source 
00110   clock_source_ = clock_source;
00111 
00112   return true;
00113 }
00114 
00115 bool SimClockThread::useROSClockTopic() 
00116 {
00117   return this->setClockSource(SIM_CLOCK_SOURCE_ROS_CLOCK_TOPIC);
00118 }
00119 
00120 bool SimClockThread::useManualClock() 
00121 {
00122   return this->setClockSource(SIM_CLOCK_SOURCE_MANUAL);
00123 }
00124 
00125 bool SimClockThread::simTimeEnabled() const 
00126 { 
00127   return this->isActive();
00128 }
00129 
00130 void SimClockThread::clockMsgCallback(const rosgraph_msgs::ClockConstPtr& clock)
00131 {
00132   // Update the RTT clock
00133   updateClockInternal(ros::Time(clock->clock.sec, clock->clock.nsec));
00134 }
00135 
00136 bool SimClockThread::updateClock(const ros::Time new_time)
00137 {
00138   if(clock_source_ != SIM_CLOCK_SOURCE_MANUAL) {
00139     RTT::log(RTT::Error) << "Cannot update simulation clock manually unless the clock source is set to MANUAL_CLOCK." << RTT::endlog();
00140     return false;
00141   }
00142 
00143   return this->updateClockInternal(new_time);
00144 }
00145 
00146 bool SimClockThread::updateClockInternal(const ros::Time new_time)
00147 {
00148   // Make sure the system time isn't being used
00149   if(time_service_->systemClockEnabled()) {
00150     time_service_->enableSystemClock(false);
00151   }
00152 
00153   // Check if time restarted
00154   if(new_time.isZero()) {
00155     
00156     RTT::log(RTT::Warning) << "Time has reset to 0! Re-setting time service..." << RTT::endlog();
00157 
00158     // Re-set the time service and don't update the activities
00159     this->resetTimeService();
00160 
00161   } else {
00162     // Update the RTT time to match the sim time
00163     using namespace RTT::os;
00164     //TimeService::ticks rtt_ticks = time_service_->getTicks();
00165     //TimeService::Seconds rtt_secs = RTT::nsecs_to_Seconds(TimeService::ticks2nsecs(rtt_ticks));
00166 
00167     // Compute the time update
00168     TimeService::Seconds dt = (new_time - rtt_rosclock::rtt_now()).toSec();
00169 
00170     // Check if time went backwards
00171     if(dt < 0) {
00172       RTT::log(RTT::Warning) << "Time went backwards by " << dt << " seconds! (" << rtt_rosclock::rtt_now() << " --> " << new_time <<")" << RTT::endlog();
00173     }
00174 
00175     // Update the RTT clock
00176     time_service_->secondsChange(dt);
00177 
00178     // Trigger all SimClockActivities
00179     boost::shared_ptr<SimClockActivityManager> manager = SimClockActivityManager::GetInstance();
00180     if (manager) {
00181       // Update the simulation period
00182       manager->setSimulationPeriod(dt);
00183       // Update all the SimClockActivities
00184       manager->update();
00185     }
00186   }
00187 
00188   return true;
00189 }
00190 
00191 void SimClockThread::resetTimeService()
00192 {
00193   // We have to set the Logger reference time to zero in order to get correct logging timestamps.
00194   // RTT::Logger::Instance()->setReferenceTime(0);
00195   //
00196   // Unfortunately this method is not available, therefore shutdown and restart logging.
00197   // This workaround is not exact.
00198 
00199   // Shutdown the RTT Logger
00200   RTT::Logger::Instance()->shutdown();
00201 
00202   // Disable the RTT system clock so Gazebo can manipulate time and reset it to 0
00203   time_service_->enableSystemClock(false);
00204   assert(time_service_->systemClockEnabled() == false);
00205 
00206   time_service_->ticksChange(-time_service_->ticksSince(0));
00207   assert(time_service_->getTicks() == 0);
00208 
00209   // Restart the RTT Logger with reference time 0
00210   RTT::Logger::Instance()->startup();
00211   assert(RTT::Logger::Instance()->getReferenceTime() == 0);
00212 }
00213 
00214 bool SimClockThread::initialize()
00215 {
00216 
00217   RTT::log(RTT::Debug) << "[rtt_rosclock] Attempting to enable global simulation clock source..." << RTT::endlog();
00218 
00219   switch(clock_source_) 
00220   {
00221     case SIM_CLOCK_SOURCE_ROS_CLOCK_TOPIC:
00222       {
00223         // Get /use_sim_time parameter from ROS
00224         bool use_sim_time = false;
00225         ros::param::get("/use_sim_time", use_sim_time);
00226 
00227         if(!use_sim_time) {
00228           RTT::log(RTT::Error) << "[rtt_rosclock] Did not enable ROS simulation clock because the ROS parameter '/use_sim_time' is not set to true." << RTT::endlog();
00229           process_callbacks_ = false;
00230           return false;
00231         }
00232 
00233         RTT::log(RTT::Debug) << "[rtt_rosclock] Switching to simulated time based on ROS /clock topic..." << RTT::endlog();
00234 
00235         // Reset the timeservice and logger
00236         this->resetTimeService();
00237 
00238         // Subscribe the /clock topic (simulation time, e.g. published by Gazebo)
00239         ros::SubscribeOptions ops = ros::SubscribeOptions::create<rosgraph_msgs::Clock>(
00240             "/clock", 1, boost::bind(&SimClockThread::clockMsgCallback, this, _1),
00241             ros::VoidConstPtr(), &callback_queue_);
00242         clock_subscriber_ = nh_.subscribe(ops);
00243 
00244         // The loop needs to run in order to call the callback queue
00245         process_callbacks_ = true;
00246       }
00247       break;
00248 
00249     case SIM_CLOCK_SOURCE_MANUAL:
00250       {
00251         RTT::log(RTT::Debug) << "[rtt_rosclock] Switching to simulated time based on a manual clock source..." << RTT::endlog();
00252 
00253         // Reset the timeservice and logger
00254         this->resetTimeService();
00255 
00256         // We're not processing the callback queue, so we won't loop.
00257         process_callbacks_ = false;
00258       }
00259       break;
00260 
00261     default:
00262       {
00263         RTT::log(RTT::Error) << "Unknown simulation clock source for SimClockThread!" << RTT::endlog();
00264         return false;
00265       }
00266   };
00267 
00268   return true;
00269 }
00270 
00271 void SimClockThread::loop()
00272 {
00273   static const ros::WallDuration timeout(0.1);
00274 
00275   // Service callbacks while 
00276   while(process_callbacks_) {
00277     callback_queue_.callAvailable(timeout);
00278   }
00279 }
00280 
00281 bool SimClockThread::breakLoop()
00282 {
00283   process_callbacks_ = false;
00284   return true;
00285 }
00286 
00287 void SimClockThread::finalize()
00288 {
00289   RTT::log(RTT::Info) << "Disabling simulated time..." << RTT::endlog();
00290 
00291   // Shutdown the subscriber so no more clock message events will be handled
00292   clock_subscriber_.shutdown();
00293 
00294   // Shutdown the RTT Logger
00295   RTT::Logger::Instance()->shutdown();
00296 
00297   // Re-enable system clock
00298   time_service_->enableSystemClock(true);
00299   
00300   // Restart the RTT Logger with reference walltime
00301   RTT::Logger::Instance()->startup();
00302 }
00303 


rtt_rosclock
Author(s):
autogenerated on Wed Sep 16 2015 06:59:15