void FlashStream::dataHandler(DataReader& data, double lostRate) {
if(!_pPublication) {
ERROR("a data packet has been received on a no publishing stream ",id,", certainly a publication currently closing");
return;
}
// necessary AMF0 here!
if (*data.packet.current() == AMF_STRING && *(data.packet.current() + 3) == '@') {
if (*(data.packet.current() + 1) == 0) {
if (*(data.packet.current() + 2) == 13 && memcmp(data.packet.current() + 3, EXPAND("@setDataFrame"))==0) {
// @setDataFrame
data.next();
_pPublication->writeProperties(data);
return;
} else if (*(data.packet.current() + 2) == 15 && memcmp(data.packet.current() + 3, EXPAND("@clearDataFrame"))==0) {
// @clearDataFrame
_pPublication->clearProperties();
return;
}
}
}
_pPublication->pushData(data,peer.ping(),lostRate);
}
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;
}
