handle collab zones

src/dummy_pickplace_provider.cpp

-/*! \file dummy_selection_provider.cpp
+/*! \file dummy_pickplace_provider.cpp
 
-    A ROS node that can trigger pick-place and pick-drop commands on cells
+    A ROS node that can trigger robot commands in a cell
 
     \author Johannes Mey
     \date 26.04.2022
@@ -37,13 +37,10 @@ int main(int argc, char** argv)
   client_connection->initializeConnection(
       [](auto t, auto m) { ROS_WARN_STREAM("ignoring message " << m << " on topic " << t); });
 
-  ros::Subscriber sub_select = n.subscribe<std_msgs::String>(prefix + "/pp", 1000, [&](auto& m) {
+  ros::Subscriber sub_select = n.subscribe<std_msgs::String>(prefix + "/pickplace", 1000, [&](auto& m) {
     std::string command = m->data;
 
-    ROS_INFO_STREAM("Selecting pick/place command: " << command);
-
     Command c;
-
     size_t pos = command.find(',');
     c.mutable_pickandplace()->set_idpick(command.substr(0, pos));
     command.erase(0, pos + 1);
@@ -56,18 +53,10 @@ int main(int argc, char** argv)
     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>\"");
-
   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);
@@ -77,12 +66,27 @@ int main(int argc, char** argv)
     client_connection->send("/" + target_cell + "/command", c.SerializeAsString());
   });
 
-  ROS_INFO_STREAM("DUMMY PICK-PLACE-PROVIDER" << std::endl
-                                              << "To trigger a commend, use" << std::endl
+  ros::Subscriber sub_change_zone = n.subscribe<std_msgs::String>(prefix + "/configzone", 1000, [&](auto& m) {
+    std::string command = m->data;
+
+    Command c;
+    size_t pos = command.find(',');
+    c.mutable_configchange()->set_idcollaborationzone(command.substr(0, pos));
+    command.erase(0, pos + 1);
+    c.mutable_configchange()->set_idrobotnewowner(command);
+
+    ROS_INFO_STREAM("sending command to assign robot " << c.configchange().idrobotnewowner() << " to zone " << c.configchange().idcollaborationzone());
+    client_connection->send("/" + target_cell + "/command", c.SerializeAsString());
+  });
+
+  ROS_INFO_STREAM("ROBOT COMMAND PROVIDER" << std::endl
+                                              << "To trigger a command, use" << std::endl
+                                              << "  rostopic pub -1 /ccf/" << prefix
+                                              << "/pickplace std_msgs/String \"<object>,<target>,<robot>\"" << std::endl
                                               << "  rostopic pub -1 /ccf/" << prefix
-                                              << "/pp std_msgs/String \"<object>,<target>,<robot>\"" << std::endl
+                                              << "/evacuate std_msgs/String \"<collaboration_zone>,<robot>\"" << std::endl
                                               << "  rostopic pub -1 /ccf/" << prefix
-                                              << "/evacuate std_msgs/String \"<collaboration_zone>,<robot>\"");
+                                              << "/configzone std_msgs/String \"<collaboration_zone>,<robot>\"");
 
   ros::spin();
 

src/moveit_sorting_controller.cpp

@@ -139,6 +139,21 @@ int main(int argc, char **argv)
       pose.position.z = 1.3;
       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());
     }