diff --git a/aruco_detect/src/aruco_detect.cpp b/aruco_detect/src/aruco_detect.cpp index d9e1199..e73f9e1 100644 --- a/aruco_detect/src/aruco_detect.cpp +++ b/aruco_detect/src/aruco_detect.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include #include @@ -95,6 +96,8 @@ class FiducialsNode { double fiducial_len; + std::string tf_prefix; + bool doPoseEstimation; bool haveCamInfo; bool publishFiducialTf; @@ -111,7 +114,7 @@ class FiducialsNode { ros::NodeHandle nh; ros::NodeHandle pnh; - image_transport::Publisher image_pub; + ros::Publisher image_pub; // log spam prevention int prev_detected_count; @@ -428,8 +431,8 @@ void FiducialsNode::poseEstimateCallback(const FiducialArrayConstPtr & msg) reprojectionError); for (size_t i=0; iimage, cameraMatrix, distortionCoeffs, - rvecs[i], tvecs[i], (float)fiducial_len); + cv::drawFrameAxes(cv_ptr->image, cameraMatrix, distortionCoeffs, + rvecs[i], tvecs[i], (float)fiducial_len); if(verbose){ ROS_INFO("Detected id %d T %.2f %.2f %.2f R %.2f %.2f %.2f", ids[i], tvecs[i][0], tvecs[i][1], tvecs[i][2], @@ -456,22 +459,31 @@ void FiducialsNode::poseEstimateCallback(const FiducialArrayConstPtr & msg) (reprojectionError[i] / dist(corners[i][0], corners[i][2])) * (norm(tvecs[i]) / fiducial_len); + tf2::Quaternion q; + geometry_msgs::Transform transform; + { + transform.translation.x = tvecs[i][0]; + transform.translation.y = tvecs[i][1]; + transform.translation.z = tvecs[i][2]; + 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(); + } + // Standard ROS vision_msgs fiducial_msgs::FiducialTransform ft; - tf2::Quaternion q; if (vis_msgs) { vision_msgs::Detection2D vm; + vm.header = vma.header; vision_msgs::ObjectHypothesisWithPose vmh; vmh.id = ids[i]; vmh.score = exp(-2 * object_error); // [0, infinity] -> [1,0] - vmh.pose.pose.position.x = tvecs[i][0]; - vmh.pose.pose.position.y = tvecs[i][1]; - vmh.pose.pose.position.z = tvecs[i][2]; - q.setRotation(tf2::Vector3(axis[0], axis[1], axis[2]), angle); - vmh.pose.pose.orientation.w = q.w(); - vmh.pose.pose.orientation.x = q.x(); - vmh.pose.pose.orientation.y = q.y(); - vmh.pose.pose.orientation.z = q.z(); + vmh.pose.pose.position.x = transform.translation.x; + vmh.pose.pose.position.y = transform.translation.y; + vmh.pose.pose.position.z = transform.translation.z; + vmh.pose.pose.orientation = transform.rotation; vm.results.push_back(vmh); vma.detections.push_back(vm); @@ -479,14 +491,7 @@ void FiducialsNode::poseEstimateCallback(const FiducialArrayConstPtr & msg) else { ft.fiducial_id = ids[i]; - ft.transform.translation.x = tvecs[i][0]; - ft.transform.translation.y = tvecs[i][1]; - ft.transform.translation.z = tvecs[i][2]; - q.setRotation(tf2::Vector3(axis[0], axis[1], axis[2]), angle); - ft.transform.rotation.w = q.w(); - ft.transform.rotation.x = q.x(); - ft.transform.rotation.y = q.y(); - ft.transform.rotation.z = q.z(); + ft.transform = transform; ft.fiducial_area = calcFiducialArea(corners[i]); ft.image_error = reprojectionError[i]; // Convert image_error (in pixels) to object_error (in meters) @@ -499,28 +504,13 @@ void FiducialsNode::poseEstimateCallback(const FiducialArrayConstPtr & msg) // Publish tf for the fiducial relative to the camera if (publishFiducialTf) { - if (vis_msgs) { - geometry_msgs::TransformStamped ts; - ts.transform.translation.x = tvecs[i][0]; - ts.transform.translation.y = tvecs[i][1]; - ts.transform.translation.z = tvecs[i][2]; - ts.transform.rotation.w = q.w(); - ts.transform.rotation.x = q.x(); - ts.transform.rotation.y = q.y(); - ts.transform.rotation.z = q.z(); - ts.header.frame_id = frameId; - ts.header.stamp = msg->header.stamp; - ts.child_frame_id = "fiducial_" + std::to_string(ids[i]); - broadcaster.sendTransform(ts); - } - else { - geometry_msgs::TransformStamped ts; - ts.transform = ft.transform; - ts.header.frame_id = frameId; - ts.header.stamp = msg->header.stamp; - ts.child_frame_id = "fiducial_" + std::to_string(ft.fiducial_id); - broadcaster.sendTransform(ts); - } + const auto frame = msg->header.frame_id + "_" + tf_prefix + std::to_string(ids[i]); + geometry_msgs::TransformStamped ts; + ts.transform = transform; + ts.header.frame_id = frameId; + ts.header.stamp = msg->header.stamp; + ts.child_frame_id = frame; + broadcaster.sendTransform(ts); } } } @@ -614,6 +604,8 @@ FiducialsNode::FiducialsNode() : nh(), pnh("~"), it(nh) pnh.param("vis_msgs", vis_msgs, false); pnh.param("verbose", verbose, false); + pnh.param("tf_prefix", tf_prefix, "fiducial_"); + std::string str; std::vector strs; @@ -659,7 +651,7 @@ FiducialsNode::FiducialsNode() : nh(), pnh("~"), it(nh) } } - image_pub = it.advertise("/fiducial_images", 1); + image_pub = nh.advertise("fiducial_images", 1); vertices_pub = nh.advertise("fiducial_vertices", 1); diff --git a/stag_detect/CMakeLists.txt b/stag_detect/CMakeLists.txt index fd293a0..61bfaa5 100644 --- a/stag_detect/CMakeLists.txt +++ b/stag_detect/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(catkin REQUIRED COMPONENTS std_msgs fiducial_msgs dynamic_reconfigure + vision_msgs ) find_package(OpenCV REQUIRED) @@ -36,8 +37,6 @@ catkin_package( ## Build ## ########### -add_definitions(-std=c++11) - include_directories( include ${catkin_INCLUDE_DIRS} diff --git a/stag_detect/include/stag_ros/stag_nodelet.h b/stag_detect/include/stag_ros/stag_nodelet.h index 389758d..51f84d3 100644 --- a/stag_detect/include/stag_ros/stag_nodelet.h +++ b/stag_detect/include/stag_ros/stag_nodelet.h @@ -23,7 +23,7 @@ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ #pragma once -#include +#include #include