new object locator with continuous updates for objects

src/object_locator.cpp

@@ -6,6 +6,8 @@
     \date 15.06.22
 */
 
+#include <cmath>
+
 #include <ros/ros.h>
 #include <ros/package.h>
 #include <apriltag_ros/AprilTagDetectionArray.h>
@@ -17,7 +19,6 @@
 #include "ccf/util/NodeUtil.h"
 
 #include "Scene.pb.h"
-#include "google/protobuf/text_format.h"
 
 #include <google/protobuf/util/json_util.h>
 #include <google/protobuf/message.h>
@@ -31,6 +32,8 @@ const std::string CELL_BUNDLE = "CETI_TABLE_ONE";
 const double TABLE_HEIGHT = 0.89;
 const double GRID_SIZE = 0.05;
 const double MAX_ERROR = 0.02;
+const bool RISKY_MODE = true;
+const bool SNAP_TO_GRID = true;
 
 double distanceToGrid(geometry_msgs::Point point, double grid_size)
 {
@@ -57,26 +60,23 @@ int main(int argc, char** argv)
   ros::NodeHandle n("ccf");
 
   ros::Rate loop_rate(200);
-  ros::Rate pause_rate(ros::Duration(2));  // seconds
 
-  pause_rate.sleep();
+  ros::Duration(2).sleep(); // sleep for two seconds
 
   DummyRobotArmController controller(n, "object_locator", "nobot");
 
   auto mqttServer = getPrivateParameter<std::string>("mqtt_server", "localhost:1883");
-  std::unique_ptr<MqttConnection> mqtt_connection =
+  std::shared_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_1"
-                                       "/scene/update");
-  auto other_cell_delta_update_topic =
+  auto observed_scene_listener_topic =
+      getPrivateParameter<std::string>("other_cell_listener", "/ceti_cell_1/scene/update");
+  auto observed_scene_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));
+  controller.addConnection(mqtt_connection);
+  mqtt_connection->listen(observed_scene_listener_topic);
 
   // subscribe to a scene
-  controller.addCallback(other_cell_scene_listener_topic, [&controller](auto msg) {
+  controller.addCallback(observed_scene_listener_topic, [&controller](auto msg) {
     ROS_INFO_STREAM("Updating scene from other cell.");
     Scene scene;
     scene.ParseFromString(msg);
@@ -90,7 +90,7 @@ int main(int argc, char** argv)
                                                                                                           "objects."
                                                                                                           "json"));
 
-  ROS_ERROR_STREAM("object library contains " << objectLibrary.objects_size() << " objects");
+  ROS_DEBUG_STREAM("object library contains " << objectLibrary.objects_size() << " objects");
 
   std::map<std::string, geometry_msgs::Pose> located_poses;
 
@@ -161,11 +161,13 @@ int main(int argc, char** argv)
           tf2::Quaternion q{ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w };
           std::cout.precision(3);
 
-          if (controller.resolveObject(name))
+          auto existingObject = controller.resolveObject(name);  // we need this also later to check if pose has changed
+          if (!RISKY_MODE && existingObject)
           {
             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 << " "
@@ -179,21 +181,48 @@ int main(int argc, char** argv)
             continue;
           }
           double e = distanceToGrid(pose.position, GRID_SIZE);
-          ROS_ERROR_STREAM("object " << name << " has an error of " << e);
+          ROS_DEBUG_STREAM("object " << name << " has an error of " << e);
 
           // 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])
+          if (SNAP_TO_GRID)
+          {
+            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 (RISKY_MODE && existingObject)
+            {
+              double dx = existingObject->pos().x() - relevant_poses[name].position.x;
+              double dy = existingObject->pos().y() - relevant_poses[name].position.y;
+              double distance = std::sqrt(dx * dx + dy * dy);
+              if (distance < GRID_SIZE / 2)
+              {
+                ROS_DEBUG_STREAM("Skipping object  " << name << " because its location has not changed.");
+                continue;
+              }
+              else
+              {
+                ROS_DEBUG_STREAM("Sending object  " << name << " because its location has changed by " << distance);
+              }
+            }
+          }
+
+          if (RISKY_MODE || 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);
+            if (RISKY_MODE)
+            {
+              ROS_DEBUG_STREAM("found the new pose for " + name + ": " << relevant_poses[name].position.x << ", "
+                                                                       << relevant_poses[name].position.y);
+            }
+            else
+            {
+              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)
             {
@@ -205,29 +234,22 @@ int main(int argc, char** argv)
             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_mode(existingObject ? Object_Mode_MODE_MODIFY : 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());
+          ROS_DEBUG_STREAM("Sending scene with " << updateScene.objects_size() << " newly recognized or updated objects.");
+          controller.sendToAll(observed_scene_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();
   }