Humble multi aruco detection (#44)

* feat: select from multiple tags

* fix: reject goal when invalid storage

* fix: cpp lint errors

* fix: correct median filter

* Fix: cpplint error

src/aruco_pose/include/aruco_pose/aruco_pose.hpp

@@ -16,6 +16,7 @@ BSD 3 Clause License. See LICENSE.txt for details.*/
 #include <cmath>
 #include <fstream>
 #include <vector>
+#include <unordered_map>
 
 #include <rclcpp/rclcpp.hpp>
 #include <opencv2/opencv.hpp>
@@ -56,11 +57,10 @@ class ArucoPose : public rclcpp::Node
   // This is the actual length/width of the printed tag
   double physical_marker_size_;   // height/width in meters
 
-  // Median filter related vars
-  std::shared_ptr<filters::MultiChannelFilterBase<double>> median_filter_ =
-    std::make_shared<filters::MultiChannelMedianFilter<double>>();
-
-  std::vector<double> median_filtered_rpyxyz;
+  // Median filters related vars
+  std::unordered_map<int,
+    std::shared_ptr<filters::MultiChannelFilterBase<double>>> median_filters_map_;
+  std::unordered_map<int, std::vector<double>> median_filtered_rpyxyz_map_;
 
   /// @brief converts a rpy to a quaternion
   geometry_msgs::msg::Quaternion toQuaternion(double roll, double pitch, double yaw);

src/aruco_pose/src/aruco_pose.cpp

@@ -89,16 +89,6 @@ ArucoPose::ArucoPose()
     throw std::runtime_error("Invalid dictionary name");
   }
 
-  // Initial estimates
-  median_filtered_rpyxyz = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
-
-  // Configure the median filter. 6 refers to the number of channels in the multi-channel filter
-  // Note: it is necessary to have/declare a parameter named 'number_of_observations'
-  // in the parameter server.
-  median_filter_->configure(
-    6, "", "number_of_observations",
-    this->get_node_logging_interface(), this->get_node_parameters_interface());
-
   RCLCPP_INFO(LOGGER, "Pose estimator node started!");
 }
 
@@ -116,7 +106,7 @@ void ArucoPose::image_raw_callback(
 
   // Exclude instances where no markers are detected
   try {
-    if (!markerIds_.empty() && markerIds_.size() == 1) {
+    if (!markerIds_.empty()) {
       // rvecs: rotational vector
       // tvecs: translation vector
       std::vector<cv::Vec3d> rvecs, tvecs;
@@ -126,65 +116,86 @@ void ArucoPose::image_raw_callback(
         markerCorners_, physical_marker_size_, cameraMatrix_, distCoeffs_, rvecs, tvecs);
 
       cv::Mat R;
+
       //  Convert the rvecs to a rotation matrix
       for (size_t i = 0; i < rvecs.size(); ++i) {
         cv::Rodrigues(rvecs[i], R);
-      }
 
-      // Access each element of the R matrix for easy rpy calculations
-      double r11 = R.at<double>(0, 0), r21 = R.at<double>(1, 0), r31 = R.at<double>(2, 0),
-        r32 = R.at<double>(2, 1), r33 = R.at<double>(2, 2);
-
-      double roll, pitch, yaw;
-
-      // rpy calculation
-      roll = std::atan2(r32, r33);
-      pitch = std::asin(-r31);
-      yaw = std::atan2(r21, r11);
-
-      auto tranlsation = tvecs[0];
-
-      // Construct the raw rpy_xyz of the marker
-      std::vector<double> raw_rpyxyz =
-      {roll, pitch, yaw, tranlsation[0], tranlsation[1], tranlsation[2]};
-
-      // Median filter gets applied
-      median_filter_->update(raw_rpyxyz, median_filtered_rpyxyz);
-
-      // Add to the tf frame here for the sample
-      geometry_msgs::msg::TransformStamped transformStamped_tag;
-
-      transformStamped_tag.header.stamp = this->now();
-      transformStamped_tag.header.frame_id = this->get_parameter("camera_tf_frame").as_string();
-      transformStamped_tag.child_frame_id = this->get_parameter("sample_name").as_string();
-
-      transformStamped_tag.transform.translation.x = median_filtered_rpyxyz[3] +
-        this->get_parameter("offset_on_marker_x").as_double();
-      transformStamped_tag.transform.translation.y = median_filtered_rpyxyz[4] +
-        this->get_parameter("offset_on_marker_y").as_double();
-      transformStamped_tag.transform.translation.z = median_filtered_rpyxyz[5];
-      transformStamped_tag.transform.rotation = toQuaternion(
-        median_filtered_rpyxyz[0],
-        median_filtered_rpyxyz[1],
-        median_filtered_rpyxyz[2]);
-
-      // Add a pre-pickup tf
-      geometry_msgs::msg::TransformStamped transformStamped_pre_pickup;
-      transformStamped_pre_pickup.header.stamp = this->now();
-      transformStamped_pre_pickup.header.frame_id = this->get_parameter("sample_name").as_string();
-      transformStamped_pre_pickup.child_frame_id =
-        this->get_parameter("pre_pickup_location.name").as_string();
-
-      transformStamped_pre_pickup.transform.translation.x = this->get_parameter(
-        "pre_pickup_location.x_adj").as_double();
-      transformStamped_pre_pickup.transform.translation.y = this->get_parameter(
-        "pre_pickup_location.y_adj").as_double();
-      transformStamped_pre_pickup.transform.translation.z = this->get_parameter(
-        "pre_pickup_location.z_adj").as_double();
-      transformStamped_pre_pickup.transform.rotation = toQuaternion(0, 0, 0);
-
-      static_broadcaster_.sendTransform(transformStamped_pre_pickup);
-      static_broadcaster_.sendTransform(transformStamped_tag);
+        int id = markerIds_[i];
+
+        // If a unseen ID is found, add it to the filter map
+        if (median_filters_map_.find(id) == median_filters_map_.end()) {
+          RCLCPP_INFO(this->LOGGER, "New ID found : %d ", id);
+          median_filters_map_[id] = std::make_shared<filters::MultiChannelMedianFilter<double>>();
+
+          // Configure the median filter. 6 refers to the number of
+          // channels in the multi-channel filter
+          // Note: it is necessary to have/declare a parameter named 'number_of_observations'
+          // in the parameter server.
+          median_filters_map_[id]->configure(
+            6, "", "number_of_observations",
+            this->get_node_logging_interface(), this->get_node_parameters_interface());
+          median_filtered_rpyxyz_map_.insert_or_assign(
+            id, std::vector<double>
+            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0});
+        }
+
+        // Access each element of the R matrix for easy rpy calculations
+        double r11 = R.at<double>(0, 0), r21 = R.at<double>(1, 0), r31 = R.at<double>(2, 0),
+          r32 = R.at<double>(2, 1), r33 = R.at<double>(2, 2);
+
+        double roll, pitch, yaw;
+
+        // rpy calculation
+        roll = std::atan2(r32, r33);
+        pitch = std::asin(-r31);
+        yaw = std::atan2(r21, r11);
+
+        auto tranlsation = tvecs[i];
+
+        // Construct the raw rpy_xyz of the marker
+        std::vector<double> raw_rpyxyz =
+        {roll, pitch, yaw, tranlsation[0], tranlsation[1], tranlsation[2]};
+
+        std::vector<double> median_filtered_rpyxyz = median_filtered_rpyxyz_map_[id];
+        // Median filter gets applied
+        median_filters_map_[id]->update(raw_rpyxyz, median_filtered_rpyxyz);
+
+        // Add to the tf frame here for the sample
+        geometry_msgs::msg::TransformStamped transformStamped_tag;
+
+        transformStamped_tag.header.stamp = this->now();
+        transformStamped_tag.header.frame_id = this->get_parameter("camera_tf_frame").as_string();
+        transformStamped_tag.child_frame_id = std::to_string(id);
+
+        transformStamped_tag.transform.translation.x = median_filtered_rpyxyz[3] +
+          this->get_parameter("offset_on_marker_x").as_double();
+        transformStamped_tag.transform.translation.y = median_filtered_rpyxyz[4] +
+          this->get_parameter("offset_on_marker_y").as_double();
+        transformStamped_tag.transform.translation.z = median_filtered_rpyxyz[5];
+        transformStamped_tag.transform.rotation = toQuaternion(
+          median_filtered_rpyxyz[0],
+          median_filtered_rpyxyz[1],
+          median_filtered_rpyxyz[2]);
+
+        // Add a pre-pickup tf
+        geometry_msgs::msg::TransformStamped transformStamped_pre_pickup;
+        transformStamped_pre_pickup.header.stamp = this->now();
+        transformStamped_pre_pickup.header.frame_id = std::to_string(id);
+        transformStamped_pre_pickup.child_frame_id =
+          std::to_string(id) + "_" + this->get_parameter("pre_pickup_location.name").as_string();
+
+        transformStamped_pre_pickup.transform.translation.x = this->get_parameter(
+          "pre_pickup_location.x_adj").as_double();
+        transformStamped_pre_pickup.transform.translation.y = this->get_parameter(
+          "pre_pickup_location.y_adj").as_double();
+        transformStamped_pre_pickup.transform.translation.z = this->get_parameter(
+          "pre_pickup_location.z_adj").as_double();
+        transformStamped_pre_pickup.transform.rotation = toQuaternion(0, 0, 0);
+
+        static_broadcaster_.sendTransform(transformStamped_pre_pickup);
+        static_broadcaster_.sendTransform(transformStamped_tag);
+      }
     }
 
     // Inner try-catch

src/pdf_beamtime/include/pdf_beamtime/pdf_beamtime_fidpose_server.hpp

@@ -5,6 +5,7 @@ BSD 3 Clause License. See LICENSE.txt for details.*/
 #include <memory>
 #include <string>
 #include <vector>
+#include <unordered_map>
 
 #include <pdf_beamtime/pdf_beamtime_server.hpp>
 #include <pdf_beamtime/tf_utilities.hpp>
@@ -57,4 +58,8 @@ class PdfBeamtimeFidPoseServer : public PdfBeamtimeServer
   bool pickup_pose_saved = false;
   std::vector<double> pre_pickup_approach_joints_;
   std::vector<double> pickup_joints_;
+  int sample_id = 0;
+
+  std::unordered_map<int, std::vector<double>> pickup_storage_map_;
+  std::unordered_map<int, std::vector<double>> pre_pickup_approach_storage_map_;
 };

src/pdf_beamtime/include/pdf_beamtime/tf_utilities.hpp

@@ -29,8 +29,8 @@ class TFUtilities
 
   std::shared_ptr<rclcpp::SyncParametersClient> parameters_client_;
 
-  std::string world_frame, sample_frame, grasping_point_on_gripper_frame, wrist_2_frame,
-    pre_pickup_approach_point_frame;
+  std::string world_frame, grasping_point_on_gripper_frame, wrist_2_frame,
+    pre_pickup_approach_point_frame_suffix_;
 
 public:
   explicit TFUtilities(const rclcpp::Node::SharedPtr node);
@@ -39,14 +39,14 @@ class TFUtilities
   double degreesToRadians(double degrees);
 
   std::pair<double, double> get_wrist_elbow_alignment(
-    moveit::planning_interface::MoveGroupInterface & mgi);
+    moveit::planning_interface::MoveGroupInterface & mgi, int sample_id);
 
   std::vector<geometry_msgs::msg::Pose> get_pickup_action_z_adj(
-    moveit::planning_interface::MoveGroupInterface & mgi);
+    moveit::planning_interface::MoveGroupInterface & mgi, int sample_id);
 
   std::vector<geometry_msgs::msg::Pose> get_pickup_action_pre_pickup(
-    moveit::planning_interface::MoveGroupInterface & mgi);
+    moveit::planning_interface::MoveGroupInterface & mgi, int sample_id);
 
   std::vector<geometry_msgs::msg::Pose> get_pickup_action_pickup(
-    moveit::planning_interface::MoveGroupInterface & mgi);
+    moveit::planning_interface::MoveGroupInterface & mgi, int sample_id);
 };

src/pdf_beamtime/src/pdf_beamtime_fidpose_client.py

@@ -23,28 +23,26 @@ def send_pickup_goal(self):
         """Send a working goal."""
         goal_msg = FidPoseControlMsg.Goal()
 
-        goal_msg.inbeam_approach = [55.10, -51.78, 124.84, -73.16, 52.24, 180.0]
-        goal_msg.inbeam_approach = [x / 180 * math.pi for x in goal_msg.inbeam_approach]
+        goal_msg.inbeam_approach = [x / 180 * math.pi for x in [55.10, -51.78, 124.84, -73.16, 52.24, 180.0]]
 
-        goal_msg.inbeam = [63.84, -43.13, 98.29, -55.25, 61.00, 180.0]
-        goal_msg.inbeam = [x / 180 * math.pi for x in goal_msg.inbeam]
+        goal_msg.inbeam = [x / 180 * math.pi for x in [63.84, -43.13, 98.29, -55.25, 61.00, 180.0]]
 
         goal_msg.sample_return = False
+        goal_msg.sample_id = 150
 
         self._action_client.wait_for_server()
         self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback)
 
-    def send_retrun_sample_goal(self):
+    def send_return_sample_goal(self):
         """Send a working goal."""
         goal_msg = FidPoseControlMsg.Goal()
 
-        goal_msg.inbeam_approach = [55.10, -51.78, 124.84, -73.16, 52.24, 180.0]
-        goal_msg.inbeam_approach = [x / 180 * math.pi for x in goal_msg.inbeam_approach]
+        goal_msg.inbeam_approach = [x / 180 * math.pi for x in [55.10, -51.78, 124.84, -73.16, 52.24, 180.0]]
 
-        goal_msg.inbeam = [63.84, -43.13, 98.29, -55.25, 61.00, 180.0]
-        goal_msg.inbeam = [x / 180 * math.pi for x in goal_msg.inbeam]
+        goal_msg.inbeam = [x / 180 * math.pi for x in [63.84, -43.13, 98.29, -55.25, 61.00, 180.0]]
 
         goal_msg.sample_return = True
+        goal_msg.sample_id = 150
 
         self._action_client.wait_for_server()
         self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback)
@@ -67,8 +65,8 @@ def main(args=None):
     rclpy.init(args=args)
 
     client = SimpleClient()
-    client.send_pickup_goal()
-    # client.send_retrun_sample_goal()
+    # client.send_pickup_goal()
+    client.send_return_sample_goal()
 
     rclpy.spin(client)