Merge pull request #1114 from Naoki-Hiraoka/PR-link-Rs

HrpsysSeqStateROSBridgeImpl.cpp: sensor->localR is world coords
start-jsk · Apr 9, 2022 · ee21bf5 · ee21bf5
2 parents 2d08f4b + ca8e27f
commit ee21bf5
Showing 1 changed file with 8 additions and 4 deletions.
diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp
@@ -172,24 +172,28 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridgeImpl::onInitialize()
         continue;
       }
       SensorInfo si;
-      si.transform.setOrigin( tf::Vector3(sensor->localPos(0), sensor->localPos(1), sensor->localPos(2)) );
+      // localPos is parent. https://github.com/start-jsk/rtmros_common/pull/925 https://github.com/start-jsk/rtmros_common/pull/1114
+      hrp::Vector3 localp = sensor->link->Rs.inverse() * sensor->localPos;
+      si.transform.setOrigin( tf::Vector3(localp(0), localp(1), localp(2)) );
       hrp::Vector3 rpy;
       if ( hrp::Sensor::VISION == sensor->type )
         // Rotate sensor->localR 180[deg] because OpenHRP3 camera -Z axis equals to ROS camera Z axis
         // http://www.openrtp.jp/openhrp3/jp/create_model.html
-        rpy = hrp::rpyFromRot(sensor->localR * hrp::rodrigues(hrp::Vector3(1,0,0), M_PI));
+        // localR is parent. https://github.com/start-jsk/rtmros_common/pull/925 https://github.com/start-jsk/rtmros_common/pull/1114
+        rpy = hrp::rpyFromRot(sensor->link->Rs.inverse() * sensor->localR * hrp::rodrigues(hrp::Vector3(1,0,0), M_PI));
       else if ( hrp::Sensor::RANGE == sensor->type )
         {
           // OpenHRP3 range sensor, front direction is -Z axis, and detected plane is XZ plane
           // ROS LaserScan, front direction is X axis, and detected plane is XY plane
           // http://www.openrtp.jp/openhrp3/jp/create_model.html
           hrp::Matrix33 m;
           m << 0, -1, 0, 0, 0, 1, -1, 0, 0;
-          rpy = hrp::rpyFromRot(sensor->localR * m);
+          // localR is parent. https://github.com/start-jsk/rtmros_common/pull/925 https://github.com/start-jsk/rtmros_common/pull/1114
+          rpy = hrp::rpyFromRot(sensor->link->Rs.inverse() * sensor->localR * m);
         }
       else
       {
-        // localR is parent. https://github.com/start-jsk/rtmros_common/pull/925
+        // localR is parent. https://github.com/start-jsk/rtmros_common/pull/925 https://github.com/start-jsk/rtmros_common/pull/1114
         rpy = hrp::rpyFromRot(sensor->link->Rs.inverse() * sensor->localR);
       }
       si.transform.setRotation( tf::createQuaternionFromRPY(rpy(0), rpy(1), rpy(2)) );