align with CCF improvements

src/dummy_pickplace_provider.cpp

@@ -41,18 +41,19 @@ int main(int argc, char** argv)
     std::string command = m->data;
 
     ROS_INFO_STREAM("Selecting pick/place command: " << command);
-    PickPlace p;
+
+    Command c;
 
     size_t pos = command.find(',');
-    p.set_idpick(command.substr(0, pos));
+    c.mutable_pickandplace()->set_idpick(command.substr(0, pos));
     command.erase(0, pos + 1);
     pos = command.find(',');
-    p.set_idplace(command.substr(0, pos));
+    c.mutable_pickandplace()->set_idplace(command.substr(0, pos));
     command.erase(0, pos + 1);
-    p.set_idrobot(command);
+    c.mutable_pickandplace()->set_idrobot(command);
 
-    ROS_INFO_STREAM("sending command for robot " << p.idrobot() << " to place " << p.idpick() << " at " << p.idplace());
-    client_connection->send("/" + target_cell + "/command", p.SerializeAsString());
+    ROS_INFO_STREAM("sending command for robot " << c.pickandplace().idrobot() << " to place " << c.pickandplace().idpick() << " at " << c.pickandplace().idplace());
+    client_connection->send("/" + target_cell + "/command", c.SerializeAsString());
   });
 
   ROS_INFO_STREAM("DUMMY PICK-PLACE-PROVIDER" << std::endl
@@ -60,6 +61,29 @@ int main(int argc, char** argv)
                                               << "  rostopic pub -1 /ccf/" << prefix
                                               << "/pp std_msgs/String \"<object>,<target>,<robot>\"");
 
+  ros::Subscriber sub_evacuate = n.subscribe<std_msgs::String>(prefix + "/evacuate", 1000, [&](auto& m) {
+    std::string command = m->data;
+
+    ROS_INFO_STREAM("Selecting evacuate command: " << command);
+
+    Command c;
+
+    size_t pos = command.find(',');
+    c.mutable_evacuate()->set_idcollaborationzone(command.substr(0, pos));
+    command.erase(0, pos + 1);
+    c.mutable_evacuate()->set_idrobot(command);
+
+    ROS_INFO_STREAM("sending command for robot " << c.evacuate().idrobot() << " to evacuate " << c.evacuate().idcollaborationzone());
+    client_connection->send("/" + target_cell + "/command", c.SerializeAsString());
+  });
+
+  ROS_INFO_STREAM("DUMMY PICK-PLACE-PROVIDER" << std::endl
+                                              << "To trigger a commend, use" << std::endl
+                                              << "  rostopic pub -1 /ccf/" << prefix
+                                              << "/pp std_msgs/String \"<object>,<target>,<robot>\"" << std::endl
+                                              << "  rostopic pub -1 /ccf/" << prefix
+                                              << "/evacuate std_msgs/String \"<collaboration_zone>,<robot>\"");
+
   ros::spin();
 
   return 0;

src/dummy_sorting_controller.cpp

@@ -33,7 +33,8 @@ int main(int argc, char **argv)
 
   ros::NodeHandle n("connector_node_ros_ccf"); // namespace where the connection_address is
 
-  DummyRobotArmController controller{n, NODE_NAME};
+  auto armName = getPrivateParameter<std::string>("arm", "arm1");
+  DummyRobotArmController controller{n, NODE_NAME, armName};
 
   auto mqttServer = getPrivateParameter<std::string>("mqtt_server", "localhost:1883");
   std::unique_ptr<MqttConnection>
@@ -59,8 +60,10 @@ int main(int argc, char **argv)
   { controller.sendScene(); }); // send a scene every ten seconds
 
 
-  auto pick_place_callback = [&controller](const PickPlace& pick_place)
+  auto pick_place_callback = [&controller](const Command& command)
   {
+    PickAndPlace pick_place = command.pickandplace();
+    if (command.has_pickandplace()) {
       ROS_INFO_STREAM("Got Command to pick object " << pick_place.idpick() << " and place it into " << pick_place.idplace());
       Object *pick_object = controller.resolveObject(pick_place.idpick());
       if (!pick_object)
@@ -81,20 +84,32 @@ int main(int argc, char **argv)
         ROS_WARN_STREAM(
             "Selected unsupported pick object of type " << pick_object->type());
       }
-    if (place_object->type() != Object::BIN)
+      if (place_object->type() != Object::BIN && place_object->type() != Object::DROP_OFF_LOCATION)
       {
         ROS_WARN_STREAM(
             "Selected unsupported place object of type " << place_object->type());
       }
       Object *robot = nullptr; // TODO
+      if (place_object->type() == Object::BIN) {
         if (!controller.pickAndDrop(*robot, *pick_object, *place_object, false)) {
           ROS_WARN_STREAM("Unable to remove box '" << pick_place.idpick() << "'!");
         } else {
           ROS_INFO_STREAM("Job is done! '" << pick_place.idpick() << "' is no more.");
           controller.sendScene();
         }
+      } else if (place_object->type() == Object::DROP_OFF_LOCATION) {
+        if (!controller.pickAndPlace(*robot, *pick_object, *place_object, false)) {
+          ROS_WARN_STREAM("Unable to move box '" << pick_place.idpick() << "'!");
+        } else {
+          ROS_INFO_STREAM("Job is done! '" << pick_place.idpick() << "' is at its new location.");
+          controller.sendScene();
+        }
+      }
+    } else {
+      ROS_WARN_STREAM("Ignoring command that is not PickAndPlace but " << command.msg_case());
+    }
   };
-  controller.reactToPickAndPlaceMessage(pick_place_callback);
+  controller.reactToCommandMessage(pick_place_callback);
 
   auto sceneUpdateCallback = [&currentlyPickedBox, &controller]()
   {

src/main_controller.cpp

@@ -43,7 +43,7 @@ int main(int argc, char **argv)
 
   auto connection_address = getPrivateParameter<std::string>("connection_address", "tcp://*:6576");
 
-  DummyRobotArmController controller{n, NODE_NAME};
+  DummyRobotArmController controller{n, NODE_NAME, "arm"};
 
   std::map<std::string, CellController> clients;
   std::vector<std::pair<std::string, std::string>> actions;
@@ -71,9 +71,9 @@ int main(int argc, char **argv)
     clients[client].setActionCallback([client, &controller](const Action &action)
                                       {
                                         ROS_INFO_STREAM("Action Callback Called for " << action);
-                                        PickPlace command;
-                                        command.set_idpick(action.getSource());
-                                        command.set_idplace(action.getTarget());
+                                        Command command;
+                                        command.mutable_pickandplace()->set_idpick(action.getSource());
+                                        command.mutable_pickandplace()->set_idplace(action.getTarget());
                                         controller.sendToAll("/" + client + "/command", command.SerializeAsString());
                                       });
     auto scene_update_topic = "/" + client + getParameter<std::string>(n, "topics/scene", "/scene/update");
@@ -89,7 +89,7 @@ int main(int argc, char **argv)
   auto fallback_path = ros::package::getPath("ccf_immersive_sorting") + "/config/config_scene_virtual-table.json";
   controller.loadScene(getPrivateParameter<std::string>("scene", fallback_path));
 
-  Object *robot = nullptr;
+  Object *robot = nullptr; // FIXME
   Object *selected_box = nullptr;
   Object *selected_bin = nullptr;
   std::optional<std::string> picked_box{};

src/moveit_sorting_controller.cpp

@@ -33,13 +33,13 @@ int main(int argc, char **argv)
 
   ros::NodeHandle n("connector_node_ros_ccf"); // namespace where the connection_address is
 
-  MoveItRobotArmController controller{n, NODE_NAME};
+  MoveItRobotArmController controller{n, NODE_NAME, "arm1"};
 
   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"));
-  std::string commandTopic = getParameter(n, "topics/command", ros::this_node::getName() + "/command");
+  auto commandTopic = getParameter<std::string>(n, "topics/command", "/ceti_cell_placeworld/command");
   mqtt_connection->listen(commandTopic);
   controller.setCommandTopic(commandTopic);
   controller.addConnection(std::move(mqtt_connection));
@@ -47,7 +47,7 @@ int main(int argc, char **argv)
   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.json"));
+      "/config/config_scene_ceti-table-placeworld.json"));
 
   std::optional<std::string> currentlyPickedBox{};
 
@@ -59,8 +59,10 @@ int main(int argc, char **argv)
   { controller.sendScene(); }); // send a scene every ten seconds
 
 
-  auto pick_place_callback = [&controller](const PickPlace& pick_place)
+  auto pick_place_callback = [&controller](const Command& command)
   {
+    const PickAndPlace& pick_place = command.pickandplace();
+    if (command.has_pickandplace()) {
       ROS_INFO_STREAM("Got Command to pick object " << pick_place.idpick() << " and place it into " << pick_place.idplace());
       Object *pick_object = controller.resolveObject(pick_place.idpick());
       if (!pick_object)
@@ -81,20 +83,67 @@ int main(int argc, char **argv)
         ROS_WARN_STREAM(
             "Selected unsupported pick object of type " << pick_object->type());
       }
-    if (place_object->type() != Object::BIN)
+      if (place_object->type() != Object::BIN && place_object->type() != Object::DROP_OFF_LOCATION)
       {
         ROS_WARN_STREAM(
             "Selected unsupported place object of type " << place_object->type());
       }
-    Object *robot = nullptr; // TODO
+      if (command.pickandplace().idrobot() != controller.getRobotName()) {
+        ROS_WARN_STREAM(
+            "Ignoring command pick-place command for robot " << command.pickandplace().idrobot() << " (this is " << controller.getRobotName() << ")");
+        return;
+      }
+
+      Object *robot = controller.resolveObject(command.pickandplace().idrobot());
+      if (!robot)
+      {
+        ROS_ERROR_STREAM(
+            "Inconsistent scene, robot '" << command.evacuate().idrobot() << "' not found!");
+        return;
+      }
+      if (place_object->type() == Object::BIN) {
         if (!controller.pickAndDrop(*robot, *pick_object, *place_object, false)) {
           ROS_WARN_STREAM("Unable to remove box '" << pick_place.idpick() << "'!");
         } else {
           ROS_INFO_STREAM("Job is done! '" << pick_place.idpick() << "' is no more.");
           controller.sendScene();
         }
+      } else if (place_object->type() == Object::DROP_OFF_LOCATION) {
+        if (!controller.pickAndPlace(*robot, *pick_object, *place_object, false)) {
+          ROS_WARN_STREAM("Unable to move box '" << pick_place.idpick() << "'!");
+        } else {
+          ROS_INFO_STREAM("Job is done! '" << pick_place.idpick() << "' is at its new location.");
+          controller.sendScene();
+        }
+      }
+    } else if (command.has_evacuate()) {
+
+
+      if (command.evacuate().idrobot() != controller.getRobotName()) {
+        ROS_WARN_STREAM(
+            "Ignoring command pick-place command for robot " << command.evacuate().idrobot() << " (this is " << controller.getRobotName() << ")");
+        return;
+      }
+
+      Object *robot = controller.resolveObject(command.evacuate().idrobot());
+      if (!robot)
+      {
+        ROS_ERROR_STREAM(
+            "Inconsistent scene, robot '" << command.evacuate().idrobot() << "' not found!");
+        return;
+      }
+
+      geometry_msgs::Pose pose;
+      pose.orientation.x = 1;
+      pose.position.x = .2;
+      pose.position.z = 1.3;
+      controller.moveToPose(*robot, pose, false);
+
+    } else {
+      ROS_WARN_STREAM("Ignoring command that is not PickAndPlace but " << command.msg_case());
+    }
   };
-  controller.reactToPickAndPlaceMessage(pick_place_callback);
+  controller.reactToCommandMessage(pick_place_callback);
 
   auto sceneUpdateCallback = [&currentlyPickedBox, &controller]()
   {