C++ DataReader::time方法代码示例

int main(int argc, char **argv)
{
  ros::init(argc, argv, "log_playback");
  ros::NodeHandle nh;
  scans_pub = nh.advertise<velodyne_msgs::VelodyneScan>("/driving/velodyne/packets", 100);
  spin_pub = nh.advertise<stdr_velodyne::PointCloud>("/driving/velodyne/points", 1);
  pose_pub = nh.advertise<stdr_msgs::ApplanixPose>("/driving/ApplanixPose", 100);
  gps_pub = nh.advertise<stdr_msgs::ApplanixGPS>("/driving/ApplanixGPS", 100);
  rms_pub = nh.advertise<stdr_msgs::ApplanixRMS>("/driving/ApplanixRMS", 100);
  ladybug_pub = nh.advertise<stdr_msgs::LadybugImages>("/driving/ladybug/images", 100);
  clock_pub = nh.advertise<rosgraph_msgs::Clock>("/clock", 100);

  bpo::options_description opts_desc("Allowed options");
  opts_desc.add_options()
      ("help,h", "produce help message")
      ("rate,r", bpo::value<float>(&rate)->default_value(1.f), "multiply the publish rate by the given factor")
      ("start,s", bpo::value<float>(&start_offset)->default_value(0.f), "start arg seconds into the bag files");

  bpo::options_description hidden_opts;
  hidden_opts.add_options()
      ("logs", bpo::value< std::vector<std::string> >()->required(), "log files to load (kitti or dgc logs)");

  bpo::options_description all_opts;
  all_opts.add(opts_desc).add(hidden_opts);

  bpo::positional_options_description pos_opts_desc;
  pos_opts_desc.add("logs", -1);

  bpo::variables_map opts;
  try {
    bpo::store(bpo::command_line_parser(argc, argv).options(all_opts).positional(pos_opts_desc).run(), opts);
    if( opts.count("help") ) {
      cout << "Usage: log_playback [OPTS] logs" << endl;
      cout << endl;
      cout << opts_desc << endl;
      return 0;
    }
    bpo::notify(opts);
  }
  catch(std::exception & e) {
    ROS_FATAL_STREAM(e.what());
    cout << "Usage: log_playback [OPTS] logs" << endl;
    cout << endl;
    cout << opts_desc << endl;
    return 1;
  }

  ROS_ASSERT_MSG(rate>0, "The rate factor must be >0");
  ROS_ASSERT_MSG(start_offset>=0, "The start offset must be >0");


  // if we are playing back kitti files, we need to load the calibration files here
  // for dgc logs, we publish scans, and so the calibration files will be loaded
  // by the velodyne processor node.

  bool kitti = false;
  BOOST_FOREACH(const std::string &log, opts["logs"].as< std::vector<std::string> >()) {
    if( boost::algorithm::ends_with(log, ".kit") || boost::algorithm::ends_with(log, ".imu") )
      kitti = true;
  }

  if( kitti ) {
    stdr_velodyne::Configuration::Ptr config =
        stdr_velodyne::Configuration::getStaticConfigurationInstance();

    std::string calibration_file;
    if( ! ros::param::get("/driving/velodyne/cal_file", calibration_file) )
      BOOST_THROW_EXCEPTION(stdr::ex::ExceptionBase() <<stdr::ex::MsgInfo(
                              "You must provide a configuration file, either on the command line, or as a rosparam."));
    config->readCalibration(calibration_file);

    std::string intensity_file;
    if( ! ros::param::get("/driving/velodyne/int_file", intensity_file) )
      BOOST_THROW_EXCEPTION(stdr::ex::ExceptionBase() <<stdr::ex::MsgInfo(
                              "You must provide an intensity configuration file, either on the command line, or as a rosparam."));
    config->readIntensity(intensity_file);
  }



  data_reader.load( opts["logs"].as< std::vector<std::string> >() );

  // advance into the files until we reach the desired start time
  start_time = data_reader.time();
  const ros::Time first_time = start_time + ros::Duration(start_offset);
  while( data_reader.ok() && nh.ok() && !signaled && data_reader.time() < first_time )
    data_reader.next();

  ros::WallTime paused_time;
  bool read_ok = true;
  signal(SIGINT, sighandler);
  setupTerminal();

  while ( (paused || read_ok) && nh.ok() && !signaled )
  {

    bool charsleftorpaused = true;
    while ( charsleftorpaused && nh.ok() && read_ok && !signaled )
    {
      switch (readCharFromStdin()) {
//.........这里部分代码省略.........