DotTracker::DotTracker(unsigned queueSize)
: queueSize_(queueSize),
nodeHandle_(),
imageTransport_(nodeHandle_),
image_(),
rectifiedImageTopic_(),
cameraInfoTopic_(),
pointCorrespondenceTopic_(),
model_prefix_(),
cameraSubscriber_(),
pointCorrespondenceSubscriber_(),
pointMsg_(),
points_(),
trackers_(),
trackers_status_(),
homography_(),
homography_pub_()
{
homography_ = cvCreateMat(3, 3, CV_64FC1);
homography_pub_ = nodeHandle_.advertise<std_msgs::Float64MultiArray>("homography", 10);
// find camera prefix name
ros::Rate rate (1);
while (rectifiedImageTopic_.empty ())
{
ros::param::get ("DotTracker/camera_topic", rectifiedImageTopic_);
ros::param::get ("DotTracker/camera_info_topic", cameraInfoTopic_);
ros::param::get ("DotTracker/point_correspondence_topic", pointCorrespondenceTopic_);
ros::param::get ("DotTracker/model_prefix", model_prefix_);
if (!ros::param::has ("DotTracker/camera_topic"))
{
ROS_WARN
("the camera_prefix parameter does not exist.\n"
"This may mean that:\n"
"- the tracker is not launched,\n"
"- the tracker and viewer are not running in the same namespace."
);
}
else if (rectifiedImageTopic_.empty ())
{
ROS_INFO
("tracker is not yet initialized, waiting...\n"
"You may want to launch the client to initialize the tracker.");
}
if (!ros::ok ())
return;
rate.sleep ();
}
ROS_INFO_STREAM("camera_topic is " << rectifiedImageTopic_);
ROS_INFO_STREAM("camera_info_topic is " << cameraInfoTopic_);
ROS_INFO_STREAM("point_correspondence_topic is " << pointCorrespondenceTopic_);
cameraSubscriber_ = imageTransport_.subscribeCamera
(rectifiedImageTopic_, queueSize_,
bindImageCallback(image_, header_, info_));
//cameraSubscriber_ = imageTransport_.subscribe(rectifiedImageTopic_, queueSize_, imageCallback);
// Wait for the image to be initialized.
//register callback for pointCorrespondance
pointCorrespondenceSubscriber_ = nodeHandle_.subscribe(pointCorrespondenceTopic_, 1, &DotTracker::pointCorrespondenceCallback, this);
//nodeHandle_.subscribe(pointCorrespondenceTopic_, 1, pointCorrespondenceCallbackGlobal);
waitForImage();
waitForInit();
}
TrackerViewer::TrackerViewer(ros::NodeHandle& nh,
ros::NodeHandle& privateNh,
volatile bool& exiting,
unsigned queueSize)
: exiting_ (exiting),
queueSize_(queueSize),
nodeHandle_(nh),
nodeHandlePrivate_(privateNh),
imageTransport_(nodeHandle_),
rectifiedImageTopic_(),
cameraInfoTopic_(),
checkInputs_(nodeHandle_, ros::this_node::getName()),
tracker_(),
cameraParameters_(),
image_(),
info_(),
cMo_(boost::none),
sites_(),
imageSubscriber_(),
cameraInfoSubscriber_(),
trackingResultSubscriber_(),
movingEdgeSitesSubscriber_(),
kltPointsSubscriber_(),
synchronizer_(syncPolicy_t(queueSize_)),
timer_(),
countAll_(0u),
countImages_(0u),
countCameraInfo_(0u),
countTrackingResult_(0u),
countMovingEdgeSites_(0u),
countKltPoints_(0u)
{
// Compute topic and services names.
std::string cameraPrefix;
ros::Rate rate (1);
while (cameraPrefix.empty ())
{
if (!nodeHandle_.getParam ("camera_prefix", cameraPrefix))
{
ROS_WARN
("the camera_prefix parameter does not exist.\n"
"This may mean that:\n"
"- the tracker is not launched,\n"
"- the tracker and viewer are not running in the same namespace."
);
}
else if (cameraPrefix.empty ())
{
ROS_INFO
("tracker is not yet initialized, waiting...\n"
"You may want to launch the client to initialize the tracker.");
}
if (this->exiting())
return;
rate.sleep ();
}
rectifiedImageTopic_ =
ros::names::resolve(cameraPrefix + "/image_rect");
cameraInfoTopic_ =
ros::names::resolve(cameraPrefix + "/camera_info");
boost::filesystem::ofstream modelStream;
std::string path;
while (!nodeHandle_.hasParam(visp_tracker::model_description_param))
{
if (!nodeHandle_.hasParam(visp_tracker::model_description_param))
{
ROS_WARN
("the model_description parameter does not exist.\n"
"This may mean that:\n"
"- the tracker is not launched or not initialized,\n"
"- the tracker and viewer are not running in the same namespace."
);
}
if (this->exiting())
return;
rate.sleep ();
}
if (!makeModelFile(modelStream, path))
throw std::runtime_error
("failed to load the model from the parameter server");
ROS_INFO_STREAM("Model loaded from the parameter server.");
vrmlPath_ = path;
initializeTracker();
if (this->exiting())
return;
checkInputs();
if (this->exiting())
return;
// Subscribe to camera and tracker synchronously.
imageSubscriber_.subscribe
(imageTransport_, rectifiedImageTopic_, queueSize_);
cameraInfoSubscriber_.subscribe
(nodeHandle_, cameraInfoTopic_, queueSize_);
trackingResultSubscriber_.subscribe
(nodeHandle_, visp_tracker::object_position_covariance_topic,
queueSize_);
movingEdgeSitesSubscriber_.subscribe
(nodeHandle_, visp_tracker::moving_edge_sites_topic, queueSize_);
kltPointsSubscriber_.subscribe
(nodeHandle_, visp_tracker::klt_points_topic, queueSize_);
synchronizer_.connectInput
(imageSubscriber_, cameraInfoSubscriber_,
trackingResultSubscriber_, movingEdgeSitesSubscriber_, kltPointsSubscriber_);
synchronizer_.registerCallback(boost::bind(&TrackerViewer::callback,
this, _1, _2, _3, _4, _5));
// Check for synchronization every 30s.
synchronizer_.registerCallback(boost::bind(increment, &countAll_));
imageSubscriber_.registerCallback(boost::bind(increment, &countImages_));
cameraInfoSubscriber_.registerCallback
(boost::bind(increment, &countCameraInfo_));
trackingResultSubscriber_.registerCallback
(boost::bind(increment, &countTrackingResult_));
movingEdgeSitesSubscriber_.registerCallback
(boost::bind(increment, &countMovingEdgeSites_));
kltPointsSubscriber_.registerCallback
(boost::bind(increment, &countKltPoints_));
timer_ = nodeHandle_.createWallTimer
(ros::WallDuration(30.),
boost::bind(&TrackerViewer::timerCallback, this));
// Wait for image.
waitForImage();
if (this->exiting())
return;
if (!image_.getWidth() || !image_.getHeight())
throw std::runtime_error("failed to retrieve image");
// Load camera parameters.
initializeVpCameraFromCameraInfo(cameraParameters_, info_);
tracker_.setCameraParameters(cameraParameters_);
}
Tracker::Tracker(ros::NodeHandle& nh,
ros::NodeHandle& privateNh,
volatile bool& exiting,
unsigned queueSize)
: exiting_ (exiting),
queueSize_(queueSize),
nodeHandle_(nh),
nodeHandlePrivate_(privateNh),
imageTransport_(nodeHandle_),
state_(WAITING_FOR_INITIALIZATION),
image_(),
cameraPrefix_(),
rectifiedImageTopic_(),
cameraInfoTopic_(),
modelPath_(),
cameraSubscriber_(),
mutex_ (),
//reconfigureSrv_(mutex_, nodeHandlePrivate_),
reconfigureSrv_(NULL),
reconfigureKltSrv_(NULL),
reconfigureEdgeSrv_(NULL),
resultPublisher_(),
transformationPublisher_(),
movingEdgeSitesPublisher_(),
kltPointsPublisher_(),
initService_(),
header_(),
info_(),
kltTracker_(),
movingEdge_(),
cameraParameters_(),
lastTrackedImage_(),
checkInputs_(nodeHandle_, ros::this_node::getName()),
cMo_ (),
listener_ (),
worldFrameId_ (),
compensateRobotMotion_ (false),
transformBroadcaster_ (),
childFrameId_ (),
objectPositionHintSubscriber_ (),
objectPositionHint_ ()
{
// Set cMo to identity.
cMo_.eye();
//tracker_ = new vpMbEdgeTracker();
// Parameters.
nodeHandlePrivate_.param<std::string>("camera_prefix", cameraPrefix_, "");
nodeHandlePrivate_.param<std::string>("tracker_type", trackerType_, "mbt");
if(trackerType_=="mbt")
tracker_ = new vpMbEdgeTracker();
else if(trackerType_=="klt")
tracker_ = new vpMbKltTracker();
else
tracker_ = new vpMbEdgeKltTracker();
if (cameraPrefix_.empty ())
{
ROS_FATAL
("The camera_prefix parameter not set.\n"
"Please relaunch the tracker while setting this parameter, i.e.\n"
"$ rosrun visp_tracker tracker _camera_prefix:=/my/camera");
ros::shutdown ();
return;
}
// Create global /camera_prefix param to avoid to remap in the launch files the tracker_client and tracker_viewer nodes
nodeHandle_.setParam("camera_prefix", cameraPrefix_);
nodeHandle_.param<std::string>("frame_id", childFrameId_, "object_position");
// Robot motion compensation.
nodeHandle_.param<std::string>("world_frame_id", worldFrameId_, "/odom");
nodeHandle_.param<bool>
("compensate_robot_motion", compensateRobotMotion_, false);
// Compute topic and services names.
rectifiedImageTopic_ =
ros::names::resolve(cameraPrefix_ + "/image_rect");
// Check for subscribed topics.
checkInputs();
// Result publisher.
resultPublisher_ =
nodeHandle_.advertise<geometry_msgs::PoseWithCovarianceStamped>
(visp_tracker::object_position_covariance_topic, queueSize_);
transformationPublisher_ =
nodeHandle_.advertise<geometry_msgs::TransformStamped>
(visp_tracker::object_position_topic, queueSize_);
// Moving edge sites_ publisher.
movingEdgeSitesPublisher_ =
nodeHandle_.advertise<visp_tracker::MovingEdgeSites>
(visp_tracker::moving_edge_sites_topic, queueSize_);
// Klt_points_ publisher.
kltPointsPublisher_ =
nodeHandle_.advertise<visp_tracker::KltPoints>
(visp_tracker::klt_points_topic, queueSize_);
// Camera subscriber.
cameraSubscriber_ =
imageTransport_.subscribeCamera
(rectifiedImageTopic_, queueSize_,
bindImageCallback(image_, header_, info_));
// Object position hint subscriber.
typedef boost::function<
void (const geometry_msgs::TransformStampedConstPtr&)>
objectPositionHintCallback_t;
objectPositionHintCallback_t callback =
boost::bind (&Tracker::objectPositionHintCallback, this, _1);
objectPositionHintSubscriber_ =
nodeHandle_.subscribe<geometry_msgs::TransformStamped>
("object_position_hint", queueSize_, callback);
// Initialization.
// No more necessary as it is done via the reconfigure server
// movingEdge_.initMask();
// if(trackerType_!="klt"){
// vpMbEdgeTracker* t = dynamic_cast<vpMbEdgeTracker*>(tracker_);
// t->setMovingEdge(movingEdge_);
// }
// if(trackerType_!="mbt"){
// vpMbKltTracker* t = dynamic_cast<vpMbKltTracker*>(tracker_);
// t->setKltOpencv(kltTracker_);
// }
// Dynamic reconfigure.
if(trackerType_=="mbt+klt"){ // Hybrid Tracker reconfigure
reconfigureSrv_ = new reconfigureSrvStruct<visp_tracker::ModelBasedSettingsConfig>::reconfigureSrv_t(mutex_, nodeHandlePrivate_);
reconfigureSrvStruct<visp_tracker::ModelBasedSettingsConfig>::reconfigureSrv_t::CallbackType f =
boost::bind(&reconfigureCallbackAndInitViewer,
boost::ref(nodeHandle_), boost::ref(tracker_),
boost::ref(image_), boost::ref(movingEdge_), boost::ref(kltTracker_),
boost::ref(mutex_), _1, _2);
reconfigureSrv_->setCallback(f);
}
else if(trackerType_=="mbt"){ // Edge Tracker reconfigure
reconfigureEdgeSrv_ = new reconfigureSrvStruct<visp_tracker::ModelBasedSettingsEdgeConfig>::reconfigureSrv_t(mutex_, nodeHandlePrivate_);
reconfigureSrvStruct<visp_tracker::ModelBasedSettingsEdgeConfig>::reconfigureSrv_t::CallbackType f =
boost::bind(&reconfigureEdgeCallbackAndInitViewer,
boost::ref(nodeHandle_), boost::ref(tracker_),
boost::ref(image_), boost::ref(movingEdge_),
boost::ref(mutex_), _1, _2);
reconfigureEdgeSrv_->setCallback(f);
}
else{ // KLT Tracker reconfigure
reconfigureKltSrv_ = new reconfigureSrvStruct<visp_tracker::ModelBasedSettingsKltConfig>::reconfigureSrv_t(mutex_, nodeHandlePrivate_);
reconfigureSrvStruct<visp_tracker::ModelBasedSettingsKltConfig>::reconfigureSrv_t::CallbackType f =
boost::bind(&reconfigureKltCallbackAndInitViewer,
boost::ref(nodeHandle_), boost::ref(tracker_),
boost::ref(image_), boost::ref(kltTracker_),
boost::ref(mutex_), _1, _2);
reconfigureKltSrv_->setCallback(f);
}
// Wait for the image to be initialized.
waitForImage();
if (this->exiting())
return;
if (!image_.getWidth() || !image_.getHeight())
throw std::runtime_error("failed to retrieve image");
// Tracker initialization.
initializeVpCameraFromCameraInfo(cameraParameters_, info_);
// Double check camera parameters.
if (cameraParameters_.get_px () == 0.
|| cameraParameters_.get_px () == 1.
|| cameraParameters_.get_py () == 0.
|| cameraParameters_.get_py () == 1.
|| cameraParameters_.get_u0 () == 0.
|| cameraParameters_.get_u0 () == 1.
|| cameraParameters_.get_v0 () == 0.
|| cameraParameters_.get_v0 () == 1.)
ROS_WARN ("Dubious camera parameters detected.\n"
"\n"
"It seems that the matrix P from your camera\n"
"calibration topics is wrong.\n"
"The tracker will continue anyway, but you\n"
"should double check your calibration data,\n"
"especially if the model re-projection fails.\n"
"\n"
"This warning is triggered is px, py, u0 or v0\n"
"is set to 0. or 1. (exactly).");
tracker_->setCameraParameters(cameraParameters_);
tracker_->setDisplayFeatures(false);
ROS_INFO_STREAM(cameraParameters_);
// Service declaration.
initCallback_t initCallback =
boost::bind(&Tracker::initCallback, this, _1, _2);
initService_ = nodeHandle_.advertiseService
(visp_tracker::init_service, initCallback);
}