add object for second arm and debug output

config/config_scene_ceti-table-placeworld.json

@@ -1463,6 +1463,26 @@
         "b": 1
       }
     },
+    {
+      "id": "bigGreen",
+      "type": "BOX",
+      "pos": {
+        "x": -0.3,
+        "y": 2.01,
+        "z": 0.9355
+      },
+      "size": {
+        "length": 0.0318,
+        "width": 0.0636,
+        "height": 0.091
+      },
+      "orientation": {
+        "w": 1
+      },
+      "color": {
+        "g": 1
+      }
+    },
     {
       "id": "arm1",
       "type": "ARM",

config/config_scene_ceti-table-placeworld.yaml

@@ -128,6 +128,7 @@
   { 'id': 'cz1', 'type': 'COLLABORATION_ZONE', 'pos': { 'z': 1.49, 'y': 0.6525 },'size': { 'length': 0.7,'width': 0.3,'height': 1.2 },'orientation': { 'w': 1 },'color': { 'r': 1,'g': 0.627,'b': 0 } },
 
   { 'id': 'bigBlue',  'type': 'BOX', 'pos': { 'x': 0.10,'y': -0.25, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } },
+  { 'id': 'bigGreen', 'type': 'BOX', 'pos': { 'x': -.3, 'y':  2.01, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
 
   { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.22,            'z': 0.89 },'size': { },'orientation': { 'w': 1 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } },
   { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.22, 'y': 1.31, 'z': 0.89 },'size': { },'orientation': { 'w': 1 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }

src/moveit_sorting_controller.cpp

@@ -148,6 +148,7 @@ int main(int argc, char **argv)
       pose.orientation.x = 1;
       pose.position.x = robot->pos().x() + .4;
       pose.position.z = robot->pos().z() + .4;
+      ROS_INFO_STREAM("Moving arm to pose " << pose);
       controller.moveToPose(*robot, pose, false);
 
     } else if (command.has_configchange()) {