39 #if !defined(_MSC_VER) 40 #include <sys/select.h> 43 #include <boost/foreach.hpp> 44 #include <boost/format.hpp> 46 #include "rosgraph_msgs/Clock.h" 48 #define foreach BOOST_FOREACH 61 ros::M_string::const_iterator header_iter = c->
header->find(
"latching");
62 opts.
latch = (header_iter != c->
header->end() && header_iter->second ==
"1");
79 bag_time_frequency(0.0),
90 wait_for_subscribers(false),
91 rate_control_topic(
""),
92 rate_control_max_delay(1.0
f),
93 skip_empty(
ros::DURATION_MAX)
99 throw Exception(
"You must specify at least one bag file to play from");
101 throw Exception(
"Invalid duration, must be > 0.0");
112 pause_change_requested_(false),
113 requested_pause_state_(false),
114 terminal_modified_(false)
132 ROS_INFO(
"Opening %s", filename.c_str());
138 bags_.push_back(bag);
141 std::cerr <<
"Bag file " << filename <<
" is unindexed. Run rosbag reindex." << std::endl;
180 view.
addQuery(*bag, initial_time, finish_time);
183 view.
addQuery(*bag, topics, initial_time, finish_time);
186 if (view.
size() == 0)
188 std::cerr <<
"No messages to play on specified topics. Exiting." << std::endl;
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(
""));
199 string callerid_topic = callerid + c->
topic;
201 map<string, ros::Publisher>::iterator pub_iter =
publishers_.find(callerid_topic);
215 std::cout <<
"Creating rate control topic subscriber..." << std::flush;
221 ops.
md5sum = ros::message_traits::md5sum<topic_tools::ShapeShifter>();
222 ops.
datatype = ros::message_traits::datatype<topic_tools::ShapeShifter>();
229 std::cout <<
" done." << std::endl;
235 std::cout <<
" done." << std::endl;
237 std::cout << std::endl <<
"Hit space to toggle paused, or 's' to step." << std::endl;
285 std::cout << std::endl;
289 std::cout << std::endl <<
"Done." << std::endl;
300 std::string def = ssmsg->getMessageDefinition();
304 std::istringstream
f(def);
307 while(std::getline(f, s,
'\n')) {
308 if (!s.empty() && s.find(
"#") != 0) {
310 if (s.find(
"Header ") == 0) {
318 std::cout << std::endl <<
"WARNING: Rate control topic is bad, header is not first. MSG may be malformed." << std::endl;
322 std::vector<uint8_t> buffer(length);
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;
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;
410 std::cout <<
"Finished waiting for subscribers." << std::endl;
424 string callerid_topic = callerid + topic;
426 map<string, ros::Publisher>::iterator pub_iter =
publishers_.find(callerid_topic);
435 pub_iter->second.publish(m);
479 bool charsleftorpaused =
true;
541 charsleftorpaused =
false;
550 pub_iter->second.publish(m);
573 bool charsleftorpaused =
true;
602 charsleftorpaused =
false;
618 #if defined(_MSC_VER) 619 input_handle = GetStdHandle(STD_INPUT_HANDLE);
620 if (input_handle == INVALID_HANDLE_VALUE)
622 std::cout <<
"Failed to set up standard input handle." << std::endl;
625 if (! GetConsoleMode(input_handle, &stdin_set) )
627 std::cout <<
"Failed to save the console mode." << std::endl;
639 const int fd = fileno(stdin);
643 flags.c_lflag &= ~ICANON;
644 flags.c_cc[VMIN] = 0;
645 flags.c_cc[VTIME] = 0;
646 tcsetattr(fd, TCSANOW, &flags);
659 #if defined(_MSC_VER) 660 SetConsoleMode(input_handle, stdin_set);
662 const int fd = fileno(stdin);
672 #elif !defined(_MSC_VER) 676 #if defined(_MSC_VER) 678 INPUT_RECORD input_record[1];
679 DWORD input_size = 1;
680 BOOL b = GetNumberOfConsoleInputEvents(input_handle, &events);
683 b = ReadConsoleInput(input_handle, input_record, input_size, &events);
686 for (
unsigned int i = 0; i < events; ++i)
688 if (input_record[i].EventType & KEY_EVENT & input_record[i].Event.KeyEvent.bKeyDown)
690 CHAR ch = input_record[i].Event.KeyEvent.uChar.AsciiChar;
701 if (select(
maxfd_, &testfd, NULL, NULL, &tv) <= 0)
751 rosgraph_msgs::Clock pub_msg;
814 rosgraph_msgs::Clock pub_msg;
830 rosgraph_msgs::Clock pub_msg;
static void write(Stream &stream, typename boost::call_traits< T >::param_type t)
void setWCHorizon(const ros::WallTime &horizon)
bool wait_for_subscribers
TimeTranslator time_translator_
std::string rate_control_topic
void publish(const boost::shared_ptr< M > &message) const
std::vector< const ConnectionInfo * > getConnections()
void setPublishFrequency(double publish_frequency)
ros::Time translate(ros::Time const &t)
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)
std::vector< std::string > topics
bool requested_pause_state_
std::vector< std::string > pause_topics
ros::WallDuration wall_step_
std::vector< std::string > bags
std::string const & getMD5Sum() const
static bool sleepUntil(const WallTime &end)
ros::WallTime wc_horizon_
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
float rate_control_max_delay
void setTranslatedStartTime(ros::Time const &t)
Increments the translated start time by shift. Useful for pausing.
ros::WallDuration advertise_sleep
ros::Time const & getTime() const
double bag_time_frequency
const boost::shared_ptr< ConstMessage > & getConstMessage() const
SubscriptionCallbackHelperPtr helper
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)
ros::NodeHandle node_handle_
double publish_frequency_
void runClock(const ros::WallDuration &duration)
void setRealStartTime(ros::Time const &t)
ros::Subscriber rate_control_sub_
ROSTIME_DECL const Time TIME_MAX
void setTimeScale(double time_scale)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
std::vector< boost::shared_ptr< Bag > > bags_
ros::ServiceServer pause_service_
ros::WallTime paused_time_
void updateRateTopicTime(const ros::MessageEvent< topic_tools::ShapeShifter const > &msg_event)
bool pauseCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
Player(PlayerOptions const &options)
std::string const & getMessageDefinition() const
boost::shared_ptr< ros::M_string > header
uint32_t serializationLength(const T &t)
#define ROS_INFO_STREAM(args)
std::string const & getTopic() const
ros::NodeHandle node_handle_
void setTime(const ros::Time &time)
ros::Duration bag_length_
std::string const & getDataType() const
void stepClock()
Step the clock to the horizon.
ros::AdvertiseOptions createAdvertiseOptions(MessageInstance const &msg, uint32_t queue_size, const std::string &prefix="")
Helper function to create AdvertiseOptions from a MessageInstance.
void shift(ros::Duration const &d)
Increments the translated start time by shift. Useful for pausing.
ros::Time last_rate_control_
ROSCPP_DECL void shutdown()
ros::Time const & getTime() const
bool pause_change_requested_
void waitForSubscribers() const
TimePublisher time_publisher_
std::string getCallerId() const
ROSCPP_DECL void spinOnce()
void processPause(const bool paused, ros::WallTime &horizon)
void setHorizon(const ros::Time &horizon)
void runStalledClock(const ros::WallDuration &duration)
Sleep as necessary, but don't let the click run.