rtt_rosclock_sim_clock_thread.cpp
Go to the documentation of this file.
1 /*
2  * (C) 2013, Johannes Meyer, TU Darmstadt
3  * (C) 2013, Intermodalics BVBA
4  * (C) 2014, Jonathan Bohren, The Johns Hopkins University
5  * - Generalized for multiple time sources
6  * - Integrated with rtt_rosclock package
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  * 1. Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * 2. Redistributions in binary form must reproduce the above copyright
14  * notice, this list of conditions and the following disclaimer in the
15  * documentation and/or other materials provided with the distribution.
16  * 3. Neither the name of the copyright holder nor the names of its contributors
17  * may be used to endorse or promote products derived from this software
18  * without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  */
33 
37 
38 #include <rtt/TaskContext.hpp>
40 #include <rtt/plugin/Plugin.hpp>
41 
43 
44 #include <ros/time.h>
45 #include <ros/node_handle.h>
46 #include <ros/param.h>
47 #include <ros/subscribe_options.h>
48 
50 
51 using namespace rtt_rosclock;
52 
54 
56 {
57  return singleton;
58 }
59 
61 {
62  // Create a new singleton, if necessary
64  if(!shared) {
65  shared.reset(new SimClockThread());
66  singleton = shared;
67  }
68 
69  return shared;
70 }
71 
73 {
74  singleton.reset();
75 }
76 
77 namespace {
79 }
80 
82  : RTT::os::Thread(ORO_SCHED_OTHER, RTT::os::LowestPriority, 0.0, 0, "rtt_rosclock_SimClockThread")
83  , time_service_(RTT::os::TimeService::Instance())
85  , process_callbacks_(false)
86 {
87 }
88 
90 {
91  this->stop();
92 }
93 
95 {
96  // Don't allow changing the source while running
97  if(this->isActive()) {
98  RTT::log(RTT::Error) << "The SimClockThread clock source cannot be changed while the thread is running." << RTT::endlog();
99  return false;
100  }
101 
102  // Set the clock source
103  clock_source_ = clock_source;
104 
105  return true;
106 }
107 
109 {
111 }
112 
114 {
116 }
117 
119 {
120  return this->isActive();
121 }
122 
123 void SimClockThread::clockMsgCallback(const rosgraph_msgs::ClockConstPtr& clock)
124 {
125  // Update the RTT clock
126  updateClockInternal(ros::Time(clock->clock.sec, clock->clock.nsec));
127 }
128 
130 {
132  RTT::log(RTT::Error) << "Cannot update simulation clock manually unless the clock source is set to MANUAL_CLOCK." << RTT::endlog();
133  return false;
134  }
135 
136  return this->updateClockInternal(new_time);
137 }
138 
140 {
141  // Make sure the system time isn't being used
144  }
145 
146  // Check if time restarted
147  if(new_time.isZero()) {
148 
149  RTT::log(RTT::Warning) << "Time has reset to 0! Re-setting time service..." << RTT::endlog();
150 
151  // Re-set the time service and don't update the activities
152  this->resetTimeService();
153 
154  } else {
155  // Update the RTT time to match the sim time
156  using namespace RTT::os;
157  //TimeService::ticks rtt_ticks = time_service_->getTicks();
158  //TimeService::Seconds rtt_secs = RTT::nsecs_to_Seconds(TimeService::ticks2nsecs(rtt_ticks));
159 
160  // Compute the time update
161  TimeService::Seconds dt = (new_time - rtt_rosclock::rtt_now()).toSec();
162 
163  // Check if time went backwards
164  if(dt < 0) {
165  RTT::log(RTT::Warning) << "Time went backwards by " << dt << " seconds! (" << rtt_rosclock::rtt_now() << " --> " << new_time <<")" << RTT::endlog();
166  }
167 
168  // Update the RTT clock
170 
171  // Trigger all SimClockActivities
173  if (manager) {
174  // Update the simulation period
175  manager->setSimulationPeriod(dt);
176  // Update all the SimClockActivities
177  manager->update();
178  }
179  }
180 
181  return true;
182 }
183 
185 {
186  // We have to set the Logger reference time to zero in order to get correct logging timestamps.
187  // RTT::Logger::Instance()->setReferenceTime(0);
188  //
189  // Unfortunately this method is not available, therefore shutdown and restart logging.
190  // This workaround is not exact.
191 
192  // Shutdown the RTT Logger
194 
195  // Disable the RTT system clock so Gazebo can manipulate time and reset it to 0
197  assert(time_service_->systemClockEnabled() == false);
198 
200  assert(time_service_->getTicks() == 0);
201 
202  // Restart the RTT Logger with reference time 0
204  assert(RTT::Logger::Instance()->getReferenceTime() == 0);
205 }
206 
208 {
209 
210  RTT::log(RTT::Debug) << "[rtt_rosclock] Attempting to enable global simulation clock source..." << RTT::endlog();
211 
212  switch(clock_source_)
213  {
215  {
216  ros::NodeHandle nh;
217 
218  // Get /use_sim_time parameter from ROS
219  bool use_sim_time = false;
220  ros::param::get("/use_sim_time", use_sim_time);
221 
222  if(!use_sim_time) {
223  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();
224  process_callbacks_ = false;
225  return false;
226  }
227 
228  RTT::log(RTT::Debug) << "[rtt_rosclock] Switching to simulated time based on ROS /clock topic..." << RTT::endlog();
229 
230  // Reset the timeservice and logger
231  this->resetTimeService();
232 
233  // Subscribe the /clock topic (simulation time, e.g. published by Gazebo)
234  ros::SubscribeOptions ops = ros::SubscribeOptions::create<rosgraph_msgs::Clock>(
235  "/clock", 1, boost::bind(&SimClockThread::clockMsgCallback, this, _1),
237  clock_subscriber_ = nh.subscribe(ops);
238 
239  // The loop needs to run in order to call the callback queue
240  process_callbacks_ = true;
241  }
242  break;
243 
245  {
246  RTT::log(RTT::Debug) << "[rtt_rosclock] Switching to simulated time based on a manual clock source..." << RTT::endlog();
247 
248  // Reset the timeservice and logger
249  this->resetTimeService();
250 
251  // We're not processing the callback queue, so we won't loop.
252  process_callbacks_ = false;
253  }
254  break;
255 
256  default:
257  {
258  RTT::log(RTT::Error) << "Unknown simulation clock source for SimClockThread!" << RTT::endlog();
259  return false;
260  }
261  };
262 
263  return true;
264 }
265 
267 {
268  static const ros::WallDuration timeout(0.1);
269 
270  // Service callbacks while
271  while(process_callbacks_) {
273  }
274 }
275 
277 {
278  process_callbacks_ = false;
279  return true;
280 }
281 
283 {
284  RTT::log(RTT::Info) << "Disabling simulated time..." << RTT::endlog();
285 
286  // Shutdown the subscriber so no more clock message events will be handled
288 
289  // Shutdown the RTT Logger
291 
292  // Re-enable system clock
294 
295  // Restart the RTT Logger with reference walltime
297 }
298 
boost::shared_ptr< void const > VoidConstPtr
bool useManualClock()
Set the clock source to use a manual source, i.e. call updateClock() manually.
bool updateClockInternal(const ros::Time new_time)
Update the RTT clock and SimClockActivities with a new time (see updateClock() for manually updating)...
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
static Logger * Instance(std::ostream &str=std::cerr)
void shutdown()
ticks ticksChange(ticks delta)
void resetTimeService()
Re-set the RTT::os::TimeService to zero and restart logging.
static boost::shared_ptr< SimClockThread > GetInstance()
Get an instance to the singleton SimClockThread or NULL.
bool useROSClockTopic()
Set the clock source to use the ROS /clock topic.
static boost::shared_ptr< SimClockActivityManager > GetInstance()
Get an instance of the singleton if it exists, null pointer otherwise.
bool systemClockEnabled() const
SimClockSource
Simulation clock sources.
ros::Subscriber clock_subscriber_
ROS /clock topic subscriber.
ROSCPP_DECL bool get(const std::string &key, std::string &s)
void startup()
virtual bool isActive() const
static boost::shared_ptr< SimClockThread > Instance()
Get an instance to the singleton SimClockThread or create one.
void enableSystemClock(bool yes_no)
static void Release()
Release the singleton SimClockThread.
ticks ticksSince(ticks relativeTime) const
bool updateClock(const ros::Time new_time)
ros::CallbackQueue callback_queue_
Custom callback queue used in this thread.
void clockMsgCallback(const rosgraph_msgs::ClockConstPtr &clock)
ROS message callback for /clock topic.
bool simTimeEnabled() const
Check if simulation time is enabled.
static boost::shared_ptr< SimClockThread > singleton
SimClockThread singleton.
Thread(int scheduler, int priority, double period, unsigned cpu_affinity, const std::string &name)
const int LowestPriority
virtual bool stop()
bool setClockSource(SimClockSource clock_source)
Set the simulation clock source by ID (see ClockSource enum)
const ros::Time rtt_now()
Get the current time according to RTT.
bool process_callbacks_
Keep running the thread loop if this is set to true.
static Logger & log()
#define ORO_SCHED_OTHER
Seconds secondsChange(Seconds delta)
static Logger::LogFunction endlog()
SimClockThread()
Constructor is protected, use Instance() to create and get a singleton.
RTT::os::TimeService * time_service_
Convenient pointer to RTT time service.
SimClockSource clock_source_
Current clock source.
ticks getTicks() const


rtt_rosclock
Author(s):
autogenerated on Mon May 10 2021 02:45:33