From 941e09d2568095d6b79c79e5ac0c7d8e53c16634 Mon Sep 17 00:00:00 2001
From: Yuki Onishi <yuki.st16@gmail.com>
Date: Tue, 19 Mar 2024 01:38:10 +0900
Subject: [PATCH] [BodyROS2] Correct RangeVisionSensor frames

---
 src/plugin/BodyROS2Item.cpp | 39 ++++++++++++++++++++++---------------
 1 file changed, 23 insertions(+), 16 deletions(-)

diff --git a/src/plugin/BodyROS2Item.cpp b/src/plugin/BodyROS2Item.cpp
index e5f567a..4f6df12 100644
--- a/src/plugin/BodyROS2Item.cpp
+++ b/src/plugin/BodyROS2Item.cpp
@@ -522,25 +522,32 @@ void BodyROS2Item::updateRangeVisionSensor(
     range.data.resize(points.size() * range.point_step);
     unsigned char *dst = (unsigned char *) &(range.data[0]);
 
-    Matrix3f Ro = Matrix3f::Identity();
-    bool hasRo = !sensor->opticalFrameRotation().isIdentity();
-    if(hasRo) {
-        Ro = sensor->opticalFrameRotation().cast<float>();
-    }
+    auto Ro = [&]() -> std::optional<Matrix3f> {
+        if(sensor->opticalFrameRotation().isIdentity()){
+            return std::nullopt;
+        }else{
+            return sensor->opticalFrameRotation().cast<float>();
+        }
+    }();
 
     for (size_t j = 0; j < points.size(); ++j) {
-        float x, y, z;
-
-        if (hasRo) {
-            const Vector3f point = Ro * points[j];
-            x = point.x();
-            y = - point.y();
-            z = - point.z();
-        } else {
-            x = points[j].x();
-            y = - points[j].y();
-            z = - points[j].z();
+        Vector3f point = points[j];
+
+        // transforms a point into the Choreonoid-default camera coordinate 
+        // if optical frame rotation is set
+        if (Ro) {
+            // image frame = optical frame rotation (Ro) * camera frame
+            // => camera frame = Ro^T * image frame
+            point = Ro.value().transpose() * point;
         }
+
+        // converts camera coordinate from Choreonoid to ROS (URDF)
+        //   Choreonoid: X-right, Y-upward, Z-backward
+        //   ROS (URDF): X-right, Y-downward, Z-forward
+        const float x = point.x();
+        const float y = - point.y();
+        const float z = - point.z();
+
         std::memcpy(&dst[0], &x, 4);
         std::memcpy(&dst[4], &y, 4);
         std::memcpy(&dst[8], &z, 4);