change owner handling for collab zones

src/dummy_sorting_controller.cpp

@@ -25,7 +25,6 @@ using CetiRosToolbox::getPrivateParameter;
 
 int main(int argc, char** argv)
 {
-
   GOOGLE_PROTOBUF_VERIFY_VERSION;
 
   ros::init(argc, argv, NODE_NAME);
@@ -39,8 +38,8 @@ int main(int argc, char **argv)
   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());
+  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");
   mqtt_connection->listen(commandTopic);
@@ -57,89 +56,107 @@ 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-placeworld.json"));
+  controller.loadScene(getPrivateParameter<std::string>("scene", ros::package::getPath("ccf_immersive_sorting") + "/con"
+                                                                                                                  "fig/"
+                                                                                                                  "conf"
+                                                                                                                  "ig_"
+                                                                                                                  "scen"
+                                                                                                                  "e_"
+                                                                                                                  "ceti"
+                                                                                                                  "-tab"
+                                                                                                                  "le-"
+                                                                                                                  "plac"
+                                                                                                                  "ewor"
+                                                                                                                  "ld."
+                                                                                                                  "jso"
+                                                                                                                  "n"));
 
   std::optional<std::string> currentlyPickedBox{};
 
-  ros::Subscriber sub = n.subscribe<std_msgs::Empty>("send_scene", 1000, [&controller](
-      const std_msgs::EmptyConstPtr &msg)
-  {
+  ros::Subscriber sub =
+      n.subscribe<std_msgs::Empty>("send_scene", 1000, [&controller](const std_msgs::EmptyConstPtr& msg) {
         ROS_INFO_STREAM("Sending scene manually (triggered by a message to the 'send_scene' topic)");
         controller.sendScene();
       });
 
-  auto pick_place_callback = [&controller](const Command& command)
-  {
+  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());
+    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)
       {
-        ROS_ERROR_STREAM(
-            "Selected unknown pick object '" << pick_place.idpick() << "'");
+        ROS_ERROR_STREAM("Selected unknown pick object '" << pick_place.idpick() << "'");
         return;
       }
       Object* place_object = controller.resolveObject(pick_place.idplace());
       if (!place_object)
       {
-        ROS_ERROR_STREAM(
-            "Selected unknown place object '" << pick_place.idplace() << "'");
+        ROS_ERROR_STREAM("Selected unknown place object '" << pick_place.idplace() << "'");
         return;
       }
       if (pick_object->type() != Object::BOX)
       {
-        ROS_WARN_STREAM(
-            "Selected unsupported pick object of type " << pick_object->type());
+        ROS_WARN_STREAM("Selected unsupported pick object of type " << pick_object->type());
       }
-      if (place_object->type() != Object::BIN && place_object->type() != Object::DROP_OFF_LOCATION && place_object->type() != Object::COLLABORATION_ZONE)
+      if (place_object->type() != Object::BIN && place_object->type() != Object::DROP_OFF_LOCATION &&
+          place_object->type() != Object::COLLABORATION_ZONE)
       {
-        ROS_WARN_STREAM(
-            "Selected unsupported place object of type " << place_object->type());
+        ROS_WARN_STREAM("Selected unsupported place object of type " << place_object->type());
       }
-      if (command.pickandplace().idrobot() != controller.getRobotName()) {
-        ROS_WARN_STREAM(
-            "Ignoring command pick-place command for robot " << command.pickandplace().idrobot() << " (this is " << controller.getRobotName() << ")");
+      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!");
+        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)) {
+      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 {
+        }
+        else
+        {
           ROS_INFO_STREAM("Job is done! '" << pick_place.idpick() << "' is no more.");
           controller.sendScene();
         }
-      } else if (place_object->type() == Object::DROP_OFF_LOCATION || place_object->type() == Object::COLLABORATION_ZONE) {
-        if (!controller.pickAndPlace(*robot, *pick_object, *place_object, false)) {
+      }
+      else if (place_object->type() == Object::DROP_OFF_LOCATION || place_object->type() == Object::COLLABORATION_ZONE)
+      {
+        if (!controller.pickAndPlace(*robot, *pick_object, *place_object, false))
+        {
           ROS_WARN_STREAM("Unable to move box '" << pick_place.idpick() << "'!");
-        } else {
+        }
+        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() << ")");
+    }
+    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!");
+        ROS_ERROR_STREAM("Inconsistent scene, robot '" << command.evacuate().idrobot() << "' not found!");
         return;
       }
 
@@ -149,8 +166,9 @@ int main(int argc, char **argv)
       pose.position.y = robot->pos().y();
       pose.position.z = robot->pos().z() + .4;
       controller.moveToPose(*robot, pose, false);
-
-    } else if (command.has_configchange()) {
+    }
+    else if (command.has_configchange())
+    {
       Object* zone = controller.resolveObject(command.configchange().idcollaborationzone());
       if (!zone)
       {
@@ -161,18 +179,19 @@ int main(int argc, char **argv)
       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.");
+        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 {
+      controller.configureCollaborationZone(*zone, command.configchange().idrobotnewowner());
+    }
+    else
+    {
       ROS_WARN_STREAM("Ignoring command that is not PickAndPlace but " << command.msg_case());
     }
   };
   controller.reactToCommandMessage(pick_place_callback);
 
-  auto sceneUpdateCallback = [&currentlyPickedBox, &controller]()
-  {
+  auto sceneUpdateCallback = [&currentlyPickedBox, &controller]() {
     if (currentlyPickedBox.has_value())
     {
       auto resolved = controller.resolveObject(currentlyPickedBox.value());

src/moveit_sorting_controller.cpp

@@ -164,7 +164,7 @@ int main(int argc, char **argv)
         ROS_WARN_STREAM(
             "Collaboration zone active for unknown robot '" << command.evacuate().idrobot() << "', so blocked in all.");
       }
-      controller.configureCollaborationZone(*zone, command.configchange().idrobotnewowner() != controller.getRobotName());
+      controller.configureCollaborationZone(*zone, command.configchange().idrobotnewowner());
     } else {
       ROS_WARN_STREAM("Ignoring command that is not PickAndPlace but " << command.msg_case());
     }