new functions

- display latest svg image
- updated demo commands
- updated conversion topics

cgv_connector.proto

-// cgv_connector.proto
-// this file contains the messages that are exchanged between the cgv framework and the st ROS interface
+// connector.proto
+// this file contains the messages that are exchanged between the ROS connector and other systems
 
 syntax = "proto3";
 
@@ -10,22 +10,22 @@ message Object {
 
     // Position is object-center related
     message Position {
-        float x = 1; // in m
-        float y = 2; // in m
-        float z = 3; // height in m
+        double x = 1; // in m
+        double y = 2; // in m
+        double z = 3; // height in m
     }
 
     // 3D description of the object
     message Size {
-        float length = 1; // in m
-        float width = 2;  // in m
-        float height = 3; // in m
+        double length = 1; // in m
+        double width = 2;  // in m
+        double height = 3; // in m
     }
     message Orientation {
-        float x = 1; // normalized quaternion
-        float y = 2;
-        float z = 3;
-        float w = 4;
+        double x = 1; // normalized quaternion
+        double y = 2;
+        double z = 3;
+        double w = 4;
     }
     message Color {
         float r = 1; // 0..1
@@ -38,6 +38,9 @@ message Object {
         BIN = 2;
         ARM = 3;
         DROP_OFF_LOCATION = 4;
+        HUMAN = 5;
+        ROBOT = 6;
+        COLLABORATION_ZONE = 7;
     }
 
     string id = 1;
@@ -46,26 +49,46 @@ message Object {
     Size size = 4;
     Orientation orientation = 5;
     Color color = 6;
+    bool active = 7;
 }
 
-// the scene is stored within the ROS side and sent to the CGV framework
+// the scene is stored within the ROS side and sent to clients
 message Scene {
     repeated Object objects = 1;
 }
 
 // the selection is done by the CGV framework and sent to ROS
+// FIXME can be removed
 message Selection {
     string id = 1; // the id corresponds to an id of an Object in a Scene
 }
 
-// vvv from rs vvv.
-// Merged message to contain both pick and place in one
-message MergedSelection {
-    string idRobot = 1; // the id corresponds to and id of the robot Object that should execute this operation
-    string idPick = 2; // the id corresponds to an id of the Object in a Scene to be picked
-    string idPlace = 3; // the id corresponds to an id of the Object in a Scene where the picked object shall be placed
+message Command {
+    oneof msg {
+        PickAndPlace pickAndPlace = 1;
+        ConfigChange configChange = 2;
+        Evacuate evacuate = 3;
+    }
+}
+
+message PickAndPlace {
+    string idRobot = 1;  // id of the robot that should execute this operation
+    string idPick = 2;  // id of the object in the scene to be picked
+    string idPlace = 3;  // id of the location the picked object shall be placed.
+}
+
+message ConfigChange {
+    string idCollaborationZone = 1;  // id of collaboration zone to change
+    string idRobotNewOwner = 2;  // id of robot that will become new owner
 }
+
+message Evacuate {
+    string idRobot = 1;  // id of robot that need to move out of its currently defined collision objects
+    string idCollaborationZone = 2;  // id of collaboration zone to evacuate
+}
+
 // Reachability of objects, as reported by MoveIt
+// FIXME can be removed
 message Reachability {
     message ObjectReachability {
         string idObject = 1; // the id of the object to reach
@@ -73,4 +96,4 @@ message Reachability {
     }
     string idRobot = 1; // the id of the robot arm
     repeated ObjectReachability objects = 2; // all objects reachable
-}
+}
\ No newline at end of file

utils.py

@@ -49,14 +49,24 @@ def topic_match(topics_to_filter, msg, last_match=True):
 def format_scene(scene: cgv_connector_pb2.Scene):
     result = ""
     for obj in scene.objects:
-        if obj.type == cgv_connector_pb2.Object.Type.BOX:
+        if obj.type != cgv_connector_pb2.Object.Type.DROP_OFF_LOCATION and obj.type != cgv_connector_pb2.Object.Type.UNKNOWN:
             pos = obj.pos
-            result += f"\n<obj {obj.id:15} at ({pos.x:6.2} {pos.y:6.2} {pos.z:6.2})>"
+            result += f"\n<{cgv_connector_pb2.Object.Type.Name(obj.type):20} {obj.id:15} at ({pos.x:6.2} {pos.y:6.2} {pos.z:6.2}) {obj.active}>"
+            # result += str(obj)
+    # result = scene
     return result
 
 
-def format_command(command: cgv_connector_pb2.MergedSelection):
-    return f"<cmd by {command.idRobot} of {command.idPick} to {command.idPlace}>"
+def format_command(command: cgv_connector_pb2.Command):
+    if command.HasField("pickAndPlace"):
+        pickAndPlace = command.pickAndPlace
+        return f"<PickAndPlace by {pickAndPlace.idRobot} of {pickAndPlace.idPick} to {pickAndPlace.idPlace}>"
+    if command.HasField("configChange"):
+        configChange = command.configChange
+        return f"<ConfigChange by {configChange.idRobotNewOwner} for {configChange.idCollaborationZone}>"
+    if command.HasField("evacuate"):
+        return f"<Evacuate by {command.evacuate.idRobot}>"
+    return "<unknown command>"
 
 
 def _get_reach_objects(r):