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 message_filters::sync_policies::ApproximateTime < sensor_msgs::Image, sensor_msgs::CameraInfo, visp_tracker::TrackingResult, visp_tracker::MovingEdgeSites > | syncPolicy_t |
Synchronization policy. | |
Public Member Functions | |
void | spin () |
Display camera image, tracked object position and moving edge sites. | |
TrackerViewer (unsigned queueSize=5u) | |
Constructor. | |
Protected Member Functions | |
void | callback (const sensor_msgs::ImageConstPtr &imageConst, const sensor_msgs::CameraInfoConstPtr &infoConst, const visp_tracker::TrackingResult::ConstPtr &trackingResult, const visp_tracker::MovingEdgeSites::ConstPtr &sitesConst) |
Callback used to received synchronized data. | |
void | checkInputs () |
Make sure the topics we subscribe already exist. | |
void | displayMovingEdgeSites () |
Display moving edge sites. | |
void | initializeTracker () |
Initialize the tracker. | |
void | timerCallback () |
void | waitForImage () |
Hang until the first image is received. | |
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. | |
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::NodeHandle | nodeHandle_ |
Main node handle. | |
unsigned | queueSize_ |
Queue size for all subscribers. | |
visp_tracker::MovingEdgeSites::ConstPtr | sites_ |
Shared pointer to latest received moving edge sites. | |
vpMbEdgeTracker | tracker_ |
ViSP edge tracker. | |
boost::filesystem::path | vrmlPath_ |
VRML model path. | |
Subscribers and synchronizer. | |
message_filters::Subscriber < sensor_msgs::CameraInfo > | cameraInfoSubscriber_ |
Subscriber to camera information topic. | |
image_transport::SubscriberFilter | imageSubscriber_ |
message_filters::Subscriber < visp_tracker::MovingEdgeSites > | movingEdgeSitesSubscriber_ |
Subscriber to moving edge sites topics.. | |
message_filters::Synchronizer < syncPolicy_t > | synchronizer_ |
Synchronizer with approximate time policy. | |
message_filters::Subscriber < visp_tracker::TrackingResult > | trackingResultSubscriber_ |
Subscriber to tracking result topic. | |
Topics and services strings. | |
std::string | cameraInfoTopic_ |
Full topic name for camera information. | |
std::string | rectifiedImageTopic_ |
Full topic name for rectified image. | |
Synchronization check | |
unsigned | countAll_ |
unsigned | countCameraInfo_ |
unsigned | countImages_ |
unsigned | countMovingEdgeSites_ |
unsigned | countTrackingResult_ |
ros::WallTimer | timer_ |
Monitors the tracking result provided by the tracking node.
Definition at line 28 of file tracker-viewer.hh.
typedef vpImage<unsigned char> visp_tracker::TrackerViewer::image_t |
ViSP image type.
Definition at line 32 of file tracker-viewer.hh.
typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::CameraInfo, visp_tracker::TrackingResult, visp_tracker::MovingEdgeSites > 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 44 of file tracker-viewer.hh.
visp_tracker::TrackerViewer::TrackerViewer | ( | unsigned | queueSize = 5u |
) |
Constructor.
Definition at line 33 of file tracker-viewer.cpp.
void visp_tracker::TrackerViewer::callback | ( | const sensor_msgs::ImageConstPtr & | imageConst, | |
const sensor_msgs::CameraInfoConstPtr & | infoConst, | |||
const visp_tracker::TrackingResult::ConstPtr & | trackingResult, | |||
const visp_tracker::MovingEdgeSites::ConstPtr & | sitesConst | |||
) | [protected] |
Callback used to received synchronized data.
Definition at line 259 of file tracker-viewer.cpp.
void visp_tracker::TrackerViewer::checkInputs | ( | ) | [protected] |
Make sure the topics we subscribe already exist.
Definition at line 230 of file tracker-viewer.cpp.
void visp_tracker::TrackerViewer::displayMovingEdgeSites | ( | ) | [protected] |
Display moving edge sites.
Definition at line 289 of file tracker-viewer.cpp.
void visp_tracker::TrackerViewer::initializeTracker | ( | ) | [protected] |
Initialize the tracker.
Definition at line 241 of file tracker-viewer.cpp.
void visp_tracker::TrackerViewer::spin | ( | ) |
Display camera image, tracked object position and moving edge sites.
Definition at line 167 of file tracker-viewer.cpp.
void visp_tracker::TrackerViewer::timerCallback | ( | ) | [protected] |
Definition at line 323 of file tracker-viewer.cpp.
void visp_tracker::TrackerViewer::waitForImage | ( | ) | [protected] |
Hang until the first image is received.
Definition at line 217 of file tracker-viewer.cpp.
message_filters::Subscriber<sensor_msgs::CameraInfo> visp_tracker::TrackerViewer::cameraInfoSubscriber_ [private] |
Subscriber to camera information topic.
Definition at line 119 of file tracker-viewer.hh.
std::string visp_tracker::TrackerViewer::cameraInfoTopic_ [private] |
Full topic name for camera information.
Definition at line 89 of file tracker-viewer.hh.
vpCameraParameters visp_tracker::TrackerViewer::cameraParameters_ [private] |
ViSP camera parameters.
Definition at line 103 of file tracker-viewer.hh.
image_proc::AdvertisementChecker visp_tracker::TrackerViewer::checkInputs_ [private] |
Helper used to check that subscribed topics exist.
Definition at line 98 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 110 of file tracker-viewer.hh.
unsigned visp_tracker::TrackerViewer::countAll_ [private] |
Definition at line 134 of file tracker-viewer.hh.
unsigned visp_tracker::TrackerViewer::countCameraInfo_ [private] |
Definition at line 136 of file tracker-viewer.hh.
unsigned visp_tracker::TrackerViewer::countImages_ [private] |
Definition at line 135 of file tracker-viewer.hh.
unsigned visp_tracker::TrackerViewer::countMovingEdgeSites_ [private] |
Definition at line 138 of file tracker-viewer.hh.
unsigned visp_tracker::TrackerViewer::countTrackingResult_ [private] |
Definition at line 137 of file tracker-viewer.hh.
image_t visp_tracker::TrackerViewer::image_ [private] |
ViSP image.
Definition at line 105 of file tracker-viewer.hh.
image_transport::SubscriberFilter visp_tracker::TrackerViewer::imageSubscriber_ [private] |
Definition at line 117 of file tracker-viewer.hh.
image_transport::ImageTransport visp_tracker::TrackerViewer::imageTransport_ [private] |
Image transport used to receive images.
Definition at line 80 of file tracker-viewer.hh.
sensor_msgs::CameraInfoConstPtr visp_tracker::TrackerViewer::info_ [private] |
Shared pointer to latest received camera information.
Definition at line 108 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 125 of file tracker-viewer.hh.
ros::NodeHandle visp_tracker::TrackerViewer::nodeHandle_ [private] |
Main node handle.
Definition at line 78 of file tracker-viewer.hh.
unsigned visp_tracker::TrackerViewer::queueSize_ [private] |
Queue size for all subscribers.
Definition at line 75 of file tracker-viewer.hh.
std::string visp_tracker::TrackerViewer::rectifiedImageTopic_ [private] |
Full topic name for rectified image.
Definition at line 87 of file tracker-viewer.hh.
Shared pointer to latest received moving edge sites.
Definition at line 112 of file tracker-viewer.hh.
message_filters::Synchronizer<syncPolicy_t> visp_tracker::TrackerViewer::synchronizer_ [private] |
Synchronizer with approximate time policy.
Definition at line 128 of file tracker-viewer.hh.
ros::WallTimer visp_tracker::TrackerViewer::timer_ [private] |
Definition at line 133 of file tracker-viewer.hh.
vpMbEdgeTracker visp_tracker::TrackerViewer::tracker_ [private] |
ViSP edge tracker.
Definition at line 101 of file tracker-viewer.hh.
message_filters::Subscriber<visp_tracker::TrackingResult> visp_tracker::TrackerViewer::trackingResultSubscriber_ [private] |
Subscriber to tracking result topic.
Definition at line 122 of file tracker-viewer.hh.
boost::filesystem::path visp_tracker::TrackerViewer::vrmlPath_ [private] |
VRML model path.
Definition at line 95 of file tracker-viewer.hh.