single_image_detector.cpp
Go to the documentation of this file.
1 
33 
34 #include <opencv2/highgui/highgui.hpp>
35 #include <std_msgs/Header.h>
36 
37 namespace apriltag_ros
38 {
39 
41  ros::NodeHandle& pnh) :
42  tag_detector_(pnh)
43 {
44  // Advertise the single image analysis service
46  nh.advertiseService("single_image_tag_detection",
49  nh.advertise<AprilTagDetectionArray>("tag_detections", 1);
50  ROS_INFO_STREAM("Ready to do tag detection on single images");
51 }
52 
54  apriltag_ros::AnalyzeSingleImage::Request& request,
55  apriltag_ros::AnalyzeSingleImage::Response& response)
56 {
57 
58  ROS_INFO("[ Summoned to analyze image ]");
59  ROS_INFO("Image load path: %s",
60  request.full_path_where_to_get_image.c_str());
61  ROS_INFO("Image save path: %s",
62  request.full_path_where_to_save_image.c_str());
63 
64  // Read the image
65  cv::Mat image = cv::imread(request.full_path_where_to_get_image,
66  cv::IMREAD_COLOR);
67  if (image.data == NULL)
68  {
69  // Cannot read image
70  ROS_ERROR_STREAM("Could not read image " <<
71  request.full_path_where_to_get_image.c_str());
72  return false;
73  }
74 
75  // Detect tags in the image
76  cv_bridge::CvImagePtr loaded_image(new cv_bridge::CvImage(std_msgs::Header(),
77  "bgr8", image));
78  loaded_image->header.frame_id = "camera";
79  response.tag_detections =
80  tag_detector_.detectTags(loaded_image,sensor_msgs::CameraInfoConstPtr(
81  new sensor_msgs::CameraInfo(request.camera_info)));
82 
83  // Publish detected tags (AprilTagDetectionArray, basically an array of
84  // geometry_msgs/PoseWithCovarianceStamped)
85  tag_detections_publisher_.publish(response.tag_detections);
86 
87  // Save tag detections image
88  tag_detector_.drawDetections(loaded_image);
89  cv::imwrite(request.full_path_where_to_save_image, loaded_image->image);
90 
91  ROS_INFO("Done!\n");
92 
93  return true;
94 }
95 
96 } // namespace apriltag_ros
void publish(const boost::shared_ptr< M > &message) const
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
AprilTagDetectionArray detectTags(const cv_bridge::CvImagePtr &image, const sensor_msgs::CameraInfoConstPtr &camera_info)
void drawDetections(cv_bridge::CvImagePtr image)
bool analyzeImage(apriltag_ros::AnalyzeSingleImage::Request &request, apriltag_ros::AnalyzeSingleImage::Response &response)
#define ROS_INFO(...)
SingleImageDetector(ros::NodeHandle &nh, ros::NodeHandle &pnh)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::ServiceServer single_image_analysis_service_
#define ROS_INFO_STREAM(args)
#define ROS_ERROR_STREAM(args)


apriltag_ros
Author(s): Danylo Malyuta
autogenerated on Thu Jul 16 2020 03:53:54