initial version of table position detection

CMakeLists.txt

@@ -141,6 +141,7 @@ add_executable(${PROJECT_NAME}_main_controller src/main_controller.cpp ${PROTO_S
 add_executable(${PROJECT_NAME}_virtual_scene_provider src/virtual_scene_provider.cpp ${PROTO_SRCS} ${PROTO_HDRS})
 add_executable(${PROJECT_NAME}_moveit_sorting_controller src/moveit_sorting_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS})
 add_executable(${PROJECT_NAME}_object_locator src/object_locator.cpp ${PROTO_SRCS} ${PROTO_HDRS})
+add_executable(${PROJECT_NAME}_table_locator src/table_locator.cpp ${PROTO_SRCS} ${PROTO_HDRS})
 
 
 ## Rename C++ executable without prefix
@@ -154,6 +155,7 @@ set_target_properties(${PROJECT_NAME}_main_controller PROPERTIES OUTPUT_NAME mai
 set_target_properties(${PROJECT_NAME}_virtual_scene_provider PROPERTIES OUTPUT_NAME virtual_scene_provider PREFIX "")
 set_target_properties(${PROJECT_NAME}_moveit_sorting_controller PROPERTIES OUTPUT_NAME moveit_sorting_controller PREFIX "")
 set_target_properties(${PROJECT_NAME}_object_locator PROPERTIES OUTPUT_NAME object_locator PREFIX "")
+set_target_properties(${PROJECT_NAME}_table_locator PROPERTIES OUTPUT_NAME table_locator PREFIX "")
 
 ## Add cmake target dependencies of the executable
 ## same as for the library above
@@ -164,6 +166,7 @@ add_dependencies(${PROJECT_NAME}_main_controller ${${PROJECT_NAME}_EXPORTED_TARG
 add_dependencies(${PROJECT_NAME}_virtual_scene_provider ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 add_dependencies(${PROJECT_NAME}_moveit_sorting_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 add_dependencies(${PROJECT_NAME}_object_locator ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(${PROJECT_NAME}_table_locator ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Specify libraries to link a library or executable target against
 target_link_libraries(${PROJECT_NAME}_dummy_sorting_controller
@@ -188,6 +191,9 @@ target_link_libraries(${PROJECT_NAME}_dummy_pickplace_provider
 target_link_libraries(${PROJECT_NAME}_object_locator
         ${catkin_LIBRARIES}
         )
+target_link_libraries(${PROJECT_NAME}_table_locator
+        ${catkin_LIBRARIES}
+        )
 
 #############
 ## Install ##

config/tags.yaml

@@ -58,6 +58,16 @@ tag_bundles:
           { id: 3, size: .08, x:  0.3, y:  0.3, z: 0.890, qw: 0.707, qx: 0, qy: 0, qz: 0.707 }
         ]
     },
+    {
+      name: 'CETI_TABLE_TWO',
+      layout:
+        [
+          { id: 100, size: .08, x: -0.3, y: -0.3, z: 0.890, qw: 0.707, qx: 0, qy: 0, qz: 0.707 },
+          { id: 101, size: .08, x: -0.3, y:  0.3, z: 0.890, qw: 0.707, qx: 0, qy: 0, qz: 0.707 },
+          { id: 102, size: .08, x:  0.3, y: -0.3, z: 0.890, qw: 0.707, qx: 0, qy: 0, qz: 0.707 },
+          { id: 103, size: .08, x:  0.3, y:  0.3, z: 0.890, qw: 0.707, qx: 0, qy: 0, qz: 0.707 }
+        ]
+    },
     { name: 'smallRed65',     layout: [ { id: 65, size: .024, x: 0.016, y: 0, z: 0.0350, qw: 0.5, qx: 0.5, qy: 0.5, qz: 0.5 } ] },
     { name: 'smallRed70',     layout: [ { id: 70, size: .024, x: 0.016, y: 0, z: 0.0350, qw: 0.5, qx: 0.5, qy: 0.5, qz: 0.5 } ] },
     { name: 'bigRed7',        layout: [ { id:  7, size: .052, x: 0.016, y: 0, z: 0.0525, qw: 0.5, qx: 0.5, qy: 0.5, qz: 0.5 } ] },

launch/object-localization/table_detection.launch

+<launch>
+    <arg name="launch_prefix" default=""/> <!-- set to value="gdbserver localhost:10000" for remote debugging -->
+    <arg name="node_namespace" default="apriltag_ros_continuous_node"/>
+    <arg name="camera_name" default="/usb_cam"/>
+    <arg name="image_topic" default="image_rect"/>
+    <arg name="device" default="/dev/video0"/>
+
+    <!-- Set parameters -->
+    <rosparam command="load" file="$(find ccf_immersive_sorting)/config/settings.yaml" ns="$(arg node_namespace)"/>
+    <rosparam command="load" file="$(find ccf_immersive_sorting)/config/tags.yaml" ns="$(arg node_namespace)"/>
+
+    <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="log" respawn="true" respawn_delay="3">
+        <param name="video_device" value="$(arg device)"/>
+        <param name="image_width" value="2560"/>
+        <param name="image_height" value="1440"/>
+        <param name="autofocus" value="1"/>
+        <param name="pixel_format" value="mjpeg"/>
+        <param name="framerate" value="1"/>
+        <param name="autofocus" value="true"/>
+    </node>
+
+
+    <node name="image_proc" pkg="image_proc" type="image_proc" ns="usb_cam" respawn="true"/>
+
+    <node pkg="apriltag_ros" type="apriltag_ros_continuous_node" name="$(arg node_namespace)" clear_params="true"
+          output="screen" launch-prefix="$(arg launch_prefix)" respawn="true">
+        <!-- Remap topics from those used in code to those on the ROS network -->
+        <remap from="image_rect" to="$(arg camera_name)/$(arg image_topic)"/>
+        <remap from="camera_info" to="$(arg camera_name)/camera_info"/>
+
+        <param name="publish_tag_detections_image" type="bool" value="true"/>      <!-- default: false -->
+    </node>
+
+    <node pkg="ccf_immersive_sorting" type="table_locator" name="table_locator" respawn="true"/>
+</launch>

src/table_locator.cpp

+/*! \file object_locator.cpp
+
+    A ROS node that detects objects
+
+    \author Johannes Mey
+    \date 15.06.22
+*/
+
+#include <ros/ros.h>
+#include <ros/package.h>
+#include <apriltag_ros/AprilTagDetectionArray.h>
+
+#include "std_msgs/String.h"
+
+#include "tf2/LinearMath/Transform.h"
+
+#include "ccf/util/NodeUtil.h"
+
+#include "connector.pb.h"
+#include "google/protobuf/text_format.h"
+
+#include <google/protobuf/util/json_util.h>
+#include <google/protobuf/message.h>
+#include <ccf/controller/DummyRobotArmController.h>
+#include <ccf/connection/MqttConnection.h>
+
+using CetiRosToolbox::getParameter;
+using CetiRosToolbox::getPrivateParameter;
+
+const std::string CELL_BUNDLE = "CETI_TABLE_ONE";
+const double TABLE_HEIGHT = 0.89;
+
+int main(int argc, char** argv)
+{
+  GOOGLE_PROTOBUF_VERIFY_VERSION;
+
+  ros::init(argc, argv, "table_locator");
+
+  ros::NodeHandle n("ccf");
+
+  ros::Rate loop_rate(200);
+  ros::Rate pause_rate(ros::Duration(2));  // seconds
+
+  pause_rate.sleep();
+
+  DummyRobotArmController controller(n, "object_locator", "nobot");
+
+  auto mqttServer = getPrivateParameter<std::string>("mqtt_server", "localhost:1883");
+  std::unique_ptr<MqttConnection> mqtt_connection =
+      std::make_unique<MqttConnection>(mqttServer, ros::this_node::getName());
+
+  controller.initScene({}, false);
+
+  std::map<std::string, geometry_msgs::Pose> located_poses;
+
+  // read the bundles and extract the base pose
+  std::map<int, std::string> tagBundleTags;
+  auto tagBundles = CetiRosToolbox::getParameter<XmlRpc::XmlRpcValue>(n, "/apriltag_ros_continuous_node/tag_bundles");
+  for (int bundle_index = 0; bundle_index < tagBundles.size(); bundle_index++)
+  {
+    auto& bundle = tagBundles[bundle_index];
+    for (int tag_index = 0; tag_index < bundle["layout"].size(); tag_index++)
+    {
+      auto& tag = bundle["layout"][tag_index];
+      tagBundleTags[tag["id"]] = std::string(bundle["name"]);
+    }
+  }
+
+  ros::Subscriber sub_tages = n.subscribe<apriltag_ros::AprilTagDetectionArray>(
+      getPrivateParameter<std::string>("tag_topic", "/tag_detections"), 1000, [&](auto& m) {
+        tf2::Transform base_pose;  // pose of the origin relative to the camera
+        std::map<std::string, geometry_msgs::Pose> allObjectPoses;
+
+        // check if the base frame was detected
+        bool base_tag_found = false;
+        for (const auto& detection : m->detections)
+        {
+          if (tagBundleTags[detection.id[0]] == CELL_BUNDLE)
+          {  // a base tag was found if an id of the cell bundle was found
+            base_tag_found = true;
+            base_pose.setOrigin({ detection.pose.pose.pose.position.x, detection.pose.pose.pose.position.y,
+                                  detection.pose.pose.pose.position.z });
+            base_pose.setRotation({ detection.pose.pose.pose.orientation.x, detection.pose.pose.pose.orientation.y,
+                                    detection.pose.pose.pose.orientation.z, detection.pose.pose.pose.orientation.w });
+          }
+          else
+          {
+            allObjectPoses[tagBundleTags[detection.id[0]]] = detection.pose.pose.pose;
+          }
+        }
+        if (!base_tag_found)
+        {
+          ROS_WARN_STREAM("Skipping tag detection because no base tag was found");
+          return;
+        }
+
+        ROS_INFO_STREAM("base tag and " << allObjectPoses.size() << " object poses found!");
+
+        // make pose relative to origin instead of camera
+        for (auto& [name, pose] : allObjectPoses)
+        {
+          ROS_INFO_STREAM("make pose " << name << "relative");
+          tf2::Transform object({ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w },
+                                { pose.position.x, pose.position.y, pose.position.z });
+          tf2::Transform result = base_pose.inverseTimes(object);
+
+          double roll, pitch, yaw;
+          tf2::Matrix3x3 resultMatrix(result.getRotation());
+          resultMatrix.getRPY(roll, pitch, yaw);
+
+          pose.position.x = result.getOrigin().x();
+          pose.position.y = result.getOrigin().y();
+          pose.position.z = result.getOrigin().z();
+          pose.orientation.x = result.getRotation().x();
+          pose.orientation.y = result.getRotation().y();
+          pose.orientation.z = result.getRotation().z();
+          pose.orientation.w = result.getRotation().w();
+
+          tf2::Quaternion q{ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w };
+          std::cout.precision(3);
+          ROS_WARN_STREAM("object " << name << " has pose " << pose << "and RPY " << roll << ", " << pitch << ", "
+                                    << yaw);
+        }
+      });
+
+  while (ros::ok())
+  {
+    ros::spinOnce();
+
+    loop_rate.sleep();
+  }
+
+  return 0;
+}