simple_single.cpp
Go to the documentation of this file.
00001 /*****************************
00002 Copyright 2011 Rafael Muñoz Salinas. All rights reserved.
00003 
00004 Redistribution and use in source and binary forms, with or without modification, are
00005 permitted provided that the following conditions are met:
00006 
00007    1. Redistributions of source code must retain the above copyright notice, this list of
00008       conditions and the following disclaimer.
00009 
00010    2. Redistributions in binary form must reproduce the above copyright notice, this list
00011       of conditions and the following disclaimer in the documentation and/or other materials
00012       provided with the distribution.
00013 
00014 THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED
00015 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
00016 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR
00017 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00018 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00019 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
00020 ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
00021 NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00022 ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00023 
00024 The views and conclusions contained in the software and documentation are those of the
00025 authors and should not be interpreted as representing official policies, either expressed
00026 or implied, of Rafael Muñoz Salinas.
00027 ********************************/
00036 #include <iostream>
00037 #include <aruco/aruco.h>
00038 #include <aruco/cvdrawingutils.h>
00039 
00040 #include <opencv2/core/core.hpp>
00041 #include <ros/ros.h>
00042 #include <image_transport/image_transport.h>
00043 #include <cv_bridge/cv_bridge.h>
00044 #include <sensor_msgs/image_encodings.h>
00045 #include <aruco_ros/aruco_ros_utils.h>
00046 #include <tf/transform_broadcaster.h>
00047 #include <tf/transform_listener.h>
00048 #include <visualization_msgs/Marker.h>
00049 
00050 #include <dynamic_reconfigure/server.h>
00051 #include <aruco_ros/ArucoThresholdConfig.h>
00052 using namespace aruco;
00053 
00054 class ArucoSimple
00055 {
00056 private:
00057   cv::Mat inImage;
00058   aruco::CameraParameters camParam;
00059   tf::StampedTransform rightToLeft;
00060   bool useRectifiedImages;
00061   MarkerDetector mDetector;
00062   vector<Marker> markers;
00063   ros::Subscriber cam_info_sub;
00064   bool cam_info_received;
00065   image_transport::Publisher image_pub;
00066   image_transport::Publisher debug_pub;
00067   ros::Publisher pose_pub;
00068   ros::Publisher transform_pub; 
00069   ros::Publisher position_pub;
00070   ros::Publisher marker_pub; //rviz visualization marker
00071   ros::Publisher pixel_pub;
00072   std::string marker_frame;
00073   std::string camera_frame;
00074   std::string reference_frame;
00075 
00076   double marker_size;
00077   int marker_id;
00078 
00079   ros::NodeHandle nh;
00080   image_transport::ImageTransport it;
00081   image_transport::Subscriber image_sub;
00082 
00083   tf::TransformListener _tfListener;
00084 
00085   dynamic_reconfigure::Server<aruco_ros::ArucoThresholdConfig> dyn_rec_server;
00086 
00087 public:
00088   ArucoSimple()
00089     : cam_info_received(false),
00090       nh("~"),
00091       it(nh)
00092   {
00093 
00094     std::string refinementMethod;
00095     nh.param<std::string>("corner_refinement", refinementMethod, "LINES");
00096     if ( refinementMethod == "SUBPIX" )
00097       mDetector.setCornerRefinementMethod(aruco::MarkerDetector::SUBPIX);
00098     else if ( refinementMethod == "HARRIS" )
00099       mDetector.setCornerRefinementMethod(aruco::MarkerDetector::HARRIS);
00100     else if ( refinementMethod == "NONE" )
00101       mDetector.setCornerRefinementMethod(aruco::MarkerDetector::NONE); 
00102     else      
00103       mDetector.setCornerRefinementMethod(aruco::MarkerDetector::LINES); 
00104 
00105     //Print parameters of aruco marker detector:
00106     ROS_INFO_STREAM("Corner refinement method: " << mDetector.getCornerRefinementMethod());
00107     ROS_INFO_STREAM("Threshold method: " << mDetector.getThresholdMethod());
00108     double th1, th2;
00109     mDetector.getThresholdParams(th1, th2);
00110     ROS_INFO_STREAM("Threshold method: " << " th1: " << th1 << " th2: " << th2);
00111     float mins, maxs;
00112     mDetector.getMinMaxSize(mins, maxs);
00113     ROS_INFO_STREAM("Marker size min: " << mins << "  max: " << maxs);
00114     ROS_INFO_STREAM("Desired speed: " << mDetector.getDesiredSpeed());
00115     
00116 
00117 
00118     image_sub = it.subscribe("/image", 1, &ArucoSimple::image_callback, this);
00119     cam_info_sub = nh.subscribe("/camera_info", 1, &ArucoSimple::cam_info_callback, this);
00120 
00121     image_pub = it.advertise("result", 1);
00122     debug_pub = it.advertise("debug", 1);
00123     pose_pub = nh.advertise<geometry_msgs::PoseStamped>("pose", 100);
00124     transform_pub = nh.advertise<geometry_msgs::TransformStamped>("transform", 100);
00125     position_pub = nh.advertise<geometry_msgs::Vector3Stamped>("position", 100);
00126     marker_pub = nh.advertise<visualization_msgs::Marker>("marker", 10);
00127     pixel_pub = nh.advertise<geometry_msgs::PointStamped>("pixel", 10);
00128 
00129     nh.param<double>("marker_size", marker_size, 0.05);
00130     nh.param<int>("marker_id", marker_id, 300);
00131     nh.param<std::string>("reference_frame", reference_frame, "");
00132     nh.param<std::string>("camera_frame", camera_frame, "");
00133     nh.param<std::string>("marker_frame", marker_frame, "");
00134     nh.param<bool>("image_is_rectified", useRectifiedImages, true);
00135 
00136     ROS_ASSERT(camera_frame != "" && marker_frame != "");
00137 
00138     if ( reference_frame.empty() )
00139       reference_frame = camera_frame;
00140 
00141     ROS_INFO("Aruco node started with marker size of %f m and marker id to track: %d",
00142              marker_size, marker_id);
00143     ROS_INFO("Aruco node will publish pose to TF with %s as parent and %s as child.",
00144              reference_frame.c_str(), marker_frame.c_str());
00145 
00146     dyn_rec_server.setCallback(boost::bind(&ArucoSimple::reconf_callback, this, _1, _2));
00147   }
00148 
00149   bool getTransform(const std::string& refFrame,
00150                     const std::string& childFrame,
00151                     tf::StampedTransform& transform)
00152   {
00153     std::string errMsg;
00154 
00155     if ( !_tfListener.waitForTransform(refFrame,
00156                                        childFrame,
00157                                        ros::Time(0),
00158                                        ros::Duration(0.5),
00159                                        ros::Duration(0.01),
00160                                        &errMsg)
00161          )
00162     {
00163       ROS_ERROR_STREAM("Unable to get pose from TF: " << errMsg);
00164       return false;
00165     }
00166     else
00167     {
00168       try
00169       {
00170         _tfListener.lookupTransform( refFrame, childFrame,
00171                                      ros::Time(0),                  //get latest available
00172                                      transform);
00173       }
00174       catch ( const tf::TransformException& e)
00175       {
00176         ROS_ERROR_STREAM("Error in lookupTransform of " << childFrame << " in " << refFrame);
00177         return false;
00178       }
00179 
00180     }
00181     return true;
00182   }
00183 
00184 
00185   void image_callback(const sensor_msgs::ImageConstPtr& msg)
00186   {
00187     if ((image_pub.getNumSubscribers() == 0) &&
00188         (debug_pub.getNumSubscribers() == 0) &&
00189         (pose_pub.getNumSubscribers() == 0) &&
00190         (transform_pub.getNumSubscribers() == 0) &&
00191         (position_pub.getNumSubscribers() == 0) &&
00192         (marker_pub.getNumSubscribers() == 0) &&
00193         (pixel_pub.getNumSubscribers() == 0))
00194     {
00195       ROS_DEBUG("No subscribers, not looking for aruco markers");
00196       return;
00197     }
00198 
00199     static tf::TransformBroadcaster br;
00200     if(cam_info_received)
00201     {
00202       ros::Time curr_stamp(ros::Time::now());
00203       cv_bridge::CvImagePtr cv_ptr;
00204       try
00205       {
00206         cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::RGB8);
00207         inImage = cv_ptr->image;
00208 
00209         //detection results will go into "markers"
00210         markers.clear();
00211         //Ok, let's detect
00212         mDetector.detect(inImage, markers, camParam, marker_size, false);
00213         //for each marker, draw info and its boundaries in the image
00214         for(size_t i=0; i<markers.size(); ++i)
00215         {
00216           // only publishing the selected marker
00217           if(markers[i].id == marker_id)
00218           {
00219             tf::Transform transform = aruco_ros::arucoMarker2Tf(markers[i]);
00220             tf::StampedTransform cameraToReference;
00221             cameraToReference.setIdentity();
00222 
00223             if ( reference_frame != camera_frame )
00224             {
00225               getTransform(reference_frame,
00226                            camera_frame,
00227                            cameraToReference);
00228             }
00229 
00230             transform = 
00231               static_cast<tf::Transform>(cameraToReference) 
00232               * static_cast<tf::Transform>(rightToLeft) 
00233               * transform;
00234 
00235             tf::StampedTransform stampedTransform(transform, curr_stamp,
00236                                                   reference_frame, marker_frame);
00237             br.sendTransform(stampedTransform);
00238             geometry_msgs::PoseStamped poseMsg;
00239             tf::poseTFToMsg(transform, poseMsg.pose);
00240             poseMsg.header.frame_id = reference_frame;
00241             poseMsg.header.stamp = curr_stamp;
00242             pose_pub.publish(poseMsg);
00243 
00244             geometry_msgs::TransformStamped transformMsg;
00245             tf::transformStampedTFToMsg(stampedTransform, transformMsg);
00246             transform_pub.publish(transformMsg);
00247 
00248             geometry_msgs::Vector3Stamped positionMsg;
00249             positionMsg.header = transformMsg.header;
00250             positionMsg.vector = transformMsg.transform.translation;
00251             position_pub.publish(positionMsg);
00252 
00253             geometry_msgs::PointStamped pixelMsg;
00254             pixelMsg.header = transformMsg.header;
00255             pixelMsg.point.x = markers[i].getCenter().x;
00256             pixelMsg.point.y = markers[i].getCenter().y;
00257             pixelMsg.point.z = 0;
00258             pixel_pub.publish(pixelMsg);
00259 
00260             //Publish rviz marker representing the ArUco marker patch
00261             visualization_msgs::Marker visMarker;
00262             visMarker.header = transformMsg.header;
00263             visMarker.pose = poseMsg.pose;
00264             visMarker.id = 1;
00265             visMarker.type   = visualization_msgs::Marker::CUBE;
00266             visMarker.action = visualization_msgs::Marker::ADD;
00267             visMarker.pose = poseMsg.pose;
00268             visMarker.scale.x = marker_size;
00269             visMarker.scale.y = 0.001;
00270             visMarker.scale.z = marker_size;
00271             visMarker.color.r = 1.0;
00272             visMarker.color.g = 0;
00273             visMarker.color.b = 0;
00274             visMarker.color.a = 1.0;
00275             visMarker.lifetime = ros::Duration(3.0);
00276             marker_pub.publish(visMarker);
00277 
00278           }
00279           // but drawing all the detected markers
00280           markers[i].draw(inImage,cv::Scalar(0,0,255),2);
00281         }
00282 
00283         //draw a 3d cube in each marker if there is 3d info
00284         if(camParam.isValid() && marker_size!=-1)
00285         {
00286           for(size_t i=0; i<markers.size(); ++i)
00287           {
00288             CvDrawingUtils::draw3dAxis(inImage, markers[i], camParam);
00289           }
00290         }
00291 
00292         if(image_pub.getNumSubscribers() > 0)
00293         {
00294           //show input with augmented information
00295           cv_bridge::CvImage out_msg;
00296           out_msg.header.stamp = curr_stamp;
00297           out_msg.encoding = sensor_msgs::image_encodings::RGB8;
00298           out_msg.image = inImage;
00299           image_pub.publish(out_msg.toImageMsg());
00300         }
00301 
00302         if(debug_pub.getNumSubscribers() > 0)
00303         {
00304           //show also the internal image resulting from the threshold operation
00305           cv_bridge::CvImage debug_msg;
00306           debug_msg.header.stamp = curr_stamp;
00307           debug_msg.encoding = sensor_msgs::image_encodings::MONO8;
00308           debug_msg.image = mDetector.getThresholdedImage();
00309           debug_pub.publish(debug_msg.toImageMsg());
00310         }
00311       }
00312       catch (cv_bridge::Exception& e)
00313       {
00314         ROS_ERROR("cv_bridge exception: %s", e.what());
00315         return;
00316       }
00317     }
00318   }
00319 
00320   // wait for one camerainfo, then shut down that subscriber
00321   void cam_info_callback(const sensor_msgs::CameraInfo &msg)
00322   {
00323     camParam = aruco_ros::rosCameraInfo2ArucoCamParams(msg, useRectifiedImages);
00324 
00325     // handle cartesian offset between stereo pairs
00326     // see the sensor_msgs/CamaraInfo documentation for details
00327     rightToLeft.setIdentity();
00328     rightToLeft.setOrigin(
00329         tf::Vector3(
00330             -msg.P[3]/msg.P[0],
00331             -msg.P[7]/msg.P[5],
00332             0.0));
00333 
00334     cam_info_received = true;
00335     cam_info_sub.shutdown();
00336   }
00337 
00338 
00339   void reconf_callback(aruco_ros::ArucoThresholdConfig &config, uint32_t level)
00340   {
00341     mDetector.setThresholdParams(config.param1,config.param2);
00342     if (config.normalizeImage)
00343     {
00344       ROS_WARN("normalizeImageIllumination is unimplemented!");
00345     }
00346   }
00347 };
00348 
00349 
00350 int main(int argc,char **argv)
00351 {
00352   ros::init(argc, argv, "aruco_simple");
00353 
00354   ArucoSimple node;
00355 
00356   ros::spin();
00357 }


aruco_ros
Author(s): Rafael Muñoz Salinas , Bence Magyar
autogenerated on Wed Jul 26 2017 02:17:27