aruco_mapping.h
Go to the documentation of this file.
00001 /*********************************************************************************************/
00032 /* Author: Jan Bacik */
00033 
00034 #ifndef ARUCO_MAPPING_H
00035 #define ARUCO_MAPPING_H
00036 
00037 // Standard ROS libraries
00038 #include <ros/ros.h>
00039 #include <sensor_msgs/image_encodings.h>
00040 #include <camera_calibration_parsers/parse_ini.h>
00041 #include <tf/transform_broadcaster.h>
00042 #include <tf/transform_listener.h>
00043 #include <visualization_msgs/Marker.h>
00044 #include <image_transport/image_transport.h>
00045 #include <cv_bridge/cv_bridge.h>
00046 
00047 // Aruco libraries
00048 #include <aruco/aruco.h>
00049 #include <aruco/cameraparameters.h>
00050 #include <aruco/cvdrawingutils.h>
00051 #include <aruco/arucofidmarkers.h>
00052 
00053 // OpenCV libraries
00054 #include <opencv2/opencv.hpp>
00055 #include <opencv2/core/core.hpp>
00056 #include <opencv2/calib3d/calib3d.hpp>
00057 #include <opencv2/highgui/highgui.hpp>
00058 
00059 // Custom message
00060 #include <aruco_mapping/ArucoMarker.h>
00061 
00063 namespace aruco_mapping
00064 {
00065 
00067 class ArucoMapping
00068 {
00069 public:
00070   
00072   struct MarkerInfo
00073   {
00074 
00075     bool visible;                                   // Marker visibile in actual image?
00076     int marker_id;                                  // Marker ID
00077     int previous_marker_id;                         // Used for chaining markers
00078     geometry_msgs::Pose geometry_msg_to_previous;   // Position with respect to previous marker
00079     geometry_msgs::Pose geometry_msg_to_world;      // Position with respect to world's origin
00080     tf::StampedTransform tf_to_previous;            // TF with respect to previous marker
00081     tf::StampedTransform tf_to_world;               // TF with respect to world's origin
00082     geometry_msgs::Pose current_camera_pose;        // Position of camera with respect to the marker
00083     tf::Transform current_camera_tf;                // TF of camera with respect to the marker
00084   };
00085 
00086 public:
00087   
00089   ArucoMapping(ros::NodeHandle *nh);
00090     
00091   ~ArucoMapping();
00092 
00094   void imageCallback(const sensor_msgs::ImageConstPtr &original_image);
00095 
00096 private:
00097   
00099   bool parseCalibrationFile(std::string filename);
00100 
00102   void publishTfs(bool world_option);
00103 
00105   void publishMarker(geometry_msgs::Pose markerPose, int MarkerID, int rank);
00106 
00108   ros::Publisher marker_visualization_pub_;
00109 
00111   ros::Publisher marker_msg_pub_;
00112 
00114   tf::Transform arucoMarker2Tf(const aruco::Marker &marker);
00115 
00117   bool processImage(cv::Mat input_image,cv::Mat output_image);
00118 
00119   //Launch file params
00120   std::string calib_filename_;                    
00121   std::string space_type_;                        
00122   float marker_size_;
00123   int num_of_markers_;
00124   bool roi_allowed_;
00125   int  roi_x_;                                      
00126   int  roi_y_;                                      
00127   int  roi_w_;                                     
00128   int  roi_h_;     
00129   
00131   std::vector<MarkerInfo> markers_;
00132    
00134   tf::StampedTransform world_position_transform_;
00135   
00137   geometry_msgs::Pose world_position_geometry_msg_;
00138 
00139   aruco::CameraParameters aruco_calib_params_;
00140 
00141   int marker_counter_;
00142   int marker_counter_previous_;
00143   int closest_camera_index_;
00144   int lowest_marker_id_;
00145   bool first_marker_detected_;
00146   
00147   tf::TransformListener *listener_;
00148   tf::TransformBroadcaster broadcaster_;
00149 
00150   //Consts
00151    static const int CV_WAIT_KEY = 10;
00152    static const int CV_WINDOW_MARKER_LINE_WIDTH = 2;
00153 
00154    static const double WAIT_FOR_TRANSFORM_INTERVAL = 2.0;
00155    static const double BROADCAST_WAIT_INTERVAL = 0.0001;
00156    static const double INIT_MIN_SIZE_VALUE = 1000000;
00157 
00158    static const double RVIZ_MARKER_HEIGHT = 0.01;
00159    static const double RVIZ_MARKER_LIFETIME = 0.2;
00160    static const double RVIZ_MARKER_COLOR_R = 1.0;
00161    static const double RVIZ_MARKER_COLOR_G = 1.0;
00162    static const double RVIZ_MARKER_COLOR_B = 1.0;
00163    static const double RVIZ_MARKER_COLOR_A = 1.0;
00164 
00165    static const double THIS_IS_FIRST_MARKER = -2;
00166 
00167 }; //ArucoMapping class
00168 }  //aruco_mapping namespace
00169 
00170 #endif //ARUCO_MAPPING_H


aruco_mapping
Author(s): Jan Bacik
autogenerated on Thu Jun 6 2019 21:48:04