34 #include <opencv2/highgui/highgui.hpp> 35 #include <std_msgs/Header.h> 49 nh.
advertise<AprilTagDetectionArray>(
"tag_detections", 1);
54 apriltag_ros::AnalyzeSingleImage::Request& request,
55 apriltag_ros::AnalyzeSingleImage::Response& response)
58 ROS_INFO(
"[ Summoned to analyze image ]");
60 request.full_path_where_to_get_image.c_str());
62 request.full_path_where_to_save_image.c_str());
65 cv::Mat image = cv::imread(request.full_path_where_to_get_image,
67 if (image.data == NULL)
71 request.full_path_where_to_get_image.c_str());
78 loaded_image->header.frame_id =
"camera";
79 response.tag_detections =
81 new sensor_msgs::CameraInfo(request.camera_info)));
89 cv::imwrite(request.full_path_where_to_save_image, loaded_image->image);
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)
ros::Publisher tag_detections_publisher_
void drawDetections(cv_bridge::CvImagePtr image)
bool analyzeImage(apriltag_ros::AnalyzeSingleImage::Request &request, apriltag_ros::AnalyzeSingleImage::Response &response)
SingleImageDetector(ros::NodeHandle &nh, ros::NodeHandle &pnh)
TagDetector tag_detector_
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)