config/config_scene_ceti-table-1-empty.json

@@ -223,26 +223,6 @@
         "b": 0.15
       }
     },
-    {
-      "id": "front_panel",
-      "pos": {
-        "z": 0.885,
-        "x": 0.6525
-      },
-      "size": {
-        "length": 0.5,
-        "width": 0.7,
-        "height": 0.01
-      },
-      "orientation": {
-        "w": 1
-      },
-      "color": {
-        "r": 0.15,
-        "g": 0.15,
-        "b": 0.15
-      }
-    },
     {
       "id": "binBlue",
       "type": "BIN",

config/config_scene_ceti-table-1-empty.yaml

@@ -22,7 +22,7 @@
   { 'id': 'table-top', 'pos': { 'z': 0.885 },'size': { 'length': 0.8,'width': 0.8,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
   { 'id': 'left_panel',   'pos': { 'z': 0.885, 'y': -0.6525 },'size': { 'length': 0.7,'width': 0.5,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
   { 'id': 'right_panel',  'pos': { 'z': 0.885, 'y': 0.6525 },'size': { 'length': 0.7,'width': 0.5,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
-  { 'id': 'front_panel', 'pos': { 'z': 0.885, 'x': 0.6525 },'size': { 'length': 0.5,'width': 0.7,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+#  { 'id': 'front_panel', 'pos': { 'z': 0.885, 'x': 0.6525 },'size': { 'length': 0.5,'width': 0.7,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
 
   # bins width/depth/height 28.5*19*15.5
   # their height is 89 + 7.75 = 0.9675 m

config/tags.yaml

@@ -52,8 +52,10 @@ tag_bundles:
       name: 'CETI_TABLE_ONE',
       layout:
             [
-          { id: 0, size: .10, x: -0.318, y: -0.272, z: 0.890, qw: 0.707, qx: 0, qy: 0, qz: 0.707 }, # label is on top, readable from front
-#          { id: 1, size: .10, x: 0.398, y: -0.272, z: 0.740, qw: 0.5, qx: 0.5, qy: 0.5, qz: 0.5 }, # label is on front side
+                  { id: 0, size: .08, x: -0.3, y: -0.3, z: 0.890, qw: 0.707, qx: 0, qy: 0, qz: 0.707 },
+                  { id: 1, size: .08, x: -0.3, y:  0.3, z: 0.890, qw: 0.707, qx: 0, qy: 0, qz: 0.707 },
+                  { id: 2, size: .08, x:  0.3, y: -0.3, z: 0.890, qw: 0.707, qx: 0, qy: 0, qz: 0.707 },
+                  { id: 3, size: .08, x:  0.3, y:  0.3, z: 0.890, qw: 0.707, qx: 0, qy: 0, qz: 0.707 }
             ]
     },
     { name: 'smallRed25',    layout: [ { id: 25, size: .024, x: 0.016, y: 0, z: 0.038, qw: 0.5, qx: 0.5, qy: 0.5, qz: 0.5 } ] },

launch/commands.txt

@@ -35,3 +35,6 @@ apriltag_ros/AprilTagDetection[] detections
 rosrun rviz rviz
 - Fixed Frame CETI_TABLE_ONE
 - Add -> TF
+
+# send a scene manually
+rostopic pub -1 /connector_node_ros_ccf/send_scene std_msgs/Empty

launch/noop-sim-cell_ceti-table-1-emptyworld.launch

@@ -5,7 +5,7 @@
 
     <include file="$(find ccf)/launch/noop-sim_setup.launch"/>
 
-    <param name="/connector_node_ros_ccf/topics/command" value="/ceti_cell_placeworld/command"/>
+    <param name="/connector_node_ros_ccf/topics/command" value="/ceti_cell_empty/command"/>
 
     <node pkg="ccf_immersive_sorting" type="moveit_sorting_controller" name="ceti_cell_empty" output="screen">
         <param name="arm" type="string" value="arm"/>

launch/noop-sim-cell_ceti-table-1-placeworld.launch

@@ -9,7 +9,7 @@
 
     <node pkg="ccf_immersive_sorting" type="moveit_sorting_controller" name="ceti_cell_placeworld" output="screen">
         <param name="arm" type="string" value="arm1"/>
-        <param name="other_cell" type="string" value="/ceti_cell_2_placeworld/scene/update"/>
+        <param name="other_cell" type="string" value="/ceti_cell_2_placeworld/scene/delta-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-placeworld.json"/>
     </node>

launch/noop-sim-cell_ceti-table-2-placeworld.launch

@@ -9,7 +9,7 @@
     
     <node pkg="ccf_immersive_sorting" type="moveit_sorting_controller" name="ceti_cell_2_placeworld" output="screen">
         <param name="arm" type="string" value="arm2"/>
-        <param name="other_cell" type="string" value="/ceti_cell_placeworld/scene/update"/>
+        <param name="other_cell" type="string" value="/ceti_cell_placeworld/scene/delta-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-placeworld.json"/>
     </node>

launch/robot-cell_ceti-table-1-emptyworld.launch

@@ -11,6 +11,8 @@
         <arg name="load_gripper" value="true"/>
     </include>
 
+    <param name="/connector_node_ros_ccf/topics/command" value="/ceti_cell_empty/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"/>

package.xml

@@ -31,6 +31,7 @@
   <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
   <depend>roscpp</depend>
   <depend>ccf</depend>
+  <depend>apriltag_ros</depend>
   <!--   Note that this is equivalent to the following: -->
   <!--   <build_depend>roscpp</build_depend> -->
   <!--   <exec_depend>roscpp</exec_depend> -->

src/moveit_sorting_controller.cpp

@@ -18,7 +18,7 @@
 #include "ccf/connection/MqttConnection.h"
 #include "ccf/util/NodeUtil.h"
 
-std::string NODE_NAME = "ceti_cell_placeworld";
+std::string NODE_NAME = "ceti_cell_empty";
 
 using CetiRosToolbox::getParameter;
 using CetiRosToolbox::getPrivateParameter;
@@ -41,25 +41,37 @@ int main(int argc, char** argv)
   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 commandTopic = getParameter<std::string>(n, "topics/command", "/ceti_cell_placeworld/command");
+  auto commandTopic = getParameter<std::string>(n, "topics/command", NODE_NAME + "/command");
   mqtt_connection->listen(commandTopic);
-  auto other_cell_topic = getPrivateParameter<std::string>("other_cell", "/ceti_cell_2_placeworld/scene/update");
+  auto other_cell_topic = getPrivateParameter<std::string>("other_cell", "/ceti_cell_2_placeworld/scene/delta-update");
   mqtt_connection->listen(other_cell_topic);
   auto scene_observer_topic = getPrivateParameter<std::string>("scene_observer", "object_locator/scene/update");
   mqtt_connection->listen(scene_observer_topic);
   controller.addConnection(std::move(mqtt_connection));
 
   controller.addCallback(other_cell_topic, [&controller](auto msg) {
-    ROS_INFO_STREAM("Updating scene from other cell.");
     Scene scene;
     scene.ParseFromString(msg);
+    ROS_INFO_STREAM("Updating scene from other cell. Is delta: " << scene.is_delta());
     controller.initScene(scene, false);
   });
 
   ros::WallDuration(5).sleep();  // wait 5 secs to init scene (to give moveit time to load)
 
-  controller.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") +
-      "/config/config_scene_ceti-table-1-empty.json"));
+  controller.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") + "/con"
+                                                                                                                  "fig/"
+                                                                                                                  "conf"
+                                                                                                                  "ig_"
+                                                                                                                  "scen"
+                                                                                                                  "e_"
+                                                                                                                  "ceti"
+                                                                                                                  "-tab"
+                                                                                                                  "le-"
+                                                                                                                  "1-"
+                                                                                                                  "empt"
+                                                                                                                  "y."
+                                                                                                                  "jso"
+                                                                                                                  "n"));
 
   // only add the scene observer after the base scene has been loaded
   controller.addCallback(scene_observer_topic, [&controller](auto msg) {
@@ -76,20 +88,22 @@ int main(int argc, char** argv)
       });
 
   auto pick_place_callback = [&controller](const Command& command) {
-    const PickAndPlace& pick_place = command.pickandplace();
     if (command.has_pickandplace())
     {
-      ROS_INFO_STREAM("[COMMAND] Pick object " << pick_place.idpick() << " and place it at " << pick_place.idplace());
-      Object* pick_object = controller.resolveObject(pick_place.idpick());
+      ROS_INFO_STREAM("[COMMAND] for robot " << command.pickandplace().idrobot() << ": Pick object "
+                                             << command.pickandplace().idpick() << " and place it at "
+                                             << command.pickandplace().idplace());
+      Object* pick_object = controller.resolveObject(command.pickandplace().idpick());
       if (!pick_object)
       {
-        ROS_ERROR_STREAM("[COMMAND] FAILED! Selected unknown pick object '" << pick_place.idpick() << "'");
+        ROS_ERROR_STREAM("[COMMAND] FAILED! Selected unknown pick object '" << command.pickandplace().idpick() << "'");
         return;
       }
-      Object* place_object = controller.resolveObject(pick_place.idplace());
+      Object* place_object = controller.resolveObject(command.pickandplace().idplace());
       if (!place_object)
       {
-        ROS_ERROR_STREAM("[COMMAND] FAILED! Selected unknown place object '" << pick_place.idplace() << "'");
+        ROS_ERROR_STREAM("[COMMAND] FAILED! Selected unknown place object '" << command.pickandplace().idplace()
+                                                                             << "'");
         return;
       }
       if (pick_object->type() != Object::BOX)
@@ -119,11 +133,11 @@ int main(int argc, char** argv)
       {
         if (!controller.pickAndDrop(*robot, *pick_object, *place_object, false))
         {
-          ROS_WARN_STREAM("[COMMAND] FAILED! Unable to remove box '" << pick_place.idpick() << "'!");
+          ROS_WARN_STREAM("[COMMAND] FAILED! Unable to remove box '" << command.pickandplace().idpick() << "'!");
         }
         else
         {
-          ROS_INFO_STREAM("[COMMAND] SUCCESS! '" << pick_place.idpick() << "' is no more.");
+          ROS_INFO_STREAM("[COMMAND] SUCCESS! '" << command.pickandplace().idpick() << "' is no more.");
           controller.sendScene();
         }
       }
@@ -131,18 +145,19 @@ int main(int argc, char** argv)
       {
         if (!controller.pickAndPlace(*robot, *pick_object, *place_object, false))
         {
-          ROS_WARN_STREAM("[COMMAND] FAILED! Unable to move box '" << pick_place.idpick() << "'!");
+          ROS_WARN_STREAM("[COMMAND] FAILED! Unable to move box '" << command.pickandplace().idpick() << "'!");
         }
         else
         {
-          ROS_INFO_STREAM("[COMMAND] SUCCESS! " << pick_place.idpick() << "' is at its new location.");
+          ROS_INFO_STREAM("[COMMAND] SUCCESS! " << command.pickandplace().idpick() << "' is at its new location.");
           controller.sendScene();
         }
       }
     }
     else if (command.has_evacuate())
     {
-      ROS_INFO_STREAM("[COMMAND] Evacuate " << command.evacuate().idrobot() << " from zone "
+      ROS_INFO_STREAM("[COMMAND] for robot " << command.evacuate().idrobot() << ": Evacuate "
+                                             << command.evacuate().idrobot() << " from zone "
                                              << command.evacuate().idcollaborationzone());
       if (command.evacuate().idrobot() != controller.getRobotName())
       {
@@ -195,10 +210,10 @@ int main(int argc, char** argv)
     }
     else if (command.has_pick())
     {
-
       Object* robot = controller.resolveObject(command.pick().idrobot());
       Object* object = controller.resolveObject(command.pick().idpick());
-      ROS_INFO_STREAM("[COMMAND] Pick " << object->id() << " using robot " << robot->id());
+      ROS_INFO_STREAM("[COMMAND] for robot " << command.pick().idrobot() << ": Pick " << object->id() << " using robot "
+                                             << robot->id());
       if (!robot)
       {
         ROS_ERROR_STREAM("[COMMAND] FAILED! Unable to pick using unknown robot '" << command.pick().idrobot() << "'.");
@@ -215,25 +230,28 @@ int main(int argc, char** argv)
     {
       Object* robot = controller.resolveObject(command.place().idrobot());
       Object* object = controller.resolveObject(command.place().idplace());
-      ROS_INFO_STREAM("[COMMAND] Place object at " << object->id() << " using robot " << robot->id());
+      ROS_INFO_STREAM("[COMMAND] for robot " << command.place().idrobot() << ": Place object at " << object->id()
+                                             << " using robot " << robot->id());
       if (!robot)
       {
-        ROS_ERROR_STREAM("[COMMAND] FAILED! Unable to place using unknown robot '" << command.place().idrobot() << "'.");
+        ROS_ERROR_STREAM("[COMMAND] FAILED! Unable to place using unknown robot '" << command.place().idrobot()
+                                                                                   << "'.");
         return;
       }
       if (!object)
       {
-        ROS_ERROR_STREAM("[COMMAND] FAILED! Unable to place at unknown location '" << command.place().idplace() << "'.");
+        ROS_ERROR_STREAM("[COMMAND] FAILED! Unable to place at unknown location '" << command.place().idplace()
+                                                                                   << "'.");
         return;
       }
       controller.place(command.place().idrobot(), command.place().idplace(), false);
     }
     else if (command.has_drop())
     {
-
       Object* robot = controller.resolveObject(command.drop().idrobot());
       Object* bin = controller.resolveObject(command.drop().idbin());
-      ROS_INFO_STREAM("[COMMAND] drop object into bin " << bin->id() << " using robot " << robot->id());
+      ROS_INFO_STREAM("[COMMAND] for robot " << command.drop().idrobot() << ":  drop object into bin " << bin->id()
+                                             << " using robot " << robot->id());
       if (!robot)
       {
         ROS_ERROR_STREAM("[COMMAND] FAILED! Unable to drop using unknown robot '" << command.drop().idrobot() << "'.");

src/object_locator.cpp

@@ -31,6 +31,14 @@ using CetiRosToolbox::getPrivateParameter;
 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;
+
+double distanceToGrid(geometry_msgs::Point point, double grid_size)
+{
+  double x_error = (std::round(point.x / grid_size)) * grid_size - point.x;
+  double y_error = (std::round(point.y / grid_size)) * grid_size - point.y;
+  return std::sqrt(x_error*x_error + y_error*y_error);
+}
 
 geometry_msgs::Point closestGridPoint(geometry_msgs::Point point, double grid_size)
 {
@@ -165,6 +173,8 @@ int main(int argc, char** argv)
                                                  << pose.position.z - TABLE_HEIGHT);
             continue;
           }
+          double e = distanceToGrid(pose.position, GRID_SIZE);
+          ROS_ERROR_STREAM("object " << name << " has an error of " << e);
 
           // sanitize pose
           relevant_poses[name] = pose;