shm_driver.cpp
Go to the documentation of this file.
1 
15 #include <ros/ros.h>
16 #include <ros/console.h>
17 
18 #include <sensor_msgs/TimeReference.h>
19 
20 #include <sys/ipc.h>
21 #include <sys/shm.h>
22 
23 #include <signal.h>
24 #include <cstring>
25 #include <cerrno>
26 
27 // there may be another library, but Poco already used by class_loader,
28 // so it definetly exist in your system.
29 #include <Poco/Process.h>
30 #include <Poco/Pipe.h>
31 #include <Poco/PipeStream.h>
32 #include <Poco/Format.h>
33 #include <Poco/StreamCopier.h>
34 
36 struct shmTime
37 {
38  int mode; /* 0 - if valid set
39  * use values,
40  * clear valid
41  * 1 - if valid set
42  * if count before and after read of values is equal,
43  * use values
44  * clear valid
45  */
46  volatile int count;
51  int leap;
52  int precision;
53  int nsamples;
54  volatile int valid;
55  unsigned clockTimeStampNSec; /* Unsigned ns timestamps */
56  unsigned receiveTimeStampNSec; /* Unsigned ns timestamps */
57  int dummy[8];
58 };
59 
60 const long int NTPD_SHM_BASE = 0x4e545030;
61 
66 static volatile struct shmTime *get_shmTime(int unit)
67 {
68  /* we definitly not root, no IPC_CREAT */
69  const int get_flags = 0666;
70 
71  int shmid = shmget(NTPD_SHM_BASE + unit, sizeof(shmTime), get_flags);
72  if (shmid < 0) {
73  ROS_FATAL("SHM shmget(0x%08lx, %zd, %o) fail: %s",
74  NTPD_SHM_BASE + unit,
75  sizeof(shmTime),
76  get_flags,
77  strerror(errno));
78  return NULL;
79  }
80 
81  /* TODO: check number of attached progs */
82 
83  void *p = shmat(shmid, 0, 0);
84  if (p == (void *) -1) {
85  ROS_FATAL("SHM shmat(%d, 0, 0) fail: %s", shmid, strerror(errno));
86  return NULL;
87  }
88 
89  ROS_INFO("SHM(%d) key 0x%08lx, successfully opened", unit, NTPD_SHM_BASE + unit);
90  return (volatile struct shmTime*) p;
91 }
92 
93 static void put_shmTime(volatile struct shmTime **shm)
94 {
95  ROS_ASSERT(shm != NULL);
96  if (*shm == NULL)
97  return;
98 
99  if (shmdt((const void*) *shm) == -1)
100  ROS_FATAL("SHM shmdt(%p) fail: %s", *shm, strerror(errno));
101  else
102  *shm = NULL;
103 }
104 
105 
107 volatile struct shmTime *g_shm = NULL;
108 static bool g_set_date = false;
109 
110 static void sig_handler(int sig)
111 {
112  put_shmTime(&g_shm);
113  ros::shutdown();
114 }
115 
122 static inline void memory_barrier(void)
123 {
124  asm volatile ("" : : : "memory");
125 }
126 
127 static void time_ref_cb(const sensor_msgs::TimeReference::ConstPtr &time_ref)
128 {
129  if (g_shm == NULL) {
130  ROS_FATAL("Got time_ref before shm opens.");
131  ros::shutdown();
132  }
133 
134  /* header */
135  g_shm->mode = 1;
136  g_shm->nsamples = 3; // stages of median filter
137 
138  g_shm->valid = 0;
139  g_shm->count += 1;
140  /* barrier */
141  memory_barrier();
142  g_shm->clockTimeStampSec = time_ref->time_ref.sec;
143  g_shm->clockTimeStampUSec = time_ref->time_ref.nsec / 1000;
144  g_shm->clockTimeStampNSec = time_ref->time_ref.nsec;
145  g_shm->receiveTimeStampSec = time_ref->header.stamp.sec;
146  g_shm->receiveTimeStampUSec = time_ref->header.stamp.nsec / 1000;
147  g_shm->receiveTimeStampNSec = time_ref->header.stamp.nsec;
148  g_shm->leap = 0; // LEAP_NOWARNING
149  g_shm->precision = -1; // initially 0.5 sec
150  memory_barrier();
151  /* barrier again */
152  g_shm->count += 1;
153  g_shm->valid = 1;
154 
155  ROS_DEBUG_THROTTLE(10, "Got time_ref: %s: %lu.%09lu",
156  time_ref->source.c_str(),
157  (long unsigned) time_ref->time_ref.sec,
158  (long unsigned) time_ref->time_ref.nsec);
159 
160  /* It is a hack for rtc-less system like Raspberry Pi
161  * We check that system time is unset (less than some magick)
162  * and set time.
163  *
164  * Sudo configuration required for that feature
165  * date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009
166  */
167  if (g_set_date && ros::Time::now().sec < 1234567890ULL) {
168 
169  const double stamp = time_ref->time_ref.toSec();
170 
171  ROS_INFO("Setting system date to: %f", stamp);
172 
173  // construct commad: sudo -n date -u -s @1234567890.000
174  Poco::Pipe outp, errp;
175  Poco::Process::Args args;
176  args.push_back("-n");
177  args.push_back("date");
178  args.push_back("-u");
179  args.push_back("-s");
180  args.push_back(Poco::format("@%f", stamp));
181  Poco::ProcessHandle ph = Poco::Process::launch("sudo", args, 0, &outp, &errp);
182 
183  int rc = ph.wait();
184  Poco::PipeInputStream outs(outp), errs(errp);
185  std::string out, err;
186 
187  Poco::StreamCopier::copyToString(outs, out, 4096);
188  Poco::StreamCopier::copyToString(errs, err, 4096);
189 
190  if (rc == 0) {
191  ROS_INFO("The system date is set.");
192  ROS_DEBUG_STREAM("OUT: " << out);
193  ROS_DEBUG_STREAM("ERR: " << err);
194  }
195  else {
196  ROS_ERROR("Setting system date failed.");
197  ROS_ERROR_STREAM("OUT: " << out);
198  ROS_ERROR_STREAM("ERR: " << err);
199  }
200  }
201 }
202 
203 int main(int argc, char *argv[])
204 {
205  ros::init(argc, argv, "ntpd_shm");
206 
207  ros::NodeHandle nh("~");
208  ros::Subscriber time_ref_sub;
209 
210  int shm_unit;
211  std::string time_ref_topic;
212 
213  // Override default ROS handler
214  signal(SIGINT, sig_handler);
215 
216  // Read Parameters
217  nh.param("shm_unit", shm_unit, 2);
218  nh.param("fixup_date", g_set_date, false);
219  nh.param<std::string>("time_ref_topic", time_ref_topic, "time_ref");
220 
221  // Report settings
222  ROS_INFO_STREAM("NTP time source: " << ros::names::resolve(time_ref_topic, true));
223  ROS_INFO_STREAM("NTP date fixup: " << ((g_set_date) ? "enabled" : "disabled"));
224 
225  g_shm = get_shmTime(shm_unit);
226  if (g_shm == NULL)
227  return 1;
228 
229  // prefer to unreliable connection, but accept tcp too.
230  time_ref_sub = nh.subscribe(time_ref_topic, 10, time_ref_cb,
232  .unreliable().maxDatagramSize(1024)
233  .reliable().tcpNoDelay(true));
234 
235  ros::spin();
236  put_shmTime(&g_shm);
237  return 0;
238 }
#define ROS_FATAL(...)
unsigned clockTimeStampNSec
Definition: shm_driver.cpp:55
#define ROS_DEBUG_THROTTLE(rate,...)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
int precision
Definition: shm_driver.cpp:52
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
static void put_shmTime(volatile struct shmTime **shm)
Definition: shm_driver.cpp:93
ROSCPP_DECL std::string resolve(const std::string &name, bool remap=true)
time_t receiveTimeStampSec
Definition: shm_driver.cpp:49
static bool g_set_date
Definition: shm_driver.cpp:108
int receiveTimeStampUSec
Definition: shm_driver.cpp:50
unsigned receiveTimeStampNSec
Definition: shm_driver.cpp:56
static volatile struct shmTime * get_shmTime(int unit)
Definition: shm_driver.cpp:66
int main(int argc, char *argv[])
Definition: shm_driver.cpp:203
int clockTimeStampUSec
Definition: shm_driver.cpp:48
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
int leap
Definition: shm_driver.cpp:51
const long int NTPD_SHM_BASE
Definition: shm_driver.cpp:60
ROSCPP_DECL void spin()
#define ROS_DEBUG_STREAM(args)
volatile int valid
Definition: shm_driver.cpp:54
#define ROS_INFO_STREAM(args)
int mode
Definition: shm_driver.cpp:38
static Time now()
ROSCPP_DECL void shutdown()
time_t clockTimeStampSec
Definition: shm_driver.cpp:47
static void memory_barrier(void)
Definition: shm_driver.cpp:122
static void sig_handler(int sig)
Definition: shm_driver.cpp:110
static void time_ref_cb(const sensor_msgs::TimeReference::ConstPtr &time_ref)
Definition: shm_driver.cpp:127
#define ROS_ERROR_STREAM(args)
#define ROS_ASSERT(cond)
int nsamples
Definition: shm_driver.cpp:53
volatile int count
Definition: shm_driver.cpp:46
#define ROS_ERROR(...)
int dummy[8]
Definition: shm_driver.cpp:57
volatile struct shmTime * g_shm
Definition: shm_driver.cpp:107


ntpd_driver
Author(s): Vladimir Ermakov
autogenerated on Thu Feb 25 2021 03:06:49