Merge pull request #14 from aprilprojecteu/non-aa-planning-scene

Placing in non axis aligned planning scence

src/grasplan/place.py

@@ -6,7 +6,7 @@
 
 import sys
 import copy
-import tf
+import tf2_ros
 import rospy
 import actionlib
 import moveit_commander
@@ -17,14 +17,17 @@
 from grasplan.tools.moveit_errors import print_moveit_error
 from std_srvs.srv import Empty, SetBool, Trigger
 from object_pose_msgs.msg import ObjectList
-from geometry_msgs.msg import Vector3Stamped, PoseStamped
+import tf2_geometry_msgs
+from geometry_msgs.msg import Vector3Stamped, PoseStamped, TransformStamped, PointStamped, Point, Transform
 from moveit_msgs.msg import PlaceAction, PlaceGoal, PlaceLocation, GripperTranslation, PlanningOptions, Constraints, OrientationConstraint
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 from visualization_msgs.msg import Marker
 from grasplan.msg import PlaceObjectAction, PlaceObjectResult
 from moveit_msgs.msg import MoveItErrorCodes
 from pose_selector.srv import GetPoses
 from visualization_msgs.msg import Marker, MarkerArray
+from typing import List
+from std_msgs.msg import Header
 
 class PlaceTools():
     def __init__(self, action_server_required=True):
@@ -94,7 +97,8 @@ def __init__(self, action_server_required=True):
             self.place_action_server = actionlib.SimpleActionServer('place_object', PlaceObjectAction, self.place_obj_action_callback, False)
             self.place_action_server.start()
 
-        self.tf_listener = tf.TransformListener()
+        self.tf_buffer = tf2_ros.Buffer()
+        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
 
     def clear_place_poses_markers(self):
         marker_array_msg = MarkerArray()
@@ -166,6 +170,40 @@ def place_obj_action_callback(self, goal):
         else:
             self.place_action_server.set_aborted(PlaceObjectResult(success=False))
 
+    def transform_obj_list(self, obj_list: ObjectList, target_frame_id: str) -> ObjectList:
+        """Transforms an object list from a source frame to a target frame."""
+        if self.tf_buffer.can_transform(obj_list.header.frame_id, target_frame_id, rospy.Time(0)):
+            for obj in obj_list.objects:
+                source_pose = PoseStamped(header=obj_list.header, pose=obj.pose)
+                tf = self.tf_buffer.lookup_transform(target_frame_id, obj_list.header.frame_id, rospy.Time(0))
+                target_pose = tf2_geometry_msgs.do_transform_pose(source_pose, tf)
+                obj.pose = target_pose.pose
+        else:
+            raise RuntimeError(f"Error while transformin obj_list, no tf from {target_frame_id} -> {obj_list.header.frame_id}")
+        obj_list.header.frame_id = target_frame_id
+        return obj_list
+
+    def transform_points(self, points: List[Point], target_frame_id: str, source_frame_id: str) -> List[Point]:
+        """Transforms a list of points from a source frame to a target frame."""
+        if self.tf_buffer.can_transform(target_frame_id, source_frame_id, rospy.Time(0)):
+            transformed_points = []
+            for point in points:
+                source_point = PointStamped(header=Header(frame_id=source_frame_id), point=point) 
+                tf = self.tf_buffer.lookup_transform(target_frame_id, source_frame_id, rospy.Time(0))
+                target_point = tf2_geometry_msgs.do_transform_point(source_point, tf)
+                transformed_points.append(target_point.point)
+        else:
+            raise RuntimeError(f"Error while transforming points, no tf from {target_frame_id} -> {source_frame_id}")
+        return transformed_points
+
+    def add_planning_scene_objects(self, planning_scene) -> None:
+        """Adds all objects in the planning scene to the local tf buffer as static transforms."""
+        for obj_name in planning_scene.get_known_object_names():
+            obj = planning_scene.get_objects([obj_name])[obj_name]
+            tf = TransformStamped(header=Header(frame_id="map"), child_frame_id=obj_name,
+                     transform=Transform(translation=obj.pose.position, rotation=obj.pose.orientation))
+            self.tf_buffer.set_transform_static(tf, "grasplan")
+
     def place_object(self, support_object, observe_before_place=False, number_of_poses=5,\
                      override_disentangle_dont_doit=False, override_observe_before_place_dont_doit=False):
         '''
@@ -182,6 +220,9 @@ def place_object(self, support_object, observe_before_place=False, number_of_pos
         object_to_be_placed = list(self.scene.get_attached_objects().keys())[0]
         rospy.loginfo(f'received request to place the object that the robot is currently holding : {object_to_be_placed}')
 
+        # TODO: find a better place for adding the static transforms, it shouldn't be done for each place
+        self.add_planning_scene_objects(self.scene)
+
         # clear pose selector before starting to place in case some data is left over from previous runs
         self.place_pose_selector_clear_srv()
 
@@ -205,15 +246,17 @@ def place_object(self, support_object, observe_before_place=False, number_of_pos
         plane = adjust_plane(plane, -0.15, -0.15)
 
         # publish plane as marker for visualization purposes
-        self.plane_vis_pub.publish(make_plane_marker_msg(self.global_reference_frame, plane))
+        self.plane_vis_pub.publish(make_plane_marker_msg(self.global_reference_frame, self.transform_points(plane, self.global_reference_frame, support_object)))
 
         # generate random places within the plane
         object_class_tbp = separate_object_class_from_id(object_to_be_placed)[0]
-        place_poses_as_object_list_msg = gen_place_poses_from_plane(object_class_tbp, support_object, plane, self.scene, \
-                frame_id=self.global_reference_frame, number_of_poses=number_of_poses, \
+        local_place_poses = gen_place_poses_from_plane(object_class_tbp, support_object, plane, self.scene, \
+                frame_id=support_object, number_of_poses=number_of_poses, \
                 min_dist=self.min_dist, ignore_min_dist_list=self.ignore_min_dist_list)
+
+        global_place_poses = self.transform_obj_list(local_place_poses, self.global_reference_frame)
 
-        self.place_poses_pub.publish(place_poses_as_object_list_msg)
+        self.place_poses_pub.publish(global_place_poses)
 
         # clear octomap before placing, this is experimental and not sure is needed
         rospy.logwarn('clearing octomap')
@@ -222,7 +265,7 @@ def place_object(self, support_object, observe_before_place=False, number_of_pos
         if action_client.wait_for_server(timeout=rospy.Duration.from_sec(2.0)):
             rospy.loginfo(f'found {self.place_object_server_name} action server')
             goal = self.make_place_goal_msg(object_to_be_placed, support_object, \
-                                            place_poses_as_object_list_msg, \
+                                            global_place_poses, \
                                             use_path_constraints=self.use_path_constraints)
 
             # allow disentangle to happen only on first place attempt, no need to do it every time

src/grasplan/tools/support_plane_tools.py

@@ -260,13 +260,12 @@ def get_obj_from_planning_scene(obj_name: str, planning_scene: PlanningScene) ->
         raise ValueError(f"Object '{obj_name}' not in planning scene")
     return planning_scene.get_objects([obj_name])[obj_name]
 
-# TODO: consider shape_msgs/Plane instead of 4 points
 def obj_to_plane(support_obj: str, planning_scene: PlanningScene, offset: float = 0.001) -> List[Point]:
     """
-    Get a top surface plane from an object in the MoveIt planning scene.
- 
-    NOTE: Currently only works for boxes parallel to the XY plane.
+    Get a surface plane from an object in the MoveIt planning scene.
 
+    NOTE: Currently only works for boxes
+ 
     Parameters
     ----------
     support_obj : str
@@ -275,36 +274,26 @@ def obj_to_plane(support_obj: str, planning_scene: PlanningScene, offset: float
         The MoveIt planning scene.
     offset : float, optional
         The offset from the MoveIt object and the created plane. Defaults to 0.001.
-
+ 
     Returns
     -------
     List[Point]
-        A list of four points representing the corners of the top surface plane.
+        A list of four points (top right, top left, buttom left, buttom right) 
+        representing the corners of the top surface plane.
     """
     collision_object = get_obj_from_planning_scene(support_obj, planning_scene)
 
     if len(collision_object.primitives) != 1 or collision_object.primitives[0].type != 1:
         raise ValueError(f"Object '{support_obj}' is not a box")
 
-    rotation_angle = tf.transformations.euler_from_quaternion(
-        [collision_object.pose.orientation.x, collision_object.pose.orientation.y,
-        collision_object.pose.orientation.z, collision_object.pose.orientation.w]
-    )
-
-    # TODO: this should be checked in a central place and not in each function
-    if rotation_angle[0] != 0 or rotation_angle[1] != 0:
-        raise ValueError(f"Object '{support_obj}' is not aligned with the XY plane")
-
     half_width = collision_object.primitives[0].dimensions[0] / 2
     half_depth = collision_object.primitives[0].dimensions[1] / 2
 
-    center_point = [collision_object.pose.position.x, collision_object.pose.position.y, collision_object.pose.position.z * 2]
+    corners = [(half_width, half_depth), (-half_width, half_depth), (-half_width, -half_depth), (half_width, -half_depth)]
 
-    corner_offsets = [(half_width, half_depth), (-half_width, half_depth), (-half_width, -half_depth), (half_width, -half_depth)]
+    half_height = collision_object.primitives[0].dimensions[2] / 2
 
-    return [Point(center_point[0] + dx * math.cos(rotation_angle[2]) - dy * math.sin(rotation_angle[2]),
-             center_point[1] + dx * math.sin(rotation_angle[2]) + dy * math.cos(rotation_angle[2]),
-             center_point[2] + offset) for dx, dy in corner_offsets]
+    return [Point(dx, dy, half_height + offset) for dx, dy in corners]
 
 def attached_obj_height(attached_obj: str, planning_scene: PlanningScene, offset: float = 0.001) -> float:
     """
@@ -329,7 +318,7 @@ def attached_obj_height(attached_obj: str, planning_scene: PlanningScene, offset
     if any('power_drill_with_grip' in key for key in planning_scene.get_attached_objects()):
         dimension_index = 1
     half_height_att_obj = list(planning_scene.get_attached_objects().values())[0].object.primitives[0].dimensions[dimension_index] / 2
-    return half_height_att_obj + collision_object.pose.position.z * 2 + offset
+    return half_height_att_obj + collision_object.primitives[0].dimensions[2] / 2 + offset
 
 # Example usage
 if __name__ == '__main__':