code cleanup

sr_moveit2_utils/sr_moveit2_utils/move_client.py

@@ -1,8 +1,7 @@
 import rclpy
 from rclpy.node import Node
-from geometry_msgs.msg import PoseStamped, Vector3, Pose
+from geometry_msgs.msg import Pose
 from sr_manipulation_interfaces.srv import MoveToPose, MoveToPoseSeq
-from visualization_msgs.msg import MarkerArray
 from std_srvs.srv import Trigger
 
 

sr_moveit2_utils/sr_moveit2_utils/moveit_client.py

@@ -43,7 +43,7 @@
 from sr_manipulation_interfaces.msg import ManipType, PlanExecState, ServiceResult
 from sr_manipulation_interfaces.srv import AttachObject, DetachObject
 
-from geometry_msgs.msg import PoseStamped, Vector3, Vector3Stamped, Pose
+from geometry_msgs.msg import PoseStamped, Vector3Stamped, Pose
 from moveit_msgs.msg import Grasp
 from sr_moveit2_utils.move_client import MoveClient
 import moveit_msgs.msg
@@ -277,7 +277,6 @@ def manip_goal_cb(self, goal: Manip.Goal):
             self.active_goal = goal
             self.get_logger().debug('Manip goal accepted.')
             return GoalResponse.ACCEPT
-        request.manipulation_sequence[0]
 
     def manip_cancel_cb(self, request):
         #TODO stop current execution cleanly ?
@@ -481,24 +480,6 @@ def did_manip_plan_succeed(self, plan_exec_success: bool, action_name: str, goal
             self.get_logger().debug(f'{action_name} failed.')
             goal_handle.abort()
             return False
-
-
-        self.get_logger().debug('PlanMoveTo execution done.')
-        if not goal_handle.is_cancel_requested:
-            if ret:
-                result.state.plan_state = PlanExecState.PLAN_SUCCESS
-                result.state.exec_state = PlanExecState.EXEC_SUCCESS
-                result.success = True
-                self.get_logger().debug('PlanMoveTo succeeded.')
-                goal_handle.succeed()
-            else:
-                result.state.plan_state = PlanExecState.PLAN_ERROR
-                result.state.exec_state = PlanExecState.EXEC_ERROR
-                result.success = False
-                self.get_logger().debug('PlanMoveTo failed.')
-                goal_handle.abort()
-        self.active_goal = None          
-        return result
 
     def send_move_request(self, pose:Pose, cartesian_trajectory:bool=True, planner_profile:str="", velocity_scaling_factor=None):
         # use default velocity scaling if not defined
@@ -511,7 +492,6 @@ def attach(self, id:str, attach_link:str, allowed_touch_links:list[str]):
         req.link_name = attach_link
         req.touch_links = allowed_touch_links
         response = self.attach_object_cli.call(req)
-        # response = wait_for_response(future, self)
         if response.result.state != ServiceResult.SUCCESS:
             self.get_logger().error(f"Attach object {id} has failed.")
             return False
@@ -522,7 +502,6 @@ def detach(self, id:str):
         req = DetachObject.Request()
         req.id = id
         response = self.detach_object_cli.call(req)
-        # response = wait_for_response(future, self)
         if response.result.state != ServiceResult.SUCCESS:
             self.get_logger().error(f"Detach object {id} has failed.")
             return False

sr_moveit2_utils/sr_moveit2_utils/scene_manager.py

@@ -296,17 +296,11 @@ 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
-        # Load in the object once more
-        # self.object_in_the_scene_storage[object_id] = attached_collision_object_to_detach.object
-        # self.object_in_the_scene_storage[object_id].pose = attached_collision_object_to_detach.object.pose
-        # self.object_in_the_scene_storage[object_id].operation = CollisionObject.ADD
-        # self.get_logger().info(f"storage id {object_id} is put in: {attached_collision_object_to_detach.object}")
         self.get_logger().info(f"Object {attached_collision_object_to_detach} is successfully detached.")
         return True
 
     def detach_collision_object(self, attached_collision_object: AttachedCollisionObject, detach_to_link: str):
 
-        #object_pose_in_attach_link_name = self.tcp_transforms.to_from_tcp_pose_conversion(object.pose, object.header.frame_id, link_name, False)
         detached_collision_object = attached_collision_object
 
         # Add object again to the scene
@@ -318,9 +312,6 @@ def detach_collision_object(self, attached_collision_object: AttachedCollisionOb
         collision_object_to_add.header.frame_id = detach_to_link # TODO(gwalck) unsure about that but is in the tuto
         collision_object_to_add.operation = CollisionObject.ADD
 
-        #object_pose_in_detach_to_link_name = self.tcp_transforms.to_from_tcp_pose_conversion(collision_object_to_add.pose, collision_object_to_add.header.frame_id, detach_to_link, False)
-        #collision_object_to_add.mesh_poses = [object_pose_in_detach_to_link_name]
-
         detached_collision_object.object.operation = CollisionObject.REMOVE
         #detached_collision_object.object.mesh_poses = [object_pose_in_attach_link_name]
 
@@ -333,10 +324,6 @@ def detach_collision_object(self, attached_collision_object: AttachedCollisionOb
         planning_scene.robot_state.attached_collision_objects.append(detached_collision_object)
         planning_scene.robot_state.is_diff = True
         ret = self.apply_planning_scene(planning_scene)
-        if ret:
-            # store the object back to the scene
-            # self.object_in_the_scene_storage[collision_object_to_add.id] = collision_object_to_add
-            pass
         return ret
 
     def attach_object_cb(self,

sr_moveit2_utils/sr_moveit2_utils/scene_manager_client.py

@@ -37,7 +37,7 @@
 from rclpy.node import Node
 from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
 from rclpy.executors import MultiThreadedExecutor
-from sr_manipulation_interfaces.srv import AddObjects, RemoveObjects, AttachObject, DetachObject
+from sr_manipulation_interfaces.srv import AttachObject, DetachObject
 from sr_manipulation_interfaces.msg import ServiceResult