Added geometry_msgs/Vector3 to OpticalFlow.msg

mavlink · Jun 30, 2023 · 6339691 · 6339691
1 parent d1b44e8
commit 6339691
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 27 deletions.
diff --git a/mavros_extras/src/plugins/optical_flow.cpp b/mavros_extras/src/plugins/optical_flow.cpp
@@ -119,11 +119,13 @@ class OpticalFlowPlugin : public plugin::Plugin
         flow.flow_x,
         flow.flow_y,
         0.0));
+
     auto int_flow_comp_m_xy = ftf::transform_frame_aircraft_baselink(
       Eigen::Vector3d(
         flow.flow_comp_m_x,
         flow.flow_comp_m_y,
         0.0));
+
     auto int_flow_rate_xy = ftf::transform_frame_aircraft_baselink(
       Eigen::Vector3d(
         flow.flow_rate_x,
@@ -134,15 +136,12 @@ class OpticalFlowPlugin : public plugin::Plugin
 
     flow_msg.header = header;
 
-    flow_msg.flow[0] = int_flow.x();
-    flow_msg.flow[1] = int_flow.y();
-
-    flow_msg.flow_comp_m[0] = int_flow_comp_m_xy.x();
-    flow_msg.flow_comp_m[1] = int_flow_comp_m_xy.y();
-
-    flow_msg.flow_rate[0] = int_flow_rate_xy.x();
-    flow_msg.flow_rate[1] = int_flow_rate_xy.y();
-
+    flow_msg.flow.x = int_flow.x();
+    flow_msg.flow.y = int_flow.y();
+    flow_msg.flow_comp_m.x = int_flow_comp_m_xy.x();
+    flow_msg.flow_comp_m.y = int_flow_comp_m_xy.y();
+    flow_msg.flow_rate.x = int_flow_rate_xy.x();
+    flow_msg.flow_rate.y = int_flow_rate_xy.y();
     flow_msg.ground_distance = flow.ground_distance;
     flow_msg.quality = flow.quality;
 
@@ -172,21 +171,12 @@ class OpticalFlowPlugin : public plugin::Plugin
   {
     mavlink::common::msg::OPTICAL_FLOW flow_msg = {};
 
-    auto int_flow = ftf::transform_frame_baselink_aircraft(
-      Eigen::Vector3d(
-        msg->flow[0],
-        msg->flow[1],
-        0.0));
-    auto int_flow_comp_m_xy = ftf::transform_frame_baselink_aircraft(
-      Eigen::Vector3d(
-        msg->flow_comp_m[0],
-        msg->flow_comp_m[1],
-        0.0));
-    auto int_flow_rate_xy = ftf::transform_frame_baselink_aircraft(
-      Eigen::Vector3d(
-        msg->flow_rate[0],
-        msg->flow_rate[1],
-        0.0));
+    auto int_flow =
+      ftf::transform_frame_baselink_aircraft(mavros::ftf::to_eigen(msg->flow));
+    auto int_flow_comp_m_xy =
+      ftf::transform_frame_baselink_aircraft(mavros::ftf::to_eigen(msg->flow_comp_m));
+    auto int_flow_rate_xy =
+      ftf::transform_frame_baselink_aircraft(mavros::ftf::to_eigen(msg->flow_rate));
 
     flow_msg.time_usec = get_time_usec(msg->header.stamp);
     flow_msg.sensor_id = 0;

diff --git a/mavros_msgs/msg/OpticalFlow.msg b/mavros_msgs/msg/OpticalFlow.msg
@@ -2,8 +2,8 @@
 
 std_msgs/Header header
 
-int16[2] flow
-float32[2] flow_comp_m
+geometry_msgs/Vector3 flow
+geometry_msgs/Vector3 flow_comp_m
 uint8 quality
 float32 ground_distance
-float32[2] flow_rate
+geometry_msgs/Vector3 flow_rate