1 #ifndef VISP_TRACKER_TRACKER_VIEWER_HH 2 # define VISP_TRACKER_TRACKER_VIEWER_HH 3 # include <boost/filesystem/path.hpp> 4 # include <boost/optional.hpp> 6 # include <geometry_msgs/PoseWithCovarianceStamped.h> 17 # include <sensor_msgs/Image.h> 18 # include <sensor_msgs/CameraInfo.h> 20 # include <visp_tracker/Init.h> 21 # include <visp_tracker/MovingEdgeSites.h> 22 # include <visp_tracker/KltPoints.h> 24 # include <visp/vpCameraParameters.h> 25 # include <visp/vpImage.h> 26 # include <visp/vpMbEdgeTracker.h> 38 typedef boost::function<bool (visp_tracker::Init::Request&,
39 visp_tracker::Init::Response& res)>
42 typedef boost::function<bool (visp_tracker::Init::Request&,
43 visp_tracker::Init::Response& res)>
54 sensor_msgs::Image, sensor_msgs::CameraInfo,
55 geometry_msgs::PoseWithCovarianceStamped,
56 visp_tracker::MovingEdgeSites,
57 visp_tracker::KltPoints
64 unsigned queueSize = 5u);
83 visp_tracker::Init::Response& res);
86 visp_tracker::Init::Response& res);
91 (
const sensor_msgs::ImageConstPtr& imageConst,
92 const sensor_msgs::CameraInfoConstPtr& infoConst,
93 const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& trackingResult,
94 const visp_tracker::MovingEdgeSites::ConstPtr& sitesConst,
95 const visp_tracker::KltPoints::ConstPtr& kltConst);
156 sensor_msgs::CameraInfoConstPtr
info_;
158 boost::optional<vpHomogeneousMatrix>
cMo_;
160 visp_tracker::MovingEdgeSites::ConstPtr
sites_;
162 visp_tracker::KltPoints::ConstPtr
klt_;
image_transport::ImageTransport imageTransport_
Image transport used to receive images.
vpImage< unsigned char > image_t
ViSP image type.
ros::NodeHandle & nodeHandle_
void displayMovingEdgeSites()
Display moving edge sites.
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.
boost::function< bool(visp_tracker::Init::Request &, visp_tracker::Init::Response &res)> initCallback_t
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.
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.
void displayKltPoints()
Display KLT points that are tracked.
boost::filesystem::path modelPath_
Model path.
image_t image_
ViSP image.
bool initCallback(visp_tracker::Init::Request &req, visp_tracker::Init::Response &res)
message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::CameraInfo, geometry_msgs::PoseWithCovarianceStamped, visp_tracker::MovingEdgeSites, visp_tracker::KltPoints > syncPolicy_t
Synchronization policy.
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.
unsigned countTrackingResult_
image_transport::SubscriberFilter imageSubscriber_
ros::ServiceServer initService_
Service called when user ends tracker_client node.
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.
Monitors the tracking result provided by the tracking node.
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.
std::string cameraInfoTopic_
Full topic name for camera information.
boost::optional< vpHomogeneousMatrix > cMo_
Last tracked object position, set to none if tracking failed.
unsigned countMovingEdgeSites_
void spin()
Display camera image, tracked object position and moving edge sites.
message_filters::Subscriber< sensor_msgs::CameraInfo > cameraInfoSubscriber_
Subscriber to camera information topic.
unsigned countCameraInfo_
visp_tracker::MovingEdgeSites::ConstPtr sites_
Shared pointer to latest received moving edge sites.
void loadCommonParameters()
Initialize the common parameters (visibility angles, etc)