Fawkes API  Fawkes Development Version
laserscan_thread.cpp
1 
2 /***************************************************************************
3  * laserscan_thread.cpp - Thread to exchange laser scans
4  *
5  * Created: Tue May 29 19:41:18 2012
6  * Copyright 2011-2012 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include "laserscan_thread.h"
23 
24 #include <core/threading/mutex_locker.h>
25 #include <ros/this_node.h>
26 #include <sensor_msgs/LaserScan.h>
27 #include <utils/math/angle.h>
28 
29 #include <fnmatch.h>
30 
31 using namespace fawkes;
32 
33 /** @class RosLaserScanThread "pcl_thread.h"
34  * Thread to exchange point clouds between Fawkes and ROS.
35  * @author Tim Niemueller
36  */
37 
38 /** Constructor. */
40 : Thread("RosLaserScanThread", Thread::OPMODE_WAITFORWAKEUP),
41  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS),
42  BlackBoardInterfaceListener("RosLaserScanThread")
43 {
44  ls_msg_queue_mutex_ = new Mutex();
45  seq_num_mutex_ = new Mutex();
46 }
47 
48 /** Destructor. */
50 {
51  delete ls_msg_queue_mutex_;
52  delete seq_num_mutex_;
53 }
54 
55 std::string
56 RosLaserScanThread::topic_name(const char *if_id, const char *suffix)
57 {
58  std::string topic_name = std::string("fawkes_scans/") + if_id + "_" + suffix;
59  std::string::size_type pos = 0;
60  while ((pos = topic_name.find("-", pos)) != std::string::npos) {
61  topic_name.replace(pos, 1, "_");
62  }
63  pos = 0;
64  while ((pos = topic_name.find(" ", pos)) != std::string::npos) {
65  topic_name.replace(pos, 1, "_");
66  }
67  return topic_name;
68 }
69 
70 void
72 {
73  active_queue_ = 0;
74  seq_num_ = 0;
75 
76  // Must do that before registering listener because we might already
77  // get events right away
78  sub_ls_ = rosnode->subscribe("scan", 100, &RosLaserScanThread::laser_scan_message_cb, this);
79 
83 
84  std::list<Laser360Interface *>::iterator i360;
85  for (i360 = ls360_ifs_.begin(); i360 != ls360_ifs_.end(); ++i360) {
86  (*i360)->read();
87  logger->log_info(name(), "Opened %s", (*i360)->uid());
91 
92  std::string topname = topic_name((*i360)->id(), "360");
93 
94  PublisherInfo pi;
95  pi.pub = rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
96 
97  logger->log_info(name(), "Publishing laser scan %s at %s", (*i360)->uid(), topname.c_str());
98 
99  pi.msg.header.frame_id = (*i360)->frame();
100  pi.msg.angle_min = 0;
101  pi.msg.angle_max = 2 * M_PI;
102  pi.msg.angle_increment = deg2rad(1);
103  pi.msg.ranges.resize(360);
104 
105  pubs_[(*i360)->uid()] = pi;
106  }
107 
108  std::list<Laser720Interface *>::iterator i720;
109  for (i720 = ls720_ifs_.begin(); i720 != ls720_ifs_.end(); ++i720) {
110  logger->log_info(name(), "Opened %s", (*i720)->uid());
114 
115  std::string topname = topic_name((*i720)->id(), "720");
116 
117  PublisherInfo pi;
118  pi.pub = rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
119 
120  logger->log_info(name(), "Publishing laser scan %s at %s", (*i720)->uid(), topname.c_str());
121 
122  pi.msg.header.frame_id = (*i720)->frame();
123  pi.msg.angle_min = 0;
124  pi.msg.angle_max = 2 * M_PI;
125  pi.msg.angle_increment = deg2rad(0.5);
126  pi.msg.ranges.resize(720);
127 
128  pubs_[(*i720)->uid()] = pi;
129  }
130 
131  std::list<Laser1080Interface *>::iterator i1080;
132  for (i1080 = ls1080_ifs_.begin(); i1080 != ls1080_ifs_.end(); ++i1080) {
133  logger->log_info(name(), "Opened %s", (*i1080)->uid());
134  bbil_add_data_interface(*i1080);
137 
138  std::string topname = topic_name((*i1080)->id(), "1080");
139 
140  PublisherInfo pi;
141  pi.pub = rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
142 
143  logger->log_info(name(),
144  "Publishing laser scan %s at %s, frame %s",
145  (*i1080)->uid(),
146  topname.c_str(),
147  (*i1080)->frame());
148 
149  pi.msg.header.frame_id = (*i1080)->frame();
150  pi.msg.angle_min = 0;
151  pi.msg.angle_max = 2 * M_PI;
152  pi.msg.angle_increment = deg2rad(1. / 3.);
153  pi.msg.ranges.resize(1080);
154 
155  pubs_[(*i1080)->uid()] = pi;
156  }
157 
159 
160  bbio_add_observed_create("Laser360Interface", "*");
161  bbio_add_observed_create("Laser720Interface", "*");
162  bbio_add_observed_create("Laser1080Interface", "*");
164 }
165 
166 void
168 {
171 
172  sub_ls_.shutdown();
173 
174  std::map<std::string, PublisherInfo>::iterator p;
175  for (p = pubs_.begin(); p != pubs_.end(); ++p) {
176  p->second.pub.shutdown();
177  }
178 
179  std::list<Laser360Interface *>::iterator i360;
180  for (i360 = ls360_ifs_.begin(); i360 != ls360_ifs_.end(); ++i360) {
181  blackboard->close(*i360);
182  }
183  ls360_ifs_.clear();
184  std::list<Laser720Interface *>::iterator i720;
185  for (i720 = ls720_ifs_.begin(); i720 != ls720_ifs_.end(); ++i720) {
186  blackboard->close(*i720);
187  }
188  ls720_ifs_.clear();
189  std::list<Laser1080Interface *>::iterator i1080;
190  for (i1080 = ls1080_ifs_.begin(); i1080 != ls1080_ifs_.end(); ++i1080) {
191  blackboard->close(*i1080);
192  }
193  ls1080_ifs_.clear();
194 }
195 
196 void
198 {
199  ls_msg_queue_mutex_->lock();
200  unsigned int queue = active_queue_;
201  active_queue_ = 1 - active_queue_;
202  ls_msg_queue_mutex_->unlock();
203 
204  while (!ls_msg_queues_[queue].empty()) {
205  const ros::MessageEvent<sensor_msgs::LaserScan const> &msg_evt = ls_msg_queues_[queue].front();
206 
207  sensor_msgs::LaserScan::ConstPtr msg = msg_evt.getConstMessage();
208 
209  // Check if interface exists, open if it does not
210  const std::string callerid = msg_evt.getPublisherName();
211 
212  // for now we only create 360 interfaces, might add on that later
213  if (callerid.empty()) {
214  logger->log_warn(name(),
215  "Received laser scan from ROS without caller ID,"
216  "ignoring");
217  } else {
218  bool have_interface = true;
219  if (ls360_wifs_.find(callerid) == ls360_wifs_.end()) {
220  try {
221  std::string id = std::string("ROS Laser ") + callerid;
223  ls360_wifs_[callerid] = ls360if;
224  } catch (Exception &e) {
225  logger->log_warn(name(),
226  "Failed to open ROS laser interface for "
227  "message from node %s, exception follows",
228  callerid.c_str());
229  logger->log_warn(name(), e);
230  have_interface = false;
231  }
232  }
233 
234  if (have_interface) {
235  // update interface with laser data
236  Laser360Interface *ls360if = ls360_wifs_[callerid];
237  ls360if->set_frame(msg->header.frame_id.c_str());
238  float distances[360];
239  for (unsigned int a = 0; a < 360; ++a) {
240  float a_rad = deg2rad(a);
241  if ((a_rad < msg->angle_min) || (a_rad > msg->angle_max)) {
242  distances[a] = 0.;
243  } else {
244  // get closest ray from message
245  int idx = (int)roundf((a_rad - msg->angle_min) / msg->angle_increment);
246  distances[a] = msg->ranges[idx];
247  }
248  }
249  ls360if->set_distances(distances);
250  ls360if->write();
251  }
252  }
253 
254  ls_msg_queues_[queue].pop();
255  }
256 }
257 
258 void
260 {
261  Laser360Interface * ls360if = dynamic_cast<Laser360Interface *>(interface);
262  Laser720Interface * ls720if = dynamic_cast<Laser720Interface *>(interface);
263  Laser1080Interface *ls1080if = dynamic_cast<Laser1080Interface *>(interface);
264 
265  PublisherInfo & pi = pubs_[interface->uid()];
266  sensor_msgs::LaserScan &msg = pi.msg;
267 
268  if (ls360if) {
269  ls360if->read();
270 
271  const Time *time = ls360if->timestamp();
272 
273  seq_num_mutex_->lock();
274  msg.header.seq = ++seq_num_;
275  seq_num_mutex_->unlock();
276  msg.header.stamp = ros::Time(time->get_sec(), time->get_nsec());
277  msg.header.frame_id = ls360if->frame();
278 
279  msg.angle_min = 0;
280  msg.angle_max = 2 * M_PI;
281  msg.angle_increment = deg2rad(1);
282  msg.range_min = 0.;
283  msg.range_max = 1000.;
284  msg.ranges.resize(360);
285  memcpy(&msg.ranges[0], ls360if->distances(), 360 * sizeof(float));
286 
287  pi.pub.publish(pi.msg);
288 
289  } else if (ls720if) {
290  ls720if->read();
291 
292  const Time *time = ls720if->timestamp();
293 
294  seq_num_mutex_->lock();
295  msg.header.seq = ++seq_num_;
296  seq_num_mutex_->unlock();
297  msg.header.stamp = ros::Time(time->get_sec(), time->get_nsec());
298  msg.header.frame_id = ls720if->frame();
299 
300  msg.angle_min = 0;
301  msg.angle_max = 2 * M_PI;
302  msg.angle_increment = deg2rad(1. / 2.);
303  msg.range_min = 0.;
304  msg.range_max = 1000.;
305  msg.ranges.resize(720);
306  memcpy(&msg.ranges[0], ls720if->distances(), 720 * sizeof(float));
307 
308  pi.pub.publish(pi.msg);
309 
310  } else if (ls1080if) {
311  ls1080if->read();
312 
313  const Time *time = ls1080if->timestamp();
314 
315  seq_num_mutex_->lock();
316  msg.header.seq = ++seq_num_;
317  seq_num_mutex_->unlock();
318  msg.header.stamp = ros::Time(time->get_sec(), time->get_nsec());
319  msg.header.frame_id = ls1080if->frame();
320 
321  msg.angle_min = 0;
322  msg.angle_max = 2 * M_PI;
323  msg.angle_increment = deg2rad(1. / 3.);
324  msg.range_min = 0.;
325  msg.range_max = 1000.;
326  msg.ranges.resize(1080);
327  memcpy(&msg.ranges[0], ls1080if->distances(), 1080 * sizeof(float));
328 
329  pi.pub.publish(pi.msg);
330  }
331 }
332 
333 void
334 RosLaserScanThread::bb_interface_created(const char *type, const char *id) throw()
335 {
336  // Ignore ID pattern of our own interfaces
337  if (fnmatch("ROS *", id, FNM_NOESCAPE) == 0)
338  return;
339 
340  if (strncmp(type, "Laser360Interface", INTERFACE_TYPE_SIZE_) == 0) {
341  Laser360Interface *ls360if;
342  try {
343  logger->log_info(name(), "Opening %s:%s", type, id);
344  ls360if = blackboard->open_for_reading<Laser360Interface>(id);
345  } catch (Exception &e) {
346  // ignored
347  logger->log_warn(name(), "Failed to open %s:%s: %s", type, id, e.what());
348  return;
349  }
350 
351  try {
352  bbil_add_data_interface(ls360if);
353  bbil_add_reader_interface(ls360if);
354  bbil_add_writer_interface(ls360if);
355 
356  std::string topname = topic_name(ls360if->id(), "360");
357 
358  PublisherInfo pi;
359  pi.pub = rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
360 
361  logger->log_info(name(), "Publishing laser scan %s at %s", ls360if->uid(), topname.c_str());
362 
363  pi.msg.header.frame_id = ls360if->frame();
364  pi.msg.angle_min = 0;
365  pi.msg.angle_max = 2 * M_PI;
366  pi.msg.angle_increment = deg2rad(1);
367  pi.msg.ranges.resize(360);
368 
369  pubs_[ls360if->uid()] = pi;
370 
371  blackboard->update_listener(this);
372  ls360_ifs_.push_back(ls360if);
373  } catch (Exception &e) {
374  blackboard->close(ls360if);
375  logger->log_warn(name(), "Failed to register for %s:%s: %s", type, id, e.what());
376  return;
377  }
378 
379  } else if (strncmp(type, "Laser720Interface", INTERFACE_TYPE_SIZE_) == 0) {
380  Laser720Interface *ls720if;
381  try {
382  logger->log_info(name(), "Opening %s:%s", type, id);
383  ls720if = blackboard->open_for_reading<Laser720Interface>(id);
384  } catch (Exception &e) {
385  // ignored
386  logger->log_warn(name(), "Failed to open %s:%s: %s", type, id, e.what());
387  return;
388  }
389 
390  try {
391  bbil_add_data_interface(ls720if);
392  bbil_add_reader_interface(ls720if);
393  bbil_add_writer_interface(ls720if);
394 
395  std::string topname = topic_name(ls720if->id(), "720");
396 
397  PublisherInfo pi;
398  pi.pub = rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
399 
400  logger->log_info(name(), "Publishing laser scan %s at %s", ls720if->uid(), topname.c_str());
401 
402  pi.msg.header.frame_id = ls720if->frame();
403  pi.msg.angle_min = 0;
404  pi.msg.angle_max = 2 * M_PI;
405  pi.msg.angle_increment = deg2rad(0.5);
406  pi.msg.ranges.resize(720);
407 
408  pubs_[ls720if->uid()] = pi;
409 
410  blackboard->update_listener(this);
411  ls720_ifs_.push_back(ls720if);
412  } catch (Exception &e) {
413  blackboard->close(ls720if);
414  logger->log_warn(name(), "Failed to register for %s:%s: %s", type, id, e.what());
415  return;
416  }
417 
418  } else if (strncmp(type, "Laser1080Interface", INTERFACE_TYPE_SIZE_) == 0) {
419  Laser1080Interface *ls1080if;
420  try {
421  logger->log_info(name(), "Opening %s:%s", type, id);
422  ls1080if = blackboard->open_for_reading<Laser1080Interface>(id);
423  } catch (Exception &e) {
424  // ignored
425  logger->log_warn(name(), "Failed to open %s:%s: %s", type, id, e.what());
426  return;
427  }
428 
429  try {
430  bbil_add_data_interface(ls1080if);
431  bbil_add_reader_interface(ls1080if);
432  bbil_add_writer_interface(ls1080if);
433 
434  std::string topname = topic_name(ls1080if->id(), "1080");
435 
436  PublisherInfo pi;
437  pi.pub = rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
438 
439  logger->log_info(name(),
440  "Publishing 1080 laser scan %s at %s",
441  ls1080if->uid(),
442  topname.c_str());
443 
444  pi.msg.header.frame_id = ls1080if->frame();
445  pi.msg.angle_min = 0;
446  pi.msg.angle_max = 2 * M_PI;
447  pi.msg.angle_increment = deg2rad(0.5);
448  pi.msg.ranges.resize(1080);
449 
450  pubs_[ls1080if->uid()] = pi;
451 
452  blackboard->update_listener(this);
453  ls1080_ifs_.push_back(ls1080if);
454  } catch (Exception &e) {
455  blackboard->close(ls1080if);
456  logger->log_warn(name(), "Failed to register for %s:%s: %s", type, id, e.what());
457  return;
458  }
459  }
460 }
461 
462 void
464  unsigned int instance_serial) throw()
465 {
466  conditional_close(interface);
467 }
468 
469 void
471  unsigned int instance_serial) throw()
472 {
473  conditional_close(interface);
474 }
475 
476 void
477 RosLaserScanThread::conditional_close(Interface *interface) throw()
478 {
479  // Verify it's a laser interface
480  Laser360Interface * ls360if = dynamic_cast<Laser360Interface *>(interface);
481  Laser720Interface * ls720if = dynamic_cast<Laser720Interface *>(interface);
482  Laser1080Interface *ls1080if = dynamic_cast<Laser1080Interface *>(interface);
483 
484  if (ls360if) {
485  std::list<Laser360Interface *>::iterator i;
486  for (i = ls360_ifs_.begin(); i != ls360_ifs_.end(); ++i) {
487  if (*ls360if == **i) {
488  if (!ls360if->has_writer() && (ls360if->num_readers() == 1)) {
489  // It's only us
490  logger->log_info(name(), "Last on %s, closing", ls360if->uid());
491  bbil_remove_data_interface(*i);
492  bbil_remove_reader_interface(*i);
493  bbil_remove_writer_interface(*i);
494  blackboard->update_listener(this);
495  blackboard->close(*i);
496  ls360_ifs_.erase(i);
497  break;
498  }
499  }
500  }
501  } else if (ls720if) {
502  std::list<Laser720Interface *>::iterator i;
503  for (i = ls720_ifs_.begin(); i != ls720_ifs_.end(); ++i) {
504  if (*ls720if == **i) {
505  if (!ls720if->has_writer() && (ls720if->num_readers() == 1)) {
506  // It's only us
507  logger->log_info(name(), "Last on %s, closing", ls720if->uid());
508  bbil_remove_data_interface(*i);
509  bbil_remove_reader_interface(*i);
510  bbil_remove_writer_interface(*i);
511  blackboard->update_listener(this);
512  blackboard->close(*i);
513  ls720_ifs_.erase(i);
514  break;
515  }
516  }
517  }
518 
519  } else if (ls1080if) {
520  std::list<Laser1080Interface *>::iterator i;
521  for (i = ls1080_ifs_.begin(); i != ls1080_ifs_.end(); ++i) {
522  if (*ls1080if == **i) {
523  if (!ls1080if->has_writer() && (ls1080if->num_readers() == 1)) {
524  // It's only us
525  logger->log_info(name(), "Last on %s, closing", ls1080if->uid());
526  bbil_remove_data_interface(*i);
527  bbil_remove_reader_interface(*i);
528  bbil_remove_writer_interface(*i);
529  blackboard->update_listener(this);
530  blackboard->close(*i);
531  ls1080_ifs_.erase(i);
532  break;
533  }
534  }
535  }
536  }
537 }
538 
539 /** Callback function for ROS laser scan message subscription.
540  * @param msg incoming message
541  */
542 void
543 RosLaserScanThread::laser_scan_message_cb(
544  const ros::MessageEvent<sensor_msgs::LaserScan const> &msg_evt)
545 {
546  MutexLocker lock(ls_msg_queue_mutex_);
547  ls_msg_queues_[active_queue_].push(msg_evt);
548 }
virtual void finalize()
Finalize the thread.
virtual void bb_interface_created(const char *type, const char *id)
BlackBoard interface created notification.
virtual void loop()
Code to execute in the thread.
virtual void bb_interface_data_changed(fawkes::Interface *interface)
BlackBoard data changed notification.
virtual ~RosLaserScanThread()
Destructor.
virtual void bb_interface_writer_removed(fawkes::Interface *interface, unsigned int instance_serial)
A writing instance has been closed for a watched interface.
virtual void bb_interface_reader_removed(fawkes::Interface *interface, unsigned int instance_serial)
A reading instance has been closed for a watched interface.
virtual void init()
Initialize the thread.
RosLaserScanThread()
Constructor.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
BlackBoard interface listener.
void bbil_add_reader_interface(Interface *interface)
Add an interface to the reader addition/removal watch list.
void bbil_add_writer_interface(Interface *interface)
Add an interface to the writer addition/removal watch list.
void bbil_add_data_interface(Interface *interface)
Add an interface to the data modification watch list.
void bbio_add_observed_create(const char *type_pattern, const char *id_pattern="*")
Add interface creation type to watch list.
virtual void unregister_observer(BlackBoardInterfaceObserver *observer)
Unregister BB interface observer.
Definition: blackboard.cpp:240
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual void update_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Update BB event listener.
Definition: blackboard.cpp:197
virtual void register_observer(BlackBoardInterfaceObserver *observer)
Register BB interface observer.
Definition: blackboard.cpp:225
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:212
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual std::list< Interface * > open_multiple_for_reading(const char *type_pattern, const char *id_pattern="*", const char *owner=NULL)=0
Open multiple interfaces for reading.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:185
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what() const
Get primary string.
Definition: exception.cpp:639
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
const Time * timestamp() const
Get timestamp of last write.
Definition: interface.cpp:705
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:494
const char * id() const
Get identifier of interface.
Definition: interface.cpp:652
const char * uid() const
Get unique identifier of interface.
Definition: interface.cpp:677
unsigned int num_readers() const
Get the number of readers.
Definition: interface.cpp:845
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:472
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:817
Laser1080Interface Fawkes BlackBoard Interface.
char * frame() const
Get frame value.
float * distances() const
Get distances value.
Laser360Interface Fawkes BlackBoard Interface.
float * distances() const
Get distances value.
void set_frame(const char *new_frame)
Set frame value.
char * frame() const
Get frame value.
void set_distances(unsigned int index, const float new_distances)
Set distances value at given index.
Laser720Interface Fawkes BlackBoard Interface.
char * frame() const
Get frame value.
float * distances() const
Get distances value.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
virtual void log_info(const char *component, const char *format,...)
Log informational message.
Definition: multi.cpp:195
virtual void log_warn(const char *component, const char *format,...)
Log warning message.
Definition: multi.cpp:216
Mutex locking helper.
Definition: mutex_locker.h:34
Mutex mutual exclusion lock.
Definition: mutex.h:33
void lock()
Lock this mutex.
Definition: mutex.cpp:87
void unlock()
Unlock the mutex.
Definition: mutex.cpp:131
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Definition: ros.h:47
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
A class for handling time.
Definition: time.h:93
Time & stamp()
Set this time to the current time.
Definition: time.cpp:704
long get_nsec() const
Get nanoseconds.
Definition: time.h:132
long get_sec() const
Get seconds.
Definition: time.h:117
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36