Go to the documentation of this file.00001 #include <apriltags_ros/apriltag_detector.h>
00002 #include <cv_bridge/cv_bridge.h>
00003 #include <sensor_msgs/image_encodings.h>
00004 #include <boost/foreach.hpp>
00005 #include <geometry_msgs/PoseStamped.h>
00006 #include <geometry_msgs/PoseArray.h>
00007 #include <apriltags_ros/AprilTagDetection.h>
00008 #include <apriltags_ros/AprilTagDetectionArray.h>
00009 #include <AprilTags/Tag16h5.h>
00010 #include <AprilTags/Tag25h7.h>
00011 #include <AprilTags/Tag25h9.h>
00012 #include <AprilTags/Tag36h9.h>
00013 #include <AprilTags/Tag36h11.h>
00014 #include <XmlRpcException.h>
00015
00016 namespace apriltags_ros{
00017
00018 AprilTagDetector::AprilTagDetector(ros::NodeHandle& nh, ros::NodeHandle& pnh): it_(nh){
00019 XmlRpc::XmlRpcValue april_tag_descriptions;
00020 if(!pnh.getParam("tag_descriptions", april_tag_descriptions)){
00021 ROS_WARN("No april tags specified");
00022 }
00023 else{
00024 try{
00025 descriptions_ = parse_tag_descriptions(april_tag_descriptions);
00026 } catch(XmlRpc::XmlRpcException e){
00027 ROS_ERROR_STREAM("Error loading tag descriptions: "<<e.getMessage());
00028 }
00029 }
00030
00031 if(!pnh.getParam("sensor_frame_id", sensor_frame_id_)){
00032 sensor_frame_id_ = "";
00033 }
00034
00035 AprilTags::TagCodes tag_codes = AprilTags::tagCodes36h11;
00036 tag_detector_= boost::shared_ptr<AprilTags::TagDetector>(new AprilTags::TagDetector(tag_codes));
00037 image_sub_ = it_.subscribeCamera("image_rect", 1, &AprilTagDetector::imageCb, this);
00038 image_pub_ = it_.advertise("tag_detections_image", 1);
00039 detections_pub_ = nh.advertise<AprilTagDetectionArray>("tag_detections", 1);
00040 pose_pub_ = nh.advertise<geometry_msgs::PoseArray>("tag_detections_pose", 1);
00041 }
00042 AprilTagDetector::~AprilTagDetector(){
00043 image_sub_.shutdown();
00044 }
00045
00046 void AprilTagDetector::imageCb(const sensor_msgs::ImageConstPtr& msg,const sensor_msgs::CameraInfoConstPtr& cam_info){
00047 cv_bridge::CvImagePtr cv_ptr;
00048 try{
00049 cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
00050 }
00051 catch (cv_bridge::Exception& e){
00052 ROS_ERROR("cv_bridge exception: %s", e.what());
00053 return;
00054 }
00055 cv::Mat gray;
00056 cv::cvtColor(cv_ptr->image, gray, CV_BGR2GRAY);
00057 std::vector<AprilTags::TagDetection> detections = tag_detector_->extractTags(gray);
00058 ROS_DEBUG("%d tag detected", (int)detections.size());
00059
00060 double fx = cam_info->K[0];
00061 double fy = cam_info->K[4];
00062 double px = cam_info->K[2];
00063 double py = cam_info->K[5];
00064
00065 if(!sensor_frame_id_.empty())
00066 cv_ptr->header.frame_id = sensor_frame_id_;
00067
00068 AprilTagDetectionArray tag_detection_array;
00069 geometry_msgs::PoseArray tag_pose_array;
00070 tag_pose_array.header = cv_ptr->header;
00071
00072 BOOST_FOREACH(AprilTags::TagDetection detection, detections){
00073 std::map<int, AprilTagDescription>::const_iterator description_itr = descriptions_.find(detection.id);
00074 if(description_itr == descriptions_.end()){
00075 ROS_WARN_THROTTLE(10.0, "Found tag: %d, but no description was found for it", detection.id);
00076 continue;
00077 }
00078 AprilTagDescription description = description_itr->second;
00079 double tag_size = description.size();
00080
00081 detection.draw(cv_ptr->image);
00082 Eigen::Matrix4d transform = detection.getRelativeTransform(tag_size, fx, fy, px, py);
00083 Eigen::Matrix3d rot = transform.block(0,0,3,3);
00084 Eigen::Quaternion<double> rot_quaternion = Eigen::Quaternion<double>(rot);
00085
00086 geometry_msgs::PoseStamped tag_pose;
00087 tag_pose.pose.position.x = transform(0,3);
00088 tag_pose.pose.position.y = transform(1,3);
00089 tag_pose.pose.position.z = transform(2,3);
00090 tag_pose.pose.orientation.x = rot_quaternion.x();
00091 tag_pose.pose.orientation.y = rot_quaternion.y();
00092 tag_pose.pose.orientation.z = rot_quaternion.z();
00093 tag_pose.pose.orientation.w = rot_quaternion.w();
00094 tag_pose.header = cv_ptr->header;
00095
00096 AprilTagDetection tag_detection;
00097 tag_detection.pose = tag_pose;
00098 tag_detection.id = detection.id;
00099 tag_detection.size = tag_size;
00100 tag_detection_array.detections.push_back(tag_detection);
00101 tag_pose_array.poses.push_back(tag_pose.pose);
00102
00103 tf::Stamped<tf::Transform> tag_transform;
00104 tf::poseStampedMsgToTF(tag_pose, tag_transform);
00105 tf_pub_.sendTransform(tf::StampedTransform(tag_transform, tag_transform.stamp_, tag_transform.frame_id_, description.frame_name()));
00106 }
00107 detections_pub_.publish(tag_detection_array);
00108 pose_pub_.publish(tag_pose_array);
00109 image_pub_.publish(cv_ptr->toImageMsg());
00110 }
00111
00112
00113 std::map<int, AprilTagDescription> AprilTagDetector::parse_tag_descriptions(XmlRpc::XmlRpcValue& tag_descriptions){
00114 std::map<int, AprilTagDescription> descriptions;
00115 ROS_ASSERT(tag_descriptions.getType() == XmlRpc::XmlRpcValue::TypeArray);
00116 for (int32_t i = 0; i < tag_descriptions.size(); ++i) {
00117 XmlRpc::XmlRpcValue& tag_description = tag_descriptions[i];
00118 ROS_ASSERT(tag_description.getType() == XmlRpc::XmlRpcValue::TypeStruct);
00119 ROS_ASSERT(tag_description["id"].getType() == XmlRpc::XmlRpcValue::TypeInt);
00120 ROS_ASSERT(tag_description["size"].getType() == XmlRpc::XmlRpcValue::TypeDouble);
00121
00122 int id = (int)tag_description["id"];
00123 double size = (double)tag_description["size"];
00124
00125 std::string frame_name;
00126 if(tag_description.hasMember("frame_id")){
00127 ROS_ASSERT(tag_description["frame_id"].getType() == XmlRpc::XmlRpcValue::TypeString);
00128 frame_name = (std::string)tag_description["frame_id"];
00129 }
00130 else{
00131 std::stringstream frame_name_stream;
00132 frame_name_stream << "tag_" << id;
00133 frame_name = frame_name_stream.str();
00134 }
00135 AprilTagDescription description(id, size, frame_name);
00136 ROS_INFO_STREAM("Loaded tag config: "<<id<<", size: "<<size<<", frame_name: "<<frame_name);
00137 descriptions.insert(std::make_pair(id, description));
00138 }
00139 return descriptions;
00140 }
00141
00142
00143 }