Bugfixes and proper link calling

sr_moveit2_utils/sr_moveit2_utils/moveit_client.py

@@ -135,6 +135,8 @@ def __init__(self):
         self.tool_link = self.get_parameter("tool_link").value
         self.declare_parameter("allowed_touch_links", ["tcp_link", "gripper_base"])
         self.allowed_touch_links = self.get_parameter("allowed_touch_links").value
+        self.declare_parameter("fixed_frame", "world")
+        self.fixed_frame = self.get_parameter("fixed_frame").value
 
         self.tcp_transforms = TCPTransforms(self)
         self.visualization_publisher = VisualizatonPublisher(self)
@@ -213,11 +215,11 @@ def plan_move_to_execute_cb(self, goal_handle: ServerGoalHandle):
 
         else:
             for i, target in enumerate(request.targets):
-                if (target.pose.header.frame_id != self.chain_base_link):
+                if (target.pose.header.frame_id != self.fixed_frame):
                     pose = self.compute_manip_pose(target.pose)
                 else:
                     pose = target.pose.pose
-
+                self.visualization_publisher.publish_pose_as_transform(pose, self.fixed_frame, "base_position", is_static=True)
                 self.plan_move_to_feedback.state.plan_message = f"planning for target {i}/{len(request.targets)}"
                 self.plan_move_to_feedback.state.exec_message = f"executing for target {i}/{len(request.targets)}"
                 goal_handle.publish_feedback(self.plan_move_to_feedback)
@@ -300,7 +302,7 @@ def compute_manip_pose(self, source_pose: PoseStamped,
         self.get_logger().debug(f'Manip computed pose source frame ({pose_source_frame.header.frame_id}).')
         return self.tcp_transforms.to_from_tcp_pose_conversion(pose_source_frame.pose,
                                                                pose_source_frame.header.frame_id,
-                                                               self.chain_base_link, apply_tool_offset)
+                                                               self.fixed_frame, apply_tool_offset)
 
     def manip_execute_cb(self, goal_handle: ServerGoalHandle):
         self.get_logger().info('executing')
@@ -341,19 +343,22 @@ def manip_execute_cb(self, goal_handle: ServerGoalHandle):
                                                                           request.pick.pre_grasp_approach.desired_distance)
                     if reach_pose_robot_base_frame is None:
                         break
-                    self.visualization_publisher.publish_pose_as_transform(reach_pose_robot_base_frame, self.chain_base_link, "pre_grasp_pose_base_link", is_static=True)
+                    self.visualization_publisher.publish_pose_as_transform(reach_pose_robot_base_frame, self.fixed_frame, "pre_grasp_pose_base_link", is_static=True)
                 if manip == ManipType.MANIP_REACH_PREPLACE:
                     # compute pre-place pose
                     reach_pose_robot_base_frame = self.compute_manip_pose(request.place.place_pose, True,
                                                                           request.place.pre_place_approach.direction,
                                                                           request.place.pre_place_approach.desired_distance)
                     if reach_pose_robot_base_frame is None:
                         break
-                    self.visualization_publisher.publish_pose_as_transform(reach_pose_robot_base_frame, self.chain_base_link, "pre_place_pose_base_link", is_static=True)
+                    self.visualization_publisher.publish_pose_as_transform(reach_pose_robot_base_frame, self.fixed_frame, "pre_place_pose_base_link", is_static=True)
 
                 # perform the action
                 # do the actual planning and execution
-
+                reach_pose_robot_base_frame.orientation.x = -reach_pose_robot_base_frame.orientation.x
+                reach_pose_robot_base_frame.orientation.y = -reach_pose_robot_base_frame.orientation.y
+                reach_pose_robot_base_frame.orientation.z = -reach_pose_robot_base_frame.orientation.z
+                reach_pose_robot_base_frame.orientation.w = -reach_pose_robot_base_frame.orientation.w
                 if manip == ManipType.MANIP_REACH_PREGRASP:
                     ret = self.move_client.send_move_request(reach_pose_robot_base_frame, cartesian_trajectory=False,
                                                  planner_profile=request.planner_profile if request.planner_profile else "ompl", plan_only=self.plan_first)

sr_moveit2_utils/sr_moveit2_utils/scene_manager.py

@@ -151,11 +151,13 @@ def planning_scene_cb(self, msg):
             # We can update the positions in our dictionary if collision objects match
             for obj in msg.world.collision_objects:
                 self.object_in_the_scene_storage[obj.id] = obj
+                self.get_logger().info(f"Found object {obj.id} with position {obj.pose.position} in scene with frame {obj.header.frame_id}")
 
         if len(msg.robot_state.attached_collision_objects)>0:
             # We can update the positions in our dictionary if collision objects match
             for attached_obj in msg.robot_state.attached_collision_objects:
                 self.attached_object_store[attached_obj.object.id] = attached_obj
+                self.get_logger().info(f"Found ATTACHED object {attached_obj.object.id} with position {attached_obj.object.pose.position} in scene")
 
 
 
@@ -235,14 +237,15 @@ def get_object_pose_cb(self,
                          request: GetObjectPose.Request,
                          response: GetObjectPose.Response) -> GetObjectPose.Response:
         # check if the object exists at all
-        self.get_logger().debug('Scene Manager get object pose') 
+        self.get_logger().debug('Scene Manager get object pose')
+        # self.get_logger().warn(f'Looking for {request.id} in\n{self.object_in_the_scene_storage.keys()}') 
         if not request.id in self.object_in_the_scene_storage:
             response.result.state = ServiceResult.NOTFOUND
         else:
             pose = self.get_object_stamped_pose(request.id)
             if pose is not None:
                 response.result.state = ServiceResult.SUCCESS
-                response.result.pose = pose
+                response.pose = pose
             else:
                 response.result.state = ServiceResult.FAILED
         return response
@@ -251,9 +254,8 @@ def get_object_stamped_pose(self, object_id: str):
         if not object_id in self.object_in_the_scene_storage.keys():
             self.get_logger().error(f"Object '{object_id}' is not known to the scene manager. Did you add it?")
             return None
-
         object = self.object_in_the_scene_storage[object_id]
-
+        # self.get_logger().warn(f'Scene Manager get object pose {object}') 
         pose = PoseStamped()
         pose.header = deepcopy(object.header)
         pose.pose = deepcopy(object.pose)
@@ -296,7 +298,7 @@ def detach_object(self, object_id: str, detach_to_link=None):
             # re-add the object to the store
             self.attached_object_store[object_id] = attached_collision_object_to_detach
             return False
-        self.get_logger().info(f"Object {attached_collision_object_to_detach} is successfully detached.")
+        self.get_logger().info(f"Object {object_id} is successfully detached.")
         return True
 
     def detach_collision_object(self, attached_collision_object: AttachedCollisionObject, detach_to_link: str):
@@ -356,7 +358,6 @@ def attach_object(self, object_id: str, link_name: str, touch_links: list[str]):
         collision_object_to_remove.header.frame_id = self.scene_base_frame # TODO(gwalck) unsure about that but is in the tuto
         collision_object_to_remove.operation = CollisionObject.REMOVE
 
-        self.get_logger().info(f"Attach_object is {object_to_attach}")
         #TODO(gwalck) cleanup the duplicate objects coming form the fusion of 2 functions
         #object_pose_in_attach_link_name = self.tcp_transforms.to_from_tcp_pose_conversion(object.pose, object.header.frame_id, link_name, False)
         #self.get_logger().debug(f"Calculated target pose of {object_to_attach.id} in {link_name} is {object_pose_in_attach_link_name}")