player.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of Willow Garage, Inc. nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 ********************************************************************/
34 
35 #include "rosbag/player.h"
37 #include "rosbag/view.h"
38 
39 #if !defined(_MSC_VER)
40  #include <sys/select.h>
41 #endif
42 
43 #include <boost/foreach.hpp>
44 #include <boost/format.hpp>
45 
46 #include "rosgraph_msgs/Clock.h"
47 
48 #define foreach BOOST_FOREACH
49 
50 using std::map;
51 using std::pair;
52 using std::string;
53 using std::vector;
54 using boost::shared_ptr;
55 using ros::Exception;
56 
57 namespace rosbag {
58 
59 ros::AdvertiseOptions createAdvertiseOptions(const ConnectionInfo* c, uint32_t queue_size, const std::string& prefix) {
60  ros::AdvertiseOptions opts(prefix + c->topic, queue_size, c->md5sum, c->datatype, c->msg_def);
61  ros::M_string::const_iterator header_iter = c->header->find("latching");
62  opts.latch = (header_iter != c->header->end() && header_iter->second == "1");
63  return opts;
64 }
65 
66 
67 ros::AdvertiseOptions createAdvertiseOptions(MessageInstance const& m, uint32_t queue_size, const std::string& prefix) {
68  return ros::AdvertiseOptions(prefix + m.getTopic(), queue_size, m.getMD5Sum(), m.getDataType(), m.getMessageDefinition());
69 }
70 
71 // PlayerOptions
72 
74  prefix(""),
75  quiet(false),
76  start_paused(false),
77  at_once(false),
78  bag_time(false),
79  bag_time_frequency(0.0),
80  time_scale(1.0),
81  queue_size(0),
82  advertise_sleep(0.2),
83  try_future(false),
84  has_time(false),
85  loop(false),
86  time(0.0f),
87  has_duration(false),
88  duration(0.0f),
89  keep_alive(false),
90  wait_for_subscribers(false),
91  rate_control_topic(""),
92  rate_control_max_delay(1.0f),
93  skip_empty(ros::DURATION_MAX)
94 {
95 }
96 
98  if (bags.size() == 0)
99  throw Exception("You must specify at least one bag file to play from");
100  if (has_duration && duration <= 0.0)
101  throw Exception("Invalid duration, must be > 0.0");
102 }
103 
104 // Player
105 
106 Player::Player(PlayerOptions const& options) :
107  options_(options),
108  paused_(false),
109  // If we were given a list of topics to pause on, then go into that mode
110  // by default (it can be toggled later via 't' from the keyboard).
111  pause_for_topics_(options_.pause_topics.size() > 0),
112  pause_change_requested_(false),
113  requested_pause_state_(false),
114  terminal_modified_(false)
115 {
116  ros::NodeHandle private_node_handle("~");
117  pause_service_ = private_node_handle.advertiseService("pause_playback", &Player::pauseCallback, this);
118 }
119 
121  foreach(shared_ptr<Bag> bag, bags_)
122  bag->close();
123 
124  restoreTerminal();
125 }
126 
128  options_.check();
129 
130  // Open all the bag files
131  foreach(string const& filename, options_.bags) {
132  ROS_INFO("Opening %s", filename.c_str());
133 
134  try
135  {
136  shared_ptr<Bag> bag(boost::make_shared<Bag>());
137  bag->open(filename, bagmode::Read);
138  bags_.push_back(bag);
139  }
140  catch (BagUnindexedException ex) {
141  std::cerr << "Bag file " << filename << " is unindexed. Run rosbag reindex." << std::endl;
142  return;
143  }
144  }
145 
146  setupTerminal();
147 
148  if (!node_handle_.ok())
149  return;
150 
151  if (!options_.prefix.empty())
152  {
153  ROS_INFO_STREAM("Using prefix '" << options_.prefix << "'' for topics ");
154  }
155 
156  if (!options_.quiet)
157  puts("");
158 
159  // Publish all messages in the bags
160  View full_view;
161  foreach(shared_ptr<Bag> bag, bags_)
162  full_view.addQuery(*bag);
163 
164  ros::Time initial_time = full_view.getBeginTime();
165 
166  initial_time += ros::Duration(options_.time);
167 
168  ros::Time finish_time = ros::TIME_MAX;
170  {
171  finish_time = initial_time + ros::Duration(options_.duration);
172  }
173 
174  View view;
175  TopicQuery topics(options_.topics);
176 
177  if (options_.topics.empty())
178  {
179  foreach(shared_ptr<Bag> bag, bags_)
180  view.addQuery(*bag, initial_time, finish_time);
181  } else {
182  foreach(shared_ptr<Bag> bag, bags_)
183  view.addQuery(*bag, topics, initial_time, finish_time);
184  }
185 
186  if (view.size() == 0)
187  {
188  std::cerr << "No messages to play on specified topics. Exiting." << std::endl;
189  ros::shutdown();
190  return;
191  }
192 
193  // Advertise all of our messages
194  foreach(const ConnectionInfo* c, view.getConnections())
195  {
196  ros::M_string::const_iterator header_iter = c->header->find("callerid");
197  std::string callerid = (header_iter != c->header->end() ? header_iter->second : string(""));
198 
199  string callerid_topic = callerid + c->topic;
200 
201  map<string, ros::Publisher>::iterator pub_iter = publishers_.find(callerid_topic);
202  if (pub_iter == publishers_.end()) {
203 
205 
207  publishers_.insert(publishers_.begin(), pair<string, ros::Publisher>(callerid_topic, pub));
208 
209  pub_iter = publishers_.find(callerid_topic);
210  }
211  }
212 
213  if (options_.rate_control_topic != "")
214  {
215  std::cout << "Creating rate control topic subscriber..." << std::flush;
216 
217  boost::shared_ptr<ros::Subscriber> sub(boost::make_shared<ros::Subscriber>());
220  ops.queue_size = 10;
221  ops.md5sum = ros::message_traits::md5sum<topic_tools::ShapeShifter>();
222  ops.datatype = ros::message_traits::datatype<topic_tools::ShapeShifter>();
223  ops.helper = boost::make_shared<ros::SubscriptionCallbackHelperT<
225  boost::bind(&Player::updateRateTopicTime, this, _1));
226 
228 
229  std::cout << " done." << std::endl;
230  }
231 
232 
233  std::cout << "Waiting " << options_.advertise_sleep.toSec() << " seconds after advertising topics..." << std::flush;
235  std::cout << " done." << std::endl;
236 
237  std::cout << std::endl << "Hit space to toggle paused, or 's' to step." << std::endl;
238 
240 
242  {
244  }
245 
246  while (true) {
247  // Set up our time_translator and publishers
248 
250 
251  start_time_ = view.begin()->getTime();
253  bag_length_ = view.getEndTime() - view.getBeginTime();
254 
255  // Set the last rate control to now, so the program doesn't start delayed.
257 
259 
262 
263 
265  if (options_.bag_time)
267  else
269 
270  paused_time_ = now_wt;
271 
272  // Call do-publish for each message
273  foreach(MessageInstance m, view) {
274  if (!node_handle_.ok())
275  break;
276 
277  doPublish(m);
278  }
279 
280  if (options_.keep_alive)
281  while (node_handle_.ok())
282  doKeepAlive();
283 
284  if (!node_handle_.ok()) {
285  std::cout << std::endl;
286  break;
287  }
288  if (!options_.loop) {
289  std::cout << std::endl << "Done." << std::endl;
290  break;
291  }
292  }
293 
294  ros::shutdown();
295 }
296 
298 {
300  std::string def = ssmsg->getMessageDefinition();
301  size_t length = ros::serialization::serializationLength(*ssmsg);
302 
303  // Check the message definition.
304  std::istringstream f(def);
305  std::string s;
306  bool flag = false;
307  while(std::getline(f, s, '\n')) {
308  if (!s.empty() && s.find("#") != 0) {
309  // Does not start with #, is not a comment.
310  if (s.find("Header ") == 0) {
311  flag = true;
312  }
313  break;
314  }
315  }
316  // If the header is not the first element in the message according to the definition, throw an error.
317  if (!flag) {
318  std::cout << std::endl << "WARNING: Rate control topic is bad, header is not first. MSG may be malformed." << std::endl;
319  return;
320  }
321 
322  std::vector<uint8_t> buffer(length);
323  ros::serialization::OStream ostream(&buffer[0], length);
325 
326  // Assuming that the header is the first several bytes of the message.
327  //uint32_t header_sequence_id = buffer[0] | (uint32_t)buffer[1] << 8 | (uint32_t)buffer[2] << 16 | (uint32_t)buffer[3] << 24;
328  int32_t header_timestamp_sec = buffer[4] | (uint32_t)buffer[5] << 8 | (uint32_t)buffer[6] << 16 | (uint32_t)buffer[7] << 24;
329  int32_t header_timestamp_nsec = buffer[8] | (uint32_t)buffer[9] << 8 | (uint32_t)buffer[10] << 16 | (uint32_t)buffer[11] << 24;
330 
331  last_rate_control_ = ros::Time(header_timestamp_sec, header_timestamp_nsec);
332 }
333 
335 {
336  if (!options_.quiet) {
337 
338  ros::Time current_time = time_publisher_.getTime();
339  ros::Duration d = current_time - start_time_;
340 
341 
342  if (paused_)
343  {
344  printf("\r [PAUSED ] Bag Time: %13.6f Duration: %.6f / %.6f \r", time_publisher_.getTime().toSec(), d.toSec(), bag_length_.toSec());
345  }
346  else if (delayed_)
347  {
348  ros::Duration time_since_rate = std::max(ros::Time::now() - last_rate_control_, ros::Duration(0));
349  printf("\r [DELAYED] Bag Time: %13.6f Duration: %.6f / %.6f Delay: %.2f \r", time_publisher_.getTime().toSec(), d.toSec(), bag_length_.toSec(), time_since_rate.toSec());
350  }
351  else
352  {
353  printf("\r [RUNNING] Bag Time: %13.6f Duration: %.6f / %.6f \r", time_publisher_.getTime().toSec(), d.toSec(), bag_length_.toSec());
354  }
355  fflush(stdout);
356  }
357 }
358 
359 bool Player::pauseCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
360 {
361  pause_change_requested_ = (req.data != paused_);
362  requested_pause_state_ = req.data;
363 
364  res.success = pause_change_requested_;
365 
366  if (res.success)
367  {
368  res.message = std::string("Playback is now ") + (requested_pause_state_ ? "paused" : "resumed");
369  }
370  else
371  {
372  res.message = std::string("Bag is already ") + (requested_pause_state_ ? "paused." : "running.");
373  }
374 
375  return true;
376 }
377 
378 void Player::processPause(const bool paused, ros::WallTime &horizon)
379 {
380  paused_ = paused;
381 
382  if (paused_)
383  {
385  }
386  else
387  {
388  // Make sure time doesn't shift after leaving pause.
391 
393 
394  horizon += shift;
395  time_publisher_.setWCHorizon(horizon);
396  }
397 }
398 
400 {
401  bool all_topics_subscribed = false;
402  std::cout << "Waiting for subscribers." << std::endl;
403  while (!all_topics_subscribed) {
404  all_topics_subscribed = true;
405  foreach(const PublisherMap::value_type& pub, publishers_) {
406  all_topics_subscribed &= pub.second.getNumSubscribers() > 0;
407  }
408  ros::Duration(0.1).sleep();
409  }
410  std::cout << "Finished waiting for subscribers." << std::endl;
411 }
412 
414  string const& topic = m.getTopic();
415  ros::Time const& time = m.getTime();
416  string callerid = m.getCallerId();
417 
418  ros::Time translated = time_translator_.translate(time);
419  ros::WallTime horizon = ros::WallTime(translated.sec, translated.nsec);
420 
422  time_publisher_.setWCHorizon(horizon);
423 
424  string callerid_topic = callerid + topic;
425 
426  map<string, ros::Publisher>::iterator pub_iter = publishers_.find(callerid_topic);
427  ROS_ASSERT(pub_iter != publishers_.end());
428 
429  // Update subscribers.
430  ros::spinOnce();
431 
432  // If immediate specified, play immediately
433  if (options_.at_once) {
435  pub_iter->second.publish(m);
436  printTime();
437  return;
438  }
439 
440  // If skip_empty is specified, skip this region and shift.
442  {
444 
445  ros::WallDuration shift = ros::WallTime::now() - horizon ;
447  horizon += shift;
448  time_publisher_.setWCHorizon(horizon);
449  (pub_iter->second).publish(m);
450  printTime();
451  return;
452  }
453 
454  if (pause_for_topics_)
455  {
456  for (std::vector<std::string>::iterator i = options_.pause_topics.begin();
457  i != options_.pause_topics.end();
458  ++i)
459  {
460  if (topic == *i)
461  {
462  paused_ = true;
464  }
465  }
466  }
467 
468  // Check if the rate control topic has posted recently enough to continue, or if a delay is needed.
469  // Delayed is separated from paused to allow more verbose printing.
470  if (rate_control_sub_ != NULL) {
472  delayed_ = true;
474  }
475  }
476 
478  {
479  bool charsleftorpaused = true;
480  while (charsleftorpaused && node_handle_.ok())
481  {
482  ros::spinOnce();
483 
485  {
487  pause_change_requested_ = false;
488  }
489 
490  switch (readCharFromStdin()){
491  case ' ':
492  processPause(!paused_, horizon);
493  break;
494  case 's':
495  if (paused_) {
497 
498  ros::WallDuration shift = ros::WallTime::now() - horizon ;
500 
502 
503  horizon += shift;
504  time_publisher_.setWCHorizon(horizon);
505 
506  (pub_iter->second).publish(m);
507 
508  printTime();
509  return;
510  }
511  break;
512  case 't':
514  break;
515  case EOF:
516  if (paused_)
517  {
518  printTime();
520  ros::spinOnce();
521  }
522  else if (delayed_)
523  {
524  printTime();
526  ros::spinOnce();
527  // You need to check the rate here too.
529  delayed_ = false;
530  // Make sure time doesn't shift after leaving delay.
533 
535 
536  horizon += shift;
537  time_publisher_.setWCHorizon(horizon);
538  }
539  }
540  else
541  charsleftorpaused = false;
542  }
543  }
544 
545  printTime();
547  ros::spinOnce();
548  }
549 
550  pub_iter->second.publish(m);
551 }
552 
553 
555  //Keep pushing ourself out in 10-sec increments (avoids fancy math dealing with the end of time)
556  ros::Time const& time = time_publisher_.getTime() + ros::Duration(10.0);
557 
558  ros::Time translated = time_translator_.translate(time);
559  ros::WallTime horizon = ros::WallTime(translated.sec, translated.nsec);
560 
562  time_publisher_.setWCHorizon(horizon);
563 
564  if (options_.at_once) {
565  return;
566  }
567 
568  // If we're done and just staying alive, don't watch the rate control topic. We aren't publishing anyway.
569  delayed_ = false;
570 
572  {
573  bool charsleftorpaused = true;
574  while (charsleftorpaused && node_handle_.ok())
575  {
576  switch (readCharFromStdin()){
577  case ' ':
578  paused_ = !paused_;
579  if (paused_) {
581  }
582  else
583  {
584  // Make sure time doesn't shift after leaving pause.
587 
589 
590  horizon += shift;
591  time_publisher_.setWCHorizon(horizon);
592  }
593  break;
594  case EOF:
595  if (paused_)
596  {
597  printTime();
599  ros::spinOnce();
600  }
601  else
602  charsleftorpaused = false;
603  }
604  }
605 
606  printTime();
608  ros::spinOnce();
609  }
610 }
611 
612 
613 
615  if (terminal_modified_)
616  return;
617 
618 #if defined(_MSC_VER)
619  input_handle = GetStdHandle(STD_INPUT_HANDLE);
620  if (input_handle == INVALID_HANDLE_VALUE)
621  {
622  std::cout << "Failed to set up standard input handle." << std::endl;
623  return;
624  }
625  if (! GetConsoleMode(input_handle, &stdin_set) )
626  {
627  std::cout << "Failed to save the console mode." << std::endl;
628  return;
629  }
630  // don't actually need anything but the default, alternatively try this
631  //DWORD event_mode = ENABLE_WINDOW_INPUT | ENABLE_MOUSE_INPUT;
632  //if (! SetConsoleMode(input_handle, event_mode) )
633  //{
634  // std::cout << "Failed to set the console mode." << std::endl;
635  // return;
636  //}
637  terminal_modified_ = true;
638 #else
639  const int fd = fileno(stdin);
640  termios flags;
641  tcgetattr(fd, &orig_flags_);
642  flags = orig_flags_;
643  flags.c_lflag &= ~ICANON; // set raw (unset canonical modes)
644  flags.c_cc[VMIN] = 0; // i.e. min 1 char for blocking, 0 chars for non-blocking
645  flags.c_cc[VTIME] = 0; // block if waiting for char
646  tcsetattr(fd, TCSANOW, &flags);
647 
648  FD_ZERO(&stdin_fdset_);
649  FD_SET(fd, &stdin_fdset_);
650  maxfd_ = fd + 1;
651  terminal_modified_ = true;
652 #endif
653 }
654 
656  if (!terminal_modified_)
657  return;
658 
659 #if defined(_MSC_VER)
660  SetConsoleMode(input_handle, stdin_set);
661 #else
662  const int fd = fileno(stdin);
663  tcsetattr(fd, TCSANOW, &orig_flags_);
664 #endif
665  terminal_modified_ = false;
666 }
667 
669 #ifdef __APPLE__
670  fd_set testfd;
671  FD_COPY(&stdin_fdset_, &testfd);
672 #elif !defined(_MSC_VER)
673  fd_set testfd = stdin_fdset_;
674 #endif
675 
676 #if defined(_MSC_VER)
677  DWORD events = 0;
678  INPUT_RECORD input_record[1];
679  DWORD input_size = 1;
680  BOOL b = GetNumberOfConsoleInputEvents(input_handle, &events);
681  if (b && events > 0)
682  {
683  b = ReadConsoleInput(input_handle, input_record, input_size, &events);
684  if (b)
685  {
686  for (unsigned int i = 0; i < events; ++i)
687  {
688  if (input_record[i].EventType & KEY_EVENT & input_record[i].Event.KeyEvent.bKeyDown)
689  {
690  CHAR ch = input_record[i].Event.KeyEvent.uChar.AsciiChar;
691  return ch;
692  }
693  }
694  }
695  }
696  return EOF;
697 #else
698  timeval tv;
699  tv.tv_sec = 0;
700  tv.tv_usec = 0;
701  if (select(maxfd_, &testfd, NULL, NULL, &tv) <= 0)
702  return EOF;
703  return getc(stdin);
704 #endif
705 }
706 
707 TimePublisher::TimePublisher() : time_scale_(1.0)
708 {
709  setPublishFrequency(-1.0);
710  time_pub_ = node_handle_.advertise<rosgraph_msgs::Clock>("clock",1);
711 }
712 
713 void TimePublisher::setPublishFrequency(double publish_frequency)
714 {
715  publish_frequency_ = publish_frequency;
716 
717  do_publish_ = (publish_frequency > 0.0);
718 
719  wall_step_.fromSec(1.0 / publish_frequency);
720 }
721 
722 void TimePublisher::setTimeScale(double time_scale)
723 {
724  time_scale_ = time_scale;
725 }
726 
728 {
729  horizon_ = horizon;
730 }
731 
733 {
734  wc_horizon_ = horizon;
735 }
736 
738 {
739  current_ = time;
740 }
741 
743 {
744  return current_;
745 }
746 
748 {
749  if (do_publish_)
750  {
751  rosgraph_msgs::Clock pub_msg;
752 
754  ros::WallTime done = t + duration;
755 
756  while (t < done && t < wc_horizon_)
757  {
758  ros::WallDuration leftHorizonWC = wc_horizon_ - t;
759 
760  ros::Duration d(leftHorizonWC.sec, leftHorizonWC.nsec);
761  d *= time_scale_;
762 
763  current_ = horizon_ - d;
764 
765  if (current_ >= horizon_)
766  current_ = horizon_;
767 
768  if (t >= next_pub_)
769  {
770  pub_msg.clock = current_;
771  time_pub_.publish(pub_msg);
772  next_pub_ = t + wall_step_;
773  }
774 
775  ros::WallTime target = done;
776  if (target > wc_horizon_)
777  target = wc_horizon_;
778  if (target > next_pub_)
779  target = next_pub_;
780 
782 
783  t = ros::WallTime::now();
784  }
785  } else {
786 
788 
789  ros::WallDuration leftHorizonWC = wc_horizon_ - t;
790 
791  ros::Duration d(leftHorizonWC.sec, leftHorizonWC.nsec);
792  d *= time_scale_;
793 
794  current_ = horizon_ - d;
795 
796  if (current_ >= horizon_)
797  current_ = horizon_;
798 
799  ros::WallTime target = ros::WallTime::now() + duration;
800 
801  if (target > wc_horizon_)
802  target = wc_horizon_;
803 
805  }
806 }
807 
809 {
810  if (do_publish_)
811  {
812  current_ = horizon_;
813 
814  rosgraph_msgs::Clock pub_msg;
815 
816  pub_msg.clock = current_;
817  time_pub_.publish(pub_msg);
818 
820  next_pub_ = t + wall_step_;
821  } else {
822  current_ = horizon_;
823  }
824 }
825 
827 {
828  if (do_publish_)
829  {
830  rosgraph_msgs::Clock pub_msg;
831 
833  ros::WallTime done = t + duration;
834 
835  while ( t < done )
836  {
837  if (t > next_pub_)
838  {
839  pub_msg.clock = current_;
840  time_pub_.publish(pub_msg);
841  next_pub_ = t + wall_step_;
842  }
843 
844  ros::WallTime target = done;
845 
846  if (target > next_pub_)
847  target = next_pub_;
848 
850 
851  t = ros::WallTime::now();
852  }
853  } else {
854  duration.sleep();
855  }
856 }
857 
859 {
860  return ros::WallTime::now() > wc_horizon_;
861 }
862 
863 } // namespace rosbag
void publish()
Definition: player.cpp:127
d
termios orig_flags_
Definition: player.h:226
static void write(Stream &stream, typename boost::call_traits< T >::param_type t)
void setupTerminal()
Definition: player.cpp:614
void setWCHorizon(const ros::WallTime &horizon)
Definition: player.cpp:732
bool wait_for_subscribers
Definition: player.h:94
ros::WallTime next_pub_
Definition: player.h:154
TimeTranslator time_translator_
Definition: player.h:231
std::string rate_control_topic
Definition: player.h:95
void publish(const boost::shared_ptr< M > &message) const
std::vector< const ConnectionInfo * > getConnections()
void setPublishFrequency(double publish_frequency)
Definition: player.cpp:713
f
ros::Time translate(ros::Time const &t)
PlayerOptions options_
Definition: player.h:197
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void doPublish(rosbag::MessageInstance const &m)
Definition: player.cpp:413
std::vector< std::string > topics
Definition: player.h:100
XmlRpcServer s
bool sleep() const
bool requested_pause_state_
Definition: player.h:210
std::vector< std::string > pause_topics
Definition: player.h:101
ros::WallDuration wall_step_
Definition: player.h:152
void doKeepAlive()
Definition: player.cpp:554
ros::Publisher time_pub_
Definition: player.h:150
std::string prefix
Definition: player.h:78
std::vector< std::string > bags
Definition: player.h:99
std::string const & getMD5Sum() const
static bool sleepUntil(const WallTime &end)
ros::WallTime wc_horizon_
Definition: player.h:156
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
float rate_control_max_delay
Definition: player.h:96
void setTranslatedStartTime(ros::Time const &t)
Increments the translated start time by shift. Useful for pausing.
ros::WallDuration advertise_sleep
Definition: player.h:86
ros::Time const & getTime() const
double bag_time_frequency
Definition: player.h:83
uint32_t size()
const boost::shared_ptr< ConstMessage > & getConstMessage() const
SubscriptionCallbackHelperPtr helper
bool sleep() const
ros::Time getBeginTime()
void addQuery(Bag const &bag, ros::Time const &start_time=ros::TIME_MIN, ros::Time const &end_time=ros::TIME_MAX)
WallDuration & fromSec(double t)
void setTimeScale(double const &s)
#define ROS_INFO(...)
ros::NodeHandle node_handle_
Definition: player.h:199
double publish_frequency_
Definition: player.h:146
ros::Time horizon_
Definition: player.h:157
void runClock(const ros::WallDuration &duration)
Definition: player.cpp:747
void setRealStartTime(ros::Time const &t)
ros::Subscriber rate_control_sub_
Definition: player.h:212
ROSTIME_DECL const Time TIME_MAX
bool paused_
Definition: player.h:203
void setTimeScale(double time_scale)
Definition: player.cpp:722
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
std::vector< boost::shared_ptr< Bag > > bags_
Definition: player.h:217
ros::ServiceServer pause_service_
Definition: player.h:201
void restoreTerminal()
Definition: player.cpp:655
void printTime()
Definition: player.cpp:334
bool delayed_
Definition: player.h:204
ros::WallTime paused_time_
Definition: player.h:215
void updateRateTopicTime(const ros::MessageEvent< topic_tools::ShapeShifter const > &msg_event)
Definition: player.cpp:297
bool pauseCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
Definition: player.cpp:359
Player(PlayerOptions const &options)
Definition: player.cpp:106
std::string const & getMessageDefinition() const
bool pause_for_topics_
Definition: player.h:206
fd_set stdin_fdset_
Definition: player.h:227
bool terminal_modified_
Definition: player.h:221
boost::shared_ptr< ros::M_string > header
uint32_t serializationLength(const T &t)
ros::Time getEndTime()
#define ROS_INFO_STREAM(args)
static WallTime now()
std::string const & getTopic() const
ros::NodeHandle node_handle_
Definition: player.h:149
iterator begin()
void setTime(const ros::Time &time)
Definition: player.cpp:737
ros::Duration bag_length_
Definition: player.h:235
std::string const & getDataType() const
void stepClock()
Step the clock to the horizon.
Definition: player.cpp:808
static Time now()
ros::AdvertiseOptions createAdvertiseOptions(MessageInstance const &msg, uint32_t queue_size, const std::string &prefix="")
Helper function to create AdvertiseOptions from a MessageInstance.
Definition: player.cpp:67
void shift(ros::Duration const &d)
Increments the translated start time by shift. Useful for pausing.
ros::Time last_rate_control_
Definition: player.h:213
ROSCPP_DECL void shutdown()
int readCharFromStdin()
Definition: player.cpp:668
PublisherMap publishers_
Definition: player.h:218
ros::Time const & getTime() const
Definition: player.cpp:742
ros::Duration skip_empty
Definition: player.h:97
bool ok() const
bool pause_change_requested_
Definition: player.h:208
#define ROS_ASSERT(cond)
void waitForSubscribers() const
Definition: player.cpp:399
ros::Time start_time_
Definition: player.h:234
TimePublisher time_publisher_
Definition: player.h:232
std::string getCallerId() const
ROSCPP_DECL void spinOnce()
void processPause(const bool paused, ros::WallTime &horizon)
Definition: player.cpp:378
void setHorizon(const ros::Time &horizon)
Definition: player.cpp:727
ros::Time current_
Definition: player.h:158
void runStalledClock(const ros::WallDuration &duration)
Sleep as necessary, but don&#39;t let the click run.
Definition: player.cpp:826


rosbag
Author(s): Tim Field, Jeremy Leibs, James Bowman
autogenerated on Sun Feb 3 2019 03:30:26