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()) { case ' ': paused = !paused; if (paused) { paused_time = ros::WallTime::now(); } else { last_wall_time += ros::WallTime::now() - paused_time; } break; case 's': if (paused) { doPublish(); printTime(); read_ok = data_reader.next(); } break; case EOF: if (paused) { printTime(); ros::WallTime::sleepUntil(ros::WallTime::now()+ros::WallDuration(.1)); } else charsleftorpaused = false; } } if( !paused ) { doPublish(); printTime(); read_ok = data_reader.next(); } } restoreTerminal(); puts(""); return 0; }