single_board.cpp
Go to the documentation of this file.
00001 
00009 #include <iostream>
00010 #include <fstream>
00011 #include <sstream>
00012 #include <aruco/aruco.h>
00013 #include <aruco/boarddetector.h>
00014 #include <aruco/cvdrawingutils.h>
00015 
00016 #include <opencv2/core/core.hpp>
00017 #include <ros/ros.h>
00018 #include <image_transport/image_transport.h>
00019 #include <cv_bridge/cv_bridge.h>
00020 #include <sensor_msgs/image_encodings.h>
00021 #include <ar_sys/utils.h>
00022 #include <tf/transform_broadcaster.h>
00023 #include <tf/transform_listener.h>
00024 
00025 using namespace aruco;
00026 
00027 class ArSysSingleBoard
00028 {
00029         private:
00030                 cv::Mat inImage, resultImg;
00031                 aruco::CameraParameters camParam;
00032                 bool useRectifiedImages;
00033                 bool draw_markers;
00034                 bool draw_markers_cube;
00035                 bool draw_markers_axis;
00036                 MarkerDetector mDetector;
00037                 vector<Marker> markers;
00038                 BoardConfiguration the_board_config;
00039                 BoardDetector the_board_detector;
00040                 Board the_board_detected;
00041                 ros::Subscriber cam_info_sub;
00042                 bool cam_info_received;
00043                 image_transport::Publisher image_pub;
00044                 image_transport::Publisher debug_pub;
00045                 ros::Publisher pose_pub;
00046                 ros::Publisher transform_pub; 
00047                 ros::Publisher position_pub;
00048                 std::string board_frame;
00049 
00050                 double marker_size;
00051                 std::string board_config;
00052 
00053                 ros::NodeHandle nh;
00054                 image_transport::ImageTransport it;
00055                 image_transport::Subscriber image_sub;
00056 
00057                 tf::TransformListener _tfListener;
00058 
00059         public:
00060                 ArSysSingleBoard()
00061                         : cam_info_received(false),
00062                         nh("~"),
00063                         it(nh)
00064                 {
00065                         image_sub = it.subscribe("/image", 1, &ArSysSingleBoard::image_callback, this);
00066                         cam_info_sub = nh.subscribe("/camera_info", 1, &ArSysSingleBoard::cam_info_callback, this);
00067 
00068                         image_pub = it.advertise("result", 1);
00069                         debug_pub = it.advertise("debug", 1);
00070                         pose_pub = nh.advertise<geometry_msgs::PoseStamped>("pose", 100);
00071                         transform_pub = nh.advertise<geometry_msgs::TransformStamped>("transform", 100);
00072                         position_pub = nh.advertise<geometry_msgs::Vector3Stamped>("position", 100);
00073 
00074                         nh.param<double>("marker_size", marker_size, 0.05);
00075                         nh.param<std::string>("board_config", board_config, "boardConfiguration.yml");
00076                         nh.param<std::string>("board_frame", board_frame, "");
00077                         nh.param<bool>("image_is_rectified", useRectifiedImages, true);
00078                         nh.param<bool>("draw_markers", draw_markers, false);
00079                         nh.param<bool>("draw_markers_cube", draw_markers_cube, false);
00080                         nh.param<bool>("draw_markers_axis", draw_markers_axis, false);
00081 
00082                         the_board_config.readFromFile(board_config.c_str());
00083 
00084                         ROS_INFO("ArSys node started with marker size of %f m and board configuration: %s",
00085                                          marker_size, board_config.c_str());
00086                 }
00087 
00088                 void image_callback(const sensor_msgs::ImageConstPtr& msg)
00089                 {
00090                         if(!cam_info_received) return;
00091 
00092                         cv_bridge::CvImagePtr cv_ptr;
00093                         try
00094                         {
00095                                 cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::RGB8);
00096                                 inImage = cv_ptr->image;
00097                                 resultImg = cv_ptr->image.clone();
00098 
00099                                 //detection results will go into "markers"
00100                                 markers.clear();
00101                                 //Ok, let's detect
00102                                 mDetector.detect(inImage, markers, camParam, marker_size, false);
00103                                 //Detection of the board
00104                                 float probDetect=the_board_detector.detect(markers, the_board_config, the_board_detected, camParam, marker_size);
00105                                 if (probDetect > 0.0)
00106                                 {
00107                                         tf::Transform transform = ar_sys::getTf(the_board_detected.Rvec, the_board_detected.Tvec);
00108 
00109                                         tf::StampedTransform stampedTransform(transform, msg->header.stamp, msg->header.frame_id, board_frame);
00110 
00111                                         geometry_msgs::PoseStamped poseMsg;
00112                                         tf::poseTFToMsg(transform, poseMsg.pose);
00113                                         poseMsg.header.frame_id = msg->header.frame_id;
00114                                         poseMsg.header.stamp = msg->header.stamp;
00115                                         pose_pub.publish(poseMsg);
00116 
00117                                         geometry_msgs::TransformStamped transformMsg;
00118                                         tf::transformStampedTFToMsg(stampedTransform, transformMsg);
00119                                         transform_pub.publish(transformMsg);
00120 
00121                                         geometry_msgs::Vector3Stamped positionMsg;
00122                                         positionMsg.header = transformMsg.header;
00123                                         positionMsg.vector = transformMsg.transform.translation;
00124                                         position_pub.publish(positionMsg);
00125                                 }
00126                                 //for each marker, draw info and its boundaries in the image
00127                                 for(size_t i=0; draw_markers && i < markers.size(); ++i)
00128                                 {
00129                                         markers[i].draw(resultImg,cv::Scalar(0,0,255),2);
00130                                 }
00131 
00132 
00133                                 if(camParam.isValid() && marker_size != -1)
00134                                 {
00135                                         //draw a 3d cube in each marker if there is 3d info
00136                                         for(size_t i=0; i<markers.size(); ++i)
00137                                         {
00138                                                 if (draw_markers_cube) CvDrawingUtils::draw3dCube(resultImg, markers[i], camParam);
00139                                                 if (draw_markers_axis) CvDrawingUtils::draw3dAxis(resultImg, markers[i], camParam);
00140                                         }
00141                                         //draw board axis
00142                                         if (probDetect > 0.0) CvDrawingUtils::draw3dAxis(resultImg, the_board_detected, camParam);
00143                                 }
00144 
00145                                 if(image_pub.getNumSubscribers() > 0)
00146                                 {
00147                                         //show input with augmented information
00148                                         cv_bridge::CvImage out_msg;
00149                                         out_msg.header.frame_id = msg->header.frame_id;
00150                                         out_msg.header.stamp = msg->header.stamp;
00151                                         out_msg.encoding = sensor_msgs::image_encodings::RGB8;
00152                                         out_msg.image = resultImg;
00153                                         image_pub.publish(out_msg.toImageMsg());
00154                                 }
00155 
00156                                 if(debug_pub.getNumSubscribers() > 0)
00157                                 {
00158                                         //show also the internal image resulting from the threshold operation
00159                                         cv_bridge::CvImage debug_msg;
00160                                         debug_msg.header.frame_id = msg->header.frame_id;
00161                                         debug_msg.header.stamp = msg->header.stamp;
00162                                         debug_msg.encoding = sensor_msgs::image_encodings::MONO8;
00163                                         debug_msg.image = mDetector.getThresholdedImage();
00164                                         debug_pub.publish(debug_msg.toImageMsg());
00165                                 }
00166                         }
00167                         catch (cv_bridge::Exception& e)
00168                         {
00169                                 ROS_ERROR("cv_bridge exception: %s", e.what());
00170                                 return;
00171                         }
00172                 }
00173 
00174                 // wait for one camerainfo, then shut down that subscriber
00175                 void cam_info_callback(const sensor_msgs::CameraInfo &msg)
00176                 {
00177                         camParam = ar_sys::getCamParams(msg, useRectifiedImages);
00178                         cam_info_received = true;
00179                         cam_info_sub.shutdown();
00180                 }
00181 };
00182 
00183 
00184 int main(int argc,char **argv)
00185 {
00186         ros::init(argc, argv, "ar_single_board");
00187 
00188         ArSysSingleBoard node;
00189 
00190         ros::spin();
00191 }


ar_sys
Author(s): Hamdi Sahloul , Rafael Muñoz Salinas , Bence Magyar
autogenerated on Sun Oct 5 2014 22:12:49