VisualOdometry::VisualOdometry(
const ros::NodeHandle& nh,
const ros::NodeHandle& nh_private):
nh_(nh),
nh_private_(nh_private),
initialized_(false),
frame_count_(0)
{
ROS_INFO("Starting RGBD Visual Odometry");
// **** initialize ROS parameters
initParams();
// **** inititialize state variables
f2b_.setIdentity();
// **** publishers
odom_publisher_ = nh_.advertise<OdomMsg>(
"vo", queue_size_);
pose_stamped_publisher_ = nh_.advertise<geometry_msgs::PoseStamped>(
"pose", queue_size_);
path_pub_ = nh_.advertise<PathMsg>(
"path", queue_size_);
feature_cloud_publisher_ = nh_.advertise<PointCloudFeature>(
"feature/cloud", 1);
feature_cov_publisher_ = nh_.advertise<visualization_msgs::Marker>(
"feature/covariances", 1);
model_cloud_publisher_ = nh_.advertise<PointCloudFeature>(
"model/cloud", 1);
model_cov_publisher_ = nh_.advertise<visualization_msgs::Marker>(
"model/covariances", 1);
reset_pos_ = nh_.advertiseService(
"reset_pos_", &VisualOdometry::resetposSrvCallback, this);
// **** subscribers
ImageTransport rgb_it(nh_);
ImageTransport depth_it(nh_);
sub_rgb_.subscribe(rgb_it, "/camera/rgb/image_rect_color", queue_size_);
sub_depth_.subscribe(depth_it, "/camera/depth_registered/image_rect_raw", queue_size_);
sub_info_.subscribe(nh_, "/camera/rgb/camera_info", queue_size_);
// Synchronize inputs.
sync_.reset(new RGBDSynchronizer3(
RGBDSyncPolicy3(queue_size_), sub_rgb_, sub_depth_, sub_info_));
sync_->registerCallback(boost::bind(&VisualOdometry::RGBDCallback, this, _1, _2, _3));
}
virtual void onInit()
{
ros::NodeHandle& nh = getNodeHandle();
ros::NodeHandle& private_nh = getPrivateNodeHandle();
ros::NodeHandle rgb_nh(nh, "rgb");
ros::NodeHandle depth_nh(nh, "depth");
ros::NodeHandle rgb_pnh(private_nh, "rgb");
ros::NodeHandle depth_pnh(private_nh, "depth");
image_transport::ImageTransport rgb_it(rgb_nh);
image_transport::ImageTransport depth_it(depth_nh);
image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
int queueSize = 10;
bool approxSync = true;
if(private_nh.getParam("max_rate", rate_))
{
NODELET_WARN("\"max_rate\" is now known as \"rate\".");
}
private_nh.param("rate", rate_, rate_);
private_nh.param("queue_size", queueSize, queueSize);
private_nh.param("approx_sync", approxSync, approxSync);
private_nh.param("decimation", decimation_, decimation_);
ROS_ASSERT(decimation_ >= 1);
NODELET_INFO("Rate=%f Hz", rate_);
NODELET_INFO("Decimation=%d", decimation_);
NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), image_sub_, image_depth_sub_, info_sub_);
approxSync_->registerCallback(boost::bind(&DataThrottleNodelet::callback, this, _1, _2, _3));
}
else
{
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), image_sub_, image_depth_sub_, info_sub_);
exactSync_->registerCallback(boost::bind(&DataThrottleNodelet::callback, this, _1, _2, _3));
}
image_sub_.subscribe(rgb_it, rgb_nh.resolveName("image_in"), 1, hintsRgb);
image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image_in"), 1, hintsDepth);
info_sub_.subscribe(rgb_nh, "camera_info_in", 1);
imagePub_ = rgb_it.advertise("image_out", 1);
imageDepthPub_ = depth_it.advertise("image_out", 1);
infoPub_ = rgb_nh.advertise<sensor_msgs::CameraInfo>("camera_info_out", 1);
};
FeatureViewer::FeatureViewer(
const ros::NodeHandle& nh,
const ros::NodeHandle& nh_private):
nh_(nh),
nh_private_(nh_private),
frame_count_(0)
{
ROS_INFO("Starting RGBD Feature Viewer");
// dynamic reconfigure
FeatureDetectorConfigServer::CallbackType f = boost::bind(
&FeatureViewer::reconfigCallback, this, _1, _2);
config_server_.setCallback(f);
// **** initialize ROS parameters
initParams();
mutex_.lock();
resetDetector();
mutex_.unlock();
// **** publishers
cloud_publisher_ = nh_.advertise<PointCloudFeature>(
"feature/cloud", 1);
covariances_publisher_ = nh_.advertise<visualization_msgs::Marker>(
"feature/covariances", 1);
// **** subscribers
ImageTransport rgb_it(nh_);
ImageTransport depth_it(nh_);
sub_rgb_.subscribe(rgb_it, "/rgbd/rgb", queue_size_);
sub_depth_.subscribe(depth_it, "/rgbd/depth", queue_size_);
sub_info_.subscribe(nh_, "/rgbd/info", queue_size_);
// Synchronize inputs.
sync_.reset(new RGBDSynchronizer3(
RGBDSyncPolicy3(queue_size_), sub_rgb_, sub_depth_, sub_info_));
sync_->registerCallback(boost::bind(&FeatureViewer::RGBDCallback, this, _1, _2, _3));
}
virtual void onInit()
{
ros::NodeHandle & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle();
int queueSize = 10;
bool approxSync = true;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("queue_size", queueSize, queueSize);
NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
rgbdImagePub_ = nh.advertise<rtabmap_ros::RGBDImage>("rgbd_image", 1);
rgbdImageCompressedPub_ = nh.advertise<rtabmap_ros::RGBDImage>("rgbd_image/compressed", 1);
if(approxSync)
{
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
approxSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, _1, _2, _3));
}
else
{
exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
exactSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, _1, _2, _3));
}
ros::NodeHandle rgb_nh(nh, "rgb");
ros::NodeHandle depth_nh(nh, "depth");
ros::NodeHandle rgb_pnh(pnh, "rgb");
ros::NodeHandle depth_pnh(pnh, "depth");
image_transport::ImageTransport rgb_it(rgb_nh);
image_transport::ImageTransport depth_it(depth_nh);
image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
}
void DriverNodelet::onInitImpl ()
{
ros::NodeHandle& nh = getNodeHandle(); // topics
ros::NodeHandle& param_nh = getPrivateNodeHandle(); // parameters
// Allow remapping namespaces rgb, ir, depth, depth_registered
image_transport::ImageTransport it(nh);
ros::NodeHandle rgb_nh(nh, "rgb");
image_transport::ImageTransport rgb_it(rgb_nh);
ros::NodeHandle ir_nh(nh, "ir");
image_transport::ImageTransport ir_it(ir_nh);
ros::NodeHandle depth_nh(nh, "depth");
image_transport::ImageTransport depth_it(depth_nh);
ros::NodeHandle depth_registered_nh(nh, "depth_registered");
image_transport::ImageTransport depth_registered_it(depth_registered_nh);
ros::NodeHandle projector_nh(nh, "projector");
rgb_frame_counter_ = depth_frame_counter_ = ir_frame_counter_ = 0;
publish_rgb_ = publish_ir_ = publish_depth_ = true;
// Check to see if we should enable debugging messages in libfreenect
// libfreenect_debug_ should be set before calling setupDevice
param_nh.param("debug" , libfreenect_debug_, false);
// Initialize the sensor, but don't start any streams yet. That happens in the connection callbacks.
updateModeMaps();
setupDevice();
// Initialize dynamic reconfigure
reconfigure_server_.reset( new ReconfigureServer(param_nh) );
reconfigure_server_->setCallback(boost::bind(&DriverNodelet::configCb, this, _1, _2));
// Camera TF frames
param_nh.param("rgb_frame_id", rgb_frame_id_, string("/openni_rgb_optical_frame"));
param_nh.param("depth_frame_id", depth_frame_id_, string("/openni_depth_optical_frame"));
NODELET_INFO("rgb_frame_id = '%s' ", rgb_frame_id_.c_str());
NODELET_INFO("depth_frame_id = '%s' ", depth_frame_id_.c_str());
// Pixel offset between depth and IR images.
// By default assume offset of (5,4) from 9x7 correlation window.
// NOTE: These are now (temporarily?) dynamically reconfigurable, to allow tweaking.
//param_nh.param("depth_ir_offset_x", depth_ir_offset_x_, 5.0);
//param_nh.param("depth_ir_offset_y", depth_ir_offset_y_, 4.0);
// The camera names are set to [rgb|depth]_[serial#], e.g. depth_B00367707227042B.
// camera_info_manager substitutes this for ${NAME} in the URL.
std::string serial_number = device_->getSerialNumber();
std::string rgb_name, ir_name;
if (serial_number.empty())
{
rgb_name = "rgb";
ir_name = "depth"; /// @todo Make it ir instead?
}
else
{
rgb_name = "rgb_" + serial_number;
ir_name = "depth_" + serial_number;
}
std::string rgb_info_url, ir_info_url;
param_nh.param("rgb_camera_info_url", rgb_info_url, std::string());
param_nh.param("depth_camera_info_url", ir_info_url, std::string());
double diagnostics_max_frequency, diagnostics_min_frequency;
double diagnostics_tolerance, diagnostics_window_time;
param_nh.param("enable_rgb_diagnostics", enable_rgb_diagnostics_, false);
param_nh.param("enable_ir_diagnostics", enable_ir_diagnostics_, false);
param_nh.param("enable_depth_diagnostics", enable_depth_diagnostics_, false);
param_nh.param("diagnostics_max_frequency", diagnostics_max_frequency, 30.0);
param_nh.param("diagnostics_min_frequency", diagnostics_min_frequency, 30.0);
param_nh.param("diagnostics_tolerance", diagnostics_tolerance, 0.05);
param_nh.param("diagnostics_window_time", diagnostics_window_time, 5.0);
// Suppress some of the output from loading camera calibrations (kinda hacky)
log4cxx::LoggerPtr logger_ccp = log4cxx::Logger::getLogger("ros.camera_calibration_parsers");
log4cxx::LoggerPtr logger_cim = log4cxx::Logger::getLogger("ros.camera_info_manager");
logger_ccp->setLevel(log4cxx::Level::getFatal());
logger_cim->setLevel(log4cxx::Level::getWarn());
// Also suppress sync warnings from image_transport::CameraSubscriber. When subscribing to
// depth_registered/foo with Freenect registration disabled, the rectify nodelet for depth_registered/
// will complain because it receives depth_registered/camera_info (from the register nodelet), but
// the driver is not publishing depth_registered/image_raw.
log4cxx::LoggerPtr logger_its = log4cxx::Logger::getLogger("ros.image_transport.sync");
logger_its->setLevel(log4cxx::Level::getError());
ros::console::notifyLoggerLevelsChanged();
// Load the saved calibrations, if they exist
rgb_info_manager_ = boost::make_shared<camera_info_manager::CameraInfoManager>(rgb_nh, rgb_name, rgb_info_url);
ir_info_manager_ = boost::make_shared<camera_info_manager::CameraInfoManager>(ir_nh, ir_name, ir_info_url);
if (!rgb_info_manager_->isCalibrated())
NODELET_WARN("Using default parameters for RGB camera calibration.");
if (!ir_info_manager_->isCalibrated())
NODELET_WARN("Using default parameters for IR camera calibration.");
// Advertise all published topics
{
// Prevent connection callbacks from executing until we've set all the publishers. Otherwise
// connectCb() can fire while we're advertising (say) "depth/image_raw", but before we actually
// assign to pub_depth_. Then pub_depth_.getNumSubscribers() returns 0, and we fail to start
// the depth generator.
boost::lock_guard<boost::mutex> lock(connect_mutex_);
// Instantiate the diagnostic updater
pub_freq_max_ = diagnostics_max_frequency;
pub_freq_min_ = diagnostics_min_frequency;
diagnostic_updater_.reset(new diagnostic_updater::Updater);
// Set hardware id
std::string hardware_id = std::string(device_->getProductName()) + "-" +
std::string(device_->getSerialNumber());
diagnostic_updater_->setHardwareID(hardware_id);
// Asus Xtion PRO does not have an RGB camera
if (device_->hasImageStream())
{
image_transport::SubscriberStatusCallback itssc = boost::bind(&DriverNodelet::rgbConnectCb, this);
ros::SubscriberStatusCallback rssc = boost::bind(&DriverNodelet::rgbConnectCb, this);
pub_rgb_ = rgb_it.advertiseCamera("image_raw", 1, itssc, itssc, rssc, rssc);
if (enable_rgb_diagnostics_) {
pub_rgb_freq_.reset(new TopicDiagnostic("RGB Image", *diagnostic_updater_,
FrequencyStatusParam(&pub_freq_min_, &pub_freq_max_,
diagnostics_tolerance, diagnostics_window_time)));
}
}
if (device_->hasIRStream())
{
image_transport::SubscriberStatusCallback itssc = boost::bind(&DriverNodelet::irConnectCb, this);
ros::SubscriberStatusCallback rssc = boost::bind(&DriverNodelet::irConnectCb, this);
pub_ir_ = ir_it.advertiseCamera("image_raw", 1, itssc, itssc, rssc, rssc);
if (enable_ir_diagnostics_) {
pub_ir_freq_.reset(new TopicDiagnostic("IR Image", *diagnostic_updater_,
FrequencyStatusParam(&pub_freq_min_, &pub_freq_max_,
diagnostics_tolerance, diagnostics_window_time)));
}
}
if (device_->hasDepthStream())
{
image_transport::SubscriberStatusCallback itssc = boost::bind(&DriverNodelet::depthConnectCb, this);
ros::SubscriberStatusCallback rssc = boost::bind(&DriverNodelet::depthConnectCb, this);
pub_depth_ = depth_it.advertiseCamera("image_raw", 1, itssc, itssc, rssc, rssc);
if (enable_depth_diagnostics_) {
pub_depth_freq_.reset(new TopicDiagnostic("Depth Image", *diagnostic_updater_,
FrequencyStatusParam(&pub_freq_min_, &pub_freq_max_,
diagnostics_tolerance, diagnostics_window_time)));
}
pub_projector_info_ = projector_nh.advertise<sensor_msgs::CameraInfo>("camera_info", 1, rssc, rssc);
if (device_->isDepthRegistrationSupported()) {
pub_depth_registered_ = depth_registered_it.advertiseCamera("image_raw", 1, itssc, itssc, rssc, rssc);
}
}
}
// Create separate diagnostics thread
close_diagnostics_ = false;
diagnostics_thread_ = boost::thread(boost::bind(&DriverNodelet::updateDiagnostics, this));
// Create watch dog timer callback
if (param_nh.getParam("time_out", time_out_) && time_out_ > 0.0)
{
time_stamp_ = ros::Time(0,0);
watch_dog_timer_ = nh.createTimer(ros::Duration(time_out_), &DriverNodelet::watchDog, this);
}
}