PythonとC++のコードに対するリファクタリングの実施

crane_plus_examples/CMakeLists.txt

@@ -17,12 +17,15 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(angles REQUIRED)
 find_package(cv_bridge REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(image_geometry REQUIRED)
+find_package(image_transport REQUIRED)
+find_package(message_filters REQUIRED)
 find_package(moveit_ros_planning_interface REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
-find_package(image_geometry REQUIRED)
 
 # Build and install node executables
 set(executable_list
@@ -37,13 +40,16 @@ set(executable_list
 foreach(loop_var IN LISTS executable_list)
   add_executable(${loop_var} src/${loop_var}.cpp)
   ament_target_dependencies(${loop_var}
+    angles
     cv_bridge
     geometry_msgs
+    image_geometry
+    image_transport
+    message_filters
     moveit_ros_planning_interface
     OpenCV
     rclcpp
     tf2_geometry_msgs
-    image_geometry
   )
 
   install(TARGETS

crane_plus_examples/package.xml

@@ -13,17 +13,20 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>angles</depend>
   <depend>crane_plus_control</depend>
   <depend>crane_plus_description</depend>
   <depend>crane_plus_moveit_config</depend>
   <depend>cv_bridge</depend>
   <depend>geometry_msgs</depend>
+  <depend>image_geometry</depend>
+  <depend>image_transport</depend>
   <depend>libopencv-dev</depend>
+  <depend>message_filters</depend>
   <depend>moveit_ros_planning_interface</depend>
   <depend>rclcpp</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>usb_cam</depend>
-  <depend>image_geometry</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

crane_plus_examples/src/aruco_detection.cpp

@@ -16,7 +16,6 @@
 // https://docs.opencv.org/4.2.0/d5/dae/tutorial_aruco_detection.html
 // https://docs.ros.org/en/humble/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Broadcaster-Cpp.html
 
-#include <cmath>
 #include <memory>
 #include <vector>
 #include <string>
@@ -27,100 +26,85 @@
 #include "sensor_msgs/msg/image.hpp"
 #include "opencv2/opencv.hpp"
 #include "opencv2/aruco.hpp"
+#include "opencv2/core/quaternion.hpp"
 #include "cv_bridge/cv_bridge.hpp"
-#include "tf2/LinearMath/Quaternion.hpp"
-#include "tf2/LinearMath/Matrix3x3.hpp"
 #include "tf2_ros/transform_broadcaster.h"
+#include "image_transport/image_transport.hpp"
+#include "image_transport/camera_subscriber.hpp"
 using std::placeholders::_1;
+using std::placeholders::_2;
 
 class ImageSubscriber : public rclcpp::Node
 {
 public:
   ImageSubscriber()
   : Node("aruco_detection")
   {
-    image_subscription_ = this->create_subscription<sensor_msgs::msg::Image>(
-      "image_raw", 10, std::bind(&ImageSubscriber::image_callback, this, _1));
+    camera_subscription_ = image_transport::create_camera_subscription(
+      this,
+      "image_raw",
+      std::bind(&ImageSubscriber::camera_callback, this, _1, _2),
+      "raw");
 
-    camera_info_subscription_ = this->create_subscription<sensor_msgs::msg::CameraInfo>(
-      "camera_info", 10, std::bind(&ImageSubscriber::camera_info_callback, this, _1));
+    // ArUcoマーカのデータセットを読み込む
+    // DICT_6x6_50は6x6ビットのマーカが50個収録されたもの
+    marker_dict_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_50);
 
     tf_broadcaster_ =
       std::make_unique<tf2_ros::TransformBroadcaster>(*this);
   }
 
 private:
-  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_subscription_;
-  rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr camera_info_subscription_;
-  sensor_msgs::msg::CameraInfo::SharedPtr camera_info_;
+  image_transport::CameraSubscriber camera_subscription_;
   std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+  cv::Ptr<cv::aruco::Dictionary> marker_dict_;
 
-  void image_callback(const sensor_msgs::msg::Image::SharedPtr msg)
+  void camera_callback(
+    const sensor_msgs::msg::Image::ConstSharedPtr & img_msg,
+    const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info_msg)
   {
-    auto cv_img = cv_bridge::toCvShare(msg, msg->encoding);
+    auto cv_img = cv_bridge::toCvShare(img_msg, img_msg->encoding);
     cv::cvtColor(cv_img->image, cv_img->image, cv::COLOR_RGB2BGR);
 
-    if (camera_info_) {
-      // ArUcoマーカのデータセットを読み込む
-      // DICT_6x6_50は6x6ビットのマーカが50個収録されたもの
-      const auto MARKER_DICT = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_50);
-      // マーカID
-      std::vector<int> ids;
-      // 画像座標系上のマーカ頂点位置
-      std::vector<std::vector<cv::Point2f>> corners;
-      // マーカの検出
-      cv::aruco::detectMarkers(cv_img->image, MARKER_DICT, corners, ids);
-      // マーカの検出数
-      int n_markers = ids.size();
-      // カメラパラメータ
-      const auto CAMERA_MATRIX = cv::Mat(3, 3, CV_64F, camera_info_->k.data());
-      const auto DIST_COEFFS = cv::Mat(1, 5, CV_64F, camera_info_->d.data());
-      // マーカ一辺の長さ 0.04 [m]
-      const float MARKER_LENGTH = 0.04;
+    // マーカID
+    std::vector<int> ids;
+    // 画像座標系上のマーカ頂点位置
+    std::vector<std::vector<cv::Point2f>> corners;
+    // マーカの検出
+    cv::aruco::detectMarkers(cv_img->image, marker_dict_, corners, ids);
+    // マーカの検出数
+    int n_markers = ids.size();
 
-      // マーカが一つ以上検出された場合、マーカの位置姿勢をtfで配信
-      if (n_markers > 0) {
-        for (int i = 0; i < n_markers; i++) {
-          // マーカの回転ベクトルと位置ベクトル
-          std::vector<cv::Vec3d> rvecs, tvecs;
-          // 画像座標系上のマーカ位置を三次元のカメラ座標系に変換
-          cv::aruco::estimatePoseSingleMarkers(
-            corners, MARKER_LENGTH, CAMERA_MATRIX, DIST_COEFFS, rvecs, tvecs);
-
-          // tfの配信
-          geometry_msgs::msg::TransformStamped t;
-          t.header = msg->header;
-          t.child_frame_id = "target_" + std::to_string(ids[i]);
-          t.transform.translation.x = tvecs[i][0];
-          t.transform.translation.y = tvecs[i][1];
-          t.transform.translation.z = tvecs[i][2];
-          tf2::Quaternion q;
-          cv::Mat cv_rotation_matrix;
-          cv::Rodrigues(rvecs[i], cv_rotation_matrix);
-          tf2::Matrix3x3 tf2_rotation_matrix = tf2::Matrix3x3(
-            cv_rotation_matrix.at<double>(0, 0),
-            cv_rotation_matrix.at<double>(0, 1),
-            cv_rotation_matrix.at<double>(0, 2),
-            cv_rotation_matrix.at<double>(1, 0),
-            cv_rotation_matrix.at<double>(1, 1),
-            cv_rotation_matrix.at<double>(1, 2),
-            cv_rotation_matrix.at<double>(2, 0),
-            cv_rotation_matrix.at<double>(2, 1),
-            cv_rotation_matrix.at<double>(2, 2));
-          tf2_rotation_matrix.getRotation(q);
-          t.transform.rotation.x = q.x();
-          t.transform.rotation.y = q.y();
-          t.transform.rotation.z = q.z();
-          t.transform.rotation.w = q.w();
-          tf_broadcaster_->sendTransform(t);
-        }
-      }
+    if (n_markers <= 0) {
+      return;
     }
-  }
 
-  void camera_info_callback(const sensor_msgs::msg::CameraInfo::SharedPtr msg)
-  {
-    camera_info_ = msg;
+    // カメラパラメータの読み込み
+    const auto CAMERA_MATRIX = cv::Mat(3, 3, CV_64F, const_cast<double *>(info_msg->k.data()));
+    const auto DIST_COEFFS = cv::Mat(1, 5, CV_64F, const_cast<double *>(info_msg->d.data()));
+    // マーカ一辺の長さ 0.04 [m]
+    const float MARKER_LENGTH = 0.04;
+    // マーカの回転ベクトルと位置ベクトル
+    std::vector<cv::Vec3d> rvecs, tvecs;
+    // 画像座標系上のマーカ位置を三次元のカメラ座標系に変換
+    cv::aruco::estimatePoseSingleMarkers(
+      corners, MARKER_LENGTH, CAMERA_MATRIX, DIST_COEFFS, rvecs, tvecs);
+
+    // tfの配信
+    for (int i = 0; i < n_markers; i++) {
+      geometry_msgs::msg::TransformStamped t;
+      t.header = img_msg->header;
+      t.child_frame_id = "target_" + std::to_string(ids[i]);
+      t.transform.translation.x = tvecs[i][0];
+      t.transform.translation.y = tvecs[i][1];
+      t.transform.translation.z = tvecs[i][2];
+      cv::Quatd cv_q = cv::Quatd::createFromRvec(rvecs[i]);
+      t.transform.rotation.x = cv_q.x;
+      t.transform.rotation.y = cv_q.y;
+      t.transform.rotation.z = cv_q.z;
+      t.transform.rotation.w = cv_q.w;
+      tf_broadcaster_->sendTransform(t);
+    }
   }
 };