rtt_rosclock.cpp
Go to the documentation of this file.
00001 
00002 #include <time.h>
00003 #include <rtt/RTT.hpp>
00004 
00005 #include <rtt_rosclock/rtt_rosclock.h>
00006 #include <rtt_rosclock/rtt_rosclock_sim_clock_activity.h>
00007 #include <rtt_rosclock/rtt_rosclock_sim_clock_thread.h>
00008 
00009 namespace rtt_rosclock {
00010   boost::shared_ptr<rtt_rosclock::SimClockThread> sim_clock_thread;
00011 }
00012 
00013 const ros::Time rtt_rosclock::host_now()
00014 {
00015   if(SimClockThread::GetInstance() && SimClockThread::GetInstance()->simTimeEnabled()) {
00016     return rtt_now();
00017   }
00018 
00019   #ifdef __XENO__
00020     // Use Xenomai 2.6 feature to get the NTP-synched real-time clock
00021     timespec ts = {0,0};
00022     int ret = clock_gettime(CLOCK_HOST_REALTIME, &ts);
00023     if(ret) {
00024       RTT::log(RTT::Error) << "Could not query CLOCK_HOST_REALTIME (" << CLOCK_HOST_REALTIME <<"): "<< errno << RTT::endlog();
00025       return rtt_rosclock::rtt_now();
00026     }
00027 
00028     return ros::Time(ts.tv_sec, ts.tv_nsec);
00029   #else
00030     return ros::Time::now();
00031   #endif
00032 }
00033 
00034 const ros::Time rtt_rosclock::host_wall_now()
00035 {
00036   #ifdef __XENO__
00037     // Use Xenomai 2.6 feature to get the NTP-synched real-time clock
00038     timespec ts = {0,0};
00039     int ret = clock_gettime(CLOCK_HOST_REALTIME, &ts);
00040     if(ret) {
00041       RTT::log(RTT::Error) << "Could not query CLOCK_HOST_REALTIME (" << CLOCK_HOST_REALTIME <<"): "<< errno << RTT::endlog();
00042       return rtt_rosclock::rtt_wall_now();
00043     }
00044 
00045     return ros::Time(ts.tv_sec, ts.tv_nsec);
00046   #else
00047     ros::WallTime now(ros::WallTime::now());
00048     return ros::Time(now.sec, now.nsec);
00049   #endif
00050 }
00051 
00052 const ros::Time rtt_rosclock::rtt_now()
00053 {
00054   // count the zeros...   -987654321--
00055   const uint64_t one_E9 = 1000000000ULL;
00056   // NOTE: getNSecs returns wall time, getTicks returns offset time
00057   uint64_t nsec64 = RTT::os::TimeService::ticks2nsecs(RTT::os::TimeService::Instance()->getTicks());
00058   uint32_t sec32_part = nsec64 / one_E9;
00059   uint32_t nsec32_part = nsec64 % one_E9;
00060   //RTT::log(RTT::Error) << "sec: " << sec32_part << " nsec: " << nsec32_part << RTT::endlog();
00061   return ros::Time(sec32_part, nsec32_part);
00062 }
00063 
00064 const ros::Time rtt_rosclock::rtt_wall_now()
00065 {
00066   const uint64_t one_E9 = 1000000000ll;
00067   // NOTE: getNSecs returns wall time, getTicks returns offset time
00068   uint64_t nsec64 = RTT::os::TimeService::Instance()->getNSecs();
00069   uint64_t sec64_part = nsec64 / one_E9;
00070   uint64_t nsec64_part = nsec64 % one_E9;
00071   //RTT::log(RTT::Error) << "sec: " << sec32 << " nsec: " << nsec32 << RTT::endlog();
00072   return ros::Time(sec64_part, nsec64_part);
00073 }
00074 
00075 const RTT::Seconds rtt_rosclock::host_offset_from_rtt()
00076 {
00077   return (rtt_rosclock::host_wall_now() - rtt_rosclock::rtt_wall_now()).toSec();
00078 }
00079 
00080 void rtt_rosclock::use_ros_clock_topic()
00081 {
00082   SimClockThread::Instance()->useROSClockTopic();
00083 }
00084 
00085 void rtt_rosclock::use_manual_clock()
00086 {
00087   SimClockThread::Instance()->useManualClock();
00088 }
00089 
00090 const bool rtt_rosclock::set_sim_clock_activity(RTT::TaskContext *t)
00091 {
00092   if (!t) return false;
00093   return t->setActivity(new SimClockActivity(t->getPeriod()));
00094 }
00095 
00096 const bool rtt_rosclock::enable_sim()
00097 {
00098   return SimClockThread::Instance()->start();
00099 }
00100 
00101 const bool rtt_rosclock::disable_sim()
00102 {
00103   return SimClockThread::Instance()->stop();
00104 }
00105 
00106 void rtt_rosclock::update_sim_clock(const ros::Time new_time)
00107 {
00108   SimClockThread::Instance()->updateClock(new_time);
00109 }


rtt_rosclock
Author(s):
autogenerated on Thu Jun 6 2019 18:06:39