Go to the documentation of this file.00001
00019 #include "marker_creator.h"
00020 #include <ros_uri/ros_uri.hpp>
00021 #include <iostream>
00022 #include <fstream>
00023 #include <string>
00024 #include <opencv2/highgui/highgui.hpp>
00025 #include "aruco/arucofidmarkers.h"
00026
00027 namespace aruco_marker_recognition {
00028
00029 MarkerCreator::MarkerCreator() : nh_(NODE_NAME) {
00030 ROS_DEBUG_STREAM("Initialize parameters");
00031
00032 nh_.getParam("marker_ids", marker_ids_);
00033 nh_.getParam("marker_pixel_size", marker_pixel_size_);
00034 nh_.getParam("output_rel_path", output_rel_path_);
00035
00036 ROS_DEBUG_STREAM("Initializing done");
00037
00038 createMarkers();
00039 }
00040
00041 void MarkerCreator::createMarkers() {
00042 ROS_DEBUG_STREAM("Creating markers");
00043 std::string output_path = ros::package::getPath(PACKAGE_NAME) + output_rel_path_;
00044
00045 for (int id : marker_ids_) {
00046 if (id >= 0 && id <= 1023) {
00047 std::ofstream marker_file;
00048 std::string marker_path = output_path + "marker_" + std::to_string(id) + ".png";
00049 marker_file.open (marker_path);
00050 if (marker_file.is_open()) {
00051 marker_file.close();
00052 cv::Mat marker = aruco::FiducidalMarkers::createMarkerImage(id, marker_pixel_size_, true, false);
00053 cv::imwrite(marker_path, marker);
00054 } else {
00055 ROS_ERROR_STREAM("Could not find the given output directory");
00056 break;
00057 }
00058 } else {
00059 ROS_ERROR_STREAM("Can't create a marker with id " << id << " (only ids from 0-1023 are allowed");
00060 }
00061 }
00062
00063 ROS_DEBUG_STREAM("Marker creation completed");
00064
00065 }
00066
00067 }
00068
00069 int main(int argc, char** argv) {
00070
00071 ros::init(argc, argv, "marker_creator");
00072
00073 aruco_marker_recognition::MarkerCreator marker_creator;
00074
00075 return 0;
00076 }