diff --git a/image_proc/package.xml b/image_proc/package.xml index 9457ed95..5df57c4d 100644 --- a/image_proc/package.xml +++ b/image_proc/package.xml @@ -30,6 +30,8 @@ rclcpp_components rcutils sensor_msgs + tf2 + tf2_geometry_msgs tracetools_image_pipeline ament_lint_auto diff --git a/image_proc/src/track_marker.cpp b/image_proc/src/track_marker.cpp index e103de06..7150870b 100644 --- a/image_proc/src/track_marker.cpp +++ b/image_proc/src/track_marker.cpp @@ -40,9 +40,10 @@ #include #include #include -#include #include #include +#include +#include namespace image_proc { @@ -135,11 +136,9 @@ void TrackMarkerNode::imageCb( pose.pose.position.y = tvecs[0][1]; pose.pose.position.z = tvecs[0][2]; // Convert angle-axis to quaternion - cv::Quatd q = cv::Quatd::createFromRvec(rvecs[0]); - pose.pose.orientation.x = q.x; - pose.pose.orientation.y = q.y; - pose.pose.orientation.z = q.z; - pose.pose.orientation.w = q.w; + const tf2::Vector3 rvec(rvecs[0][0], rvecs[0][1], rvecs[0][2]); + const tf2::Quaternion q(rvec.normalized(), rvec.length()); + tf2::convert(q, pose.pose.orientation); pub_->publish(pose); } }