diff --git a/CMakeLists.txt b/CMakeLists.txt index f5c9aeeb7aea7c0d029f9531af41a85f6600d4a9..8e90bd2aef5cd259ab2f4f152cd3f6a217571147 100644 --- a/CMakeLists.txt +++ b/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 ## diff --git a/config/make_tag/tags-80mm-100-103.pdf b/config/make_tag/tags-80mm-100-103.pdf new file mode 100644 index 0000000000000000000000000000000000000000..a3d7a8ffb879e346ab401099056107ad2b718eaf Binary files /dev/null and b/config/make_tag/tags-80mm-100-103.pdf differ diff --git a/config/tags.yaml b/config/tags.yaml index ea70499893692327f36a9295ed8aed9839a9f38b..4b0172dd9401a78e76638a38ce82d06c7e540cb8 100644 --- a/config/tags.yaml +++ b/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 } ] }, diff --git a/launch/object-localization/table_detection.launch b/launch/object-localization/table_detection.launch new file mode 100644 index 0000000000000000000000000000000000000000..01f43051b4c7c56bb3999af1ba9495615051ce68 --- /dev/null +++ b/launch/object-localization/table_detection.launch @@ -0,0 +1,35 @@ +<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> diff --git a/src/table_locator.cpp b/src/table_locator.cpp new file mode 100644 index 0000000000000000000000000000000000000000..ed4867b260f9b8d10d8be11598937feda3b6ebe4 --- /dev/null +++ b/src/table_locator.cpp @@ -0,0 +1,134 @@ +/*! \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; +}