tracker-viewer.cpp
Go to the documentation of this file.
1 #include <cstdlib>
2 #include <fstream>
3 #include <sstream>
4 
5 #include <boost/bind.hpp>
6 #include <boost/filesystem.hpp>
7 #include <boost/filesystem/fstream.hpp>
8 #include <boost/filesystem/path.hpp>
9 #include <boost/format.hpp>
10 #include <boost/optional.hpp>
11 
12 #include <ros/ros.h>
13 #include <ros/param.h>
15 
16 #include <visp/vpDisplayX.h>
17 
18 #include "conversion.hh"
19 #include "callbacks.hh"
20 #include "file.hh"
21 #include "names.hh"
22 
23 #include "tracker-viewer.hh"
24 
25 namespace visp_tracker
26 {
27  namespace
28  {
29  static void increment(unsigned int* value)
30  {
31  ++(*value);
32  }
33  } // end of anonymous namespace.
34 
35  // Callback to fix ROS bug when /model_description (not properly cleared) doesn't contain the right model of the object to track.
36  // Bug only occurs when viewer is started too early and with a different model than the previous call.
37  bool
38  TrackerViewer::initCallback(visp_tracker::Init::Request& req,
39  visp_tracker::Init::Response& res)
40  {
41  boost::filesystem::ofstream modelStream;
42  std::string path;
43 
44  if (!makeModelFile(modelStream, path))
45  throw std::runtime_error("failed to load the model from the callback");
46  //ROS_WARN_STREAM("Make model Viewer: " << path.c_str());
47  ROS_INFO_STREAM("Model loaded from the service.");
48  modelPath_ = path;
49 
50  tracker_.resetTracker();
52 
53  // Common parameters
55 
56  res.initialization_succeed = true;
57  return true;
58  }
59 
60  bool
61  TrackerViewer::reconfigureCallback(visp_tracker::Init::Request& req,
62  visp_tracker::Init::Response& res)
63  {
64  // Common parameters
65  ROS_INFO_STREAM("Reconfiguring Tracker Viewer.");
67 
68  res.initialization_succeed = true;
69  return true;
70  }
71 
73  ros::NodeHandle& privateNh,
74  volatile bool& exiting,
75  unsigned queueSize)
76  : exiting_ (exiting),
77  queueSize_(queueSize),
78  nodeHandle_(nh),
79  nodeHandlePrivate_(privateNh),
81  frameSize_(0.1),
84  checkInputs_(nodeHandle_, ros::this_node::getName()),
85  tracker_(),
87  image_(),
88  info_(),
89  initService_(),
91  trackerName_(),
92  cMo_(boost::none),
93  sites_(),
100  timer_(),
101  countAll_(0u),
102  countImages_(0u),
103  countCameraInfo_(0u),
106  countKltPoints_(0u)
107  {
108  // Compute topic and services names.
109  std::string cameraPrefix;
110 
111  ros::Rate rate (1);
112  while (cameraPrefix.empty ())
113  {
114  // Check for the global parameter /camera_prefix set by visp_tracker node
115  if (!nodeHandle_.getParam ("camera_prefix", cameraPrefix) && !ros::param::get ("~camera_prefix", cameraPrefix))
116  {
117  ROS_WARN
118  ("the camera_prefix parameter does not exist.\n"
119  "This may mean that:\n"
120  "- the tracker is not launched,\n"
121  "- the tracker and viewer are not running in the same namespace."
122  );
123  }
124  else if (cameraPrefix.empty ())
125  {
126  ROS_INFO
127  ("tracker is not yet initialized, waiting...\n"
128  "You may want to launch the client to initialize the tracker.");
129  }
130  if (this->exiting())
131  return;
132  rate.sleep ();
133  }
134  nodeHandlePrivate_.param<double>("frame_size", frameSize_, 0.1);
135 
137  ros::names::resolve(cameraPrefix + "/image_rect");
139  ros::names::resolve(cameraPrefix + "/camera_info");
140 
142  boost::bind(&TrackerViewer::initCallback, this, _1, _2);
143 
145  boost::bind(&TrackerViewer::reconfigureCallback, this, _1, _2);
146 
148  (visp_tracker::init_service_viewer, initCallback);
149 
151  (visp_tracker::reconfigure_service_viewer, reconfigureCallback);
152 
153 
154  boost::filesystem::ofstream modelStream;
155  std::string path;
156 
157  unsigned int cpt = 0;
159  {
161  {
162  if(cpt%10 == 0){
164  ("[Node: " << ros::this_node::getName() << "]\n"
165  "The model_description parameter does not exist.\n"
166  "This may mean that:\n"
167  "- the tracker is not launched or not initialized,\n"
168  "- the tracker and viewer are not running in the same namespace."
169  );
170  }
171  cpt++;
172  }
173  if (this->exiting())
174  return;
175  rate.sleep ();
176  }
177 
178 
179  if (!makeModelFile(modelStream, path))
180  throw std::runtime_error
181  ("failed to load the model from the parameter server");
182 
183  ROS_INFO_STREAM("Model loaded from the parameter server.");
184  //ROS_WARN_STREAM("Make model Viewer: " << path.c_str());
185  modelPath_ = path;
186 
188 
189  if (this->exiting())
190  return;
191 
192  checkInputs();
193  if (this->exiting())
194  return;
195 
196  // Subscribe to camera and tracker synchronously.
203  queueSize_);
208 
209  synchronizer_.connectInput
212  synchronizer_.registerCallback(boost::bind(&TrackerViewer::callback,
213  this, _1, _2, _3, _4, _5));
214 
215  // Check for synchronization every 30s.
216  synchronizer_.registerCallback(boost::bind(increment, &countAll_));
217  imageSubscriber_.registerCallback(boost::bind(increment, &countImages_));
219  (boost::bind(increment, &countCameraInfo_));
221  (boost::bind(increment, &countTrackingResult_));
223  (boost::bind(increment, &countMovingEdgeSites_));
225  (boost::bind(increment, &countKltPoints_));
226 
228  (ros::WallDuration(30.),
229  boost::bind(&TrackerViewer::timerCallback, this));
230 
231  // Wait for image.
232  waitForImage();
233  if (this->exiting())
234  return;
235  if (!image_.getWidth() || !image_.getHeight())
236  throw std::runtime_error("failed to retrieve image");
237 
238  // Load camera parameters.
240  tracker_.setCameraParameters(cameraParameters_);
241 
242  // Load the common parameters from ROS messages
244  }
245 
246  void
248  {
249  boost::format fmtWindowTitle ("ViSP MBT tracker viewer - [ns: %s]");
250  fmtWindowTitle % ros::this_node::getNamespace ();
251 
252  vpDisplayX d(image_,
253  image_.getWidth(), image_.getHeight(),
254  fmtWindowTitle.str().c_str());
255  vpImagePoint point (10, 10);
256  vpImagePoint pointTime (22, 10);
257  vpImagePoint pointCameraTopic (34, 10);
258  ros::Rate loop_rate(80);
259 
260  boost::format fmt("tracking (x=%f y=%f z=%f)");
261  boost::format fmtTime("time = %f");
262 
263  boost::format fmtCameraTopic("camera topic = %s");
264  fmtCameraTopic % rectifiedImageTopic_;
265  while (!exiting())
266  {
267  vpDisplay::display(image_);
270  if (cMo_)
271  {
272  try
273  {
274  tracker_.initFromPose(image_, *cMo_);
275  tracker_.display(image_, *cMo_, cameraParameters_, vpColor::red);
276  vpDisplay::displayFrame(image_, *cMo_, cameraParameters_, frameSize_, vpColor::none, 2);
277  }
278  catch(...)
279  {
280  ROS_DEBUG_STREAM_THROTTLE(10, "failed to display cmo");
281  }
282 
283  ROS_DEBUG_STREAM_THROTTLE(10, "cMo viewer:\n" << *cMo_);
284 
285  fmt % (*cMo_)[0][3] % (*cMo_)[1][3] % (*cMo_)[2][3];
286  vpDisplay::displayCharString
287  (image_, point, fmt.str().c_str(), vpColor::red);
288  fmtTime % info_->header.stamp.toSec();
289  vpDisplay::displayCharString
290  (image_, pointTime, fmtTime.str().c_str(), vpColor::red);
291  vpDisplay::displayCharString
292  (image_, pointCameraTopic, fmtCameraTopic.str().c_str(),
293  vpColor::red);
294  }
295  else{
296  vpDisplay::displayCharString
297  (image_, point, "tracking failed", vpColor::red);
298  }
299 
300  vpDisplay::flush(image_);
301  ros::spinOnce();
302  loop_rate.sleep();
303  }
304  }
305 
306  void
308  {
309  ros::Rate loop_rate(10);
310  while (!exiting()
311  && (!image_.getWidth() || !image_.getHeight()))
312  {
313  ROS_INFO_THROTTLE(1, "waiting for a rectified image...");
314  ros::spinOnce();
315  loop_rate.sleep();
316  }
317  }
318 
319  void
321  {
322  ros::V_string topics;
323  topics.push_back(rectifiedImageTopic_);
324  topics.push_back(cameraInfoTopic_);
325  topics.push_back(visp_tracker::object_position_topic);
326  topics.push_back(visp_tracker::moving_edge_sites_topic);
327  topics.push_back(visp_tracker::klt_points_topic);
328  checkInputs_.start(topics, 60.0);
329  }
330 
331  void
333  {
334  nodeHandlePrivate_.param<std::string>("tracker_name", trackerName_, "");
335  std::string key;
336 
337  bool loadParam = false;
338 
339  if(trackerName_.empty())
340  {
341  if(!ros::param::search("/angle_appear",key)){
342  trackerName_ = "tracker_mbt";
343  if(!ros::param::search(trackerName_ + "/angle_appear",key))
344  {
345  ROS_WARN_STREAM("No tracker has been found with the default name value \""
346  << trackerName_ << "/angle_appear\".\n"
347  << "Tracker name parameter (tracker_name) should be provided for this node (tracker_viewer).\n"
348  << "Polygon visibility might not work well in the viewer window.");
349  }
350  else loadParam = true;
351  }
352  else loadParam = true;
353  }
354  else loadParam = true;
355 
356  // Reading common parameters
357  if(loadParam)
358  {
359  if (ros::param::search(trackerName_ + "/angle_appear",key))
360  {
361  double value;
362  if(ros::param::get(key,value)){
363  // ROS_WARN_STREAM("Angle Appear Viewer: " << value);
364  tracker_.setAngleAppear(vpMath::rad(value));
365  }
366  }
367  else
368  {
369  ROS_WARN_STREAM("No tracker has been found with the provided parameter "
370  << "(tracker_name=\"" << trackerName_ << "\")\n"
371  << "Polygon visibility might not work well in the viewer window");
372  }
373 
374  if (ros::param::search(trackerName_ + "/angle_disappear",key))
375  {
376  double value;
377  if(ros::param::get(key,value)){
378  // ROS_WARN_STREAM("Angle Disappear Viewer: " << value);
379  tracker_.setAngleDisappear(vpMath::rad(value));
380  }
381  }
382  }
383  }
384 
385  void
387  {
388  try
389  {
390  // ROS_WARN_STREAM("Trying to load the model Viewer: " << modelPath_);
391  tracker_.loadModel(modelPath_.native().c_str());
392  }
393  catch(...)
394  {
395  boost::format fmt("failed to load the model %1%");
396  fmt % modelPath_;
397  throw std::runtime_error(fmt.str());
398  }
399  // ROS_WARN("Model has been successfully loaded.");
400  }
401 
402  void
404  (const sensor_msgs::ImageConstPtr& image,
405  const sensor_msgs::CameraInfoConstPtr& info,
406  const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& trackingResult,
407  const visp_tracker::MovingEdgeSites::ConstPtr& sites,
408  const visp_tracker::KltPoints::ConstPtr& klt)
409  {
410  // Copy image.
411  try
412  {
413  rosImageToVisp(image_, image);
414  }
415  catch(std::exception& e)
416  {
417  ROS_ERROR_STREAM("dropping frame: " << e.what());
418  }
419 
420  // Copy moving camera infos, edges sites and optional KLT points.
421  info_ = info;
422  sites_ = sites;
423  klt_ = klt;
424 
425  // Copy cMo.
426  cMo_ = vpHomogeneousMatrix();
427  transformToVpHomogeneousMatrix(*cMo_, trackingResult->pose.pose);
428  }
429 
430  void
432  {
433  if (!sites_)
434  return;
435  for (unsigned i = 0; i < sites_->moving_edge_sites.size(); ++i)
436  {
437  double x = sites_->moving_edge_sites[i].x;
438  double y = sites_->moving_edge_sites[i].y;
439  int suppress = sites_->moving_edge_sites[i].suppress;
440  vpColor color = vpColor::black;
441 
442  switch(suppress)
443  {
444  case vpMeSite::NO_SUPPRESSION:
445  color = vpColor::green;
446  break;
447  case vpMeSite::CONSTRAST:
448  color = vpColor::blue;
449  break;
450  case vpMeSite::THRESHOLD:
451  color = vpColor::purple;
452  break;
453  case vpMeSite::M_ESTIMATOR:
454  color = vpColor::red;
455  break;
456  default: // vpMeSite::UNKOWN
457  color = vpColor::yellow;
458  }
459 
460  vpDisplay::displayCross(image_, vpImagePoint(x, y), 3, color, 1);
461  }
462  }
463 
464 
465  void
467  {
468  if (!klt_)
469  return;
470  vpImagePoint pos;
471 
472  for (unsigned i = 0; i < klt_->klt_points_positions.size(); ++i)
473  {
474  double ii = klt_->klt_points_positions[i].i;
475  double jj = klt_->klt_points_positions[i].j;
476  int id = klt_->klt_points_positions[i].id;
477  vpColor color = vpColor::red;
478 
479  vpDisplay::displayCross(image_, vpImagePoint(ii, jj), 15, color, 1);
480 
481  pos.set_i( vpMath::round( ii + 7 ) );
482  pos.set_j( vpMath::round( jj + 7 ) );
483  char ide[10];
484  sprintf(ide, "%d", id);
485  vpDisplay::displayCharString(image_, pos, ide, vpColor::red);
486  }
487  }
488 
489  void
491  {
494  {
495  boost::format fmt
496  ("[visp_tracker] Low number of synchronized tuples received.\n"
497  "Images: %d\n"
498  "Camera info: %d\n"
499  "Tracking result: %d\n"
500  "Moving edge sites: %d\n"
501  "KLT points: %d\n"
502  "Synchronized tuples: %d\n"
503  "Possible issues:\n"
504  "\t* The network is too slow.");
507  ROS_WARN_STREAM_THROTTLE(10, fmt.str());
508  }
509  }
510 } // end of namespace visp_tracker.
d
image_transport::ImageTransport imageTransport_
Image transport used to receive images.
ros::NodeHandle & nodeHandle_
void transformToVpHomogeneousMatrix(vpHomogeneousMatrix &dst, const geometry_msgs::Transform &src)
Definition: conversion.cpp:167
void displayMovingEdgeSites()
Display moving edge sites.
#define ROS_WARN_STREAM_THROTTLE(period, args)
void waitForImage()
Hang until the first image is received.
message_filters::Subscriber< geometry_msgs::PoseWithCovarianceStamped > trackingResultSubscriber_
Subscriber to tracking result topic.
std::string rectifiedImageTopic_
Full topic name for rectified image.
void start(const ros::V_string &topics, double duration)
std::string moving_edge_sites_topic
boost::function< bool(visp_tracker::Init::Request &, visp_tracker::Init::Response &res)> initCallback_t
void convertInitRequestToVpMbTracker(const visp_tracker::Init::Request &req, vpMbTracker *tracker)
Definition: conversion.cpp:232
std::string getName(void *handle)
boost::function< bool(visp_tracker::Init::Request &, visp_tracker::Init::Response &res)> reconfigureCallback_t
ros::ServiceServer reconfigureService_
Service called when user is reconfiguring tracker node.
ROSCPP_DECL const std::string & getName()
ROSCPP_DECL std::string resolve(const std::string &name, bool remap=true)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
TFSIMD_FORCE_INLINE const tfScalar & y() const
#define ROS_WARN(...)
#define ROS_DEBUG_STREAM_THROTTLE(period, args)
message_filters::Subscriber< visp_tracker::KltPoints > kltPointsSubscriber_
Subscriber to KLT point topics.
visp_tracker::KltPoints::ConstPtr klt_
Shared pointer to latest received KLT point positions.
image_proc::AdvertisementChecker checkInputs_
Helper used to check that subscribed topics exist.
std::string trackerName_
Name of the tracker used in this viewer node.
TrackerViewer(ros::NodeHandle &nh, ros::NodeHandle &privateNh, volatile bool &exiting, unsigned queueSize=5u)
Constructor.
ros::NodeHandle & nodeHandlePrivate_
vpMbEdgeTracker tracker_
ViSP edge tracker.
std::vector< std::string > V_string
void displayKltPoints()
Display KLT points that are tracked.
boost::filesystem::path modelPath_
Model path.
#define ROS_INFO_THROTTLE(period,...)
ROSCPP_DECL const std::string & getNamespace()
ROSCPP_DECL bool get(const std::string &key, std::string &s)
image_t image_
ViSP image.
#define ROS_INFO(...)
bool initCallback(visp_tracker::Init::Request &req, visp_tracker::Init::Response &res)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
std::string model_description_param
bool reconfigureCallback(visp_tracker::Init::Request &req, visp_tracker::Init::Response &res)
void callback(const sensor_msgs::ImageConstPtr &imageConst, const sensor_msgs::CameraInfoConstPtr &infoConst, const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &trackingResult, const visp_tracker::MovingEdgeSites::ConstPtr &sitesConst, const visp_tracker::KltPoints::ConstPtr &kltConst)
Callback used to received synchronized data.
sensor_msgs::CameraInfoConstPtr info_
Shared pointer to latest received camera information.
WallTimer createWallTimer(WallDuration period, void(T::*callback)(const WallTimerEvent &), T *obj, bool oneshot=false, bool autostart=true) const
TFSIMD_FORCE_INLINE const tfScalar & x() const
std::string reconfigure_service_viewer
#define ROS_WARN_STREAM(args)
image_transport::SubscriberFilter imageSubscriber_
bool sleep()
ros::ServiceServer initService_
Service called when user ends tracker_client node.
void subscribe(ImageTransport &it, const std::string &base_topic, uint32_t queue_size, const TransportHints &transport_hints=TransportHints())
void checkInputs()
Make sure the topics we subscribe already exist.
message_filters::Synchronizer< syncPolicy_t > synchronizer_
Synchronizer with approximate time policy.
vpCameraParameters cameraParameters_
ViSP camera parameters.
message_filters::Subscriber< visp_tracker::MovingEdgeSites > movingEdgeSitesSubscriber_
Subscriber to moving edge sites topics.
void initializeTracker()
Initialize the tracker.
unsigned queueSize_
Queue size for all subscribers.
#define ROS_INFO_STREAM(args)
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
std::string cameraInfoTopic_
Full topic name for camera information.
boost::optional< vpHomogeneousMatrix > cMo_
Last tracked object position, set to none if tracking failed.
bool getParam(const std::string &key, std::string &s) const
void initializeVpCameraFromCameraInfo(vpCameraParameters &cam, sensor_msgs::CameraInfoConstPtr info)
Definition: conversion.cpp:348
std::string object_position_covariance_topic
void spin()
Display camera image, tracked object position and moving edge sites.
std::string init_service_viewer
message_filters::Subscriber< sensor_msgs::CameraInfo > cameraInfoSubscriber_
Subscriber to camera information topic.
std::string object_position_topic
bool hasParam(const std::string &key) const
#define ROS_ERROR_STREAM(args)
std::string klt_points_topic
ROSCPP_DECL void spinOnce()
ROSCPP_DECL bool search(const std::string &ns, const std::string &key, std::string &result)
visp_tracker::MovingEdgeSites::ConstPtr sites_
Shared pointer to latest received moving edge sites.
bool makeModelFile(boost::filesystem::ofstream &modelStream, std::string &fullModelPath)
Definition: file.cpp:62
void increment(int *value)
Connection registerCallback(const C &callback)
void loadCommonParameters()
Initialize the common parameters (visibility angles, etc)
void rosImageToVisp(vpImage< unsigned char > &dst, const sensor_msgs::Image::ConstPtr &src)
Convert a ROS image into a ViSP one.
Definition: conversion.cpp:27


visp_tracker
Author(s): Thomas Moulard
autogenerated on Wed Jul 3 2019 19:48:07