def __init__(self): print("init ManipulationNode") rospy.init_node("Manipulation_Control") self.__manipulation = Manipulation() rospy.Service(MOVE_SERVICE, Move, self.__handle_move) rospy.Service(PLAN_SERVICE, Plan, self.__handle_plan) rospy.Service(MOVE_WITH_PLAN_SERVICE, MoveWithPlan, self.__handle_move_with_plan) rospy.Service(ADD_COLLISION_OBJECTS_SERVICE, AddCollisionObjects, self.__handle_add_objects) rospy.Service(GET_COLLSISION_OBJECT_SERVICE, GetCollisionObject, self.__handle_get_collision_object) rospy.Service(MOVE_MASTCAM_SERVICE, MoveMastCam, self.__handle_mast_cam) rospy.Service(OPEN_GRIPPER_SERVICE, OpenGripper, self.__handle_open_gripper) rospy.Service(CLOSE_GRIPPER_SERVICE, CloseGripper, self.__handle_close_gripper) self.__base_publisher = rospy.Publisher(BASE_ORIGIN_TOPIC, PointStamped) self.__eef_position_publisher = rospy.Publisher( GET_EEF_POSITION_TOPIC, PointStamped) print( "ASDFASDFASFLKJKDJSFKJLASDFKJLDKFDKJLFKDSFHDSKFK############################################################################" )
def execute(self, userdata): rospy.logdebug('CamToDropzone: Executing state CamToDropzone') listener = TransformListener() if utils.manipulation is None: utils.manipulation = Manipulation() utils.manipulation.set_turbo_mode() utils.manipulation.set_movement_time(9) # TODO: Exception schmeissen wenn abort_after vergangen ist abort_after = 15 then = int(time.time()) now = int(time.time()) while not listener.frameExists("drop_point"): if not (now - then < abort_after): rospy.logdebug('drop_point frame not found!') return 'fail' rospy.loginfo("wait for drop_point frame") rospy.sleep(2.) now = int(time.time()) then = int(time.time()) now = int(time.time()) while not listener.frameExists("mdl_middle"): if not (now - then < abort_after): rospy.logdebug('CamToDropzone: mdl_middle frame not found!') return 'fail' rospy.sleep(2.) now = int(time.time()) if len(userdata.scan_conveyor_pose) == 0: rospy.logdebug('CamToDropzone: No scanPose, calculate now...') p = utils.manipulation.scan_conveyor_pose() userdata.scan_conveyor_pose.append(p) else: rospy.logdebug('CamToDropzone: scanPose found!') p = userdata.scan_conveyor_pose[0] rospy.logdebug('CamToDropzone: Move arm to scan_conveyor_pose') for i in range(0, 3): if utils.manipulation.move_to(p): return 'scanPoseReached' else: if i == 2: rospy.logdebug('CamToDropzone: Cant move arm to scan_conveyor_pose') return 'fail'
def execute(self, userdata): rospy.loginfo('Executing state CamToDropzone') if utils.manipulation is None: utils.manipulation = Manipulation() rospy.sleep(2) # 1. Foerderband in PS haun, wenns noch nicht drin ist. subscriber = rospy.Subscriber("suturo/yaml_pars0r", Task, self.addConveyorBelt) # 1: Dropzone Punkt ueber Vektor dp finden # 2: Breite des Bandes ueber 2*drop_deviation[1] bestimmen # 3: Hoehe des Bandes ueber drop_center_point[2] (z wert) + Banddicke (0.01m) holen # 4: Lange des Bandes mit mdl Vektor bestimmen + 0.10m fuer Laenge hinter dem dp Vektor # TODO 5: Orientierung de Bandes mit mdl Vektor bestimmen return 'success'
def execute(self, userdata): rospy.loginfo('Executing state SearchObject') if self._missing_objects is None: self._missing_objects = [] for obj in userdata.yaml.objects: print("adding missing object" + obj.name) self._missing_objects.append(obj.name) if utils.manipulation is None: utils.manipulation = Manipulation() rospy.sleep(2) # Add the found objects for obj in userdata.fitted_objects: # Check if the object was already found if [ x for x in self._found_objects if x.mpe_object.id == obj.mpe_object.id ]: rospy.loginfo('Object %s was already found.' % obj.mpe_object.id) else: self._found_objects.append(obj) if obj.mpe_object.id in self._missing_objects: self._missing_objects.remove(obj.mpe_object.id) userdata.objects_found = self._found_objects rospy.loginfo('##########################################') rospy.loginfo('Objects found:') for obj in self._found_objects: rospy.loginfo(obj.mpe_object.id) rospy.loginfo('##########################################') # Check if all objects were found if not self._missing_objects: return 'noObjectsLeft' return 'searchObject'
def execute(self, userdata): rospy.logdebug('PlaceTask6: Executing state PlaceTask6') listener = TransformListener() if utils.manipulation is None: utils.manipulation = Manipulation() # TODO: Exception schmeissen wenn abort_after vergangen ist abort_after = 15 then = int(time.time()) now = int(time.time()) while not listener.frameExists("target_zone"): if not now - then < abort_after: rospy.logdebug("PlaceTask6: target_zone frame not found") return 'fail' rospy.loginfo("wait for target_zone frame") rospy.sleep(2.) now = int(time.time()) self.__place_pose = self.get_place_point(userdata) rospy.logdebug('PlaceTask6: Move Arm to Place Pose') rospy.logdebug(self.__place_pose) for i in range(0, 7): if utils.manipulation.move_to(self.__place_pose): rospy.logdebug('PlaceTask6: OpenGripper') utils.manipulation.open_gripper() return 'success' else: rospy.logdebug('PlaceTask6: Calculate new place position...') if i <= 2: self.__place_pose = self.get_place_point(userdata) if i > 2: self.__place_pose = self.get_place_point(userdata, 3) if i == 6: rospy.logdebug('PlaceTask6: Cant find place position!') utils.manipulation.open_gripper() return 'fail'
def create_manipulation(self): utils.manipulation = Manipulation()
#!/usr/bin/env python import rospy from suturo_planning_manipulation.manipulation import Manipulation if __name__ == '__main__': # print (lambda x: a(2, x))(3) rospy.init_node('head_mover', anonymous=True) # mani = Manipulation() print mani.get_arm_base_move_group().get_current_joint_values()
# dest.header.frame_id = "/odom_combined" # dest.point = Point( -0.85, 0.85, 0.00) # mani.place_and_move(dest) # mani.grasp_and_move("green_cylinder") # # dest = PointStamped() # dest.header.frame_id = "/odom_combined" # dest.point = Point(-0.85, -0.85, 0) # mani.place_and_move(dest) if __name__ == '__main__': rospy.init_node('head_mover', log_level=rospy.DEBUG) m = Manipulation() # print m.get_arm_base_move_group().get_goal_orientation_tolerance() # print m.get_arm_base_move_group().get_goal_position_tolerance() # print m.get_arm_base_move_group().get_goal_joint_tolerance() # print m.get_arm_base_move_group().get_goal_tolerance() # print m.get_arm_move_group().get # m.grasp("cyan_cylinder") # m.open_gripper() # m.close_gripper() # m.get_arm_move_group().set_named_target("scan_pose1") # print m.get_arm_move_group().get_goal_tolerance() # print m.get_arm_move_group().get_goal_position_tolerance() # print m.get_arm_move_group().get_goal_orientation_tolerance() # rospy.sleep(2) # test_task1(m)
def execute(self, userdata): rospy.loginfo('Executing state ScanMapMastCam') if utils.manipulation is None: utils.manipulation = Manipulation(userdata.yaml) rospy.sleep(2) utils.map = Map(2) # arm_base = utils.manipulation.get_base_origin() # rospy.sleep(4) utils.manipulation.pan_tilt(0.2, 0.5) rospy.sleep(utils.waiting_time_before_scan) utils.map.add_point_cloud(scene_cam=True) utils.manipulation.pan_tilt(0.2825, 0.775) rospy.sleep(utils.waiting_time_before_scan) utils.map.add_point_cloud(scene_cam=True) utils.manipulation.pan_tilt(0, 1.1) rospy.sleep(utils.waiting_time_before_scan) utils.map.add_point_cloud(scene_cam=True) utils.manipulation.pan_tilt(-0.2825, 0.775) rospy.sleep(utils.waiting_time_before_scan) utils.map.add_point_cloud(scene_cam=True) utils.manipulation.pan_tilt(-0.2, 0.5) rospy.sleep(utils.waiting_time_before_scan) utils.map.add_point_cloud(scene_cam=True) utils.manipulation.pan_tilt(0, 0.45) if not userdata.enable_movement: return 'mapScanned' co = utils.map.to_collision_object() utils.manipulation.get_planning_scene().add_object(co) # utils.manipulation.get_planning_scene().add_object(co) rospy.sleep(2) n = 8 for i in range(0, n): a = 2 * pi * ((i + 0.0) / (n + 0.0)) goal_base_pose = PoseStamped() goal_base_pose.header.frame_id = "/odom_combined" goal_base_pose.pose.position = Point(cos(a), sin(a), 0) if goal_base_pose.pose.position.x < 0 and goal_base_pose.pose.position.y < 0 or \ goal_base_pose.pose.position.x > 0 and goal_base_pose.pose.position.y > 0: continue goal_base_pose.pose.position = set_vector_length( 0.25, goal_base_pose.pose.position) goal_base_pose.pose.orientation.w = 1 if utils.manipulation.move_base(goal_base_pose): rospy.sleep(utils.waiting_time_before_scan) utils.map.add_point_cloud( arm_origin=utils.manipulation.get_base_origin().point, scene_cam=True) break # utils.map.add_point_cloud(arm_origin=arm_base, scene_cam=True) # utils.map.add_point_cloud(arm_origin=arm_base, scene_cam=True) # utils.map.publish_as_marker() if userdata.yaml.task_type == Task.TASK_5: utils.map.all_unknowns_to_obstacle() #utils.map.remove_puzzle_fixture(userdata.yaml) co = utils.map.to_collision_object() utils.manipulation.get_planning_scene().add_object(co) return 'mapScanned'