diff --git a/aruco_ros/CMakeLists.txt b/aruco_ros/CMakeLists.txt index eee2428..c7f094f 100644 --- a/aruco_ros/CMakeLists.txt +++ b/aruco_ros/CMakeLists.txt @@ -53,11 +53,21 @@ add_executable(marker_publisher src/marker_publish.cpp add_dependencies(marker_publisher ${PROJECT_NAME}_gencfg ${catkin_EXPORTED_TARGETS}) target_link_libraries(marker_publisher ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) +add_executable(single_board src/single_board.cpp + src/aruco_ros_utils.cpp) +add_dependencies(single_board ${PROJECT_NAME}_gencfg) +target_link_libraries(single_board ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) + +add_executable(multi_board src/multi_board.cpp + src/aruco_ros_utils.cpp) +add_dependencies(multi_board ${PROJECT_NAME}_gencfg) +target_link_libraries(multi_board ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) + ############# ## Install ## ############# -install(TARGETS single double marker_publisher aruco_ros_utils +install(TARGETS single double marker_publisher single_board multi_board aruco_ros_utils ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/aruco_ros/include/aruco_ros/aruco_ros_utils.h b/aruco_ros/include/aruco_ros/aruco_ros_utils.h index e622284..154b1b4 100644 --- a/aruco_ros/include/aruco_ros/aruco_ros_utils.h +++ b/aruco_ros/include/aruco_ros/aruco_ros_utils.h @@ -21,5 +21,7 @@ namespace aruco_ros //FIXME: make parameter const as soon as the used function is also const tf::Transform arucoMarker2Tf(const aruco::Marker& marker, bool rotate_marker_axis=true); + tf::Transform arucoBoard2Tf(const aruco::Board& board, bool rotate_board_axis=true); + } #endif // ARUCO_ROS_UTILS_H diff --git a/aruco_ros/launch/multi_board.launch b/aruco_ros/launch/multi_board.launch new file mode 100644 index 0000000..b8ed0e6 --- /dev/null +++ b/aruco_ros/launch/multi_board.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aruco_ros/launch/single_board.launch b/aruco_ros/launch/single_board.launch new file mode 100644 index 0000000..9462086 --- /dev/null +++ b/aruco_ros/launch/single_board.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aruco_ros/samples/board_1.png b/aruco_ros/samples/board_1.png new file mode 100644 index 0000000..f399645 Binary files /dev/null and b/aruco_ros/samples/board_1.png differ diff --git a/aruco_ros/samples/board_1.yml b/aruco_ros/samples/board_1.yml new file mode 100644 index 0000000..3b41e96 --- /dev/null +++ b/aruco_ros/samples/board_1.yml @@ -0,0 +1,8 @@ +%YAML:1.0 +aruco_bc_nmarkers: 4 +aruco_bc_mInfoType: 1 +aruco_bc_markers: + - { id: 000, corners:[ [ -0.17, -0.17, 0. ], [ -0.01, -0.17, 0. ], [ -0.01, -0.01, 0. ], [ -0.17, -0.01, 0. ] ] } + - { id: 001, corners:[ [ 0.01, -0.17, 0. ], [ 0.17, -0.17, 0. ], [ 0.17, -0.01, 0. ], [ 0.01, -0.01, 0. ] ] } + - { id: 002, corners:[ [ -0.17, 0.01, 0. ], [ -0.01, 0.01, 0. ], [ -0.01, 0.17, 0. ], [ -0.17, 0.17, 0. ] ] } + - { id: 003, corners:[ [ 0.01, 0.01, 0. ], [ 0.17, 0.01, 0. ], [ 0.17, 0.17, 0. ], [ 0.01, 0.17, 0. ] ] } diff --git a/aruco_ros/samples/board_2.png b/aruco_ros/samples/board_2.png new file mode 100644 index 0000000..791f33e Binary files /dev/null and b/aruco_ros/samples/board_2.png differ diff --git a/aruco_ros/samples/board_2.yml b/aruco_ros/samples/board_2.yml new file mode 100644 index 0000000..80971fc --- /dev/null +++ b/aruco_ros/samples/board_2.yml @@ -0,0 +1,8 @@ +%YAML:1.0 +aruco_bc_nmarkers: 4 +aruco_bc_mInfoType: 1 +aruco_bc_markers: + - { id: 004, corners:[ [ -0.17, -0.17, 0. ], [ -0.01, -0.17, 0. ], [ -0.01, -0.01, 0. ], [ -0.17, -0.01, 0. ] ] } + - { id: 005, corners:[ [ 0.01, -0.17, 0. ], [ 0.17, -0.17, 0. ], [ 0.17, -0.01, 0. ], [ 0.01, -0.01, 0. ] ] } + - { id: 006, corners:[ [ -0.17, 0.01, 0. ], [ -0.01, 0.01, 0. ], [ -0.01, 0.17, 0. ], [ -0.17, 0.17, 0. ] ] } + - { id: 007, corners:[ [ 0.01, 0.01, 0. ], [ 0.17, 0.01, 0. ], [ 0.17, 0.17, 0. ], [ 0.01, 0.17, 0. ] ] } diff --git a/aruco_ros/samples/multi_board.yml b/aruco_ros/samples/multi_board.yml new file mode 100644 index 0000000..6c9391a --- /dev/null +++ b/aruco_ros/samples/multi_board.yml @@ -0,0 +1,4 @@ +%YAML:1.0 +aruco_boards: + - { frame_id: "/board1", filename: "board_1.yml"} + - { frame_id: "/board2", filename: "board_2.yml"} diff --git a/aruco_ros/src/aruco_ros_utils.cpp b/aruco_ros/src/aruco_ros_utils.cpp index 2fc00b8..5f3446e 100644 --- a/aruco_ros/src/aruco_ros_utils.cpp +++ b/aruco_ros/src/aruco_ros_utils.cpp @@ -78,3 +78,39 @@ tf::Transform aruco_ros::arucoMarker2Tf(const aruco::Marker &marker, bool rotate return tf::Transform(tf_rot, tf_orig); } + +tf::Transform aruco_ros::arucoBoard2Tf(const aruco::Board &board, bool rotate_board_axis) +{ + cv::Mat rot(3, 3, CV_64FC1); + cv::Mat Rvec64; + board.Rvec.convertTo(Rvec64, CV_64FC1); + cv::Rodrigues(Rvec64, rot); + cv::Mat tran64; + board.Tvec.convertTo(tran64, CV_64FC1); + + // Rotate axis direction as to fit ROS (?) + if (rotate_board_axis) + { + cv::Mat rotate_to_ros(3, 3, CV_64FC1); + // -1 0 0 + // 0 0 1 + // 0 1 0 + rotate_to_ros.at(0, 0) = -1.0; + rotate_to_ros.at(0, 1) = 0.0; + rotate_to_ros.at(0, 2) = 0.0; + rotate_to_ros.at(1, 0) = 0.0; + rotate_to_ros.at(1, 1) = 0.0; + rotate_to_ros.at(1, 2) = 1.0; + rotate_to_ros.at(2, 0) = 0.0; + rotate_to_ros.at(2, 1) = 1.0; + rotate_to_ros.at(2, 2) = 0.0; + rot = rot * rotate_to_ros.t(); + } + tf::Matrix3x3 tf_rot(rot.at(0, 0), rot.at(0, 1), rot.at(0, 2), + rot.at(1, 0), rot.at(1, 1), rot.at(1, 2), + rot.at(2, 0), rot.at(2, 1), rot.at(2, 2)); + + tf::Vector3 tf_orig(tran64.at(0, 0), tran64.at(1, 0), tran64.at(2, 0)); + + return tf::Transform(tf_rot, tf_orig); +} \ No newline at end of file diff --git a/aruco_ros/src/multi_board.cpp b/aruco_ros/src/multi_board.cpp new file mode 100644 index 0000000..2c84baa --- /dev/null +++ b/aruco_ros/src/multi_board.cpp @@ -0,0 +1,384 @@ +/***************************** +Copyright 2011 Rafael Muñoz Salinas. All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are +permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this list of + conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, this list + of conditions and the following disclaimer in the documentation and/or other materials + provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND +FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR +CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +The views and conclusions contained in the software and documentation are those of the +authors and should not be interpreted as representing official policies, either expressed +or implied, of Rafael Muñoz Salinas. +********************************/ +/** +* @file multi_board.cpp +* @author Sarthak Mittal +* @date April 2020 +* @version 0.2.4 +* @brief ROS version of the example named "simple" in the Aruco software package. +*/ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace aruco; + +struct board_t { + int uid; + std::string frame_id; + BoardConfiguration config; +}; + + +class ArucoMultiBoard { + + private: + + cv::Mat inImage; + aruco::CameraParameters camParam; + tf::StampedTransform rightToLeft; + bool useRectifiedImages; + bool draw_markers; + bool rotate_marker_axis; + MarkerDetector mDetector; + vector markers; + BoardDetector board_detector; + vector boards; + vector boards_detected; + bool cam_info_received; + ros::Subscriber cam_info_sub; + image_transport::Publisher image_pub; + image_transport::Publisher debug_pub; + ros::Publisher pose_pub; + ros::Publisher transform_pub; + ros::Publisher position_pub; + ros::Publisher marker_pub; //rviz visualization marker + std::string reference_frame; + std::string camera_frame; + + double marker_size; + std::string board_config_file; + std::string board_config_dir; + + ros::NodeHandle nh; + image_transport::ImageTransport it; + image_transport::Subscriber image_sub; + + tf::TransformListener _tfListener; + + dynamic_reconfigure::Server dyn_rec_server; + + public: + ArucoMultiBoard() + : cam_info_received(false), + nh("~"), + it(nh) { + + std::string refinementMethod; + nh.param("corner_refinement", refinementMethod, "LINES"); + if (refinementMethod == "SUBPIX") + mDetector.setCornerRefinementMethod(aruco::MarkerDetector::SUBPIX); + else if (refinementMethod == "HARRIS") + mDetector.setCornerRefinementMethod(aruco::MarkerDetector::HARRIS); + else if (refinementMethod == "NONE") + mDetector.setCornerRefinementMethod(aruco::MarkerDetector::NONE); + else + mDetector.setCornerRefinementMethod(aruco::MarkerDetector::LINES); + + //Print parameters of aruco marker detector: + ROS_INFO_STREAM("Corner refinement method: " << mDetector.getCornerRefinementMethod()); + ROS_INFO_STREAM("Threshold method: " << mDetector.getThresholdMethod()); + double th1, th2; + mDetector.getThresholdParams(th1, th2); + ROS_INFO_STREAM("Threshold method: " << " th1: " << th1 << " th2: " << th2); + float mins, maxs; + mDetector.getMinMaxSize(mins, maxs); + ROS_INFO_STREAM("Marker size min: " << mins << " max: " << maxs); + ROS_INFO_STREAM("Desired speed: " << mDetector.getDesiredSpeed()); + + image_sub = it.subscribe("/image", 1, &ArucoMultiBoard::image_callback, this); + cam_info_sub = nh.subscribe("/camera_info", 1, &ArucoMultiBoard::cam_info_callback, this); + + image_pub = it.advertise("result", 1); + debug_pub = it.advertise("debug", 1); + pose_pub = nh.advertise("pose", 100); + transform_pub = nh.advertise("transform", 100); + position_pub = nh.advertise("position", 100); + marker_pub = nh.advertise("marker", 10); + + nh.param("marker_size", marker_size, 0.05); + nh.param("board_config", board_config_file, ""); + nh.param("board_dir", board_config_dir, ""); + nh.param("reference_frame", reference_frame, ""); + nh.param("camera_frame", camera_frame, ""); + nh.param("image_is_rectified", useRectifiedImages, true); + nh.param("draw_markers", draw_markers, false); + nh.param("rotate_marker_axis", rotate_marker_axis, false); + + ROS_ASSERT(board_config_file != "" && board_config_dir != ""); + + readBoardConfigList(); + ROS_ASSERT(!camera_frame.empty()); + + if (reference_frame.empty()) + reference_frame = camera_frame; + + ROS_INFO("Aruco node will publish pose to TF with %s as parent", + reference_frame.c_str()); + + dyn_rec_server.setCallback(boost::bind(&ArucoMultiBoard::reconf_callback, this, _1, _2)); + } + + void readBoardConfigList() { + try { + cv::FileStorage fs(board_config_file, cv::FileStorage::READ); + + if (fs["aruco_boards"].name() != "aruco_boards") + throw cv::Exception(81818, "ArucoMultiBoard::readBoardConfigList", "invalid file type", __FILE__, __LINE__); + + cv::FileNode FnBoards = fs["aruco_boards"]; + for (cv::FileNodeIterator it = FnBoards.begin(); it != FnBoards.end(); ++it) { + board_t board; + board.uid = (int) boards.size(); + board.frame_id = (std::string) (*it)["frame_id"]; + std::string path(board_config_dir); + if (path.back() != '/') + path.append("/"); + path.append((std::string) (*it)["filename"]); + board.config.readFromFile(path); + boards.push_back(board); + } + ROS_ASSERT(!boards.empty()); + } + catch (std::exception &ex) { + throw cv::Exception(81818, + "ArucoMultiBoard::readBoardConfigList", + ex.what() + string(" file=)") + board_config_file, + __FILE__, + __LINE__); + } + } + + bool getTransform(const std::string &refFrame, + const std::string &childFrame, + tf::StampedTransform &transform) { + std::string errMsg; + + if (!_tfListener.waitForTransform(refFrame, + childFrame, + ros::Time(0), + ros::Duration(0.5), + ros::Duration(0.01), + &errMsg) + ) { + ROS_ERROR_STREAM("Unable to get pose from TF: " << errMsg); + return false; + } else { + try { + _tfListener.lookupTransform(refFrame, childFrame, + ros::Time(0), //get latest available + transform); + } + catch (const tf::TransformException &e) { + ROS_ERROR_STREAM("Error in lookupTransform of " << childFrame << " in " << refFrame); + return false; + } + + } + return true; + } + + void image_callback(const sensor_msgs::ImageConstPtr &msg) { + + if ((image_pub.getNumSubscribers() == 0) && + (debug_pub.getNumSubscribers() == 0) && + (pose_pub.getNumSubscribers() == 0) && + (transform_pub.getNumSubscribers() == 0) && + (position_pub.getNumSubscribers() == 0) && + (marker_pub.getNumSubscribers() == 0)) + { + ROS_DEBUG("No subscribers, not looking for aruco markers"); + return; + } + + static tf::TransformBroadcaster br; + + if (!cam_info_received) return; + + ros::Time curr_stamp(ros::Time::now()); + + cv_bridge::CvImagePtr cv_ptr; + try { + cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::RGB8); + inImage = cv_ptr->image; + + //detection results will go into "markers" + markers.clear(); + + //Ok, let's detect + mDetector.detect(inImage, markers, camParam, marker_size, false); + + for (int board_index = 0; board_index < boards.size(); board_index++) { + Board board_detected; + + //Detection of the board + float probDetect = board_detector.detect(markers, + boards[board_index].config, + board_detected, + camParam, + marker_size); + if (probDetect > 0.0) { + tf::Transform transform = aruco_ros::arucoBoard2Tf(board_detected, rotate_marker_axis); + + tf::StampedTransform cameraToReference; + cameraToReference.setIdentity(); + + if (reference_frame != camera_frame) { + getTransform(reference_frame, + camera_frame, + cameraToReference); + } + + transform = + static_cast(cameraToReference) + * static_cast(rightToLeft) + * transform; + + tf::StampedTransform stampedTransform(transform, curr_stamp, reference_frame, boards[board_index].frame_id); + br.sendTransform(stampedTransform); + + geometry_msgs::PoseStamped poseMsg; + tf::poseTFToMsg(transform, poseMsg.pose); + poseMsg.header.frame_id = reference_frame; + poseMsg.header.stamp = curr_stamp; + pose_pub.publish(poseMsg); + + geometry_msgs::TransformStamped transformMsg; + tf::transformStampedTFToMsg(stampedTransform, transformMsg); + transform_pub.publish(transformMsg); + + geometry_msgs::Vector3Stamped positionMsg; + positionMsg.header = transformMsg.header; + positionMsg.vector = transformMsg.transform.translation; + position_pub.publish(positionMsg); + + //Publish rviz marker representing the ArUco marker patch + visualization_msgs::Marker visMarker; + visMarker.header = transformMsg.header; + visMarker.id = 1; + visMarker.type = visualization_msgs::Marker::CUBE; + visMarker.action = visualization_msgs::Marker::ADD; + visMarker.pose = poseMsg.pose; + visMarker.scale.x = marker_size; + visMarker.scale.y = 0.001; + visMarker.scale.z = marker_size; + visMarker.color.r = 1.0; + visMarker.color.g = 0; + visMarker.color.b = 0; + visMarker.color.a = 1.0; + visMarker.lifetime = ros::Duration(3.0); + marker_pub.publish(visMarker); + + if (camParam.isValid()) { + //draw board axis + CvDrawingUtils::draw3dAxis(inImage, board_detected, camParam); + } + } + } + + //for each marker, draw info and its boundaries in the image + for (size_t i = 0; draw_markers && i < markers.size(); ++i) { + markers[i].draw(inImage, cv::Scalar(0, 0, 255), 2); + } + + if (image_pub.getNumSubscribers() > 0) { + //show input with augmented information + cv_bridge::CvImage out_msg; + out_msg.header.frame_id = msg->header.frame_id; + out_msg.header.stamp = msg->header.stamp; + out_msg.encoding = sensor_msgs::image_encodings::RGB8; + out_msg.image = inImage; + image_pub.publish(out_msg.toImageMsg()); + } + + if (debug_pub.getNumSubscribers() > 0) { + //show also the internal image resulting from the threshold operation + cv_bridge::CvImage debug_msg; + debug_msg.header.frame_id = msg->header.frame_id; + debug_msg.header.stamp = msg->header.stamp; + debug_msg.encoding = sensor_msgs::image_encodings::MONO8; + debug_msg.image = mDetector.getThresholdedImage(); + debug_pub.publish(debug_msg.toImageMsg()); + } + } + catch (cv_bridge::Exception &e) { + ROS_ERROR("cv_bridge exception: %s", e.what()); + return; + } + } + + // wait for one camerainfo, then shut down that subscriber + void cam_info_callback(const sensor_msgs::CameraInfo &msg) { + camParam = aruco_ros::rosCameraInfo2ArucoCamParams(msg, useRectifiedImages); + // handle cartesian offset between stereo pairs + // see the sensor_msgs/CamaraInfo documentation for details + rightToLeft.setIdentity(); + rightToLeft.setOrigin( + tf::Vector3( + -msg.P[3] / msg.P[0], + -msg.P[7] / msg.P[5], + 0.0)); + + cam_info_received = true; + cam_info_sub.shutdown(); + ROS_INFO("Received camera info"); + } + + void reconf_callback(aruco_ros::ArucoThresholdConfig &config, uint32_t level) { + mDetector.setThresholdParams(config.param1, config.param2); + if (config.normalizeImage) { + ROS_WARN("normalizeImageIllumination is unimplemented!"); + } + } + +}; + +int main(int argc, char **argv) { + ros::init(argc, argv, "aruco_multi_board"); + + ArucoMultiBoard node; + + ros::spin(); +} diff --git a/aruco_ros/src/single_board.cpp b/aruco_ros/src/single_board.cpp new file mode 100644 index 0000000..3ac7315 --- /dev/null +++ b/aruco_ros/src/single_board.cpp @@ -0,0 +1,355 @@ +/***************************** +Copyright 2011 Rafael Muñoz Salinas. All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are +permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this list of + conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, this list + of conditions and the following disclaimer in the documentation and/or other materials + provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND +FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR +CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +The views and conclusions contained in the software and documentation are those of the +authors and should not be interpreted as representing official policies, either expressed +or implied, of Rafael Muñoz Salinas. +********************************/ +/** +* @file single_board.cpp +* @author Sarthak Mittal +* @date April 2020 +* @version 0.2.4 +* @brief ROS version of the example named "simple" in the Aruco software package. +*/ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace aruco; + +class ArucoSingleBoard { + + private: + + cv::Mat inImage; + aruco::CameraParameters camParam; + tf::StampedTransform rightToLeft; + bool useRectifiedImages; + bool draw_markers; + bool rotate_marker_axis; + MarkerDetector mDetector; + vector markers; + BoardConfiguration board_config; + BoardDetector board_detector; + Board board_detected; + bool cam_info_received; + ros::Subscriber cam_info_sub; + image_transport::Publisher image_pub; + image_transport::Publisher debug_pub; + ros::Publisher pose_pub; + ros::Publisher transform_pub; + ros::Publisher position_pub; + ros::Publisher marker_pub; //rviz visualization marker + std::string reference_frame; + std::string camera_frame; + std::string board_frame; + + double marker_size; + std::string board_config_file; + + ros::NodeHandle nh; + image_transport::ImageTransport it; + image_transport::Subscriber image_sub; + + tf::TransformListener _tfListener; + + dynamic_reconfigure::Server dyn_rec_server; + + public: + + ArucoSingleBoard() + : cam_info_received(false), + nh("~"), + it(nh) { + + std::string refinementMethod; + nh.param("corner_refinement", refinementMethod, "LINES"); + if (refinementMethod == "SUBPIX") + mDetector.setCornerRefinementMethod(aruco::MarkerDetector::SUBPIX); + else if (refinementMethod == "HARRIS") + mDetector.setCornerRefinementMethod(aruco::MarkerDetector::HARRIS); + else if (refinementMethod == "NONE") + mDetector.setCornerRefinementMethod(aruco::MarkerDetector::NONE); + else + mDetector.setCornerRefinementMethod(aruco::MarkerDetector::LINES); + + //Print parameters of aruco marker detector: + ROS_INFO_STREAM("Corner refinement method: " << mDetector.getCornerRefinementMethod()); + ROS_INFO_STREAM("Threshold method: " << mDetector.getThresholdMethod()); + double th1, th2; + mDetector.getThresholdParams(th1, th2); + ROS_INFO_STREAM("Threshold method: " << " th1: " << th1 << " th2: " << th2); + float mins, maxs; + mDetector.getMinMaxSize(mins, maxs); + ROS_INFO_STREAM("Marker size min: " << mins << " max: " << maxs); + ROS_INFO_STREAM("Desired speed: " << mDetector.getDesiredSpeed()); + + image_sub = it.subscribe("/image", 1, &ArucoSingleBoard::image_callback, this); + cam_info_sub = nh.subscribe("/camera_info", 1, &ArucoSingleBoard::cam_info_callback, this); + + image_pub = it.advertise("result", 1); + debug_pub = it.advertise("debug", 1); + pose_pub = nh.advertise("pose", 100); + transform_pub = nh.advertise("transform", 100); + position_pub = nh.advertise("position", 100); + marker_pub = nh.advertise("marker", 10); + + nh.param("marker_size", marker_size, 0.05); + nh.param("board_config", board_config_file, ""); + nh.param("board_frame", board_frame, ""); + nh.param("reference_frame", reference_frame, ""); + nh.param("camera_frame", camera_frame, ""); + nh.param("image_is_rectified", useRectifiedImages, true); + nh.param("draw_markers", draw_markers, true); + nh.param("rotate_marker_axis", rotate_marker_axis, false); + + ROS_ASSERT(board_config_file != ""); + + try { + board_config.readFromFile(board_config_file); + } + catch (std::exception &exp) { + ROS_ERROR("Could not read '%s'", board_config_file.c_str()); + exit(1); + } + + ROS_ASSERT(!camera_frame.empty() && !board_frame.empty()); + + if (reference_frame.empty()) + reference_frame = camera_frame; + + ROS_INFO("Aruco node started with marker size of %f m and marker board to track: %s", + marker_size, board_config_file.c_str()); + ROS_INFO("Aruco node will publish pose to TF with %s as parent and %s as child.", + reference_frame.c_str(), board_frame.c_str()); + + dyn_rec_server.setCallback(boost::bind(&ArucoSingleBoard::reconf_callback, this, _1, _2)); + } + + bool getTransform(const std::string& refFrame, + const std::string& childFrame, + tf::StampedTransform& transform) + { + std::string errMsg; + + if ( !_tfListener.waitForTransform(refFrame, + childFrame, + ros::Time(0), + ros::Duration(0.5), + ros::Duration(0.01), + &errMsg) + ) + { + ROS_ERROR_STREAM("Unable to get pose from TF: " << errMsg); + return false; + } + else + { + try + { + _tfListener.lookupTransform( refFrame, childFrame, + ros::Time(0), //get latest available + transform); + } + catch ( const tf::TransformException& e) + { + ROS_ERROR_STREAM("Error in lookupTransform of " << childFrame << " in " << refFrame); + return false; + } + + } + return true; + } + + void image_callback(const sensor_msgs::ImageConstPtr &msg) { + + if ((image_pub.getNumSubscribers() == 0) && + (debug_pub.getNumSubscribers() == 0) && + (pose_pub.getNumSubscribers() == 0) && + (transform_pub.getNumSubscribers() == 0) && + (position_pub.getNumSubscribers() == 0) && + (marker_pub.getNumSubscribers() == 0)) + { + ROS_DEBUG("No subscribers, not looking for aruco markers"); + return; + } + + static tf::TransformBroadcaster br; + + if (!cam_info_received) + return; + + ros::Time curr_stamp(ros::Time::now()); + cv_bridge::CvImagePtr cv_ptr; + try { + cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::RGB8); + inImage = cv_ptr->image; + + //detection results will go into "markers" + markers.clear(); + //Ok, let's detect + mDetector.detect(inImage, markers, camParam, marker_size, false); + //Detection of the board + float probDetect = board_detector.detect(markers, board_config, board_detected, camParam, marker_size); + + if (probDetect > 0.0) { + tf::Transform transform = aruco_ros::arucoBoard2Tf(board_detected, rotate_marker_axis); + + tf::StampedTransform cameraToReference; + cameraToReference.setIdentity(); + + if (reference_frame != camera_frame) { + getTransform(reference_frame, + camera_frame, + cameraToReference); + } + + transform = + static_cast(cameraToReference) + * static_cast(rightToLeft) + * transform; + + tf::StampedTransform stampedTransform(transform, curr_stamp, reference_frame, board_frame); + br.sendTransform(stampedTransform); + + geometry_msgs::PoseStamped poseMsg; + tf::poseTFToMsg(transform, poseMsg.pose); + poseMsg.header.frame_id = reference_frame; + poseMsg.header.stamp = curr_stamp; + pose_pub.publish(poseMsg); + + geometry_msgs::TransformStamped transformMsg; + tf::transformStampedTFToMsg(stampedTransform, transformMsg); + transform_pub.publish(transformMsg); + + geometry_msgs::Vector3Stamped positionMsg; + positionMsg.header = transformMsg.header; + positionMsg.vector = transformMsg.transform.translation; + position_pub.publish(positionMsg); + + //Publish rviz marker representing the ArUco marker patch + visualization_msgs::Marker visMarker; + visMarker.header = transformMsg.header; + visMarker.id = 1; + visMarker.type = visualization_msgs::Marker::CUBE; + visMarker.action = visualization_msgs::Marker::ADD; + visMarker.pose = poseMsg.pose; + visMarker.scale.x = marker_size; + visMarker.scale.y = 0.001; + visMarker.scale.z = marker_size; + visMarker.color.r = 1.0; + visMarker.color.g = 0; + visMarker.color.b = 0; + visMarker.color.a = 1.0; + visMarker.lifetime = ros::Duration(3.0); + marker_pub.publish(visMarker); + } + + //for each marker, draw info and its boundaries in the image + for (size_t i = 0; draw_markers && i < markers.size(); ++i) { + markers[i].draw(inImage,cv::Scalar(0,0,255),2); + } + + //draw a 3d cube in each marker if there is 3d info + if(camParam.isValid() && probDetect > 0.0) + { + CvDrawingUtils::draw3dAxis(inImage, board_detected, camParam); + } + + if(image_pub.getNumSubscribers() > 0) + { + //show input with augmented information + cv_bridge::CvImage out_msg; + out_msg.header.stamp = curr_stamp; + out_msg.encoding = sensor_msgs::image_encodings::RGB8; + out_msg.image = inImage; + image_pub.publish(out_msg.toImageMsg()); + } + + if(debug_pub.getNumSubscribers() > 0) + { + //show also the internal image resulting from the threshold operation + cv_bridge::CvImage debug_msg; + debug_msg.header.stamp = curr_stamp; + debug_msg.encoding = sensor_msgs::image_encodings::MONO8; + debug_msg.image = mDetector.getThresholdedImage(); + debug_pub.publish(debug_msg.toImageMsg()); + } + } + catch (cv_bridge::Exception &e) { + ROS_ERROR("cv_bridge exception: %s", e.what()); + return; + } + } + + // wait for one camerainfo, then shut down that subscriber + void cam_info_callback(const sensor_msgs::CameraInfo &msg) { + camParam = aruco_ros::rosCameraInfo2ArucoCamParams(msg, useRectifiedImages); + + // handle cartesian offset between stereo pairs + // see the sensor_msgs/CamaraInfo documentation for details + rightToLeft.setIdentity(); + rightToLeft.setOrigin( + tf::Vector3( + -msg.P[3] / msg.P[0], + -msg.P[7] / msg.P[5], + 0.0)); + + cam_info_received = true; + cam_info_sub.shutdown(); + ROS_INFO("Received camera info"); + } + + void reconf_callback(aruco_ros::ArucoThresholdConfig &config, uint32_t level) { + mDetector.setThresholdParams(config.param1, config.param2); + if (config.normalizeImage) { + ROS_WARN("normalizeImageIllumination is unimplemented!"); + } + } +}; + +int main(int argc, char **argv) { + ros::init(argc, argv, "aruco_single_board"); + + ArucoSingleBoard node; + + ros::spin(); +}