| 87 | |
| 88 | public: |
| 89 | virtual void onInit() |
| 90 | { |
| 91 | ros::NodeHandle &nh_ = getNodeHandle(); |
| 92 | ros::NodeHandle &nh_priv_ = getPrivateNodeHandle(); |
| 93 | |
| 94 | image_transport::ImageTransport it_priv(nh_priv_); |
| 95 | |
| 96 | // TODO: why image_transport doesn't work here? |
| 97 | img_pub_ = nh_priv_.advertise<sensor_msgs::Image>("image", 1, true); |
| 98 | markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("map", 1, true); |
| 99 | |
| 100 | board_ = cv::makePtr<cv::aruco::Board>(); |
| 101 | board_->dictionary = cv::aruco::getPredefinedDictionary( |
| 102 | static_cast<cv::aruco::PREDEFINED_DICTIONARY_NAME>(nh_priv_.param("dictionary", 2))); |
| 103 | camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F); |
| 104 | |
| 105 | type_ = nh_priv_.param<std::string>("type", "map"); |
| 106 | transform_.child_frame_id = nh_priv_.param<std::string>("frame_id", "aruco_map"); |
| 107 | known_vertical_ = nh_priv_.param("known_vertical", nh_priv_.param("known_tilt", std::string(""))); // known_tilt is an old name |
| 108 | flip_vertical_ = nh_priv_.param<bool>("flip_vertical", false); |
| 109 | auto_flip_ = nh_priv_.param("auto_flip", false); |
| 110 | image_width_ = nh_priv_.param("image_width" , 2000); |
| 111 | image_height_ = nh_priv_.param("image_height", 2000); |
| 112 | image_margin_ = nh_priv_.param("image_margin", 200); |
| 113 | image_axis_ = nh_priv_.param("image_axis", true); |
| 114 | put_markers_count_to_covariance_ = nh_priv_.param("put_markers_count_to_covariance", false); |
| 115 | markers_parent_frame_ = nh_priv_.param<std::string>("markers/frame_id", transform_.child_frame_id); |
| 116 | markers_frame_ = nh_priv_.param<std::string>("markers/child_frame_id_prefix", ""); |
| 117 | |
| 118 | // createStripLine(); |
| 119 | |
| 120 | if (type_ == "map") { |
| 121 | map_ = nh_priv_.param<std::string>("map" , ""); |
| 122 | loadMap(map_); |
| 123 | } else if (type_ == "gridboard") { |
| 124 | createGridBoard(nh_priv_); |
| 125 | } else { |
| 126 | NODELET_FATAL("unknown type: %s", type_.c_str()); |
| 127 | ros::shutdown(); |
| 128 | } |
| 129 | |
| 130 | pose_pub_ = nh_priv_.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose", 1); |
| 131 | vis_markers_pub_ = nh_priv_.advertise<visualization_msgs::MarkerArray>("visualization", 1, true); |
| 132 | debug_pub_ = it_priv.advertise("debug", 1); |
| 133 | |
| 134 | publishMap(); |
| 135 | |
| 136 | image_sub_.subscribe(nh_, "image_raw", 1); |
| 137 | info_sub_.subscribe(nh_, "camera_info", 1); |
| 138 | markers_sub_.subscribe(nh_, "markers", 1); |
| 139 | |
| 140 | sync_.reset(new message_filters::Synchronizer<SyncPolicy>(SyncPolicy(10), image_sub_, info_sub_, markers_sub_)); |
| 141 | sync_->registerCallback(boost::bind(&ArucoMap::callback, this, _1, _2, _3)); |
| 142 | |
| 143 | dyn_srv_ = std::make_shared<dynamic_reconfigure::Server<aruco_pose::MapConfig>>(nh_priv_); |
| 144 | dynamic_reconfigure::Server<aruco_pose::MapConfig>::CallbackType cb; |
| 145 | |
| 146 | cb = std::bind(&ArucoMap::paramCallback, this, std::placeholders::_1, std::placeholders::_2); |