1st version of auto attach, fixed id related bug

include/ccf/util/scene_constructor_util.h

@@ -12,12 +12,6 @@
 #include "connector.pb.h"
 #include "scene_collision_object.h"
 
-static const std::string TABLE_PILLAR_1 = "tablePillar1";
-static const std::string TABLE_PILLAR_2 = "tablePillar2";
-static const std::string TABLE_PILLAR_3 = "tablePillar3";
-static const std::string TABLE_PILLAR_4 = "tablePillar4";
-static const std::string TABLE_PLATE = "table";
-
 /**
 * Util for the construction of the scene.
 */

src/util/scene_controller_node.cpp

@@ -372,9 +372,35 @@ bool pick(const std::string &objectId,
     std::map<std::string, moveit_msgs::CollisionObject> known_objects = planning_scene_interface.getObjects(
             known_object_ids);
 
-    for (auto const &i : known_objects) {
+    std::map<std::string, moveit_msgs::CollisionObject> o_map = planning_scene_interface.getObjects();
+
+    for (auto const &x : o_map) {
+
+        if (x.first == objectId) {
+            ROS_INFO("[SCENECONTROLLER] Found object to pick in planning scene");
+            object_to_pick = x.second;
+        }
+    }
+
 
-        moveit_msgs::CollisionObject co = i.second;
+    std::string attached_surface_id = "";
+    std::string support_surface_id = "";
+
+  /*  for (auto const &i : world_state::collision_objects) {
+
+        if (i.getType() == SceneCollisionObject::ENVIRONMENT_TYPE) {
+
+            moveit_msgs::CollisionObject co = i.getCollisionObject();
+            group.attachObject(i.getCollisionObject().id, "panda_link0", touch_links);
+        }
+    }
+*/
+
+    for (auto const &i : world_state::collision_objects) {
+
+        if(i.getType() == SceneCollisionObject::ENVIRONMENT_TYPE) {
+
+            moveit_msgs::CollisionObject co = i.getCollisionObject();
 
             // we can do this because the tables are planes in x/y
             float max_x = co.primitive_poses[0].position.x + (co.primitives[0].dimensions[0] / 2);
@@ -386,21 +412,45 @@ bool pick(const std::string &objectId,
 
             // after transformation the robot is on x=0 and y=0
             if (0 > min_x && 0 < max_x && 0 > min_y && 0 < max_y) {
-            group.attachObject(i.first, "panda_link0", touch_links);
+                std::cout << "attaching surface: " << i.getCollisionObject().id << std::endl;
+                attached_surface_id = i.getCollisionObject().id;
+              //  if(!group.attachObject(i.getCollisionObject().id, "panda_link0", touch_links)){
+             //       group.detachObject(i.getCollisionObject().id);
+                    group.attachObject(i.getCollisionObject().id, "panda_link0", touch_links);
+             //   }
+                break;
+            }
         }
     }
 
-    if (world_state::current_picked_object_id.empty()) {
+    for (auto const &i : world_state::collision_objects) {
 
-        std::map<std::string, moveit_msgs::CollisionObject> o_map = planning_scene_interface.getObjects();
+        if(i.getType() == SceneCollisionObject::ENVIRONMENT_TYPE) {
 
-        for (auto const &x : o_map) {
+            moveit_msgs::CollisionObject co = i.getCollisionObject();
 
-            if (x.first == objectId) {
-                ROS_INFO("[SCENECONTROLLER] Found object to pick in planning scene");
-                object_to_pick = x.second;
+            float max_x = co.primitive_poses[0].position.x + (co.primitives[0].dimensions[0] / 2);
+            float max_y = co.primitive_poses[0].position.y + (co.primitives[0].dimensions[0] / 2) +
+                          (co.primitives[0].dimensions[1] / 2);
+            float min_x = co.primitive_poses[0].position.x - (co.primitives[0].dimensions[0] / 2);
+            float min_y = co.primitive_poses[0].position.y - (co.primitives[0].dimensions[0] / 2) -
+                          (co.primitives[0].dimensions[1] / 2);
+
+            // set the support surface for the pick task
+            if (object_to_pick.primitive_poses[0].position.x > min_x &&
+                object_to_pick.primitive_poses[0].position.x < max_x
+                && object_to_pick.primitive_poses[0].position.y > min_y &&
+                object_to_pick.primitive_poses[0].position.y < max_y) {
+                std::cout << "trying to set support surface: " << i.getCollisionObject().id << std::endl;
+                if (i.getCollisionObject().id != attached_surface_id) {
+                    std::cout << "setting support surface: " << i.getCollisionObject().id << std::endl;
+                    support_surface_id = i.getCollisionObject().id;
+                }
             }
         }
+    }
+
+    if (world_state::current_picked_object_id.empty()) {
 
         dimensions.x = object_to_pick.primitives[0].dimensions[0];
         dimensions.y = object_to_pick.primitives[0].dimensions[1];
@@ -414,7 +464,7 @@ bool pick(const std::string &objectId,
                                                                         gripper_transform.rotation);
 
         bool success = grasp_util.pickFromAbove(group, pick_pose, dimensions, world_state::open_amount,
-                                                TABLE_PLATE, objectId);
+                                                support_surface_id, objectId);
         if (success) {
             world_state::current_picked_object_id = objectId;
             return true;
@@ -560,7 +610,13 @@ bool drop(const std::string &bin_id,
 
         ROS_INFO("[SCENECONTROLLER] Removing placed object from scene.");
         std::vector<std::string> object_remove_vector{world_state::current_picked_object_id};
-        planning_scene_interface.removeCollisionObjects(object_remove_vector);
+     //   planning_scene_interface.removeCollisionObjects(object_remove_vector);
+
+        moveit_msgs::CollisionObject del_obj = planning_scene_interface.getObjects(object_remove_vector).at(world_state::current_picked_object_id);
+        del_obj.operation = del_obj.REMOVE;
+        std::vector<moveit_msgs::CollisionObject> del_obj_vec{del_obj};
+
+        planning_scene_interface.applyCollisionObjects(del_obj_vec);
 
         for (int i = 0; i < world_state::collision_objects.size(); i++) {
             if (world_state::collision_objects.at(i).getCollisionObject().id == world_state::current_picked_object_id) {