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);
}
}