StereoVslamNode(const std::string& vocab_tree_file, const std::string& vocab_weights_file,
const std::string& calonder_trees_file)
: it_(nh_), sync_(3),
vslam_system_(vocab_tree_file, vocab_weights_file),
detector_(new vslam_system::AnyDetector)
{
// Use calonder descriptor
typedef cv::CalonderDescriptorExtractor<float> Calonder;
vslam_system_.frame_processor_.setFrameDescriptor(new Calonder(calonder_trees_file));
// Advertise outputs
cam_marker_pub_ = nh_.advertise<visualization_msgs::Marker>("/vslam/cameras", 1);
point_marker_pub_ = nh_.advertise<visualization_msgs::Marker>("/vslam/points", 1);
vo_tracks_pub_ = it_.advertiseCamera("/vslam/vo_tracks/image", 1);
odom_pub_ = nh_.advertise<nav_msgs::Odometry>("/vo", 1);
pointcloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/vslam/pointcloud", 1);
// Synchronize inputs
l_image_sub_.subscribe(it_, "left/image_rect", 1);
l_info_sub_ .subscribe(nh_, "left/camera_info", 1);
r_image_sub_.subscribe(it_, "right/image_rect", 1);
r_info_sub_ .subscribe(nh_, "right/camera_info", 1);
sync_.connectInput(l_image_sub_, l_info_sub_, r_image_sub_, r_info_sub_);
sync_.registerCallback( boost::bind(&StereoVslamNode::imageCb, this, _1, _2, _3, _4) );
// Dynamic reconfigure for parameters
ReconfigureServer::CallbackType f = boost::bind(&StereoVslamNode::configCb, this, _1, _2);
reconfigure_server_.setCallback(f);
}
void checkInputsSynchronized()
{
int threshold = 3 * all_received_;
if (left_received_ >= threshold || right_received_ >= threshold ||
left_info_received_ >= threshold || right_info_received_ >= threshold) {
ROS_WARN("[stereo_processor] Low number of synchronized left/right/left_info/right_info tuples received.\n"
"Left images received: %d (topic '%s')\n"
"Right images received: %d (topic '%s')\n"
"Left camera info received: %d (topic '%s')\n"
"Right camera info received: %d (topic '%s')\n"
"Synchronized tuples: %d\n"
"Possible issues:\n"
"\t* stereo_image_proc is not running.\n"
"\t Does `rosnode info %s` show any connections?\n"
"\t* The cameras are not synchronized.\n"
"\t Try restarting the node with parameter _approximate_sync:=True\n"
"\t* The network is too slow. One or more images are dropped from each tuple.\n"
"\t Try restarting the node, increasing parameter 'queue_size' (currently %d)",
left_received_, left_sub_.getTopic().c_str(),
right_received_, right_sub_.getTopic().c_str(),
left_info_received_, left_info_sub_.getTopic().c_str(),
right_info_received_, right_info_sub_.getTopic().c_str(),
all_received_, ros::this_node::getName().c_str(), queue_size_);
}
}
/**
* Constructor, subscribes to input topics using image transport and registers
* callbacks.
* \param transport The image transport to use
*/
StereoProcessor(const std::string& transport) :
left_received_(0), right_received_(0), left_info_received_(0), right_info_received_(0), all_received_(0)
{
// Read local parameters
ros::NodeHandle local_nh("~");
// Resolve topic names
ros::NodeHandle nh;
std::string stereo_ns = nh.resolveName("stereo");
std::string left_topic = ros::names::clean(stereo_ns + "/left/" + nh.resolveName("image"));
std::string right_topic = ros::names::clean(stereo_ns + "/right/" + nh.resolveName("image"));
std::string left_info_topic = stereo_ns + "/left/camera_info";
std::string right_info_topic = stereo_ns + "/right/camera_info";
// Subscribe to four input topics.
ROS_INFO("Subscribing to:\n\t* %s\n\t* %s\n\t* %s\n\t* %s",
left_topic.c_str(), right_topic.c_str(),
left_info_topic.c_str(), right_info_topic.c_str());
image_transport::ImageTransport it(nh);
left_sub_.subscribe(it, left_topic, 1, transport);
right_sub_.subscribe(it, right_topic, 1, transport);
left_info_sub_.subscribe(nh, left_info_topic, 1);
right_info_sub_.subscribe(nh, right_info_topic, 1);
// Complain every 15s if the topics appear unsynchronized
left_sub_.registerCallback(boost::bind(StereoProcessor::increment, &left_received_));
right_sub_.registerCallback(boost::bind(StereoProcessor::increment, &right_received_));
left_info_sub_.registerCallback(boost::bind(StereoProcessor::increment, &left_info_received_));
right_info_sub_.registerCallback(boost::bind(StereoProcessor::increment, &right_info_received_));
check_synced_timer_ = nh.createWallTimer(ros::WallDuration(15.0),
boost::bind(&StereoProcessor::checkInputsSynchronized, this));
// Synchronize input topics. Optionally do approximate synchronization.
local_nh.param("queue_size", queue_size_, 5);
bool approx;
local_nh.param("approximate_sync", approx, false);
if (approx)
{
approximate_sync_.reset(new ApproximateSync(ApproximatePolicy(queue_size_),
left_sub_, right_sub_, left_info_sub_, right_info_sub_) );
approximate_sync_->registerCallback(boost::bind(&StereoProcessor::dataCb, this, _1, _2, _3, _4));
}
else
{
exact_sync_.reset(new ExactSync(ExactPolicy(queue_size_),
left_sub_, right_sub_, left_info_sub_, right_info_sub_) );
exact_sync_->registerCallback(boost::bind(&StereoProcessor::dataCb, this, _1, _2, _3, _4));
}
}
/**
* Constructor, subscribes to input topics using image transport and registers
* callbacks.
* \param transport The image transport to use
*/
MonoDepthProcessor(const std::string& transport) :
image_received_(0), depth_received_(0), image_info_received_(0), depth_info_received_(0), all_received_(0)
{
// Read local parameters
ros::NodeHandle local_nh("~");
// Resolve topic names
ros::NodeHandle nh;
std::string camera_ns = nh.resolveName("camera");
std::string image_topic = ros::names::clean(camera_ns + "/rgb/image_rect");
std::string depth_topic = ros::names::clean(camera_ns + "/depth_registered/image_rect");
std::string image_info_topic = camera_ns + "/rgb/camera_info";
std::string depth_info_topic = camera_ns + "/depth_registered/camera_info";
// Subscribe to four input topics.
ROS_INFO("Subscribing to:\n\t* %s\n\t* %s\n\t* %s\n\t* %s",
image_topic.c_str(), depth_topic.c_str(),
image_info_topic.c_str(), depth_info_topic.c_str());
image_transport::ImageTransport it(nh);
image_sub_.subscribe(it, image_topic, 1, transport);
depth_sub_.subscribe(it, depth_topic, 1, transport);
image_info_sub_.subscribe(nh, image_info_topic, 1);
depth_info_sub_.subscribe(nh, depth_info_topic, 1);
// Complain every 15s if the topics appear unsynchronized
image_sub_.registerCallback(boost::bind(MonoDepthProcessor::increment, &image_received_));
depth_sub_.registerCallback(boost::bind(MonoDepthProcessor::increment, &depth_received_));
image_info_sub_.registerCallback(boost::bind(MonoDepthProcessor::increment, &image_info_received_));
depth_info_sub_.registerCallback(boost::bind(MonoDepthProcessor::increment, &depth_info_received_));
check_synced_timer_ = nh.createWallTimer(ros::WallDuration(15.0),
boost::bind(&MonoDepthProcessor::checkInputsSynchronized, this));
// Synchronize input topics. Optionally do approximate synchronization.
local_nh.param("queue_size", queue_size_, 5);
bool approx;
local_nh.param("approximate_sync", approx, true);
if (approx)
{
approximate_sync_.reset(new ApproximateSync(ApproximatePolicy(queue_size_),
image_sub_, depth_sub_, image_info_sub_, depth_info_sub_) );
approximate_sync_->registerCallback(boost::bind(&MonoDepthProcessor::dataCb, this, _1, _2, _3, _4));
}
else
{
exact_sync_.reset(new ExactSync(ExactPolicy(queue_size_),
image_sub_, depth_sub_, image_info_sub_, depth_info_sub_) );
exact_sync_->registerCallback(boost::bind(&MonoDepthProcessor::dataCb, this, _1, _2, _3, _4));
}
}
// Constructor
PcPublisher()
: image_transport_(n_),
tof_sync_(SyncPolicy(3)),
c_xyz_image_32F3_(0),
c_grey_image_32F1_(0)
{
topicPub_pointCloud_ = n_.advertise<sensor_msgs::PointCloud>("point_cloud", 1);
xyz_image_subscriber_.subscribe(image_transport_, "image_xyz", 1);
grey_image_subscriber_.subscribe(image_transport_,"image_grey", 1);
tof_sync_.connectInput(xyz_image_subscriber_, grey_image_subscriber_);
tof_sync_.registerCallback(boost::bind(&PcPublisher::syncCallback, this, _1, _2));
// topicSub_xyzImage_ = n_.subscribe("xyz_image", 1, &PcPublisher::topicCallback_xyzImage, this);
}
/////////////////////////////////////////////////////////////////
// Constructor
PedestrianDetectorHOG(ros::NodeHandle nh):
nh_(nh),
local_nh_("~"),
it_(nh_),
stereo_sync_(4),
// cam_model_(NULL),
counter(0)
{
hog_.setSVMDetector(cv::HOGDescriptor::getDefaultPeopleDetector());
// Get parameters from the server
local_nh_.param("hit_threshold",hit_threshold_,0.0);
local_nh_.param("group_threshold",group_threshold_,2);
local_nh_.param("use_depth", use_depth_, true);
local_nh_.param("use_height",use_height_,true);
local_nh_.param("max_height_m",max_height_m_,2.2);
local_nh_.param("ground_frame",ground_frame_,std::string("base_link"));
local_nh_.param("do_display", do_display_, true);
// Advertise a 3d position measurement for each head.
people_pub_ = nh_.advertise<people_msgs::PositionMeasurement>("people_tracker_measurements",1);
// Subscribe to tf & the images
if (use_depth_) {
tf_.setExtrapolationLimit(ros::Duration().fromSec(0.01));
std::string left_topic = nh_.resolveName("stereo") + "/left/image";
std::string disp_topic = nh_.resolveName("stereo") + "/disparity";
std::string left_info_topic = nh_.resolveName("stereo") + "/left/camera_info";
std::string right_info_topic = nh_.resolveName("stereo") + "/right/camera_info";
left_sub_.subscribe(it_, left_topic, 10);
disp_sub_.subscribe(it_, disp_topic, 10);
left_info_sub_.subscribe(nh_, left_info_topic, 10);
right_info_sub_.subscribe(nh_, right_info_topic, 10);
stereo_sync_.connectInput(left_sub_, left_info_sub_, disp_sub_, right_info_sub_);
stereo_sync_.registerCallback(boost::bind(&PedestrianDetectorHOG::imageCBAll, this, _1, _2, _3, _4));
}
else {
std::string topic = nh_.resolveName("image");
image_sub_ = it_.subscribe("image",10,&PedestrianDetectorHOG::imageCB,this);
}
}
void checkInputsSynchronized()
{
int threshold = 3 * all_received_;
if (image_received_ >= threshold || depth_received_ >= threshold ||
image_info_received_ >= threshold || depth_info_received_ >= threshold) {
ROS_WARN("[stereo_processor] Low number of synchronized image/depth/image_info/depth_info tuples received.\n"
"Images received: %d (topic '%s')\n"
"Depth images received: %d (topic '%s')\n"
"Image camera info received: %d (topic '%s')\n"
"Depth camera info received: %d (topic '%s')\n"
"Synchronized tuples: %d\n",
image_received_, image_sub_.getTopic().c_str(),
depth_received_, depth_sub_.getTopic().c_str(),
image_info_received_, image_info_sub_.getTopic().c_str(),
depth_info_received_, depth_info_sub_.getTopic().c_str(),
all_received_);
}
}
void connectCb()
{
num_subs++;
if(num_subs > 0)
{
color_image_sub_.subscribe(image_transport_,"/camera/rgb/image_raw", 1);
pc_sub_.subscribe(n_, "/camera/depth/points", 1);
}
}
void disconnectCb()
{
num_subs--;
if(num_subs <= 0)
{
color_image_sub_.unsubscribe();
pc_sub_.unsubscribe();
}
}
void connectCb()
{
num_subs++;
if(num_subs > 0)
{
color_image_sub_.subscribe(image_transport_,"image_color", 1);
pc_sub_.subscribe(n_, "point_cloud2", 1);
}
}
unsigned long init()
{
color_camera_image_sub_.subscribe(*it_, "colorimage", 1);
point_cloud_sub_.subscribe(node_handle_, "pointcloud", 1);
sync_pointcloud_->connectInput(point_cloud_sub_, color_camera_image_sub_);
sync_pointcloud_callback_connection_ = sync_pointcloud_->registerCallback(boost::bind(&CobKinectImageFlipNodelet::inputCallback, this, _1, _2));
return ipa_Utils::RET_OK;
}
void initNode()
{
pc_sync_ = boost::shared_ptr<message_filters::Synchronizer<SyncPolicy> >(new message_filters::Synchronizer<SyncPolicy>(SyncPolicy(3)));
pc_pub_ = n_.advertise<sensor_msgs::PointCloud2>("colored_point_cloud2", 1);
color_image_sub_.subscribe(image_transport_,"image_color", 1);
pc_sub_.subscribe(n_, "point_cloud2", 1);
pc_sync_->connectInput(color_image_sub_, pc_sub_);
pc_sync_->registerCallback(boost::bind(&CreateColoredPointCloud::syncCallback, this, _1, _2));
}
StereoSynchronizer () : nh_(), it_(nh_), sync_(15) {
std::string left_raw = nh_.resolveName("left_raw");
std::string right_raw = nh_.resolveName("right_raw");
image_sub_l_.subscribe(it_, left_raw + "/image_raw", 4);
info_sub_l_ .subscribe(nh_, left_raw + "/camera_info", 4);
image_sub_r_.subscribe(it_, right_raw + "/image_raw", 4);
info_sub_r_ .subscribe(nh_, right_raw + "/camera_info", 4);
left_ns_ = nh_.resolveName("left");
right_ns_ = nh_.resolveName("right");
ros::NodeHandle cam_l_nh(left_ns_);
ros::NodeHandle cam_r_nh(right_ns_);
it_l_ = new image_transport::ImageTransport(cam_l_nh);
it_r_ = new image_transport::ImageTransport(cam_r_nh);
pub_left_ = it_l_->advertiseCamera("image_raw", 1);
pub_right_ = it_r_->advertiseCamera("image_raw", 1);
sync_.connectInput(image_sub_l_, info_sub_l_, image_sub_r_, info_sub_r_);
sync_.registerCallback(boost::bind(&StereoSynchronizer::imageCB, this, _1, _2, _3, _4));
}
// Handles (un)subscribing when clients (un)subscribe
void DisparityWideNodelet::connectCb()
{
boost::lock_guard<boost::mutex> lock(connect_mutex_);
if (pub_disparity_.getNumSubscribers() == 0)
{
sub_l_image_.unsubscribe();
sub_l_info_ .unsubscribe();
sub_r_image_.unsubscribe();
sub_r_info_ .unsubscribe();
}
else if (!sub_l_image_.getSubscriber())
{
ros::NodeHandle &nh = getNodeHandle();
// Queue size 1 should be OK; the one that matters is the synchronizer queue size.
/// @todo Allow remapping left, right?
image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
sub_l_image_.subscribe(*it_, "left_wide/image_rect", 1, hints);
sub_l_info_ .subscribe(nh, "left_wide/camera_info", 1);
sub_r_image_.subscribe(*it_, "right/image_rect", 1, hints);
sub_r_info_ .subscribe(nh, "right/camera_info", 1);
}
}
/// Unsubscribe from camera topics if possible.
void disconnectCallback()
{
if (sub_counter_ > 0)
{
ROS_INFO("[fiducials] Unsubscribing from camera topics");
color_camera_image_sub_.unsubscribe();
color_camera_info_sub_.unsubscribe();
sub_counter_--;
ROS_INFO("[fiducials] %i subscribers on camera topics [OK]", sub_counter_);
}
}
/// Unsubscribe from camera topics if possible.
void disconnectCallback()
{
sub_counter_--;
if (sub_counter_ == 0)
{
ROS_DEBUG("[all_camera_viewer] Unsubscribing from camera topics");
if (use_right_color_camera_)
{
right_color_camera_image_sub_.unsubscribe();
right_camera_info_sub_.unsubscribe();
}
if (use_left_color_camera_)
{
left_color_camera_image_sub_.unsubscribe();
left_camera_info_sub_.unsubscribe();
}
if (use_tof_camera_)
{
tof_camera_grey_image_sub_.unsubscribe();
}
}
}
/// Subscribe to camera topics if not already done.
void connectCallback()
{
if (sub_counter_ == 0)
{
sub_counter_++;
ROS_DEBUG("[all_camera_viewer] Subscribing to camera topics");
if (use_right_color_camera_)
{
right_color_camera_image_sub_.subscribe(image_transport_, "right/image_color", 1);
right_camera_info_sub_.subscribe(node_handle_, "right/camera_info", 1);
}
if (use_left_color_camera_)
{
left_color_camera_image_sub_.subscribe(image_transport_, "left/image_color", 1);
left_camera_info_sub_.subscribe(node_handle_, "left/camera_info", 1);
}
if (use_tof_camera_)
{
tof_camera_grey_image_sub_.subscribe(image_transport_, "image_grey", 1);
}
}
}
/// Subscribe to camera topics if not already done.
void connectCallback()
{
ROS_INFO("[fiducials] Subscribing to camera topics");
color_camera_image_sub_.subscribe(*image_transport_0_, "image_color", 1);
color_camera_info_sub_.subscribe(node_handle_, "camera_info", 1);
color_image_sub_sync_ = boost::shared_ptr<message_filters::Synchronizer<ColorImageSyncPolicy> >(new message_filters::Synchronizer<ColorImageSyncPolicy>(ColorImageSyncPolicy(3)));
color_image_sub_sync_->connectInput(color_camera_image_sub_, color_camera_info_sub_);
color_image_sub_sync_->registerCallback(boost::bind(&CobFiducialsNode::colorImageCallback, this, _1, _2));
sub_counter_++;
ROS_INFO("[fiducials] %i subscribers on camera topics [OK]", sub_counter_);
}
// Connection callback that unsubscribes from the tracker if no one is subscribed.
void connectCallback(ros::Subscriber &sub_msg,
ros::NodeHandle &n,
string gp_topic,
string img_topic,
Subscriber<GroundPlane> &sub_gp,
Subscriber<CameraInfo> &sub_cam,
image_transport::SubscriberFilter &sub_col,
image_transport::ImageTransport &it){
if(!pub_message.getNumSubscribers() && !pub_result_image.getNumSubscribers() && !pub_detected_persons.getNumSubscribers()) {
ROS_DEBUG("HOG: No subscribers. Unsubscribing.");
sub_msg.shutdown();
sub_gp.unsubscribe();
sub_cam.unsubscribe();
sub_col.unsubscribe();
} else {
ROS_DEBUG("HOG: New subscribers. Subscribing.");
if(strcmp(gp_topic.c_str(), "") == 0) {
sub_msg = n.subscribe(img_topic.c_str(), 1, &imageCallback);
}
sub_cam.subscribe();
sub_gp.subscribe();
sub_col.subscribe(it,sub_col.getTopic().c_str(),1);
}
}
// Handles (un)subscribing when clients (un)subscribe
void PointCloud2Nodelet::connectCb()
{
boost::lock_guard<boost::mutex> lock(connect_mutex_);
if (pub_points2_.getNumSubscribers() == 0)
{
sub_l_image_ .unsubscribe();
sub_l_info_ .unsubscribe();
sub_r_info_ .unsubscribe();
sub_disparity_.unsubscribe();
}
else if (!sub_l_image_.getSubscriber())
{
ros::NodeHandle &nh = getNodeHandle();
// Queue size 1 should be OK; the one that matters is the synchronizer queue size.
sub_l_image_ .subscribe(*it_, "left/image_rect_color", 1);
sub_l_info_ .subscribe(nh, "left/camera_info", 1);
sub_r_info_ .subscribe(nh, "right/camera_info", 1);
sub_disparity_.subscribe(nh, "disparity", 1);
}
}
// Connection callback that unsubscribes from the tracker if no one is subscribed.
void connectCallback(message_filters::Subscriber<CameraInfo> &sub_cam,
message_filters::Subscriber<GroundPlane> &sub_gp,
image_transport::SubscriberFilter &sub_col,
image_transport::SubscriberFilter &sub_dep,
image_transport::ImageTransport &it) {
if(!pub_message.getNumSubscribers() && !pub_result_image.getNumSubscribers() && !pub_centres.getNumSubscribers() && !pub_detected_persons.getNumSubscribers()) {
ROS_DEBUG("Upper Body Detector: No subscribers. Unsubscribing.");
sub_cam.unsubscribe();
sub_gp.unsubscribe();
sub_col.unsubscribe();
sub_dep.unsubscribe();
} else {
ROS_DEBUG("Upper Body Detector: New subscribers. Subscribing.");
sub_cam.subscribe();
sub_gp.subscribe();
sub_col.subscribe(it,sub_col.getTopic().c_str(),1);
sub_dep.subscribe(it,sub_dep.getTopic().c_str(),1);
}
}
