FileSeqPubCore::FileSeqPubCore(ros::NodeHandle *_n) :
it_(*_n)
{
// Initialise node parameters from launch file or command line.
// Use a private node handle so that multiple instances of the node can be run simultaneously
// while using different parameters.
ros::NodeHandle private_node_handle("~");
private_node_handle.param("rate", rate_, double(1));
private_node_handle.param("pub_image_topic_name", pub_image_topic_name_, string("cam_image"));
private_node_handle.param("pub_odom_topic_name", pub_odom_topic_name_, string("odom"));
private_node_handle.param("loop", loop_f_, bool(false));
private_node_handle.param("one_frame", one_frame_, int(-1));
string _tmppath = ros::package::getPath("sscrovers_file_sequence_publisher");
//add '/' wjen missed at the end of path
private_node_handle.param("path_to_images", path_to_images_, string(_tmppath + "/input/"));
if (*path_to_images_.rbegin() != '/') //last element get by reverse_begin iterator
path_to_images_.push_back('/');
private_node_handle.param("image_file_name_prefix", image_file_name_prefix_, string("frame_orig_"));
private_node_handle.param("start_no", start_no_, int(1000000));
//private_node_handle.param("start_no", start_no_, int(0));
private_node_handle.param("image_extension", image_extension_, string(".jpg"));
//add '.' when missed in extension
if (image_extension_[0] != '.')
image_extension_.insert(0, ".");
private_node_handle.param("path_to_trajectory", path_to_poses_, string(_tmppath + "/input/"));
//add '/' wjen missed at the end of path
if (*path_to_poses_.rbegin() != '/') //last element get by reverse_begin iterator
path_to_poses_.push_back('/');
private_node_handle.param("trajectory_file_name", trajectory_file_name_, string("trajectory.fli"));
image_pub_ = it_.advertise(pub_image_topic_name_.c_str(), 0);
odom_pub_ = _n->advertise<nav_msgs::Odometry>(pub_odom_topic_name_.c_str(), 0);
cv_img_ptr_.reset(new cv_bridge::CvImage);
step_ = -1;
loadTrajectory();
}
Evidence::Evidence(string filename, Grid &_grid, int factor)
: grid(_grid), factor_(factor) {
if (strstr(filename.c_str(), ".trajectory")) {
cout << " .trajectory file: "<<filename.c_str()<<endl;
loadTrajectory(filename, factor);
}
else if (strstr(filename.c_str(), ".track")) {
cout << "Loading .track file: "<<filename.c_str()<<endl;
loadTrack(filename, factor);
}
else if (strstr(filename.c_str(), ".sicktraj")) {
cout << "Loading .sicktraj file: "<<filename.c_str()<<endl;
loadSICKTraj(filename);
}
else {
cout << "Unexpected file format: "<<filename<<endl;
cout << "Expected either .trajectory or .track"<<endl;
exit(0);
}
}
GroundTruthOdometry::GroundTruthOdometry(const std::string & filename)
: last_utime(0)
{
loadTrajectory(filename);
}
