diff --git a/aruco_pose/CMakeLists.txt b/aruco_pose/CMakeLists.txt index 90403cf51..9b7bb84ae 100644 --- a/aruco_pose/CMakeLists.txt +++ b/aruco_pose/CMakeLists.txt @@ -16,11 +16,15 @@ find_package(catkin REQUIRED COMPONENTS image_transport cv_bridge tf - #tf2 - #tf2_ros - #aruco_msgs + tf2 + tf2_ros + tf2_geometry_msgs + sensor_msgs + message_generation ) +find_package(OpenCV REQUIRED) + ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -55,11 +59,12 @@ find_package(catkin REQUIRED COMPONENTS ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) ## Generate messages in the 'msg' folder -#add_message_files( -# FILES -# Marker.msg -# MarkerArray.msg -#) +add_message_files( + FILES + Point2D.msg + Marker.msg + MarkerArray.msg +) ## Generate services in the 'srv' folder # add_service_files( @@ -76,10 +81,11 @@ find_package(catkin REQUIRED COMPONENTS # ) ## Generate added messages and services with any dependencies listed here -#generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -#) +generate_messages( + DEPENDENCIES + std_msgs + geometry_msgs +) ################################################ ## Declare ROS dynamic reconfigure parameters ## @@ -111,9 +117,9 @@ find_package(catkin REQUIRED COMPONENTS ## CATKIN_DEPENDS: catkin_packages dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( -# INCLUDE_DIRS include + INCLUDE_DIRS DEPENDS OpenCV LIBRARIES aruco_pose -# CATKIN_DEPENDS other_catkin_pkg + CATKIN_DEPENDS message_runtime # DEPENDS system_lib ) @@ -126,11 +132,13 @@ catkin_package( include_directories( # include ${catkin_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} ) ## Declare a C++ library -add_library(${PROJECT_NAME} - src/aruco_pose.cpp +add_library(aruco_pose + src/aruco_detect.cpp + src/aruco_map.cpp ) ## Add cmake target dependencies of the library @@ -154,11 +162,9 @@ add_library(${PROJECT_NAME} # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Specify libraries to link a library or executable target against -link_directories(/opt/ros/kinetic/lib) - -target_link_libraries(${PROJECT_NAME} +target_link_libraries(aruco_pose ${catkin_LIBRARIES} - "/opt/ros/kinetic/lib/libopencv_aruco3.so" # TODO: fix launch fails with .so loading + ${OpenCV_LIBS} ) ############# diff --git a/aruco_pose/map/map.txt b/aruco_pose/map/map.txt new file mode 100644 index 000000000..3d4dd1ee0 --- /dev/null +++ b/aruco_pose/map/map.txt @@ -0,0 +1,4 @@ +1 0.33 0 0 0 0 0 0 +2 0.33 1 0 0 0 0 0 +3 0.33 0 1 0 0 0 0 +4 0.33 1 1 0 0 0 0 diff --git a/aruco_pose/msg/Marker.msg b/aruco_pose/msg/Marker.msg new file mode 100644 index 000000000..3bad576d1 --- /dev/null +++ b/aruco_pose/msg/Marker.msg @@ -0,0 +1,6 @@ +uint32 id +geometry_msgs/PoseWithCovariance pose +Point2D c1 +Point2D c2 +Point2D c3 +Point2D c4 diff --git a/aruco_pose/msg/MarkerArray.msg b/aruco_pose/msg/MarkerArray.msg new file mode 100644 index 000000000..b870027fd --- /dev/null +++ b/aruco_pose/msg/MarkerArray.msg @@ -0,0 +1,2 @@ +Header header +Marker[] markers diff --git a/aruco_pose/msg/Point2D.msg b/aruco_pose/msg/Point2D.msg new file mode 100644 index 000000000..bc944ef8d --- /dev/null +++ b/aruco_pose/msg/Point2D.msg @@ -0,0 +1,2 @@ +float32 x +float32 y diff --git a/aruco_pose/nodelet_plugins.xml b/aruco_pose/nodelet_plugins.xml index 79d5ddad6..40f8e16aa 100644 --- a/aruco_pose/nodelet_plugins.xml +++ b/aruco_pose/nodelet_plugins.xml @@ -1,5 +1,8 @@ - + + + + diff --git a/aruco_pose/package.xml b/aruco_pose/package.xml index f3dece62d..a3dbdad40 100644 --- a/aruco_pose/package.xml +++ b/aruco_pose/package.xml @@ -2,7 +2,7 @@ aruco_pose 0.0.0 - ArUco maps precise pose estimation nodelet + Positioning by ArUco markers Oleg Kalachev MIT @@ -11,21 +11,30 @@ Oleg Kalachev Artem Smirnov - + catkin + + tf + tf2 + tf2_ros + tf2_geometry_msgs + cv_bridge + dynamic_reconfigure + image_transport + message_generation + message_runtime nodelet roscpp - image_transport - cv_bridge - tf - - catkin - + std_msgs nodelet roscpp image_transport cv_bridge - - + message_runtime + std_msgs + tf + tf2 + tf2_ros + tf2_geometry_msgs diff --git a/aruco_pose/src/aruco_detect.cpp b/aruco_pose/src/aruco_detect.cpp new file mode 100644 index 000000000..c0c0c43d3 --- /dev/null +++ b/aruco_pose/src/aruco_detect.cpp @@ -0,0 +1,293 @@ +/* + * Detecting and positioning ArUco markers + * Copyright (C) 2018 Copter Express Technologies + * + * Author: Oleg Kalachev + * + * Distributed under MIT License (available at https://opensource.org/licenses/MIT). + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + */ + +/* + * Code is based on https://github.com/UbiquityRobotics/fiducials, which is distributed + * under the BSD license. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include "utils.h" + +using std::vector; +using cv::Mat; + +class ArucoDetect : public nodelet::Nodelet { +private: + ros::NodeHandle nh_, nh_priv_; + tf2_ros::TransformBroadcaster br_; + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_{tf_buffer_}; + cv::Ptr dictionary_; + cv::Ptr parameters_; + image_transport::Publisher debug_pub_; + image_transport::CameraSubscriber img_sub_; + ros::Publisher markers_pub_, vis_markers_pub_; + bool estimate_poses_, send_tf_; + double length_; + std::string snap_orientation_; + Mat camera_matrix_, dist_coeffs_; + aruco_pose::MarkerArray array_; + visualization_msgs::MarkerArray vis_array_; + +public: + virtual void onInit() + { + nh_ = getNodeHandle(); + nh_priv_ = getPrivateNodeHandle(); + + int dictionary; + nh_priv_.param("dictionary", dictionary, 2); + nh_priv_.param("estimate_poses", estimate_poses_, true); + nh_priv_.param("send_tf", send_tf_, true); + if (estimate_poses_ && !nh_priv_.getParam("length", length_)) { + ROS_FATAL("aruco_detect: can't estimate marker's poses as ~length parameter is not defined"); + ros::shutdown(); + } + nh_priv_.param("snap_orientation", snap_orientation_, ""); + + camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F); + dist_coeffs_ = cv::Mat::zeros(8, 1, CV_64F); + + dictionary_ = cv::aruco::getPredefinedDictionary(static_cast(dictionary)); + parameters_ = cv::aruco::DetectorParameters::create(); + parameters_->cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX; + + image_transport::ImageTransport it(nh_); + image_transport::ImageTransport it_priv(nh_priv_); + + debug_pub_ = it_priv.advertise("debug", 1); + markers_pub_ = nh_priv_.advertise("markers", 1); + vis_markers_pub_ = nh_priv_.advertise("visualization", 1); + img_sub_ = it.subscribeCamera("image_raw", 1, &ArucoDetect::imageCallback, this); + + ROS_INFO("aruco_detect: ready"); + } + +private: + void imageCallback(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr &cinfo) + { + Mat image = cv_bridge::toCvShare(msg, "bgr8")->image; + + vector ids; + vector> corners, rejected; + vector rvecs, tvecs; + vector obj_points; + geometry_msgs::TransformStamped snap_to; + + // Detect markers + cv::aruco::detectMarkers(image, dictionary_, corners, ids, parameters_, rejected); + + array_.header.stamp = msg->header.stamp; + array_.header.frame_id = msg->header.frame_id; + array_.markers.clear(); + + if (ids.size() != 0) { + parseCameraInfo(cinfo, camera_matrix_, dist_coeffs_); + + // Estimate individual markers' poses + if (estimate_poses_) { + cv::aruco::estimatePoseSingleMarkers(corners, length_, camera_matrix_, dist_coeffs_, + rvecs, tvecs); + + if (!snap_orientation_.empty()) { + try { + snap_to = tf_buffer_.lookupTransform(msg->header.frame_id, snap_orientation_, + msg->header.stamp, ros::Duration(0.02)); + } catch (const tf2::TransformException& e) { + ROS_WARN_THROTTLE(5, "aruco_detect: can't snap: %s", e.what()); + } + } + } + + array_.markers.reserve(ids.size()); + aruco_pose::Marker marker; + geometry_msgs::TransformStamped transform; + transform.header.stamp = msg->header.stamp; + transform.header.frame_id = msg->header.frame_id; + + for (unsigned int i = 0; i < ids.size(); i++) { + marker.id = ids[i]; + fillCorners(marker, corners[i]); + + if (estimate_poses_) { + fillPose(marker.pose.pose, rvecs[i], tvecs[i]); + + // snap orientation (if enabled and snap frame avaiable) + if (!snap_orientation_.empty() && !snap_to.header.frame_id.empty()) { + snapOrientation(marker.pose.pose, snap_to.transform.rotation); + } + + // TODO: check IDs are unique + if (send_tf_) { + transform.child_frame_id = getChildFrameId(ids[i]); + transform.transform.rotation = marker.pose.pose.orientation; + fillTranslation(transform.transform.translation, tvecs[i]); + br_.sendTransform(transform); + } + } + array_.markers.push_back(marker); + } + } + + markers_pub_.publish(array_); + + // Publish visualization markers + if (estimate_poses_ && vis_markers_pub_.getNumSubscribers() != 0) { + // Delete all markers + visualization_msgs::Marker vis_marker; + vis_marker.action = visualization_msgs::Marker::DELETEALL; + vis_array_.markers.clear(); + vis_array_.markers.reserve(ids.size() + 1); + vis_array_.markers.push_back(vis_marker); + + for (unsigned int i = 0; i < ids.size(); i++) + pushVisMarkers(msg->header.frame_id, msg->header.stamp, array_.markers[i].pose.pose, + length_, ids[i], i); + + vis_markers_pub_.publish(vis_array_); + } + + // Publish debug image + if (debug_pub_.getNumSubscribers() != 0) { + Mat debug = image.clone(); + cv::aruco::drawDetectedMarkers(debug, corners, ids); // draw markers + if (estimate_poses_) + for (unsigned int i = 0; i < ids.size(); i++) + cv::aruco::drawAxis(debug, camera_matrix_, dist_coeffs_, rvecs[i], tvecs[i], length_); + + 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::BGR8; + out_msg.image = debug; + debug_pub_.publish(out_msg.toImageMsg()); + } + } + + inline void fillCorners(aruco_pose::Marker& marker, const vector& corners) const + { + marker.c1.x = corners[0].x; + marker.c2.x = corners[1].x; + marker.c3.x = corners[2].x; + marker.c4.x = corners[3].x; + marker.c1.y = corners[0].y; + marker.c2.y = corners[1].y; + marker.c3.y = corners[2].y; + marker.c4.y = corners[3].y; + } + + inline void fillPose(geometry_msgs::Pose& pose, const cv::Vec3d& rvec, const cv::Vec3d& tvec) const + { + pose.position.x = tvec[0]; + pose.position.y = tvec[1]; + pose.position.z = tvec[2]; + + double angle = norm(rvec); + cv::Vec3d axis = rvec / angle; + + tf2::Quaternion q; + q.setRotation(tf2::Vector3(axis[0], axis[1], axis[2]), angle); + + pose.orientation.w = q.w(); + pose.orientation.x = q.x(); + pose.orientation.y = q.y(); + pose.orientation.z = q.z(); + } + + inline void fillTranslation(geometry_msgs::Vector3& translation, const cv::Vec3d& tvec) const + { + translation.x = tvec[0]; + translation.y = tvec[1]; + translation.z = tvec[2]; + } + + void pushVisMarkers(const std::string& frame_id, const ros::Time& stamp, + const geometry_msgs::Pose &pose, double length, int id, int index) + { + visualization_msgs::Marker marker; + marker.header.frame_id = frame_id; + marker.header.stamp = stamp; + marker.action = visualization_msgs::Marker::ADD; + marker.id = index; + + // Marker + marker.ns = "aruco_marker"; + marker.type = visualization_msgs::Marker::CUBE; + marker.scale.x = length; + marker.scale.y = length; + marker.scale.z = 0.001; + marker.color.r = 1; + marker.color.g = 1; + marker.color.b = 1; + marker.color.a = 0.9; + marker.pose = pose; + vis_array_.markers.push_back(marker); + + // Label + marker.ns = "aruco_marker_label"; + marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING; + marker.scale.z = length * 0.6; + marker.color.r = 0; + marker.color.g = 0; + marker.color.b = 0; + marker.color.a = 1; + marker.text = std::to_string(id); + marker.pose = pose; + vis_array_.markers.push_back(marker); + } + + void snapOrientation(geometry_msgs::Pose& pose, const geometry_msgs::Quaternion orientation) + { + tf::Quaternion q; + q.setRPY(0, 0, -tf::getYaw(pose.orientation) + tf::getYaw(orientation) + M_PI / 2); + tf::Quaternion pq; + tf::quaternionMsgToTF(orientation, pq); + pq = pq * q; + tf::quaternionTFToMsg(pq, pose.orientation); + } + + inline std::string getChildFrameId(int id) const + { + return "aruco_" + std::to_string(id); + } +}; + +PLUGINLIB_EXPORT_CLASS(ArucoDetect, nodelet::Nodelet) diff --git a/aruco_pose/src/aruco_map.cpp b/aruco_pose/src/aruco_map.cpp new file mode 100644 index 000000000..f6d48e745 --- /dev/null +++ b/aruco_pose/src/aruco_map.cpp @@ -0,0 +1,307 @@ +/* + * Positioning ArUco markers maps + * Copyright (C) 2018 Copter Express Technologies + * + * Author: Oleg Kalachev + * + * Distributed under MIT License (available at https://opensource.org/licenses/MIT). + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + */ + +/* + * Code is based on https://github.com/UbiquityRobotics/fiducials, which is distributed + * under the BSD license. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include "utils.h" +#include "gridboard.h" + +using std::vector; +using cv::Mat; + +class ArucoMap : public nodelet::Nodelet { +private: + ros::NodeHandle nh_, nh_priv_; + ros::Publisher img_pub_, pose_pub_; + ros::Subscriber markers_sub_, cinfo_sub; + cv::Ptr board_; + Mat camera_matrix_, dist_coeffs_; + geometry_msgs::TransformStamped transform_; + geometry_msgs::PoseWithCovarianceStamped pose_; + tf2_ros::TransformBroadcaster br_; + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_{tf_buffer_}; + visualization_msgs::MarkerArray vis_markers; + std::string snap_orientation_; + bool has_camera_info_ = false; + +public: + virtual void onInit() + { + nh_ = getNodeHandle(); + nh_priv_ = getPrivateNodeHandle(); + + image_transport::ImageTransport it_priv(nh_priv_); + + // TODO: why image_transport doesn't work here? + img_pub_ = nh_priv_.advertise("image", 1, true); + + board_ = cv::makePtr(); + board_->dictionary = cv::aruco::getPredefinedDictionary( + static_cast(nh_priv_.param("dictionary", 2))); + camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F); + dist_coeffs_ = cv::Mat::zeros(8, 1, CV_64F); + + std::string type, map, map_name; + nh_priv_.param("type", type, "map"); + + if (type == "map") { + param(nh_priv_, "map", map); + loadMap(map); + } else if (type == "gridboard") { + createGridBoard(); + } else { + ROS_FATAL("aruco_map: unknown type: %s", type.c_str()); + ros::shutdown(); + } + + nh_priv_.param("name", map_name, "map"); + nh_priv_.param("frame_id", transform_.child_frame_id, "aruco_map"); + nh_priv_.param("snap_orientation", snap_orientation_, ""); + + pose_pub_ = nh_priv_.advertise("pose", 1); + + // TODO: use synchronised subscriber + markers_sub_ = nh_.subscribe("markers", 1, &ArucoMap::markersCallback, this); + cinfo_sub = nh_.subscribe("camera_info", 1, &ArucoMap::cinfoCallback, this); + + publishMapImage(); + ROS_INFO("aruco_map: ready"); + } + + void markersCallback(const aruco_pose::MarkerArray& markers) + { + if (!has_camera_info_) return; + if (markers.markers.empty()) return; + + int count = markers.markers.size(); + std::vector ids; + std::vector> corners; + ids.reserve(count); + corners.reserve(count); + + for(auto const &marker : markers.markers) { + ids.push_back(marker.id); + std::vector marker_corners = { + cv::Point2f(marker.c1.x, marker.c1.y), + cv::Point2f(marker.c2.x, marker.c2.y), + cv::Point2f(marker.c3.x, marker.c3.y), + cv::Point2f(marker.c4.x, marker.c4.y) + }; + corners.push_back(marker_corners); + } + + Mat obj_points, img_points; + cv::Vec3d rvec, tvec; + + if (snap_orientation_.empty()) { + // simple estimation + int valid = cv::aruco::estimatePoseBoard(corners, ids, board_, camera_matrix_, dist_coeffs_, + rvec, tvec, false); + if (!valid) return; + + transform_.header.stamp = markers.header.stamp; + transform_.header.frame_id = markers.header.frame_id; + pose_.header = transform_.header; + fillPose(pose_.pose.pose, rvec, tvec); + fillTransform(transform_.transform, rvec, tvec); + + } else { + // estimation with "snapping" + cv::aruco::getBoardObjectAndImagePoints(board_, corners, ids, obj_points, img_points); + if (obj_points.empty()) return; + + double center_x = 0, center_y = 0; + alignObjPointsToCenter(obj_points, center_x, center_y); + + int res = solvePnP(obj_points, img_points, camera_matrix_, dist_coeffs_, rvec, tvec, false); + if (!res) return; + + fillTransform(transform_.transform, rvec, tvec); + try { + geometry_msgs::TransformStamped snap_to = tf_buffer_.lookupTransform(markers.header.frame_id, + snap_orientation_, markers.header.stamp, ros::Duration(0.02)); + snapOrientation(transform_.transform.rotation, snap_to.transform.rotation); + } catch (const tf2::TransformException& e) { + ROS_WARN_THROTTLE(1, "aruco_map: can't snap: %s", e.what()); + } + + geometry_msgs::TransformStamped shift; + shift.transform.translation.x = -center_x; + shift.transform.translation.y = -center_y; + shift.transform.rotation.w = 1; + tf2::doTransform(shift, transform_, transform_); + + transform_.header.stamp = markers.header.stamp; + transform_.header.frame_id = markers.header.frame_id; + pose_.header = transform_.header; + transformToPose(transform_.transform, pose_.pose.pose); + } + + br_.sendTransform(transform_); + pose_pub_.publish(pose_); + } + + void cinfoCallback(const sensor_msgs::CameraInfoConstPtr& cinfo) + { + parseCameraInfo(cinfo, camera_matrix_, dist_coeffs_); + has_camera_info_ = true; + } + + void alignObjPointsToCenter(Mat &obj_points, double ¢er_x, double ¢er_y) const + { + // Align object points to the center of mass + double sum_x = 0; + double sum_y = 0; + + for (int i = 0; i < obj_points.rows; i++) { + sum_x += obj_points.at(i, 0); + sum_y += obj_points.at(i, 1); + } + + center_x = sum_x / obj_points.rows; + center_y = sum_y / obj_points.rows; + + for (int i = 0; i < obj_points.rows; i++) { + obj_points.at(i, 0) -= center_x; + obj_points.at(i, 1) -= center_y; + } + } + + void loadMap(std::string filename) + { + std::ifstream f(filename); + std::string line; + + if (!f.good()) { + ROS_FATAL("aruco_map: %s - %s", strerror(errno), filename.c_str()); + ros::shutdown(); + } + + while (std::getline(f, line)) { + int id; + double length, x, y, z, yaw, pitch, roll; + + std::istringstream s(line); + ROS_INFO("aruco_map: parse line: %s", line.c_str()); + + if (!(s >> id >> length >> x >> y >> z >> yaw >> pitch >> roll)) { + ROS_ERROR("aruco_map: cannot parse line: %s", line.c_str()); + continue; + } + addMarker(id, length, x, y, z, yaw, pitch, roll); + } + + ROS_INFO("aruco_map: loading %s complete (%d markers)", filename.c_str(), static_cast(board_->ids.size())); + } + + void createGridBoard() + { + ROS_INFO("aruco_map: generate gridboard"); + ROS_WARN("aruco_map: gridboard maps are deprecated"); + + int markers_x, markers_y, first_marker; + double markers_side, markers_sep_x, markers_sep_y; + std::vector marker_ids; + nh_priv_.param("markers_x", markers_x, 10); + nh_priv_.param("markers_y", markers_y, 10); + nh_priv_.param("first_marker", first_marker, 0); + + param(nh_priv_, "markers_side", markers_side); + param(nh_priv_, "markers_sep_x", markers_sep_x); + param(nh_priv_, "markers_sep_y", markers_sep_y); + + if (nh_priv_.getParam("marker_ids", marker_ids)) { + if ((unsigned int)(markers_x * markers_y) != marker_ids.size()) { + ROS_FATAL("~marker_ids length should be equal to ~markers_x * ~markers_y"); + exit(1); + } + } else { + // Fill marker_ids automatically + marker_ids.resize(markers_x * markers_y); + for (int i = 0; i < markers_x * markers_y; i++) + { + marker_ids.at(i) = first_marker++; + } + } + + createCustomGridBoard(board_, markers_x, markers_y, markers_side, markers_sep_x, markers_sep_y, marker_ids); + } + + void addMarker(int id, double length, double x, double y, double z, + double yaw, double pitch, double roll) + { + // Create transform + geometry_msgs::TransformStamped t; + t.transform.translation.x = x; + t.transform.translation.y = y; + t.transform.translation.z = z; + tf::Quaternion q; + q.setRPY(roll, pitch, yaw); + tf::quaternionTFToMsg(q, t.transform.rotation); + + // TODO: process roll pitch yaw + vector obj_points(4); + setMarkerObjectPoints(x, y, z, yaw, length, obj_points); + board_->ids.push_back(id); + board_->objPoints.push_back(obj_points); + } + + void setMarkerObjectPoints(float x, float y, float z, float yaw, float length, + vector& obj_points) + { + obj_points[0] = cv::Point3f(x - length / 2, y + length / 2, 0); + obj_points[1] = obj_points[0] + cv::Point3f(length, 0, 0); + obj_points[2] = obj_points[0] + cv::Point3f(length, -length, 0); + obj_points[3] = obj_points[0] + cv::Point3f(0, -length, 0); + } + + void publishMapImage() + { + cv::Mat image; + cv_bridge::CvImage msg; + cv::aruco::drawPlanarBoard(board_, cv::Size(2000, 2000), image, 50, 1); + cv::cvtColor(image, image, CV_GRAY2BGR); + msg.encoding = sensor_msgs::image_encodings::BGR8; + msg.image = image; + img_pub_.publish(msg.toImageMsg()); + } +}; + +PLUGINLIB_EXPORT_CLASS(ArucoMap, nodelet::Nodelet) diff --git a/aruco_pose/src/aruco_pose.cpp b/aruco_pose/src/aruco_pose.cpp deleted file mode 100644 index 8f1818485..000000000 --- a/aruco_pose/src/aruco_pose.cpp +++ /dev/null @@ -1,350 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "util.h" - -using std::vector; -using std::string; - -namespace aruco_pose { - -class ArucoPose : public nodelet::Nodelet { - tf::TransformBroadcaster br; - cv::Ptr dictionary; - cv::Ptr parameters; - cv::Ptr board; - std::string frame_id_; - image_transport::CameraSubscriber img_sub; - image_transport::Publisher img_pub; - ros::Publisher marker_pub; - ros::Publisher pose_pub; - ros::NodeHandle nh_, nh_priv_; - - virtual void onInit(); - void createBoard(); - cv::Point3f getObjPointsCenter(cv::Mat objPoints); - void detect(const sensor_msgs::ImageConstPtr&, const sensor_msgs::CameraInfoConstPtr&); - void parseCameraInfo(const sensor_msgs::CameraInfoConstPtr&, cv::Mat&, cv::Mat&); - tf::Transform aruco2tf(cv::Mat rvec, cv::Mat tvec); -}; - -void ArucoPose::onInit() { - ROS_INFO("Initializing aruco_pose"); - nh_ = getNodeHandle(); - nh_priv_ = getPrivateNodeHandle(); - - nh_priv_.param("frame_id", frame_id_, std::string("aruco_map")); - - dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_1000); - parameters = cv::aruco::DetectorParameters::create(); - - try - { - createBoard(); - } - catch (const std::exception &exc) - { - std::cerr << exc.what(); - exit(0); - } - - image_transport::ImageTransport it(nh_); - img_sub = it.subscribeCamera("image", 1, &ArucoPose::detect, this); - - image_transport::ImageTransport it_priv(nh_priv_); - img_pub = it_priv.advertise("debug", 1); - - pose_pub = nh_priv_.advertise("pose", 1); - - ROS_INFO("aruco_pose nodelet inited"); -} - -cv::Ptr createCustomGridBoard(int markersX, int markersY, float markerLength, float markerSeparationX, float markerSeparationY, - const cv::Ptr &dictionary, std::vector ids) { - - CV_Assert(markersX > 0 && markersY > 0 && markerLength > 0 && markerSeparationX > 0 && markerSeparationY > 0); - - cv::Ptr res = cv::makePtr(); - - res->dictionary = dictionary; - - size_t totalMarkers = (size_t) markersX * markersY; - res->ids = ids; - res->objPoints.reserve(totalMarkers); - - // calculate Board objPoints - float maxY = (float)markersY * markerLength + (markersY - 1) * markerSeparationY; - for(int y = 0; y < markersY; y++) { - for(int x = 0; x < markersX; x++) { - std::vector< cv::Point3f > corners; - corners.resize(4); - corners[0] = cv::Point3f(x * (markerLength + markerSeparationX), - maxY - y * (markerLength + markerSeparationY), 0); - corners[1] = corners[0] + cv::Point3f(markerLength, 0, 0); - corners[2] = corners[0] + cv::Point3f(markerLength, -markerLength, 0); - corners[3] = corners[0] + cv::Point3f(0, -markerLength, 0); - res->objPoints.push_back(corners); - } - } - - return res; -} - -cv::Ptr createCustomBoard(std::map markers, const cv::Ptr &dictionary) { - cv::Ptr res = cv::makePtr(); - - res->dictionary = dictionary; - - size_t total_markers = markers.size(); - res->ids.reserve(total_markers); - res->objPoints.reserve(total_markers); - - // Generate ids and objPoints - for(auto const &marker : markers) { - res->ids.push_back(std::stoi(marker.first)); - - vector parts; - parts = strSplit(marker.second, " "); - - float size = std::stof(parts.at(0)); - float x = std::stof(parts.at(1)); - float y = std::stof(parts.at(2)); - float z = std::stof(parts.at(3)); - float yaw = std::stof(parts.at(4)); - float pitch = std::stof(parts.at(5)); - float roll = std::stof(parts.at(6)); - - vector corners; - corners.resize(4); - corners[0] = cv::Point3f(x - size / 2, y + size / 2, 0); - corners[1] = corners[0] + cv::Point3f(size, 0, 0); - corners[2] = corners[0] + cv::Point3f(size, -size, 0); - corners[3] = corners[0] + cv::Point3f(0, -size, 0); - - // TODO: process yaw, pitch, roll - - res->objPoints.push_back(corners); - } - - return res; -} - -#include "fix.cpp" - -void ArucoPose::createBoard() -{ - static auto map_image_pub = nh_priv_.advertise("map_image", 1, true); - cv_bridge::CvImage map_image_msg; - cv::Mat map_image; - - std::string type; - - nh_priv_.param("type", type, "gridboard"); - if (type == "gridboard") - { - ROS_INFO("Initialize gridboard"); - - int markers_x, markers_y, first_marker; - float markers_side, markers_sep_x, markers_sep_y; - std::vector marker_ids; - nh_priv_.param("markers_x", markers_x, 10); - nh_priv_.param("markers_y", markers_y, 10); - nh_priv_.param("first_marker", first_marker, 0); - - if (!nh_priv_.getParam("markers_side", markers_side)) - { - ROS_ERROR("gridboard: required parameter ~markers_side is not set."); - exit(1); - } - - if (!nh_priv_.getParam("markers_sep_x", markers_sep_x)) - { - if (!nh_priv_.getParam("markers_sep", markers_sep_x)) - { - ROS_ERROR("gridboard: ~markers_sep_x or ~markers_sep parameters are required"); - exit(1); - } - } - - if (!nh_priv_.getParam("markers_sep_y", markers_sep_y)) - { - if (!nh_priv_.getParam("markers_sep", markers_sep_y)) - { - ROS_ERROR("gridboard: ~markers_sep_y or ~markers_sep parameters are required"); - exit(1); - } - } - - if (nh_priv_.getParam("marker_ids", marker_ids)) - { - if (markers_x * markers_y != marker_ids.size()) - { - ROS_FATAL("~marker_ids length should be equal to ~markers_x * ~markers_y"); - exit(1); - } - } - else - { - // Fill marker_ids automatically - marker_ids.resize(markers_x * markers_y); - for(int i = 0; i < markers_x * markers_y; i++) - { - marker_ids.at(i) = first_marker++; - } - } - - // Create grid board - board = createCustomGridBoard(markers_x, markers_y, markers_side, markers_sep_x, markers_sep_y, dictionary, marker_ids); - - // Publish map image for debugging - _drawPlanarBoard(board, cv::Size(2000, 2000), map_image, 50, 1); - - cv::cvtColor(map_image, map_image, CV_GRAY2BGR); - - map_image_msg.encoding = sensor_msgs::image_encodings::BGR8; - map_image_msg.image = map_image; - map_image_pub.publish(map_image_msg.toImageMsg()); - } - else if (type == "custom") - { - ROS_INFO("Initialize a custom board"); - - std::map markers; - nh_priv_.getParam("markers", markers); - - board = createCustomBoard(markers, dictionary); - - ROS_INFO("Draw a custom board"); - // Publish map image for debugging - _drawPlanarBoard(board, cv::Size(2000, 2000), map_image, 50, 1); - - cv::cvtColor(map_image, map_image, CV_GRAY2BGR); - - map_image_msg.encoding = sensor_msgs::image_encodings::BGR8; - map_image_msg.image = map_image; - map_image_pub.publish(map_image_msg.toImageMsg()); - } - else - { - ROS_ERROR("Incorrect map type '%s'", type.c_str()); - } -} - -cv::Point3f ArucoPose::getObjPointsCenter(cv::Mat objPoints) { - float min_x = std::numeric_limits::max(); - float max_x = std::numeric_limits::min(); - float min_y = min_x, max_y = max_x; - for (int i = 0; i < objPoints.rows; i++) { - max_x = std::max(max_x, objPoints.at(i, 0)); - max_y = std::max(max_y, objPoints.at(i, 1)); - min_x = std::min(min_x, objPoints.at(i, 0)); - min_y = std::min(min_y, objPoints.at(i, 1)); - } - cv::Point3f res((min_x + max_x) / 2, (min_y + max_y) / 2, 0); - return res; -} - -void ArucoPose::detect(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr &cinfo) { - cv::Mat image = cv_bridge::toCvShare(msg, "bgr8")->image; - - std::vector markerIds; - std::vector> markerCorners; - std::vector> rejectedCandidates; - - cv::aruco::detectMarkers(image, dictionary, markerCorners, markerIds, parameters, rejectedCandidates); - - cv::Mat cameraMatrix(3, 3, CV_64F); - cv::Mat distCoeffs(8, 1, CV_64F); - parseCameraInfo(cinfo, cameraMatrix, distCoeffs); - - int valid = 0; - cv::Mat rvec, tvec, objPoints; - - if (markerIds.size() > 0) { - - valid = _estimatePoseBoard(markerCorners, markerIds, board, cameraMatrix, distCoeffs, - rvec, tvec, false, objPoints); - - if (valid) { - // Send map transform - tf::StampedTransform transform(aruco2tf(rvec, tvec), msg->header.stamp, cinfo->header.frame_id, frame_id_); - br.sendTransform(transform); - - // Publish map pose - static geometry_msgs::PoseStamped ps; - ps.header.frame_id = frame_id_; - ps.header.stamp = msg->header.stamp; - ps.pose.orientation.w = 1; - pose_pub.publish(ps); - - // Send reference point - cv::Point3f ref = getObjPointsCenter(objPoints); - tf::Vector3 ref_vector3 = tf::Vector3(ref.x, ref.y, ref.z); - tf::Quaternion q(0, 0, 0); - static tf::StampedTransform ref_transform; - ref_transform.stamp_ = msg->header.stamp; - ref_transform.frame_id_ = frame_id_; - ref_transform.child_frame_id_ = "aruco_map_reference"; - ref_transform.setOrigin(ref_vector3); - ref_transform.setRotation(q); - br.sendTransform(ref_transform); - } - } - - if (img_pub.getNumSubscribers() > 0) - { - cv::aruco::drawDetectedMarkers(image, markerCorners, markerIds); // draw markers - if (valid) - { - cv::aruco::drawAxis(image, cameraMatrix, distCoeffs, rvec, tvec, 0.3); // draw board axis - } - 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::BGR8; - out_msg.image = image; - img_pub.publish(out_msg.toImageMsg()); - } -} - -void ArucoPose::parseCameraInfo(const sensor_msgs::CameraInfoConstPtr &cinfo, cv::Mat &cameraMat, cv::Mat &distCoeffs) { - for (int i = 0; i < 3; ++i) { - for (int j = 0; j < 3; ++j) { - cameraMat.at(i, j) = cinfo->K[3 * i + j]; - } - } - for (int k = 0; k < cinfo->D.size(); k++) { - distCoeffs.at(k) = cinfo->D[k]; - } -} - -tf::Transform ArucoPose::aruco2tf(cv::Mat rvec, cv::Mat tvec) { - - cv::Mat rot; - cv::Rodrigues(rvec, rot); - - 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(tvec.at(0,0), tvec.at(1,0), tvec.at(2,0)); - return tf::Transform(tf_rot, tf_orig); -} - -PLUGINLIB_EXPORT_CLASS(ArucoPose, nodelet::Nodelet) - -} diff --git a/aruco_pose/src/fix.cpp b/aruco_pose/src/fix.cpp deleted file mode 100644 index c15bf3b72..000000000 --- a/aruco_pose/src/fix.cpp +++ /dev/null @@ -1,145 +0,0 @@ -using namespace cv; -using namespace cv::aruco; - -// Temporal fix! -// TODO: remove -// fix strange bug in our OpenCV version - -void _getBoardObjectAndImagePoints(const Ptr &board, InputArrayOfArrays detectedCorners, - InputArray detectedIds, OutputArray objPoints, OutputArray imgPoints) { - - CV_Assert(board->ids.size() == board->objPoints.size()); - CV_Assert(detectedIds.total() == detectedCorners.total()); - - size_t nDetectedMarkers = detectedIds.total(); - - std::vector< Point3f > objPnts; - objPnts.reserve(nDetectedMarkers); - - std::vector< Point2f > imgPnts; - imgPnts.reserve(nDetectedMarkers); - - // look for detected markers that belong to the board and get their information - for(unsigned int i = 0; i < nDetectedMarkers; i++) { - int currentId = detectedIds.getMat().ptr< int >(0)[i]; - for(unsigned int j = 0; j < board->ids.size(); j++) { - if(currentId == board->ids[j]) { - for(int p = 0; p < 4; p++) { - objPnts.push_back(board->objPoints[j][p]); - imgPnts.push_back(detectedCorners.getMat(i).ptr< Point2f >(0)[p]); - } - } - } - } - - // create output - Mat(objPnts).copyTo(objPoints); - Mat(imgPnts).copyTo(imgPoints); -} - -int _estimatePoseBoard(InputArrayOfArrays _corners, InputArray _ids, const Ptr &board, - InputArray _cameraMatrix, InputArray _distCoeffs, OutputArray _rvec, - OutputArray _tvec, bool useExtrinsicGuess, Mat &objPoints) { - - CV_Assert(_corners.total() == _ids.total()); - - // get object and image points for the solvePnP function - Mat /*objPoints, */imgPoints; - _getBoardObjectAndImagePoints(board, _corners, _ids, objPoints, imgPoints); - - CV_Assert(imgPoints.total() == objPoints.total()); - - if(objPoints.total() == 0) // 0 of the detected markers in board - return 0; - -// std::cout << "objPoints: " << objPoints << std::endl; -// std::cout << "imgPoints: " << imgPoints << std::endl; - - solvePnP(objPoints, imgPoints, _cameraMatrix, _distCoeffs, _rvec, _tvec, useExtrinsicGuess); - - // divide by four since all the four corners are concatenated in the array for each marker - return (int)objPoints.total() / 4; -} - -void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginSize, - int borderBits) { - - CV_Assert(outSize.area() > 0); - CV_Assert(marginSize >= 0); - - _img.create(outSize, CV_8UC1); - Mat out = _img.getMat(); - out.setTo(Scalar::all(255)); - out.adjustROI(-marginSize, -marginSize, -marginSize, -marginSize); - - // calculate max and min values in XY plane - CV_Assert(_board->objPoints.size() > 0); - float minX, maxX, minY, maxY; - minX = maxX = _board->objPoints[0][0].x; - minY = maxY = _board->objPoints[0][0].y; - - for(unsigned int i = 0; i < _board->objPoints.size(); i++) { - for(int j = 0; j < 4; j++) { - minX = min(minX, _board->objPoints[i][j].x); - maxX = max(maxX, _board->objPoints[i][j].x); - minY = min(minY, _board->objPoints[i][j].y); - maxY = max(maxY, _board->objPoints[i][j].y); - } - } - - float sizeX = maxX - minX; - float sizeY = maxY - minY; - - // proportion transformations - float xReduction = sizeX / float(out.cols); - float yReduction = sizeY / float(out.rows); - - // determine the zone where the markers are placed - if(xReduction > yReduction) { - int nRows = int(sizeY / xReduction); - int rowsMargins = (out.rows - nRows) / 2; - out.adjustROI(-rowsMargins, -rowsMargins, 0, 0); - } else { - int nCols = int(sizeX / yReduction); - int colsMargins = (out.cols - nCols) / 2; - out.adjustROI(0, 0, -colsMargins, -colsMargins); - } - - // now paint each marker - Dictionary &dictionary = *(_board->dictionary); - Mat marker; - Point2f outCorners[3]; - Point2f inCorners[3]; - for(unsigned int m = 0; m < _board->objPoints.size(); m++) { - // transform corners to markerZone coordinates - for(int j = 0; j < 3; j++) { - Point2f pf = Point2f(_board->objPoints[m][j].x, _board->objPoints[m][j].y); - // move top left to 0, 0 - pf -= Point2f(minX, minY); - pf.x = pf.x / sizeX * float(out.cols); - pf.y = (1.0f - pf.y / sizeY) * float(out.rows); - outCorners[j] = pf; - } - - // get marker - Size dst_sz(outCorners[2] - outCorners[0]); // assuming CCW order - dst_sz.width = dst_sz.height = std::min(dst_sz.width, dst_sz.height); //marker should be square - dictionary.drawMarker(_board->ids[m], dst_sz.width, marker, borderBits); - - if((outCorners[0].y == outCorners[1].y) && (outCorners[1].x == outCorners[2].x)) { - // marker is aligned to image axes - marker.copyTo(out(Rect(outCorners[0], dst_sz))); - continue; - } - - // interpolate tiny marker to marker position in markerZone - inCorners[0] = Point2f(-0.5f, -0.5f); - inCorners[1] = Point2f(marker.cols - 0.5f, -0.5f); - inCorners[2] = Point2f(marker.cols - 0.5f, marker.rows - 0.5f); - - // remove perspective - Mat transformation = getAffineTransform(inCorners, outCorners); - warpAffine(marker, out, transformation, out.size(), INTER_LINEAR, - BORDER_TRANSPARENT); - } -} diff --git a/aruco_pose/src/gridboard.h b/aruco_pose/src/gridboard.h new file mode 100644 index 000000000..87c428ffe --- /dev/null +++ b/aruco_pose/src/gridboard.h @@ -0,0 +1,22 @@ +void createCustomGridBoard(cv::Ptr& board, int markersX, int markersY, float markerLength, + float markerSeparationX, float markerSeparationY, std::vector ids) +{ + size_t totalMarkers = (size_t) markersX * markersY; + board->ids = ids; + board->objPoints.reserve(totalMarkers); + + // calculate Board objPoints + float maxY = (float)markersY * markerLength + (markersY - 1) * markerSeparationY; + for(int y = 0; y < markersY; y++) { + for(int x = 0; x < markersX; x++) { + std::vector< cv::Point3f > corners; + corners.resize(4); + corners[0] = cv::Point3f(x * (markerLength + markerSeparationX), + maxY - y * (markerLength + markerSeparationY), 0); + corners[1] = corners[0] + cv::Point3f(markerLength, 0, 0); + corners[2] = corners[0] + cv::Point3f(markerLength, -markerLength, 0); + corners[3] = corners[0] + cv::Point3f(0, -markerLength, 0); + board->objPoints.push_back(corners); + } + } +} diff --git a/aruco_pose/src/util.h b/aruco_pose/src/util.h deleted file mode 100644 index 2371c59a1..000000000 --- a/aruco_pose/src/util.h +++ /dev/null @@ -1,20 +0,0 @@ -#pragma once - -#include -#include - -std::vector strSplit(const std::string& str, const std::string& delim) -{ - std::vector tokens; - size_t prev = 0, pos = 0; - do - { - pos = str.find(delim, prev); - if (pos == std::string::npos) pos = str.length(); - std::string token = str.substr(prev, pos-prev); - if (!token.empty()) tokens.push_back(token); - prev = pos + delim.length(); - } - while (pos < str.length() && prev < str.length()); - return tokens; -} diff --git a/aruco_pose/src/utils.h b/aruco_pose/src/utils.h new file mode 100644 index 000000000..2822cca3e --- /dev/null +++ b/aruco_pose/src/utils.h @@ -0,0 +1,109 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +// Read required param or shutdown the node +template +static void param(ros::NodeHandle nh, const std::string& param_name, T& param_val) +{ + if (!nh.getParam(param_name, param_val)) { + ROS_FATAL("Required param %s is not defined", param_name.c_str()); + ros::shutdown(); + } +} + +static void parseCameraInfo(const sensor_msgs::CameraInfoConstPtr& cinfo, + cv::Mat& matrix, cv::Mat& dist) +{ + for (unsigned int i = 0; i < 3; ++i) + for (unsigned int j = 0; j < 3; ++j) + matrix.at(i, j) = cinfo->K[3 * i + j]; + + for (unsigned int k = 0; k < cinfo->D.size(); k++) + dist.at(k) = cinfo->D[k]; +} + +inline void rotatePoint(cv::Point3f& p, cv::Point3f origin, float angle) +{ + float s = sin(angle); + float c = cos(angle); + + // translate point back to origin: + p.x -= origin.x; + p.y -= origin.y; + + // rotate point + float xnew = p.x * c - p.y * s; + float ynew = p.x * s + p.y * c; + + // translate point back: + p.x = xnew + origin.x; + p.y = ynew + origin.y; +} + +inline void fillPose(geometry_msgs::Pose& pose, const cv::Vec3d& rvec, const cv::Vec3d& tvec) +{ + pose.position.x = tvec[0]; + pose.position.y = tvec[1]; + pose.position.z = tvec[2]; + + double angle = norm(rvec); + cv::Vec3d axis = rvec / angle; + + tf2::Quaternion q; + q.setRotation(tf2::Vector3(axis[0], axis[1], axis[2]), angle); + + pose.orientation.w = q.w(); + pose.orientation.x = q.x(); + pose.orientation.y = q.y(); + pose.orientation.z = q.z(); +} + +inline void fillTransform(geometry_msgs::Transform& transform, const cv::Vec3d& rvec, const cv::Vec3d& tvec) +{ + transform.translation.x = tvec[0]; + transform.translation.y = tvec[1]; + transform.translation.z = tvec[2]; + + double angle = norm(rvec); + cv::Vec3d axis = rvec / angle; + + tf2::Quaternion q; + q.setRotation(tf2::Vector3(axis[0], axis[1], axis[2]), angle); + + transform.rotation.w = q.w(); + transform.rotation.x = q.x(); + transform.rotation.y = q.y(); + transform.rotation.z = q.z(); +} + +inline void fillTranslation(geometry_msgs::Vector3& translation, const cv::Vec3d& tvec) +{ + translation.x = tvec[0]; + translation.y = tvec[1]; + translation.z = tvec[2]; +} + +inline void snapOrientation(geometry_msgs::Quaternion& to, const geometry_msgs::Quaternion& from) +{ + tf::Quaternion q; + q.setRPY(0, 0, -tf::getYaw(to) + tf::getYaw(from)); + tf::Quaternion pq; + tf::quaternionMsgToTF(from, pq); + pq = pq * q; + tf::quaternionTFToMsg(pq, to); +} + +inline void transformToPose(const geometry_msgs::Transform& transform, geometry_msgs::Pose& pose) +{ + pose.position.x = transform.translation.x; + pose.position.y = transform.translation.y; + pose.position.z = transform.translation.z; + pose.orientation = transform.rotation; +} diff --git a/clever/CMakeLists.txt b/clever/CMakeLists.txt index 6ce7ec76a..4febd5b32 100644 --- a/clever/CMakeLists.txt +++ b/clever/CMakeLists.txt @@ -167,12 +167,16 @@ add_executable(camera_markers src/camera_markers.cpp) add_executable(frames src/frames.cpp) +add_executable(vpe_publisher src/vpe_publisher.cpp) + target_link_libraries(rc ${catkin_LIBRARIES}) target_link_libraries(camera_markers ${catkin_LIBRARIES}) target_link_libraries(frames ${catkin_LIBRARIES}) +target_link_libraries(vpe_publisher ${catkin_LIBRARIES}) + ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the ## target back to the shorter version for ease of user use diff --git a/clever/launch/aruco.launch b/clever/launch/aruco.launch index f9dc88c51..0430b3c5c 100644 --- a/clever/launch/aruco.launch +++ b/clever/launch/aruco.launch @@ -1,24 +1,35 @@ - - + + + - - - - - - - - + + + + - - + + + - - - + + + + + + + + - + + + + + + + + + diff --git a/clever/launch/clever.launch b/clever/launch/clever.launch index 8610bcb4d..3cfa2f533 100644 --- a/clever/launch/clever.launch +++ b/clever/launch/clever.launch @@ -22,7 +22,7 @@ - + @@ -33,7 +33,7 @@ - + diff --git a/clever/src/selfcheck.py b/clever/src/selfcheck.py index 314cc095b..69a8b5b15 100755 --- a/clever/src/selfcheck.py +++ b/clever/src/selfcheck.py @@ -9,6 +9,7 @@ from sensor_msgs.msg import Image, CameraInfo, NavSatFix, Imu, Range from mavros_msgs.msg import State, OpticalFlowRad from geometry_msgs.msg import PoseStamped, TwistStamped +from aruco_pose.msg import MarkerArray # TODO: roscore is running @@ -82,9 +83,9 @@ def check_camera(name): @check('Aruco detector') def check_aruco(): try: - rospy.wait_for_message('aruco_pose/debug', Image, timeout=1) + rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=0.5) except rospy.ROSException: - failure('No aruco_pose/debug messages') + failure('No aruco markers detection') @check('Visual position estimate') diff --git a/clever/src/utils.h b/clever/src/utils.h new file mode 100644 index 000000000..013bcf121 --- /dev/null +++ b/clever/src/utils.h @@ -0,0 +1,36 @@ +#pragma once +// TODO: merge with util.h + +#include +#include +#include +#include + +// Read required param or shutdown the node +template +static void param(ros::NodeHandle nh, const std::string& param_name, T& param_val) +{ + if (!nh.getParam(param_name, param_val)) { + ROS_FATAL("Required param %s is not defined", param_name); + ros::shutdown(); + } +} + +static inline double hypot(double x, double y, double z) +{ + return std::sqrt(x*x + y*y + z*z); +} + +static inline void vectorToPoint(const geometry_msgs::Vector3& vector, geometry_msgs::Point& point) +{ + point.x = vector.x; + point.y = vector.y; + point.z = vector.z; +} + +static inline void pointToVector(const geometry_msgs::Point& point, geometry_msgs::Vector3& vector) +{ + vector.x = point.x; + vector.y = point.y; + vector.z = point.z; +} diff --git a/clever/src/vpe_publisher.cpp b/clever/src/vpe_publisher.cpp new file mode 100644 index 000000000..a13f331bd --- /dev/null +++ b/clever/src/vpe_publisher.cpp @@ -0,0 +1,99 @@ +/* + * Universal VPE publisher node + * Copyright (C) 2018 Copter Express Technologies + * + * Author: Oleg Kalachev + * + * Distributed under MIT License (available at https://opensource.org/licenses/MIT). + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "utils.h" + +using std::string; + +static string frame_id, child_frame_id; +static tf2_ros::Buffer tf_buffer; +static ros::Publisher vpe_pub; +static ros::Subscriber local_position_sub; +static ros::Timer zero_timer; +static geometry_msgs::PoseStamped vpe, pose; +static ros::Time local_pose_stamp(0); +static ros::Duration publish_zero_timout; +tf2_ros::TransformBroadcaster br; + +void publishZero(const ros::TimerEvent&) +{ + if ((ros::Time::now() - pose.header.stamp < publish_zero_timout) || + (ros::Time::now() - vpe.header.stamp < publish_zero_timout)) + return; + + ROS_INFO_THROTTLE(10, "vpe_publisher: publish zero"); + vpe.header.stamp = ros::Time::now(); + vpe.pose.orientation.w = 1; + vpe_pub.publish(vpe); +} + +void localPositionCallback(const geometry_msgs::PoseStamped& msg) +{ + pose = msg; +} + +template +void callback(const T& msg) +{ + try { + auto transform = tf_buffer.lookupTransform(frame_id, child_frame_id, + msg->header.stamp, ros::Duration(0.02)); + vpe.header.stamp = msg->header.stamp; + vpe.pose.position.x = transform.transform.translation.x; + vpe.pose.position.y = transform.transform.translation.y; + vpe.pose.position.z = transform.transform.translation.z; + vpe.pose.orientation = transform.transform.rotation; + vpe.header.stamp = msg->header.stamp; + vpe_pub.publish(vpe); + } catch (const tf2::TransformException& e) { + ROS_WARN_THROTTLE(10, "vpe_publisher: %s", e.what()); + } +} + +int main(int argc, char **argv) { + ros::init(argc, argv, "vpe_publisher"); + ros::NodeHandle nh, nh_priv("~"); + + tf2_ros::TransformListener tf_listener(tf_buffer); + + param(nh_priv, "frame_id", frame_id); + param(nh_priv, "child_frame_id", child_frame_id); + nh_priv.param("mavros/local_position/frame_id", vpe.header.frame_id, "map"); + + auto pose_sub = nh_priv.subscribe("pose", 1, &callback); + auto pose_cov_sub = nh_priv.subscribe("pose_cov", 1, &callback); + auto markers_sub = nh_priv.subscribe("markers", 1, &callback); + //auto pose_cov_sub = nh_priv.subscribe("pose_cov", 1, &callback); + + vpe_pub = nh_priv.advertise("pose_pub", 1); + //vpe_cov_pub = nh_priv_.advertise("pose_cov_pub", 1); + + if (nh_priv.param("publish_zero", false)) { + // publish zero to initialize the local position + vpe.header.stamp = ros::Time(0); + zero_timer = nh.createTimer(ros::Duration(0.1), &publishZero); + publish_zero_timout = ros::Duration(nh_priv.param("publish_zero_timout", 5.0)); + local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &localPositionCallback); + } + + ROS_INFO("vpe_publisher: ready"); + ros::spin(); +}