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