void TimePublisher::runStalledClock(const ros::WallDuration& duration)
{
if (do_publish_)
{
rosgraph_msgs::Clock pub_msg;
ros::WallTime t = ros::WallTime::now();
ros::WallTime done = t + duration;
while ( t < done )
{
if (t > next_pub_)
{
pub_msg.clock = current_;
time_pub_.publish(pub_msg);
next_pub_ = t + wall_step_;
}
ros::WallTime target = done;
if (target > next_pub_)
target = next_pub_;
ros::WallTime::sleepUntil(target);
t = ros::WallTime::now();
}
} else {
duration.sleep();
}
}
void AmclNode::runFromBag(const std::string &in_bag_fn)
{
rosbag::Bag bag;
bag.open(in_bag_fn, rosbag::bagmode::Read);
std::vector<std::string> topics;
topics.push_back(std::string("tf"));
std::string scan_topic_name = "base_scan"; // TODO determine what topic this actually is from ROS
topics.push_back(scan_topic_name);
rosbag::View view(bag, rosbag::TopicQuery(topics));
ros::Publisher laser_pub = nh_.advertise<sensor_msgs::LaserScan>(scan_topic_name, 100);
ros::Publisher tf_pub = nh_.advertise<tf2_msgs::TFMessage>("/tf", 100);
// Sleep for a second to let all subscribers connect
ros::WallDuration(1.0).sleep();
ros::WallTime start(ros::WallTime::now());
// Wait for map
while (ros::ok())
{
{
boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);
if (map_)
{
ROS_INFO("Map is ready");
break;
}
}
ROS_INFO("Waiting for map...");
ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration(1.0));
}
BOOST_FOREACH(rosbag::MessageInstance const msg, view)
{
if (!ros::ok())
{
break;
}
// Process any ros messages or callbacks at this point
ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration());
tf2_msgs::TFMessage::ConstPtr tf_msg = msg.instantiate<tf2_msgs::TFMessage>();
if (tf_msg != NULL)
{
tf_pub.publish(msg);
for (size_t ii=0; ii<tf_msg->transforms.size(); ++ii)
{
tf_->getBuffer().setTransform(tf_msg->transforms[ii], "rosbag_authority");
}
continue;
}
sensor_msgs::LaserScan::ConstPtr base_scan = msg.instantiate<sensor_msgs::LaserScan>();
if (base_scan != NULL)
{
laser_pub.publish(msg);
laser_scan_filter_->add(base_scan);
if (bag_scan_period_ > ros::WallDuration(0))
{
bag_scan_period_.sleep();
}
continue;
}
ROS_WARN_STREAM("Unsupported message type" << msg.getTopic());
}
bag.close();
double runtime = (ros::WallTime::now() - start).toSec();
ROS_INFO("Bag complete, took %.1f seconds to process, shutting down", runtime);
const geometry_msgs::Quaternion & q(last_published_pose.pose.pose.orientation);
double yaw, pitch, roll;
tf::Matrix3x3(tf::Quaternion(q.x, q.y, q.z, q.w)).getEulerYPR(yaw,pitch,roll);
ROS_INFO("Final location %.3f, %.3f, %.3f with stamp=%f",
last_published_pose.pose.pose.position.x,
last_published_pose.pose.pose.position.y,
yaw, last_published_pose.header.stamp.toSec()
);
ros::shutdown();
}