void Player::doPublish(MessageInstance const& m) {
string const& topic = m.getTopic();
ros::Time const& time = m.getTime();
string callerid = m.getCallerId();
ros::Time translated = time_translator_.translate(time);
ros::WallTime horizon = ros::WallTime(translated.sec, translated.nsec);
time_publisher_.setHorizon(time);
time_publisher_.setWCHorizon(horizon);
string callerid_topic = callerid + topic;
map<string, ros::Publisher>::iterator pub_iter = publishers_.find(callerid_topic);
ROS_ASSERT(pub_iter != publishers_.end());
// If immediate specified, play immediately
if (options_.at_once) {
time_publisher_.stepClock();
pub_iter->second.publish(m);
printTime();
return;
}
// If skip_empty is specified, skip this region and shift.
if (time - time_publisher_.getTime() > options_.skip_empty)
{
time_publisher_.stepClock();
ros::WallDuration shift = ros::WallTime::now() - horizon ;
time_translator_.shift(ros::Duration(shift.sec, shift.nsec));
horizon += shift;
time_publisher_.setWCHorizon(horizon);
(pub_iter->second).publish(m);
printTime();
return;
}
while ((paused_ || !time_publisher_.horizonReached()) && node_handle_.ok())
{
bool charsleftorpaused = true;
while (charsleftorpaused && node_handle_.ok())
{
switch (readCharFromStdin()){
case ' ':
paused_ = !paused_;
if (paused_) {
paused_time_ = ros::WallTime::now();
}
else
{
ros::WallDuration shift = ros::WallTime::now() - paused_time_;
paused_time_ = ros::WallTime::now();
time_translator_.shift(ros::Duration(shift.sec, shift.nsec));
horizon += shift;
time_publisher_.setWCHorizon(horizon);
}
break;
case 's':
if (paused_) {
time_publisher_.stepClock();
ros::WallDuration shift = ros::WallTime::now() - horizon ;
paused_time_ = ros::WallTime::now();
time_translator_.shift(ros::Duration(shift.sec, shift.nsec));
horizon += shift;
time_publisher_.setWCHorizon(horizon);
(pub_iter->second).publish(m);
printTime();
return;
}
break;
case EOF:
if (paused_)
{
printTime();
time_publisher_.runStalledClock(ros::WallDuration(.1));
}
else
charsleftorpaused = false;
}
}
printTime();
time_publisher_.runClock(ros::WallDuration(.1));
}
pub_iter->second.publish(m);
}
ros::AdvertiseOptions createAdvertiseOptions(MessageInstance const& m, uint32_t queue_size, const std::string& prefix) {
return ros::AdvertiseOptions(prefix + m.getTopic(), queue_size, m.getMD5Sum(), m.getDataType(), m.getMessageDefinition());
}
