def __init__(self): # initialize GripperCommandClient GripperCommandClient.__init__(self) # initialize Moveit Scene self.scene = PlanningSceneInterface("base_link") self.scene2 = moveit_commander.PlanningSceneInterface() find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient( find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server() # creat target pose publisher self.marker_pub = rospy.Publisher("/TargetMarker", MarkerArray, queue_size=1) # creat basket pos publisher self.marker_pub2 = rospy.Publisher("/basket", MarkerArray, queue_size=1) # creat publisher for requesting grasp pose self.request_cloud_client = rospy.Publisher("/request_cloud", Int32, queue_size=1) # instantiate a RobotCommander self.robot = moveit_commander.RobotCommander() # instantiate two MoveGroupCommander self.group = moveit_commander.MoveGroupCommander("arm") # basket's parameters self.basket_pos = 'left' self.basket_found = False self.basket_pos_x = 0 self.basket_pos_y = 0 self.basket_search_t = 0
def tuck(): if not is_moveit_running(): rospy.loginfo("starting moveit") move_thread = MoveItThread() rospy.loginfo("Waiting for MoveIt...") client = MoveGroupInterface("arm_with_torso", "base_link") rospy.loginfo("...connected") # Padding does not work (especially for self collisions) # So we are adding a box above the base of the robot scene = PlanningSceneInterface("base_link") scene.addBox("keepout", 0.2, 0.5, 0.05, 0.15, 0.0, 0.375) joints = ["torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = client.moveToJointPosition(joints, pose, 0.0, max_velocity_scaling_factor=0.5) if result.error_code.val == MoveItErrorCodes.SUCCESS: scene.removeCollisionObject("keepout") move_thread.stop() return
def set_init_pose(self): #set init pose move_group = MoveGroupInterface( "manipulator", "base_link") #MoveGroupInterface のインスタンスの作成 planning_scene = PlanningSceneInterface( "base_link") #PlanningSceneInterface のインスタンスの作成 joint_names = [ "shoulder_pan_joint", "shoulder_lift_joint", #ジョイントの名前を定義 "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint" ] # pose = [-1.26 , -0.64 , -2.44 , -0.66 , 1.56 , 0.007] # move_group.moveToJointPosition(joint_names, pose, wait=False)#joint_names を pose に動かす # move_group.get_move_action().wait_for_result() #wait result # result = move_group.get_move_action().get_result() #result を代入 move_group.get_move_action().cancel_all_goals() #すべてのゴールをキャンセル self.state = { 'default': 0, 'hold': 1, 'way_standby': 2, 'plan_standby': 3 } # default : 何もしていない target のセットか hold button を待つ # hold : waypoint のセットを受け付ける hold button が離れたら終了 #way_standby : execute button が押されるのを待つ 押されたら default へ waypoint の設置をした場合 #plan_standby : execute button が押されるのを待つ 押されたら default へ waypoint を使わなかった場合 self.now_state = self.state['default'] #現在フレームの状態
def gotoInit(self): move_group = MoveGroupInterface("arm", "base_link") planning_scene = PlanningSceneInterface("base_link") joint_names = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] # Plans the joints in joint_names to angles in pose init = self.trajActionGoal.trajectory.points[0].positions move_group.moveToJointPosition(joint_names, init, wait=False) move_group.get_move_action().wait_for_result() result = move_group.get_move_action().get_result() if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Moved to Init") else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Arm goal in state: %s", move_group.get_move_action().get_state()) else: rospy.logerr("MoveIt! failure no result returned.")
def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True) self.move_group = MoveGroupInterface("arm", "base_link") grasp_topic = "fetch_grasp_planner_node/plan" rospy.loginfo("Waiting for %s..." % grasp_topic) self.grasp_planner_client = actionlib.SimpleActionClient( grasp_topic, GraspPlanningAction) wait = self.grasp_planner_client.wait_for_server(rospy.Duration(5)) if (wait): print("successfully connected to grasp server") else: print("failed to connect to grasp server") rospy.Subscriber("fetch_fruit_harvest/apple_pose", Pose, self.apple_pose_callback) self.pub = rospy.Publisher("fetch_fruit_harvest/grasp_msg", String, queue_size=10) self.object = Object() self.grasps = Grasp() self.ready_for_grasp = False self.pick_place_finished = False self.first_time_grasp = True self.scene.addBox("ground", 300, 300, 0.02, 0, 0, 0) self.tuck()
def __init__(self): self.moveGroup = MoveGroupInterface("arm_with_torso", "base_link") self.planning_scene = PlanningSceneInterface("base_link") self.planning_scene.addCube("my_front_ground", 2, 1.4, 0.0, -0.48) self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -0.6) self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -0.6) self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -0.6)
def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True) self.move_group = MoveGroupInterface("arm", "base_link") # 初始化需要使用move group控制的机械臂中的arm group self.arm = MoveGroupCommander('arm') # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 self.arm.set_goal_position_tolerance(0.01) self.arm.set_goal_orientation_tolerance(0.05) # 当运动规划失败后,允许重新规划 self.arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' self.arm.set_pose_reference_frame(reference_frame) # 设置每次运动规划的时间限制:5s self.arm.set_planning_time(5) find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server()
def __init__(self, boundaries=False): self.scene = PlanningSceneInterface("/base_link") self.robot = RobotCommander() self.group_commander = MoveGroupCommander("manipulator") #self.gripper = MoveGroupCommander("gripper") self.group_commander.set_end_effector_link('ee_link') self.get_basic_infomation() if boundaries is True: self.add_bounds() self.joint_state_subscriber = rospy.Subscriber("/joint_states", JointState, self.joint_callback) self.joint_pubs = [ rospy.Publisher('/%s/command' % name, Float64, queue_size=1) for name in CONTROLLER_NAMES ] self.gripper_pub = rospy.Publisher('/gripper_prismatic_joint/command', Float64, queue_size=1) # create a DisplayTrajectory publisher which is used later to publish trajectories for RViz to visualize self.display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) self.reset_subscriber = rospy.Subscriber("/ur/reset", String, self.reset) rospy.sleep(2)
def set_forbidden(self): #set forbidden erea self.planning_scene = PlanningSceneInterface( "base_link") #PlanningSceneInterface のインスタンスの作成 self.planning_scene.removeCollisionObject("my_front_ground") self.planning_scene.removeCollisionObject("my_back_ground") self.planning_scene.removeCollisionObject("my_right_ground") self.planning_scene.removeCollisionObject("my_left_ground") #オブジェクトの削除 self.planning_scene.addCube("my_front_ground", 2, 1.1, 0, -1.0) self.planning_scene.addCube("my_back_ground", 2, -1.2, 0, -1.0) self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.0, -1.0) self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.0, -1.0) #座標に立方体を挿入 self.planning_scene.removeCollisionObject("demo_cube") self.planning_scene.removeCollisionObject("obstacle") #オブジェクトの削除 print dir(self.planning_scene) #インスタンス内のあらゆるオブジェクトの属性とメソッドのリストを表示 import inspect print "addBox's variable is ", inspect.getargspec( self.planning_scene.addBox) print "attachBox's variable is ", inspect.getargspec( self.planning_scene.attachBox) print "addCube's variable is ", inspect.getargspec( self.planning_scene.addCube) print "setColor's variable is ", inspect.getargspec( self.planning_scene.setColor) #python関数のパラメータの名前とデフォルト値を取得
def main(): rospy.init_node('part1_obstacles') wait_for_time() planning_scene = PlanningSceneInterface('base_link') planning_scene.clear() planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('floor') planning_scene.addBox('floor', 2, 2, 0.01, 0, 0, 0.01 / 2) table_height = 0.767 table_width = 0.7 table_x = 0.95 planning_scene.addBox('table', table_width, 2, table_height, table_x, 0, table_height / 2) planning_scene.addBox('robot_base', 0.54, 0.52, 0.37, 0, 0, 0.37 / 2) microwave_height = 0.28 microwave_width = 0.48 # microwave_depth = 0.33 microwave_depth = 0.27 microwave_x = 0.97 microwave_z = 0.06 microwave_y = 0.18 planning_scene.addBox('microwave', microwave_depth, microwave_width, microwave_height, microwave_x, microwave_y, table_height + microwave_z + microwave_height / 2) rospy.sleep(2) rospy.spin()
def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True) self.move_group = MoveGroupInterface("arm", "base_link") find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server()
def __init__(self): self.planning_scene = PlanningSceneInterface("base_link") self.planning_scene.removeCollisionObject("my_front_ground") self.planning_scene.removeCollisionObject("my_back_ground") self.planning_scene.removeCollisionObject("my_right_ground") self.planning_scene.removeCollisionObject("my_left_ground") self.planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
def arm_move(self): move_group = MoveGroupInterface("arm_with_torso", "base_link") # Define ground plane # This creates objects in the planning scene that mimic the ground # If these were not in place gripper could hit the ground planning_scene = PlanningSceneInterface("base_link") planning_scene.removeCollisionObject("my_front_ground") planning_scene.removeCollisionObject("my_back_ground") planning_scene.removeCollisionObject("my_right_ground") planning_scene.removeCollisionObject("my_left_ground") planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) #planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) planning_scene.addCube("my_left_ground", 1, 1.5, self.translation[2][0], self.translation[2][1], self.translation[2][2]) planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) # TF joint names joint_names = [ "torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] # Lists of joint angles in the same order as in joint_names disco_poses = [[0, 2.5, -0.1, 3.0, 1.5, 3.0, 1.0, 3.0]] for pose in disco_poses: if rospy.is_shutdown(): break # Plans the joints in joint_names to angles in pose move_group.moveToJointPosition(joint_names, pose, wait=False) # Since we passed in wait=False above we need to wait here move_group.get_move_action().wait_for_result() result = move_group.get_move_action().get_result() if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Disco!") else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Arm goal in state: %s", move_group.get_move_action().get_state()) else: rospy.logerr("MoveIt! failure no result returned.") # This stops all arm movement goals # It should be called when a program is exiting so movement stops move_group.get_move_action().cancel_all_goals()
def set_forbidden(self): #set forbidden erea self.planning_scene = PlanningSceneInterface("base_link") self.planning_scene.removeCollisionObject("my_front_ground") self.planning_scene.removeCollisionObject("my_back_ground") self.planning_scene.removeCollisionObject("my_right_ground") self.planning_scene.removeCollisionObject("my_left_ground") self.planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
def main(args): rospy.init_node('simple_move') move_group = MoveGroupInterface('manipulator', "base_link") planning_scene = PlanningSceneInterface("base_link") joint_names = [ 'shoulder_1_joint', 'shoulder_2_joint', 'elbow_1_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ] # This is the wrist link not the gripper itself gripper_frame = "wrist_3_link" poses, pose_name = get_pose(args.reset) # Construct a "pose_stamped" message as required by moveToPose gripper_pose_stamped = PoseStamped() gripper_pose_stamped.header.frame_id = "base_link" while not rospy.is_shutdown(): for pose in poses: # Finish building the Pose_stamped message # If the message stampe is not current it could be ignored gripper_pose_stamped.header.stamp = rospy.Time.now() # Set the message pose gripper_pose_stamped.pose = pose move_group.moveToPose(gripper_pose_stamped, gripper_frame) result = move_group.get_move_action().get_result() if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: log_output = pose_name + " Done!" rospy.loginfo(log_output) else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Arm goal in state: %s", move_group.get_move_action().get_state()) else: rospy.logerr("MoveIt! failure no result returned.") # This stops all arm movement goals # It should be called when a program is exiting so movement stops move_group.get_move_action().cancel_all_goals()
def main(): rospy.init_node('a5_obstacles') wait_for_time() planning_scene = PlanningSceneInterface('base_link') planning_scene.clear() planning_scene.removeCollisionObject('top_shelf') planning_scene.removeCollisionObject('bottom_shelf') planning_scene.addBox('top_shelf', 0.32, 1, 0.4, 0.99, 0, 1.64) planning_scene.addBox('bottom_shelf', 0.5, 1, 1.09, 0.9, 0, 0.545) rospy.sleep(2)
def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True) self.move_group = MoveGroupInterface("arm", "base_link") ##self.objectname_sub= rospy.Subscriber("/objects",Float32MultiArray,self.callback) ##self.interestedObjectName = "" #self.objects = [] find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server()
def set_init_pose(sefl): #set init pose move_group = MoveGroupInterface("manipulator","base_link") planning_scene = PlanningSceneInterface("base_link") joint_names = ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"] pose = [0.0, -1.98, 1.0, -0.64, -1.57, 0.0] move_group.moveToJointPosition(joint_names, pose, wait=False) move_group.get_move_action().wait_for_result() result = move_group.get_move_action().get_result() move_group.get_move_action().cancel_all_goals()
def set_init_pose(self): #set init pose move_group = MoveGroupInterface("manipulator","base_link") #MoveGroupInterface のインスタンスの作成 planning_scene = PlanningSceneInterface("base_link") #PlanningSceneInterface のインスタンスの作成 joint_names = ["shoulder_pan_joint", "shoulder_lift_joint", #ジョイントの名前を定義 "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"] pose = [0.0, -1.98, 1.0, -0.64, -1.57, 0.0] move_group.moveToJointPosition(joint_names, pose, wait=False)#joint_names を pose に動かす move_group.get_move_action().wait_for_result() #wait result result = move_group.get_move_action().get_result() #result を代入 move_group.get_move_action().cancel_all_goals() #すべてのゴールをキャンセル
def main(): rospy.init_node('a5_obstacles') wait_for_time() planning_scene = PlanningSceneInterface('base_link') planning_scene.clear() planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('floor') planning_scene.addBox('floor', 2, 2, 0.01, 0, 0, 0.01 / 2) planning_scene.addBox('table', 0.5, 1, 0.72, 1, 0, 0.72 / 2) rospy.sleep(2)
def __init__(self, save_file_path="food_items.pkl", nav_file_path="annotator_positions.pkl"): self._food_items_pub = rospy.Publisher(FOOD_ITEMS_TOPIC, FoodItems, queue_size=10, latch=True) rospy.loginfo("Given save file path: " + save_file_path) if os.path.isfile(save_file_path): rospy.loginfo("File already exists, loading saved positions.") with open(save_file_path, "rb") as save_file: try: self._food_items = pickle.load(save_file) except EOFError: # this can be caused if the file is empty. self._food_items = {} rospy.loginfo("File loaded...") else: rospy.loginfo("File doesn't exist.") self._food_items = {} self.__print_food_items__() self._save_file_path = save_file_path self.__pub_food_items__() rospy.loginfo("initializing arm...") self.arm = robot_api.Arm() rospy.loginfo("initializing gripper...") self.gripper = robot_api.Gripper() rospy.loginfo("initializing head...") self.head = robot_api.Head() rospy.loginfo("initializing torso...") self.torso = robot_api.Torso() rospy.loginfo("initializing planning scene...") self.planning_scene = PlanningSceneInterface('base_link') self.curr_obstacle = None rospy.loginfo("Starting map annotator...") # We should expect the nav file given to contain the annotated positions: # MICROWAVE_LOCATION_NAME - starting location in front of the microwave. # DROPOFF_LOCATION_NAME - ending dropoff location. # TODO: Remember to uncomment this section when we get the map working. # self._map_annotator = Annotator(save_file_path=nav_file_path) # if not self._map_annotator.exists(self.MICROWAVE_LOCATION_NAME): # rospy.logwarn("Annotator is missing location '%s'" % # (self.MICROWAVE_LOCATION_NAME)) # if not self._map_annotator.exists(self.DROPOFF_LOCATION_NAME): # rospy.logwarn("Annotator is missing location '%s'" % # (self.DROPOFF_LOCATION_NAME)) rospy.loginfo("Initialization finished...")
def __init__(self): self.moving_mode = False self.plan_only = False self.prev_joy_buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] self.joint_names = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] self.joy_subscriber = rospy.Subscriber("/fetch_joy", Joy, self.joy_callback) self.joints_subscriber = rospy.Subscriber("/joint_states", JointState, self.joints_states_callback) self.rgbimage_subscriber = rospy.Subscriber("/head_camera/depth/image", Image, self.depthimage_callback) self.depthimage_subscriber = rospy.Subscriber( "/head_camera/depth/image", Image, self.rgbimage_callback) self.reset_subscriber = rospy.Subscriber("/fetch_ai_arm_reset_pos", Empty, self.arm_reset_callback) self.reset_subscriber = rospy.Subscriber("/fetch_ai_arm_start_pos", Empty, self.arm_start_callback) self.arm_effort_pub = rospy.Publisher( "/arm_controller/weightless_torque/command", JointTrajectory, queue_size=2) self.gripper_client = actionlib.SimpleActionClient( "gripper_controller/gripper_action", GripperCommandAction) self.arm_cartesian_twist_pub = rospy.Publisher( "/arm_controller/cartesian_twist/command", Twist, queue_size=2) self.head_point_client = actionlib.SimpleActionClient( "head_controller/point_head", PointHeadAction) self.arm_move_group = MoveGroupInterface("arm", "base_link", plan_only=self.plan_only) planning_scene = PlanningSceneInterface("base_link") planning_scene.removeCollisionObject("my_front_ground") planning_scene.removeCollisionObject("my_back_ground") planning_scene.removeCollisionObject("my_right_ground") planning_scene.removeCollisionObject("my_left_ground") planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
def run(self): move_thread = None if not is_moveit_running(): rospy.loginfo("starting moveit") move_thread = MoveItThread() else: rospy.loginfo("moveit already started") rospy.loginfo("Waiting for MoveIt...") self.client = MoveGroupInterface("arm_with_torso", "base_link") rospy.loginfo("...connected") # Padding does not work (especially for self collisions) # So we are adding a box above the base of the robot scene = PlanningSceneInterface("base_link") keepout_pose = Pose() keepout_pose.position.z = 0.375 keepout_pose.orientation.w = 1.0 ground_pose = Pose() ground_pose.position.z = -0.03 ground_pose.orientation.w = 1.0 rospack = rospkg.RosPack() mesh_dir = os.path.join(rospack.get_path('fetch_teleop'), 'mesh') scene.addMesh('keepout', keepout_pose, os.path.join(mesh_dir, 'keepout.stl')) scene.addMesh('ground', ground_pose, os.path.join(mesh_dir, 'ground.stl')) joints = [ "torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = self.client.moveToJointPosition( joints, pose, 0.0, max_velocity_scaling_factor=0.5) if result and result.error_code.val == MoveItErrorCodes.SUCCESS: scene.removeCollisionObject("keepout") scene.removeCollisionObject("ground") if move_thread: move_thread.stop() # On success quit # Stopping the MoveIt thread works, however, the action client # does not get shut down, and future tucks will not work. # As a work-around, we die and roslaunch will respawn us. rospy.signal_shutdown("done") sys.exit(0) return
def __init__(self): self.joint_names = ["l_gripper_finger_joint", "r_gripper_finger_joint"] self.client = actionlib.SimpleActionClient("gripper_controller/gripper_action", GripperCommandAction) rospy.loginfo("Waiting for gripper_controller...") self.client.wait_for_server() self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True, plan_only=True) find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server()
def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("left_arm", "left_hand", verbose = True) self.move_group = MoveGroupInterface("left_arm", "base_link") find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server(rospy.Duration(15.0)) self.cubes = [] self.tf_listener = TransformListener() # self.gripper_client = actionlib.SimpleActionClient("/robot/end_effector/left_gripper/gripper_action", GripperCommandAction) # self.gripper_client.wait_for_server() # rospy.loginfo("...connected.") rospy.sleep(2.0)
def clean_up(self, close=False): if close: # get the actual list after game self.planning_scene_interface = PlanningSceneInterface( REFERENCE_FRAME) for name in self.planning_scene_interface.getKnownCollisionObjects(): if self.DEBUG is 1: print("[DEBUG] Removing object %s" % name) self.planning_scene_interface.removeCollisionObject(name, False) for name in self.planning_scene_interface.getKnownAttachedObjects(): if self.DEBUG is 1: print("[DEBUG] Removing attached object %s" % name) self.planning_scene_interface.removeAttachedObject(name, False) if close: self.planning_scene_interface.waitForSync(5.0) pass
def __init__(self, tower_array, disk_array, config, debug=0): print "[INFO] Initialise World" self.DEBUG = debug # initialise arrays self.tower_array = tower_array self.disk_array = disk_array # configs self.max_gripper = config.get_max_gripper() self.disk_height = config.disk_height self.tower_positions = config.tower_positions self.planning_scene_interface = PlanningSceneInterface(REFERENCE_FRAME) # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
def __init__(self): self.planning_scene = PlanningSceneInterface("map") self.tf_broacaster = tf.TransformBroadcaster() self.tf_listener = tf.TransformListener() self.pub = rospy.Subscriber('/gazebo/link_states', LinkStates, self.simulated_link_state_callback, queue_size=1) self.update_planning_scene = True self.last_update = rospy.Time.now() self.update_period = rospy.Duration(10) self.table_collision = True self.cube_collision = False
def __init__(self): # Create move group interface for a fetch robot self._move_group = MoveGroupInterface("arm_with_torso", "base_link") # Define ground plane # This creates objects in the planning scene that mimic the ground # If these were not in place gripper could hit the ground self._planning_scene = PlanningSceneInterface("base_link") self._planning_scene.removeCollisionObject("my_front_ground") self._planning_scene.removeCollisionObject("my_back_ground") self._planning_scene.removeCollisionObject("my_right_ground") self._planning_scene.removeCollisionObject("my_left_ground") self._planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) self._planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) self._planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) self._planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
def setBoundaries(): ''' This is a fix for the FETCH colliding with itself Define ground plane This creates objects in the planning scene that mimic the ground If these were not in place gripper could hit the ground ''' planning_scene = PlanningSceneInterface("base_link") planning_scene.removeCollisionObject("my_front_ground") planning_scene.removeCollisionObject("my_back_ground") planning_scene.removeCollisionObject("my_right_ground") planning_scene.removeCollisionObject("my_left_ground") planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)