diff --git a/CMakeLists.txt b/CMakeLists.txt
index 43877941d5464c35f5ef1c0fa15dad60b2f81ddb..cd1041f6ef30396fb5e856b9468091c13532932a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -139,6 +139,7 @@ add_executable(${PROJECT_NAME}_dummy_pickplace_provider src/dummy_pickplace_prov
add_executable(${PROJECT_NAME}_dummy_sorting_controller src/dummy_sorting_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS})
add_executable(${PROJECT_NAME}_main_controller src/main_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS} src/cell.cpp src/action.cpp src/task.cpp src/cell_controller.cpp include/cell_controller.h)
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})
## Rename C++ executable without prefix
@@ -150,6 +151,7 @@ set_target_properties(${PROJECT_NAME}_dummy_pickplace_provider PROPERTIES OUTPUT
set_target_properties(${PROJECT_NAME}_dummy_sorting_controller PROPERTIES OUTPUT_NAME dummy_sorting_controller PREFIX "")
set_target_properties(${PROJECT_NAME}_main_controller PROPERTIES OUTPUT_NAME main_controller 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 "")
## Add cmake target dependencies of the executable
## same as for the library above
@@ -158,6 +160,7 @@ add_dependencies(${PROJECT_NAME}_dummy_pickplace_provider ${${PROJECT_NAME}_EXPO
add_dependencies(${PROJECT_NAME}_dummy_sorting_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(${PROJECT_NAME}_main_controller ${${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})
## Specify libraries to link a library or executable target against
target_link_libraries(${PROJECT_NAME}_dummy_sorting_controller
@@ -176,7 +179,9 @@ target_link_libraries(${PROJECT_NAME}_dummy_selection_provider
target_link_libraries(${PROJECT_NAME}_dummy_pickplace_provider
${catkin_LIBRARIES}
)
-
+target_link_libraries(${PROJECT_NAME}_object_locator
+ ${catkin_LIBRARIES}
+ )
#############
## Install ##
diff --git a/launch/noop-sim-cell_ceti-table-1-emptyworld.launch b/launch/noop-sim-cell_ceti-table-1-emptyworld.launch
new file mode 100644
index 0000000000000000000000000000000000000000..e2f3c349a50116cada51a2f43856672c7a7ee223
--- /dev/null
+++ b/launch/noop-sim-cell_ceti-table-1-emptyworld.launch
@@ -0,0 +1,17 @@
+<launch>
+
+ <!-- MQTT server for communication with client cells -->
+ <arg name="mqtt_server" default="tcp://localhost:1883" doc="address of the mqtt server for the connection to the controller"/>
+
+ <include file="$(find ccf)/launch/noop-sim_setup.launch"/>
+
+ <param name="/connector_node_ros_ccf/topics/command" value="/ceti_cell_placeworld/command"/>
+
+ <node pkg="ccf_immersive_sorting" type="moveit_sorting_controller" name="ceti_cell_empty" output="screen">
+ <param name="arm" type="string" value="arm"/>
+ <param name="scene_observer" type="string" value="object_locator/scene/update"/>
+ <param name="mqtt_server" type="yaml" value="$(arg mqtt_server)"/>
+ <param name="scene" type="string" value="$(find ccf_immersive_sorting)/config/config_scene_ceti-table-1-empty.json"/>
+ </node>
+
+</launch>
diff --git a/launch/object_locator.launch b/launch/object_locator.launch
new file mode 100644
index 0000000000000000000000000000000000000000..6ef056b5303160e623aff6c62bf535393ed7f6bb
--- /dev/null
+++ b/launch/object_locator.launch
@@ -0,0 +1,28 @@
+<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" />
+
+ <!-- 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">
+ <param name="video_device" value="/dev/video2" />
+ <param name="image_width" value="2560" />
+ <param name="image_height" value="1440" />
+ <param name="autofocus" value="0" />
+ </node>
+
+
+ <node name="image_proc" pkg="image_proc" type="image_proc" ns="usb_cam"/>
+
+ <node pkg="apriltag_ros" type="apriltag_ros_continuous_node" name="$(arg node_namespace)" clear_params="true" output="screen" launch-prefix="$(arg launch_prefix)" >
+ <!-- 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>
+</launch>
diff --git a/src/object_locator.cpp b/src/object_locator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ce893025183cb5f438563f48b6536825cad6fc2e
--- /dev/null
+++ b/src/object_locator.cpp
@@ -0,0 +1,218 @@
+/*! \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;
+const double GRID_SIZE = 0.05;
+
+geometry_msgs::Point closestGridPoint(geometry_msgs::Point point, double grid_size)
+{
+ geometry_msgs::Point grid_point;
+ grid_point.x = (std::round(point.x / grid_size)) * grid_size;
+ grid_point.y = (std::round(point.y / grid_size)) * grid_size;
+ grid_point.z = point.z;
+ return grid_point;
+}
+
+int main(int argc, char** argv)
+{
+ GOOGLE_PROTOBUF_VERIFY_VERSION;
+
+ ros::init(argc, argv, "object_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());
+ mqtt_connection->listen(getParameter<std::string>(n, "topics/selection", "selection"));
+ auto other_cell_scene_listener_topic = getPrivateParameter<std::string>("other_cell_listener", "/ceti_cell_empty/scene/update");
+ auto other_cell_delta_update_topic = getPrivateParameter<std::string>("other_cell_delta_updates", "object_locator/scene/update");
+ mqtt_connection->listen(other_cell_scene_listener_topic);
+ controller.addConnection(std::move(mqtt_connection));
+
+ // subscribe to a scene
+ controller.addCallback(other_cell_scene_listener_topic, [&controller](auto msg) {
+ ROS_INFO_STREAM("Updating scene from other cell.");
+ Scene scene;
+ scene.ParseFromString(msg);
+ controller.initScene(scene, false);
+ });
+
+ controller.initScene({}, false);
+
+ Scene objectLibrary = RobotArmController::loadSceneFromFile(getPrivateParameter<std::string>("object_library", ros::package::getPath("ccf_immersive_sorting") +
+ "/config/objects.json"));
+
+ ROS_ERROR_STREAM("object library contains " << objectLibrary.objects_size() << " objects");
+
+ 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;
+ }
+
+ // make pose relative to origin instead of camera
+ for (auto& [name, pose] : allObjectPoses)
+ {
+ 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);
+ 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();
+ }
+
+ // create a delta scene to send later (if not empty)
+ Scene updateScene;
+ updateScene.set_is_delta(true);
+
+ // filter object poses
+ std::map<std::string, geometry_msgs::Pose> relevant_poses;
+ for (const auto& [name, pose] : allObjectPoses)
+ {
+ tf2::Quaternion q{ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w };
+ std::cout.precision(3);
+
+ if (controller.resolveObject(name)) {
+ ROS_DEBUG_STREAM("skipping pose for " << name << " the object is already in the scene.");
+ continue;
+ }
+ if (pose.orientation.w < 0.99)
+ {
+ ROS_WARN_STREAM("skipping pose for " << name << " because the object is rotated " << pose.orientation << " "
+ << q.getAxis() << " " << q.angleShortestPath({ 0, 0, 0, 1 }));
+ continue;
+ }
+ if (std::abs(pose.position.z - TABLE_HEIGHT) > 0.02)
+ {
+ ROS_WARN_STREAM("skipping pose for " << name << " because the object is not on table level "
+ << pose.position.z - TABLE_HEIGHT);
+ continue;
+ }
+
+ // sanitize pose
+ relevant_poses[name] = pose;
+ relevant_poses[name].position.z = TABLE_HEIGHT;
+ relevant_poses[name].position = closestGridPoint(relevant_poses[name].position, GRID_SIZE);
+ relevant_poses[name].orientation.x = 0;
+ relevant_poses[name].orientation.y = 0;
+ relevant_poses[name].orientation.z = 0;
+ relevant_poses[name].orientation.w = 1;
+
+ if (located_poses[name] == relevant_poses[name]) {
+ ROS_DEBUG_STREAM("found the same pose for " + name + " two times in a row: " << relevant_poses[name].position.x << ", "
+ << relevant_poses[name].position.y);
+ auto object = DummyRobotArmController::resolveObject(name, objectLibrary);
+ if (!object) {
+ ROS_WARN_STREAM("skipping pose for " << name << " because the object is not in the object library.");
+ continue;
+ }
+ auto newObject = updateScene.add_objects();
+ *newObject = *object;
+ newObject->mutable_pos()->set_x(located_poses[name].position.x);
+ newObject->mutable_pos()->set_y(located_poses[name].position.y);
+ newObject->mutable_pos()->set_z(located_poses[name].position.z + newObject->size().height()/2);
+ newObject->set_mode(Object_Mode_MODE_ADD);
+ newObject->set_state(Object_State_STATE_STATIONARY);
+ }
+ }
+
+ if (updateScene.objects_size() > 0) {
+ ROS_ERROR_STREAM("Sending scene with " << updateScene.objects_size() << " newly recognized objects.");
+ controller.sendToAll(other_cell_delta_update_topic, updateScene.SerializeAsString());
+ }
+ located_poses = relevant_poses;
+
+ });
+
+ // receive a tag position
+ // if no "base tag" is contained, abort
+ // if the contained objects are already in the robot scene, abort (we only detect new objects)
+ // if the object tag is not in the base scene, abort
+ // find the drop-of location in the scene that is closest to the
+
+ while (ros::ok())
+ {
+ ros::spinOnce();
+
+ loop_rate.sleep();
+ }
+
+ return 0;
+}
\ No newline at end of file