Monitors the tracking result provided by the tracking node. More...
#include <tracker-viewer.hh>
Public Types | |
typedef vpImage< unsigned char > | image_t |
ViSP image type. | |
typedef boost::function< bool(visp_tracker::Init::Request &, visp_tracker::Init::Response &res)> | initCallback_t |
typedef boost::function< bool(visp_tracker::Init::Request &, visp_tracker::Init::Response &res)> | reconfigureCallback_t |
typedef message_filters::sync_policies::ApproximateTime < sensor_msgs::Image, sensor_msgs::CameraInfo, geometry_msgs::PoseWithCovarianceStamped, visp_tracker::MovingEdgeSites, visp_tracker::KltPoints > | syncPolicy_t |
Synchronization policy. | |
Public Member Functions | |
void | spin () |
Display camera image, tracked object position and moving edge sites. | |
TrackerViewer (ros::NodeHandle &nh, ros::NodeHandle &privateNh, volatile bool &exiting, unsigned queueSize=5u) | |
Constructor. | |
Protected Member Functions | |
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. | |
void | checkInputs () |
Make sure the topics we subscribe already exist. | |
void | displayKltPoints () |
Display KLT points that are tracked. | |
void | displayMovingEdgeSites () |
Display moving edge sites. | |
bool | initCallback (visp_tracker::Init::Request &req, visp_tracker::Init::Response &res) |
void | initializeTracker () |
Initialize the tracker. | |
void | loadCommonParameters () |
Initialize the common parameters (visibility angles, etc) | |
bool | reconfigureCallback (visp_tracker::Init::Request &req, visp_tracker::Init::Response &res) |
void | timerCallback () |
void | waitForImage () |
Hang until the first image is received. | |
Private Member Functions | |
bool | exiting () |
Private Attributes | |
vpCameraParameters | cameraParameters_ |
ViSP camera parameters. | |
image_proc::AdvertisementChecker | checkInputs_ |
Helper used to check that subscribed topics exist. | |
boost::optional < vpHomogeneousMatrix > | cMo_ |
Last tracked object position, set to none if tracking failed. | |
volatile bool & | exiting_ |
double | frameSize_ |
image_t | image_ |
ViSP image. | |
image_transport::ImageTransport | imageTransport_ |
Image transport used to receive images. | |
sensor_msgs::CameraInfoConstPtr | info_ |
Shared pointer to latest received camera information. | |
ros::ServiceServer | initService_ |
Service called when user ends tracker_client node. | |
visp_tracker::KltPoints::ConstPtr | klt_ |
Shared pointer to latest received KLT point positions. | |
boost::filesystem::path | modelPath_ |
Model path. | |
ros::NodeHandle & | nodeHandle_ |
ros::NodeHandle & | nodeHandlePrivate_ |
unsigned | queueSize_ |
Queue size for all subscribers. | |
ros::ServiceServer | reconfigureService_ |
Service called when user is reconfiguring tracker node. | |
visp_tracker::MovingEdgeSites::ConstPtr | sites_ |
Shared pointer to latest received moving edge sites. | |
vpMbEdgeTracker | tracker_ |
ViSP edge tracker. | |
std::string | trackerName_ |
Name of the tracker used in this viewer node. | |
Topics and services strings. | |
std::string | rectifiedImageTopic_ |
Full topic name for rectified image. | |
std::string | cameraInfoTopic_ |
Full topic name for camera information. | |
Subscribers and synchronizer. | |
Subscriber to image topic. | |
image_transport::SubscriberFilter | imageSubscriber_ |
message_filters::Subscriber < sensor_msgs::CameraInfo > | cameraInfoSubscriber_ |
Subscriber to camera information topic. | |
message_filters::Subscriber < geometry_msgs::PoseWithCovarianceStamped > | trackingResultSubscriber_ |
Subscriber to tracking result topic. | |
message_filters::Subscriber < visp_tracker::MovingEdgeSites > | movingEdgeSitesSubscriber_ |
Subscriber to moving edge sites topics. | |
message_filters::Subscriber < visp_tracker::KltPoints > | kltPointsSubscriber_ |
Subscriber to KLT point topics. | |
message_filters::Synchronizer < syncPolicy_t > | synchronizer_ |
Synchronizer with approximate time policy. | |
Synchronization check | |
} | |
ros::WallTimer | timer_ |
unsigned | countAll_ |
unsigned | countImages_ |
unsigned | countCameraInfo_ |
unsigned | countTrackingResult_ |
unsigned | countMovingEdgeSites_ |
unsigned | countKltPoints_ |
Monitors the tracking result provided by the tracking node.
Definition at line 31 of file tracker-viewer.hh.
typedef vpImage<unsigned char> visp_tracker::TrackerViewer::image_t |
ViSP image type.
Definition at line 35 of file tracker-viewer.hh.
typedef boost::function<bool (visp_tracker::Init::Request&, visp_tracker::Init::Response& res)> visp_tracker::TrackerViewer::initCallback_t |
Definition at line 40 of file tracker-viewer.hh.
typedef boost::function<bool (visp_tracker::Init::Request&, visp_tracker::Init::Response& res)> visp_tracker::TrackerViewer::reconfigureCallback_t |
Definition at line 44 of file tracker-viewer.hh.
typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::CameraInfo, geometry_msgs::PoseWithCovarianceStamped, visp_tracker::MovingEdgeSites, visp_tracker::KltPoints > visp_tracker::TrackerViewer::syncPolicy_t |
Synchronization policy.
This is used to make sure that the image, the object position and the moving edge sites are synchronized. This may not be the case as these informations are published on different topics. The approximate time allows light differences in timestamps which are not critical as this is only a viewer.
Definition at line 58 of file tracker-viewer.hh.
visp_tracker::TrackerViewer::TrackerViewer | ( | ros::NodeHandle & | nh, |
ros::NodeHandle & | privateNh, | ||
volatile bool & | exiting, | ||
unsigned | queueSize = 5u |
||
) |
Constructor.
Definition at line 72 of file tracker-viewer.cpp.
void visp_tracker::TrackerViewer::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 | ||
) | [protected] |
Callback used to received synchronized data.
Definition at line 404 of file tracker-viewer.cpp.
void visp_tracker::TrackerViewer::checkInputs | ( | ) | [protected] |
Make sure the topics we subscribe already exist.
Definition at line 320 of file tracker-viewer.cpp.
void visp_tracker::TrackerViewer::displayKltPoints | ( | ) | [protected] |
Display KLT points that are tracked.
Definition at line 466 of file tracker-viewer.cpp.
void visp_tracker::TrackerViewer::displayMovingEdgeSites | ( | ) | [protected] |
Display moving edge sites.
Definition at line 431 of file tracker-viewer.cpp.
bool visp_tracker::TrackerViewer::exiting | ( | ) | [inline, private] |
Definition at line 105 of file tracker-viewer.hh.
bool visp_tracker::TrackerViewer::initCallback | ( | visp_tracker::Init::Request & | req, |
visp_tracker::Init::Response & | res | ||
) | [protected] |
Definition at line 38 of file tracker-viewer.cpp.
void visp_tracker::TrackerViewer::initializeTracker | ( | ) | [protected] |
Initialize the tracker.
Definition at line 386 of file tracker-viewer.cpp.
void visp_tracker::TrackerViewer::loadCommonParameters | ( | ) | [protected] |
Initialize the common parameters (visibility angles, etc)
Definition at line 332 of file tracker-viewer.cpp.
bool visp_tracker::TrackerViewer::reconfigureCallback | ( | visp_tracker::Init::Request & | req, |
visp_tracker::Init::Response & | res | ||
) | [protected] |
Definition at line 61 of file tracker-viewer.cpp.
void visp_tracker::TrackerViewer::spin | ( | ) |
Display camera image, tracked object position and moving edge sites.
Definition at line 247 of file tracker-viewer.cpp.
void visp_tracker::TrackerViewer::timerCallback | ( | ) | [protected] |
Definition at line 490 of file tracker-viewer.cpp.
void visp_tracker::TrackerViewer::waitForImage | ( | ) | [protected] |
Hang until the first image is received.
Definition at line 307 of file tracker-viewer.cpp.
message_filters::Subscriber<sensor_msgs::CameraInfo> visp_tracker::TrackerViewer::cameraInfoSubscriber_ [private] |
Subscriber to camera information topic.
Definition at line 169 of file tracker-viewer.hh.
std::string visp_tracker::TrackerViewer::cameraInfoTopic_ [private] |
Full topic name for camera information.
Definition at line 129 of file tracker-viewer.hh.
vpCameraParameters visp_tracker::TrackerViewer::cameraParameters_ [private] |
ViSP camera parameters.
Definition at line 151 of file tracker-viewer.hh.
Helper used to check that subscribed topics exist.
Definition at line 146 of file tracker-viewer.hh.
boost::optional<vpHomogeneousMatrix> visp_tracker::TrackerViewer::cMo_ [private] |
Last tracked object position, set to none if tracking failed.
Definition at line 158 of file tracker-viewer.hh.
unsigned visp_tracker::TrackerViewer::countAll_ [private] |
Definition at line 187 of file tracker-viewer.hh.
unsigned visp_tracker::TrackerViewer::countCameraInfo_ [private] |
Definition at line 189 of file tracker-viewer.hh.
unsigned visp_tracker::TrackerViewer::countImages_ [private] |
Definition at line 188 of file tracker-viewer.hh.
unsigned visp_tracker::TrackerViewer::countKltPoints_ [private] |
Definition at line 192 of file tracker-viewer.hh.
unsigned visp_tracker::TrackerViewer::countMovingEdgeSites_ [private] |
Definition at line 191 of file tracker-viewer.hh.
unsigned visp_tracker::TrackerViewer::countTrackingResult_ [private] |
Definition at line 190 of file tracker-viewer.hh.
volatile bool& visp_tracker::TrackerViewer::exiting_ [private] |
Definition at line 110 of file tracker-viewer.hh.
double visp_tracker::TrackerViewer::frameSize_ [private] |
Definition at line 121 of file tracker-viewer.hh.
image_t visp_tracker::TrackerViewer::image_ [private] |
ViSP image.
Definition at line 153 of file tracker-viewer.hh.
Definition at line 167 of file tracker-viewer.hh.
Image transport used to receive images.
Definition at line 119 of file tracker-viewer.hh.
sensor_msgs::CameraInfoConstPtr visp_tracker::TrackerViewer::info_ [private] |
Shared pointer to latest received camera information.
Definition at line 156 of file tracker-viewer.hh.
Service called when user ends tracker_client node.
Definition at line 134 of file tracker-viewer.hh.
visp_tracker::KltPoints::ConstPtr visp_tracker::TrackerViewer::klt_ [private] |
Shared pointer to latest received KLT point positions.
Definition at line 162 of file tracker-viewer.hh.
message_filters::Subscriber<visp_tracker::KltPoints> visp_tracker::TrackerViewer::kltPointsSubscriber_ [private] |
Subscriber to KLT point topics.
Definition at line 178 of file tracker-viewer.hh.
boost::filesystem::path visp_tracker::TrackerViewer::modelPath_ [private] |
Model path.
Definition at line 143 of file tracker-viewer.hh.
message_filters::Subscriber<visp_tracker::MovingEdgeSites> visp_tracker::TrackerViewer::movingEdgeSitesSubscriber_ [private] |
Subscriber to moving edge sites topics.
Definition at line 175 of file tracker-viewer.hh.
Definition at line 115 of file tracker-viewer.hh.
Definition at line 116 of file tracker-viewer.hh.
unsigned visp_tracker::TrackerViewer::queueSize_ [private] |
Queue size for all subscribers.
Definition at line 113 of file tracker-viewer.hh.
Service called when user is reconfiguring tracker node.
Definition at line 137 of file tracker-viewer.hh.
std::string visp_tracker::TrackerViewer::rectifiedImageTopic_ [private] |
Full topic name for rectified image.
Definition at line 127 of file tracker-viewer.hh.
visp_tracker::MovingEdgeSites::ConstPtr visp_tracker::TrackerViewer::sites_ [private] |
Shared pointer to latest received moving edge sites.
Definition at line 160 of file tracker-viewer.hh.
Synchronizer with approximate time policy.
Definition at line 181 of file tracker-viewer.hh.
Definition at line 186 of file tracker-viewer.hh.
vpMbEdgeTracker visp_tracker::TrackerViewer::tracker_ [private] |
ViSP edge tracker.
Definition at line 149 of file tracker-viewer.hh.
std::string visp_tracker::TrackerViewer::trackerName_ [private] |
Name of the tracker used in this viewer node.
Definition at line 140 of file tracker-viewer.hh.
message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped> visp_tracker::TrackerViewer::trackingResultSubscriber_ [private] |
Subscriber to tracking result topic.
Definition at line 172 of file tracker-viewer.hh.