continuous_detector.cpp
Go to the documentation of this file.
1 
33 
35 
37 
38 namespace apriltag_ros
39 {
41 {
44 
45  tag_detector_ = std::shared_ptr<TagDetector>(new TagDetector(pnh));
46  draw_tag_detections_image_ = getAprilTagOption<bool>(pnh,
47  "publish_tag_detections_image", false);
48  it_ = std::shared_ptr<image_transport::ImageTransport>(
50 
52  it_->subscribeCamera("image_rect", 1,
55  nh.advertise<AprilTagDetectionArray>("tag_detections", 1);
56  if (draw_tag_detections_image_)
57  {
58  tag_detections_image_publisher_ = it_->advertise("tag_detections_image", 1);
59  }
60 }
61 
63  const sensor_msgs::ImageConstPtr& image_rect,
64  const sensor_msgs::CameraInfoConstPtr& camera_info)
65 {
66  // Lazy updates:
67  // When there are no subscribers _and_ when tf is not published,
68  // skip detection.
71  !tag_detector_->get_publish_tf())
72  {
73  // ROS_INFO_STREAM("No subscribers and no tf publishing, skip processing.");
74  return;
75  }
76 
77  // Convert ROS's sensor_msgs::Image to cv_bridge::CvImagePtr in order to run
78  // AprilTag 2 on the iamge
79  try
80  {
81  cv_image_ = cv_bridge::toCvCopy(image_rect, image_rect->encoding);
82  }
83  catch (cv_bridge::Exception& e)
84  {
85  ROS_ERROR("cv_bridge exception: %s", e.what());
86  return;
87  }
88 
89  // Publish detected tags in the image by AprilTag 2
91  tag_detector_->detectTags(cv_image_,camera_info));
92 
93  // Publish the camera image overlaid by outlines of the detected tags and
94  // their payload values
96  {
97  tag_detector_->drawDetections(cv_image_);
99  }
100 }
101 
102 } // namespace apriltag_ros
void publish(const boost::shared_ptr< M > &message) const
image_transport::CameraSubscriber camera_image_subscriber_
uint32_t getNumSubscribers() const
ros::NodeHandle & getPrivateNodeHandle() const
std::shared_ptr< TagDetector > tag_detector_
void publish(const sensor_msgs::Image &message) const
image_transport::Publisher tag_detections_image_publisher_
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
std::shared_ptr< image_transport::ImageTransport > it_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::NodeHandle & getNodeHandle() const
uint32_t getNumSubscribers() const
void imageCallback(const sensor_msgs::ImageConstPtr &image_rect, const sensor_msgs::CameraInfoConstPtr &camera_info)
#define ROS_ERROR(...)
PLUGINLIB_EXPORT_CLASS(apriltag_ros::ContinuousDetector, nodelet::Nodelet)


apriltag_ros
Author(s): Danylo Malyuta
autogenerated on Sat Apr 10 2021 02:59:26