void OpenNIListener::setupSubscribers(){
ParameterServer* ps = ParameterServer::instance();
int q = ps->get<int>("subscriber_queue_size");
ros::NodeHandle nh;
tflistener_ = new tf::TransformListener(nh);
std::string bagfile_name = ps->get<std::string>("bagfile_name");
if(bagfile_name.empty()){
std::string visua_tpc = ps->get<std::string>("topic_image_mono");
std::string depth_tpc = ps->get<std::string>("topic_image_depth");
std::string cinfo_tpc = ps->get<std::string>("camera_info_topic");
std::string cloud_tpc = ps->get<std::string>("topic_points");
std::string widev_tpc = ps->get<std::string>("wide_topic");
std::string widec_tpc = ps->get<std::string>("wide_cloud_topic");
//All information from Kinect
if(!visua_tpc.empty() && !depth_tpc.empty() && !cloud_tpc.empty())
{
visua_sub_ = new image_sub_type(nh, visua_tpc, q);
depth_sub_ = new image_sub_type (nh, depth_tpc, q);
cloud_sub_ = new pc_sub_type (nh, cloud_tpc, q);
kinect_sync_ = new message_filters::Synchronizer<KinectSyncPolicy>(KinectSyncPolicy(q), *visua_sub_, *depth_sub_, *cloud_sub_),
kinect_sync_->registerCallback(boost::bind(&OpenNIListener::kinectCallback, this, _1, _2, _3));
ROS_INFO_STREAM_NAMED("OpenNIListener", "Listening to " << visua_tpc << ", " << depth_tpc << " and " << cloud_tpc);
}
//No cloud, but visual image and depth
else if(!visua_tpc.empty() && !depth_tpc.empty() && !cinfo_tpc.empty() && cloud_tpc.empty())
{
visua_sub_ = new image_sub_type(nh, visua_tpc, q);
depth_sub_ = new image_sub_type(nh, depth_tpc, q);
cinfo_sub_ = new cinfo_sub_type(nh, cinfo_tpc, q);
no_cloud_sync_ = new message_filters::Synchronizer<NoCloudSyncPolicy>(NoCloudSyncPolicy(q), *visua_sub_, *depth_sub_, *cinfo_sub_);
no_cloud_sync_->registerCallback(boost::bind(&OpenNIListener::noCloudCallback, this, _1, _2, _3));
ROS_INFO_STREAM_NAMED("OpenNIListener", "Listening to " << visua_tpc << " and " << depth_tpc);
}
//All information from stereo
if(!widec_tpc.empty() && !widev_tpc.empty())
{
visua_sub_ = new image_sub_type(nh, widev_tpc, q);
cloud_sub_ = new pc_sub_type(nh, widec_tpc, q);
stereo_sync_ = new message_filters::Synchronizer<StereoSyncPolicy>(StereoSyncPolicy(q), *visua_sub_, *cloud_sub_);
stereo_sync_->registerCallback(boost::bind(&OpenNIListener::stereoCallback, this, _1, _2));
ROS_INFO_STREAM_NAMED("OpenNIListener", "Listening to " << widev_tpc << " and " << widec_tpc );
}
if(ps->get<bool>("use_robot_odom")){
odom_sub_= new odom_sub_type(nh, ps->get<std::string>("odometry_tpc"), 3);
ROS_INFO_STREAM_NAMED("OpenNIListener", "Listening to odometry on " << ps->get<std::string>("odometry_tpc"));
odom_sub_->registerCallback(boost::bind(&OpenNIListener::odomCallback,this,_1));
}
}
else {
ROS_WARN("RGBDSLAM loads a bagfile - therefore doesn't subscribe to topics.");
}
}
void visual_slam::OpenNIListener::setupsubscribers(){
int q;
_node.getParam("/queue_size", q);
std::string bagfile_name;
_node.getParam("/bagfile_name", bagfile_name);
if(bagfile_name.empty()){
std::string visua_tpc, depth_tpc, cinfo_tpc, cloud_tpc;
_node.getParam("topic_image_mono", visua_tpc);
_node.getParam("topic_image_depth", depth_tpc);
_node.getParam("camera_info_topic", cinfo_tpc);
_node.getParam("topic_points", cloud_tpc);
//All information from Kinect
if(!visua_tpc.empty() && !depth_tpc.empty() && !cloud_tpc.empty())
{
visua_sub_ = new image_sub_type(_node, visua_tpc, q);
depth_sub_ = new image_sub_type (_node, depth_tpc, q);
cloud_sub_ = new pc_sub_type (_node, cloud_tpc, q);
kinect_sync_ = new message_filters::Synchronizer<KinectSyncPolicy>(KinectSyncPolicy(q), *visua_sub_, *depth_sub_, *cloud_sub_),
kinect_sync_->registerCallback(boost::bind(&visual_slam::OpenNIListener::kinectCallback, this, _1, _2, _3));
}
//No cloud, but visual image and depth
else if(!visua_tpc.empty() && !depth_tpc.empty() && !cinfo_tpc.empty() && cloud_tpc.empty())
{
visua_sub_ = new image_sub_type(_node, visua_tpc, q);
depth_sub_ = new image_sub_type(_node, depth_tpc, q);
cinfo_sub_ = new cinfo_sub_type(_node, cinfo_tpc, q);
no_cloud_sync_ = new message_filters::Synchronizer<NoCloudSyncPolicy>(NoCloudSyncPolicy(q), *visua_sub_, *depth_sub_, *cinfo_sub_);
no_cloud_sync_->registerCallback(boost::bind(&visual_slam::OpenNIListener::noCloudCallback, this, _1, _2, _3));
}
bool use_robot_odom;
_node.getParam("use_robot_odom", use_robot_odom);
if(use_robot_odom){
std::string odometry_tpc;
_node.getParam("odometry_tpc", odometry_tpc);
odom_sub_= new odom_sub_type(_node,odometry_tpc , 3);
odom_sub_->registerCallback(boost::bind(&OpenNIListener::odomCallback,this,_1));
}
}
else {
ROS_WARN("RGBDSLAM loads a bagfile - therefore doesn't subscribe to topics.");
}
}
OpenNIListener::OpenNIListener(GraphManager* graph_mgr)
: graph_mgr_(graph_mgr),
stereo_sync_(NULL), kinect_sync_(NULL), no_cloud_sync_(NULL),
visua_sub_(NULL), depth_sub_(NULL), cloud_sub_(NULL),
depth_mono8_img_(cv::Mat()),
save_bag_file(false),
pause_(ParameterServer::instance()->get<bool>("start_paused")),
getOneFrame_(false),
first_frame_(true),
data_id_(0),
num_processed_(0),
image_encoding_("rgb8")
{
ParameterServer* ps = ParameterServer::instance();
int q = ps->get<int>("subscriber_queue_size");
if(ps->get<bool>("encoding_bgr")){
image_encoding_ = "bgr8";//used in conversion to qimage. exact value does not matter, just not rgb8
}
std::string bagfile_name = ps->get<std::string>("bagfile_name");
std::string visua_tpc = ps->get<std::string>("topic_image_mono");
std::string depth_tpc = ps->get<std::string>("topic_image_depth");
std::string cinfo_tpc = ps->get<std::string>("camera_info_topic");
ros::NodeHandle nh;
tflistener_ = new tf::TransformListener(nh);
if(bagfile_name.empty()){
std::string cloud_tpc = ps->get<std::string>("topic_points");
std::string widev_tpc = ps->get<std::string>("wide_topic");
std::string widec_tpc = ps->get<std::string>("wide_cloud_topic");
//All information from Kinect
if(!visua_tpc.empty() && !depth_tpc.empty() && !cloud_tpc.empty())
{
visua_sub_ = new image_sub_type(nh, visua_tpc, q);
depth_sub_ = new image_sub_type (nh, depth_tpc, q);
cloud_sub_ = new pc_sub_type (nh, cloud_tpc, q);
kinect_sync_ = new message_filters::Synchronizer<KinectSyncPolicy>(KinectSyncPolicy(q), *visua_sub_, *depth_sub_, *cloud_sub_),
kinect_sync_->registerCallback(boost::bind(&OpenNIListener::kinectCallback, this, _1, _2, _3));
ROS_INFO_STREAM("Listening to " << visua_tpc << ", " << depth_tpc << " and " << cloud_tpc);
}
//No cloud, but visual image and depth
else if(!visua_tpc.empty() && !depth_tpc.empty() && !cinfo_tpc.empty() && cloud_tpc.empty())
{
visua_sub_ = new image_sub_type(nh, visua_tpc, q);
depth_sub_ = new image_sub_type(nh, depth_tpc, q);
cinfo_sub_ = new cinfo_sub_type(nh, cinfo_tpc, q);
no_cloud_sync_ = new message_filters::Synchronizer<NoCloudSyncPolicy>(NoCloudSyncPolicy(q), *visua_sub_, *depth_sub_, *cinfo_sub_);
no_cloud_sync_->registerCallback(boost::bind(&OpenNIListener::noCloudCallback, this, _1, _2, _3));
ROS_INFO_STREAM("Listening to " << visua_tpc << " and " << depth_tpc);
}
//All information from stereo
if(!widec_tpc.empty() && !widev_tpc.empty())
{
visua_sub_ = new image_sub_type(nh, widev_tpc, q);
cloud_sub_ = new pc_sub_type(nh, widec_tpc, q);
stereo_sync_ = new message_filters::Synchronizer<StereoSyncPolicy>(StereoSyncPolicy(q), *visua_sub_, *cloud_sub_);
stereo_sync_->registerCallback(boost::bind(&OpenNIListener::stereoCallback, this, _1, _2));
ROS_INFO_STREAM("Listening to " << widev_tpc << " and " << widec_tpc );
}
}
detector_ = cv::Ptr<cv::FeatureDetector>( createDetector(ps->get<std::string>("feature_detector_type")) );
extractor_ = cv::Ptr<cv::DescriptorExtractor>( createDescriptorExtractor(ps->get<std::string>("feature_extractor_type")) );
}
/**This function reads the sensor input from a bagfile specified in the parameter bagfile_name.
* It is meant for offline processing of each frame */
void OpenNIListener::loadBag(std::string filename)
{
ScopedTimer s(__FUNCTION__);
ros::NodeHandle nh;
ParameterServer* ps = ParameterServer::instance();
std::string visua_tpc = ps->get<std::string>("topic_image_mono");
std::string depth_tpc = ps->get<std::string>("topic_image_depth");
std::string points_tpc = ps->get<std::string>("topic_points");
std::string cinfo_tpc = ps->get<std::string>("camera_info_topic");
int q = ps->get<int>("subscriber_queue_size");
std::string tf_tpc = std::string("/tf");
tf_pub_ = nh.advertise<tf::tfMessage>(tf_tpc, 10);
//All information from Kinect
if(!visua_tpc.empty() && !depth_tpc.empty() && !cinfo_tpc.empty() && points_tpc.empty())
{
// Set up fake subscribers to capture images
depth_img_sub_ = new BagSubscriber<sensor_msgs::Image>();
rgb_img_sub_ = new BagSubscriber<sensor_msgs::Image>();
cam_info_sub_ = new BagSubscriber<sensor_msgs::CameraInfo>();
no_cloud_sync_ = new message_filters::Synchronizer<NoCloudSyncPolicy>(NoCloudSyncPolicy(q), *rgb_img_sub_, *depth_img_sub_, *cam_info_sub_);
no_cloud_sync_->registerCallback(boost::bind(&OpenNIListener::noCloudCallback, this, _1, _2, _3));
ROS_INFO_STREAM("Listening to " << visua_tpc << ", " << depth_tpc << " and " << cinfo_tpc);
}
else if(!visua_tpc.empty() && !depth_tpc.empty() && !points_tpc.empty())
{
// Set up fake subscribers to capture images
depth_img_sub_ = new BagSubscriber<sensor_msgs::Image>();
rgb_img_sub_ = new BagSubscriber<sensor_msgs::Image>();
pc_sub_ = new BagSubscriber<sensor_msgs::PointCloud2>();
kinect_sync_ = new message_filters::Synchronizer<KinectSyncPolicy>(KinectSyncPolicy(q), *rgb_img_sub_, *depth_img_sub_, *pc_sub_);
kinect_sync_->registerCallback(boost::bind(&OpenNIListener::kinectCallback, this, _1, _2, _3));
ROS_INFO_STREAM("Listening to " << visua_tpc << ", " << depth_tpc << " and " << points_tpc);
}
else {
ROS_ERROR("Missing required information: Topic names.");
ROS_ERROR_STREAM("Visual: " << visua_tpc);
ROS_ERROR_STREAM("Camera Info (Optional if Points given): " << cinfo_tpc);
ROS_ERROR_STREAM("Depth: " << depth_tpc);
ROS_ERROR_STREAM("Points (Optional if Cam Info given): " << points_tpc);
}
bool eval_landmarks = ps->get<bool>("optimize_landmarks");
ps->set<bool>("optimize_landmarks", false);
ROS_INFO("Loading Bagfile %s", filename.c_str());
Q_EMIT iamBusy(4, "Loading Bagfile", 0);
{ //bag will be destructed after this block (hopefully frees memory for the optimizer)
rosbag::Bag bag;
try{
bag.open(filename, rosbag::bagmode::Read);
} catch(rosbag::BagIOException ex) {
ROS_FATAL("Opening Bagfile %s failed: %s Quitting!", filename.c_str(), ex.what());
ros::shutdown();
return;
}
ROS_INFO("Opened Bagfile %s", filename.c_str());
// Image topics to load for bagfiles
std::vector<std::string> topics;
topics.push_back(visua_tpc);
topics.push_back(depth_tpc);
if(points_tpc.empty()){
topics.push_back(cinfo_tpc);
} else {
topics.push_back(points_tpc);
}
topics.push_back(tf_tpc);
rosbag::View view(bag, rosbag::TopicQuery(topics));
Q_EMIT iamBusy(4, "Processing Bagfile", view.size());
// Simulate sending of the messages in the bagfile
std::deque<sensor_msgs::Image::ConstPtr> vis_images;
std::deque<sensor_msgs::Image::ConstPtr> dep_images;
std::deque<sensor_msgs::CameraInfo::ConstPtr> cam_infos;
std::deque<sensor_msgs::PointCloud2::ConstPtr> pointclouds;
//ros::Time last_tf=ros::Time(0);
ros::Time last_tf=ros::Time::now();
int counter = 0;
BOOST_FOREACH(rosbag::MessageInstance const m, view)
{
Q_EMIT progress(4, "Processing Bagfile", counter++);
do{
usleep(150);
if(!ros::ok()) return;
} while(pause_);
if (m.getTopic() == visua_tpc || ("/" + m.getTopic() == visua_tpc))
{
sensor_msgs::Image::ConstPtr rgb_img = m.instantiate<sensor_msgs::Image>();
if (rgb_img) vis_images.push_back(rgb_img);
ROS_DEBUG("Found Message of %s", visua_tpc.c_str());
}
if (m.getTopic() == depth_tpc || ("/" + m.getTopic() == depth_tpc))
{
sensor_msgs::Image::ConstPtr depth_img = m.instantiate<sensor_msgs::Image>();
//if (depth_img) depth_img_sub_->newMessage(depth_img);
if (depth_img) dep_images.push_back(depth_img);
ROS_DEBUG("Found Message of %s", depth_tpc.c_str());
}
if (m.getTopic() == points_tpc || ("/" + m.getTopic() == points_tpc))
{
sensor_msgs::PointCloud2::ConstPtr pointcloud = m.instantiate<sensor_msgs::PointCloud2>();
//if (cam_info) cam_info_sub_->newMessage(cam_info);
if (pointcloud) pointclouds.push_back(pointcloud);
ROS_DEBUG("Found Message of %s", points_tpc.c_str());
}
if (m.getTopic() == cinfo_tpc || ("/" + m.getTopic() == cinfo_tpc))
{
sensor_msgs::CameraInfo::ConstPtr cam_info = m.instantiate<sensor_msgs::CameraInfo>();
//if (cam_info) cam_info_sub_->newMessage(cam_info);
if (cam_info) cam_infos.push_back(cam_info);
ROS_DEBUG("Found Message of %s", cinfo_tpc.c_str());
}
if (m.getTopic() == tf_tpc|| ("/" + m.getTopic() == tf_tpc)){
tf::tfMessage::ConstPtr tf_msg = m.instantiate<tf::tfMessage>();
if (tf_msg) {
#if ROS_VERSION_MINIMUM(1,11,0)
//use MessageEvent for indigo
#else
//if(tf_msg->transforms[0].header.frame_id == "/kinect") continue;//avoid destroying tf tree if odom is used
//prevents missing callerid warning
boost::shared_ptr<std::map<std::string, std::string> > msg_header_map = tf_msg->__connection_header;
(*msg_header_map)["callerid"] = "rgbdslam";
tf_pub_.publish(tf_msg);
ROS_DEBUG("Found Message of %s", tf_tpc.c_str());
last_tf = tf_msg->transforms[0].header.stamp;
last_tf -= ros::Duration(1.0);
#endif
}
}
while(!vis_images.empty() && vis_images.front()->header.stamp < last_tf){
rgb_img_sub_->newMessage(vis_images.front());
vis_images.pop_front();
}
while(!dep_images.empty() && dep_images.front()->header.stamp < last_tf){
depth_img_sub_->newMessage(dep_images.front());
dep_images.pop_front();
}
while(!cam_infos.empty() && cam_infos.front()->header.stamp < last_tf){
cam_info_sub_->newMessage(cam_infos.front());
cam_infos.pop_front();
}
while(!pointclouds.empty() && pointclouds.front()->header.stamp < last_tf){
pc_sub_->newMessage(pointclouds.front());
pointclouds.pop_front();
}
}
ROS_WARN_NAMED("eval", "Finished processing of Bagfile");
bag.close();
}
