update dummy controller

src/dummy_sorting_controller.cpp

@@ -33,36 +33,49 @@ int main(int argc, char **argv)
 
   ros::NodeHandle n("connector_node_ros_ccf"); // namespace where the connection_address is
 
-  auto armName = getPrivateParameter<std::string>("arm", "arm1");
-  DummyRobotArmController controller{n, NODE_NAME, armName};
+  auto robotName = getPrivateParameter<std::string>("arm", "arm1");
+  ROS_INFO_STREAM("This cell controls arm " << robotName);
+
+  DummyRobotArmController controller{n, NODE_NAME, robotName};
 
   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(n, "topics/command", ros::this_node::getName() + "/command");
   mqtt_connection->listen(commandTopic);
-  controller.setCommandTopic(commandTopic);
+  auto other_cell_topic = getPrivateParameter<std::string>("other_cell", "/ceti_cell_2_placeworld/scene/update");
+  mqtt_connection->listen(other_cell_topic);
   controller.addConnection(std::move(mqtt_connection));
 
+  controller.addCallback(other_cell_topic, [&controller](auto msg) {
+    ROS_WARN_STREAM("Updating scene from other cell");
+    Scene scene;
+    scene.ParseFromString(msg);
+    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.json"));
+      "/config/config_scene_ceti-table-placeworld.json"));
 
   std::optional<std::string> currentlyPickedBox{};
 
   ros::Subscriber sub = n.subscribe<std_msgs::Empty>("send_scene", 1000, [&controller](
       const std_msgs::EmptyConstPtr &msg)
-  { controller.sendScene(); });
-  ros::Timer timer = n.createTimer(ros::Duration(10), [&controller](
-      const ros::TimerEvent &event)
-  { controller.sendScene(); }); // send a scene every ten seconds
+  {
+      ROS_INFO_STREAM("Sending scene manually (triggered by a message to the 'send_scene' topic)");
+      controller.sendScene();
+  });
+  //ros::Timer timer = n.createTimer(ros::Duration(10), [&controller](
+  //    const ros::TimerEvent &event)
+  //{ controller.sendScene(); }); // send a scene every ten seconds
 
 
   auto pick_place_callback = [&controller](const Command& command)
   {
-    PickAndPlace pick_place = command.pickandplace();
+    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());
@@ -89,7 +102,19 @@ int main(int argc, char **argv)
         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() << "'!");
@@ -105,6 +130,45 @@ int main(int argc, char **argv)
           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 = robot->pos().x() + .4;
+      pose.position.y = robot->pos().y();
+      pose.position.z = robot->pos().z() + .4;
+      controller.moveToPose(*robot, pose, false);
+
+    } else if (command.has_configchange()) {
+      Object *zone = controller.resolveObject(command.configchange().idcollaborationzone());
+      if (!zone)
+      {
+        ROS_ERROR_STREAM("Unable to configure unknown collaboration zone '" << command.evacuate().idrobot() << "'.");
+        return;
+      }
+
+      Object *robot = controller.resolveObject(command.configchange().idrobotnewowner());
+      if (!robot)
+      {
+        ROS_WARN_STREAM(
+            "Collaboration zone active for unknown robot '" << command.evacuate().idrobot() << "', so blocked in all.");
+      }
+      controller.configureCollaborationZone(*zone, command.configchange().idrobotnewowner() != controller.getRobotName());
     } else {
       ROS_WARN_STREAM("Ignoring command that is not PickAndPlace but " << command.msg_case());
     }