[pull] master from choreonoid:master

src/node/choreonoid_ros2.cpp

@@ -1,16 +1,11 @@
-#include <ament_index_cpp/get_package_share_directory.hpp>
+#include <ament_index_cpp/get_package_prefix.hpp>
 #include <rclcpp/utilities.hpp>
 
 #include <cnoid/App>
-#include <cnoid/ExecutablePath>
-#include <cnoid/Plugin>
 #include <cnoid/PluginManager>
-#include <cnoid/ProjectManager>
-#include <cnoid/UTF8>
 
 #include <string>
 #include <vector>
-#include <cstdlib>
 
 int main(int argc, char** argv)
 {
@@ -31,14 +26,8 @@ int main(int argc, char** argv)
 
     cnoid::App app(nonRosArgc, nonRosArgv, "Choreonoid-ROS2", "Choreonoid");
 
-    auto plugin_manager = cnoid::PluginManager::instance();
-
-    if(auto pluginPath = getenv("CNOID_PLUGIN_PATH")){
-      plugin_manager->addPluginPath(cnoid::toUTF8(pluginPath));
-    }
-
-    auto plugin_path = ament_index_cpp::get_package_share_directory("choreonoid_ros") + "/../../lib/choreonoid_ros";
-    plugin_manager->addPluginPath(plugin_path);
+    cnoid::PluginManager::instance()->addPluginDirectoryAsPrefix(
+        ament_index_cpp::get_package_prefix("choreonoid_ros"));
 
     app.requirePluginToCustomizeApplication("ROS2");
 

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