aruco_mapping.h

Go to the documentation of this file.

/*********************************************************************************************/
/* Author: Jan Bacik */

#ifndef ARUCO_MAPPING_H
#define ARUCO_MAPPING_H

#include <fstream>

// Standard ROS libraries
#include <ros/ros.h>
#include <ros/package.h>
#include <sensor_msgs/image_encodings.h>
#include <camera_calibration_parsers/parse_ini.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <visualization_msgs/Marker.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>

// Aruco libraries
#include <aruco/aruco.h>
#include <aruco/cameraparameters.h>
#include <aruco/cvdrawingutils.h>
#include <aruco/arucofidmarkers.h>

// OpenCV libraries
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>

// Custom message
#include <aruco_mapping/ArucoMarker.h>

namespace aruco_mapping
{

class ArucoMapping
{
public:
  
  struct MarkerInfo
  {

    bool visible;                                   // Marker visibile in actual image?
    int marker_id;                                  // Marker ID
    int previous_marker_id;                         // Used for chaining markers
    geometry_msgs::Pose geometry_msg_to_previous;   // Position with respect to previous marker
    geometry_msgs::Pose geometry_msg_to_world;      // Position with respect to world's origin
    tf::StampedTransform tf_to_previous;            // TF with respect to previous marker
    tf::StampedTransform tf_to_world;               // TF with respect to world's origin
    geometry_msgs::Pose current_camera_pose;        // Position of camera with respect to the marker
    tf::Transform current_camera_tf;                // TF of camera with respect to the marker
  };

public:
  
  ArucoMapping(ros::NodeHandle *nh);
    
  ~ArucoMapping();

  void imageCallback(const sensor_msgs::ImageConstPtr &original_image);

private:
  
  bool parseCalibrationFile(std::string filename);

  void publishTfs(bool world_option);

  void publishMarker(geometry_msgs::Pose markerPose, int MarkerID, int rank);

  ros::Publisher marker_visualization_pub_;

  ros::Publisher marker_msg_pub_;

  ros::Publisher lidar_camera_calibration_rt;

  tf::Transform arucoMarker2Tf(const aruco::Marker &marker);

  bool processImage(cv::Mat input_image,cv::Mat output_image);

  //Launch file params
  std::string calib_filename_;                    
  std::string space_type_;                        
  float marker_size_;
  int num_of_markers_;
  bool roi_allowed_;
  int  roi_x_;                                      
  int  roi_y_;                                      
  int  roi_w_;                                     
  int  roi_h_;     
  
  std::vector<MarkerInfo> markers_;
   
  tf::StampedTransform world_position_transform_;
  
  geometry_msgs::Pose world_position_geometry_msg_;

  aruco::CameraParameters aruco_calib_params_;

  int marker_counter_;
  int marker_counter_previous_;
  int closest_camera_index_;
  int lowest_marker_id_;
  bool first_marker_detected_;
  
  tf::TransformListener *listener_;
  tf::TransformBroadcaster broadcaster_;

  //Consts
   static const int CV_WAIT_KEY = 10;
   static const int CV_WINDOW_MARKER_LINE_WIDTH = 2;

   static constexpr double WAIT_FOR_TRANSFORM_INTERVAL = 2.0;
   static constexpr double BROADCAST_WAIT_INTERVAL = 0.0001;
   static constexpr double INIT_MIN_SIZE_VALUE = 1000000;

   static constexpr double RVIZ_MARKER_HEIGHT = 0.01;
   static constexpr double RVIZ_MARKER_LIFETIME = 0.2;
   static constexpr double RVIZ_MARKER_COLOR_R = 1.0;
   static constexpr double RVIZ_MARKER_COLOR_G = 1.0;
   static constexpr double RVIZ_MARKER_COLOR_B = 1.0;
   static constexpr double RVIZ_MARKER_COLOR_A = 1.0;

   static constexpr double THIS_IS_FIRST_MARKER = -2;

}; //ArucoMapping class
}  //aruco_mapping namespace

#endif //ARUCO_MAPPING_H