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);
}
ProsilicaNode(const ros::NodeHandle& node_handle)
: nh_(node_handle),
it_(nh_),
cam_(NULL), running_(false), auto_adjust_stream_bytes_per_second_(false),
count_(0),
frames_dropped_total_(0), frames_completed_total_(0),
frames_dropped_acc_(WINDOW_SIZE),
frames_completed_acc_(WINDOW_SIZE),
packets_missed_total_(0), packets_received_total_(0),
packets_missed_acc_(WINDOW_SIZE),
packets_received_acc_(WINDOW_SIZE)
{
// Two-stage initialization: in the constructor we open the requested camera. Most
// parameters controlling capture are set and streaming started in configure(), the
// callback to dynamic_reconfig.
prosilica::init();
if (prosilica::numCameras() == 0)
ROS_WARN("Found no cameras on local subnet");
// Determine which camera to use. Opening by IP address is preferred, then guid. If both
// parameters are set we open by IP and verify the guid. If neither are set we default
// to opening the first available camera.
ros::NodeHandle local_nh("~");
unsigned long guid = 0;
std::string guid_str;
if (local_nh.getParam("guid", guid_str) && !guid_str.empty())
guid = strtol(guid_str.c_str(), NULL, 0);
std::string ip_str;
if (local_nh.getParam("ip_address", ip_str) && !ip_str.empty()) {
cam_.reset( new prosilica::Camera(ip_str.c_str()) );
// Verify guid is the one expected
unsigned long cam_guid = cam_->guid();
if (guid != 0 && guid != cam_guid)
throw prosilica::ProsilicaException(ePvErrBadParameter,
"guid does not match expected");
guid = cam_guid;
}
else {
if (guid == 0) guid = prosilica::getGuid(0);
cam_.reset( new prosilica::Camera(guid) );
}
hw_id_ = boost::lexical_cast<std::string>(guid);
ROS_INFO("Found camera, guid = %s", hw_id_.c_str());
diagnostic_.setHardwareID(hw_id_);
// Record some attributes of the camera
tPvUint32 dummy;
PvAttrRangeUint32(cam_->handle(), "Width", &dummy, &sensor_width_);
PvAttrRangeUint32(cam_->handle(), "Height", &dummy, &sensor_height_);
// Try to load intrinsics from on-camera memory.
loadIntrinsics();
// Set up self tests and diagnostics.
// NB: Need to wait until here to construct self_test_, otherwise an exception
// above from failing to find the camera gives bizarre backtraces
// (http://answers.ros.org/question/430/trouble-with-prosilica_camera-pvapi).
self_test_.reset(new self_test::TestRunner);
self_test_->add( "Info Test", this, &ProsilicaNode::infoTest );
self_test_->add( "Attribute Test", this, &ProsilicaNode::attributeTest );
self_test_->add( "Image Test", this, &ProsilicaNode::imageTest );
diagnostic_.add( "Frequency Status", this, &ProsilicaNode::freqStatus );
diagnostic_.add( "Frame Statistics", this, &ProsilicaNode::frameStatistics );
diagnostic_.add( "Packet Statistics", this, &ProsilicaNode::packetStatistics );
diagnostic_.add( "Packet Error Status", this, &ProsilicaNode::packetErrorStatus );
diagnostic_timer_ = nh_.createTimer(ros::Duration(0.1), boost::bind(&ProsilicaNode::runDiagnostics, this));
// Service call for setting calibration.
set_camera_info_srv_ = nh_.advertiseService("set_camera_info", &ProsilicaNode::setCameraInfo, this);
// Start dynamic_reconfigure
reconfigure_server_.setCallback(boost::bind(&ProsilicaNode::configure, this, _1, _2));
}
