Python PlanningSceneInterface.remove_world_object方法代码示例

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_world_object [as 别名]
def main():
    rospy.init_node('moveit_py_place', anonymous=True)
    #right_arm.set_planner_id("KPIECEkConfigDefault");
    scene = PlanningSceneInterface()
    robot = RobotCommander()
    #group = MoveGroupCommander("head")
    right_arm = MoveGroupCommander("right_arm")
    #right_arm.set_planner_id("KPIECEkConfigDefault");
    rospy.logwarn("cleaning world")
    GRIPPER_FRAME = 'gripper_bracket_f2'
    #scene.remove_world_object("table")
    scene.remove_world_object("part")
    scene.remove_attached_object(GRIPPER_FRAME, "part")
    p = PoseStamped()
    p.header.frame_id = robot.get_planning_frame()

    p.pose.position.x = 0.67
    p.pose.position.y = -0.
    p.pose.position.z = 0.75
    scene.add_box("part", p, (0.07, 0.01, 0.2))

    # move to a random target
    #group.set_named_target("ahead")
    #group.go()
    #rospy.sleep(1)

    result = False
    n_attempts = 0
       
    # repeat until will succeed
    while result == False:
        result = robot.right_arm.pick("part")      
        n_attempts += 1
        print "Attempts pickup: ", n_attempts
        rospy.sleep(0.2)
    
    #robot.right_arm.pick("part")
    #right_arm.go()
    rospy.sleep(5)

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_world_object [as 别名]
    def __init__(self):

        roscpp_initialize(sys.argv)
        rospy.init_node('moveit_py_demo', anonymous=True)
      
        scene = PlanningSceneInterface()
        robot = RobotCommander()
       
        right_arm = MoveGroupCommander(GROUP_NAME_ARM)
        #right_arm.set_planner_id("KPIECEkConfigDefault");
        right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
       
        eef = right_arm.get_end_effector_link()
       
        rospy.sleep(2)
       
        scene.remove_attached_object(GRIPPER_FRAME, "part")

   
        # clean the scene
        scene.remove_world_object("table")
        scene.remove_world_object("part")
   
        #right_arm.set_named_target("r_start")
        #right_arm.go()
      
        #right_gripper.set_named_target("right_gripper_open")
        #right_gripper.go()
      
        rospy.sleep(1)
   
        # publish a demo scene
        p = PoseStamped()
        p.header.frame_id = robot.get_planning_frame()
   
        # add a table
        #p.pose.position.x = 0.42
        #p.pose.position.y = -0.2
        #p.pose.position.z = 0.3
        #scene.add_box("table", p, (0.5, 1.5, 0.6))

   
        # add an object to be grasped
        p.pose.position.x = 0.7
        p.pose.position.y = -0.2
        p.pose.position.z = 0.85
        scene.add_box("part", p, (0.07, 0.01, 0.2))
      
        rospy.sleep(1)
             
        g = Grasp()
        g.id = "test"
        start_pose = PoseStamped()
        start_pose.header.frame_id = FIXED_FRAME
   
        # start the gripper in a neutral pose part way to the target
        start_pose.pose.position.x = 0.47636
        start_pose.pose.position.y = -0.21886
        start_pose.pose.position.z = 0.9
        start_pose.pose.orientation.x = 0.00080331
        start_pose.pose.orientation.y = 0.001589
        start_pose.pose.orientation.z = -2.4165e-06
        start_pose.pose.orientation.w = 1
          
        right_arm.set_pose_target(start_pose)
        right_arm.go()
       
        rospy.sleep(2)

        # generate a list of grasps
        grasps = self.make_grasps(start_pose)
   
        result = False
        n_attempts = 0
       
        # repeat until will succeed
        while result == False:
            result = robot.right_arm.pick("part", grasps)      
            n_attempts += 1
            print "Attempts: ", n_attempts
            rospy.sleep(0.3)
          
        rospy.spin()
        roscpp_shutdown()

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_world_object [as 别名]

#.........这里部分代码省略.........
                self.update_aborted("No grasps received")
                return
            if DEBUG_MODE:  # TODO: change to dynamic param
                publish_grasps_as_poses(grasp_list)
            self.current_goal.publish_feedback(self.as_feedback)
            ########
            self.update_feedback("Setup planning scene")
            # Add object to grasp to planning scene
            self.scene.add_box(self.current_side + "_hand_object", object_pose,
                               (obj_bbox_dims[0], obj_bbox_dims[1], obj_bbox_dims[2]))
            self.as_result.object_scene_name = self.current_side + "_hand_object"
            ########
            self.update_feedback("Execute grasps")
            pug = createPickupGoal(self.current_side + "_hand_object", grasp_list, group=goal_message_field.group)
            rospy.loginfo("Sending pickup goal")
            self.pickup_ac.send_goal(pug)
            rospy.loginfo("Waiting for result...")
            self.pickup_ac.wait_for_result()
            result = self.pickup_ac.get_result()
            rospy.loginfo("Human readable error: " + str(moveit_error_dict[result.error_code.val]))
            ########
            self.update_feedback("finished")
            self.as_result.object_pose = object_pose
            self.as_result.error_code = result.error_code
            self.as_result.error_message = str(moveit_error_dict[result.error_code.val])
            if result.error_code.val == MoveItErrorCodes.SUCCESS:
                if self.current_side == 'right':
                    self.right_hand_object = self.current_side + "_hand_object"
                elif self.current_side == 'left':
                    self.left_hand_object = self.current_side + "_hand_object"
                self.current_goal.set_succeeded(result=self.as_result)
            else:
                # Remove object as it has not been picked
                self.scene.remove_world_object(self.current_side + "_hand_object")
                self.update_aborted(text="MoveIt pick failed", manipulation_result=self.as_result)
        else:
            self.update_aborted("Goal fields not correctly fulfilled")

    def place_operation(self):
        """Execute the place operation"""
        if self.message_fields_ok():
            self.as_result = ObjectManipulationResult()
            goal_message_field = self.current_goal.get_goal()
            self.update_feedback("Checking arm to use")
            # Check which arm group was requested and if it currently has an object
            if 'right' in goal_message_field.group:
                if not self.right_hand_object:  # Something already in the hand
                    self.update_aborted("Right hand does not contain an object.")
                    return  # necessary?
                self.current_side = 'right'
                current_target = self.right_hand_object
            elif 'left' in goal_message_field.group:
                if not self.left_hand_object:
                    self.update_aborted("Left hand does not contain an object.")
                    return
                self.current_side = 'left'
                current_target = self.left_hand_object

            # transform pose to base_link if needed
            if goal_message_field.target_pose.header.frame_id != "base_link":
                self.tf_listener.waitForTransform("base_link", goal_message_field.target_pose.header.frame_id, goal_message_field.target_pose.header.stamp, rospy.Duration(5))
                placing_pose = self.tf_listener.transformPose("base_link", goal_message_field.target_pose)
            else:
                placing_pose = goal_message_field.target_pose.pose
            ####  TODO: ADD HERE LOGIC ABOUT SEARCHING GOOD PLACE POSE ####
            self.update_feedback("Sending place order to MoveIt!")

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_world_object [as 别名]
    # specify the planner
    group.set_planner_id("RRTkConfigDefault")

    rospy.sleep(3)

    ## We create this DisplayTrajectory publisher which is used below to publish
    ## trajectories for RVIZ to visualize.

    # display_trajectory_publisher = rospy.Publisher(
    #                                   '/move_group/display_planned_path',
    #                                   moveit_msgs.msg.DisplayTrajectory)

    print ">>>>> remove scenes"

    # clean the scene
    scene.remove_world_object("pole")
    scene.remove_world_object("ball")
    scene.remove_world_object("table")
    scene.remove_world_object("ground_plane")
    rospy.sleep(5)

    print ">>>>> add scenes"
    # publish a demo scene
    p = PoseStamped()
    p.header.frame_id = robot.get_planning_frame()
    p.pose.position.x = 0
    p.pose.position.y = -1
    p.pose.position.z = 0.2
    p.pose.orientation.w = 1.0
    scene.add_box("pole", p, (0.4, 0.1, 0.4))

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_world_object [as 别名]
class GraspObjectServer:

    def __init__(self, name):
        # stuff for grasp planning
        self.tf_listener = tf.TransformListener()
        self.tf_broadcaster = tf.TransformBroadcaster()
        self.cbbf = ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster, "base_link")
        self.last_objects = RecognizedObjectArray()
        #rospy.Subscriber("object_array", RecognizedObjectArray, self.objects_callback)
        self.sub = rospy.Subscriber("/recognized_object_array", RecognizedObjectArray, self.objects_callback)
        self.grasp_publisher = rospy.Publisher("generated_grasps", PoseArray)
        
        rospy.loginfo("Connecting to pickup AS")
        self.pickup_ac = SimpleActionClient('/pickup', PickupAction)
        #pickup_ac.wait_for_server() # needed?
        
        rospy.loginfo("Connecting to grasp generator AS")
        self.grasps_ac = SimpleActionClient('/grasp_generator_server/generate', GenerateBlockGraspsAction)
        #grasps_ac.wait_for_server() # needed? 
        
        #planning scene for motion planning
        self.scene = PlanningSceneInterface()
        
        # blocking action server
        self.grasp_obj_as = ActionServer(name, GraspObjectAction, self.goal_callback, self.cancel_callback, False)
        self.feedback = GraspObjectFeedback()
        self.result = GraspObjectResult()
        self.current_goal = None
        self.grasp_obj_as.start()

    def objects_callback(self, data):
        rospy.loginfo(rospy.get_name() + ": This message contains %d objects." % len(data.objects))
        self.last_objects = data
        
    def goal_callback(self, goal):      
        if self.current_goal:
          goal.set_rejected() # "Server busy"
          return
        elif len(self.last_objects.objects) - 1 < goal.get_goal().target_id:
          goal.set_rejected() # "No objects to grasp were received on the objects topic."
          return
        else:
          #store and accept new goal
          self.current_goal = goal
          self.current_goal.set_accepted()
          #run grasping state machine
          self.grasping_sm()
          #finished, get rid of goal
          self.current_goal = None
        
    def cancel_callback(self, goal):
        #TODO stop motions?
        self.current_goal.set_canceled()

    def grasping_sm(self):
      if self.current_goal:
        self.update_feedback("running clustering")
        (object_points, obj_bbox_dims, 
         object_bounding_box, object_pose) = self.cbbf.find_object_frame_and_bounding_box(self.last_objects.objects[self.current_goal.get_goal().target_id].point_clouds[0])
        #TODO visualize bbox
        #TODO publish filtered pointcloud?
        print obj_bbox_dims
        ########
        self.update_feedback("check reachability")
        ########
        self.update_feedback("generate grasps")
        #transform pose to base_link
        self.tf_listener.waitForTransform("base_link", object_pose.header.frame_id, object_pose.header.stamp, rospy.Duration(5))
        trans_pose = self.tf_listener.transformPose("base_link", object_pose)
        object_pose = trans_pose
	      #HACK remove orientation -> pose is aligned with parent(base_link)
        object_pose.pose.orientation.w = 1.0
        object_pose.pose.orientation.x = 0.0
        object_pose.pose.orientation.y = 0.0
        object_pose.pose.orientation.z = 0.0
        # shift object pose up by halfway, clustering code gives obj frame on the bottom because of too much noise on the table cropping (2 1pixel lines behind the objects)
        # TODO remove this hack, fix it in table filtering
        object_pose.pose.position.z += obj_bbox_dims[2]/2.0
        grasp_list = self.generate_grasps(object_pose, obj_bbox_dims[0]) # width is the bbox size on x
        # check if there are grasps, if not, abort
        if len(grasp_list) == 0:
          self.update_aborted("no grasps received")
          return
        self.publish_grasps_as_poses(grasp_list)
        self.feedback.grasps = grasp_list
        self.current_goal.publish_feedback(self.feedback)
        self.result.grasps = grasp_list
        ########
        self.update_feedback("setup planning scene")
        #remove old objects
        self.scene.remove_world_object("object_to_grasp")
        # add object to grasp to planning scene      
        self.scene.add_box("object_to_grasp", object_pose, 
                           (obj_bbox_dims[0], obj_bbox_dims[1], obj_bbox_dims[2]))
        self.result.object_scene_name = "object_to_grasp"
        ########
        if self.current_goal.get_goal().execute_grasp:
          self.update_feedback("execute grasps")
          pug = self.createPickupGoal("object_to_grasp", grasp_list)
          rospy.loginfo("Sending goal")
#.........这里部分代码省略.........

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_world_object [as 别名]
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        rospy.init_node('moveit_obstacles_demo')

        # Construct the initial scene object
        scene = PlanningSceneInterface()

        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5)

        # Create a dictionary to hold object colors
        self.colors = dict()

        # Pause for the scene to get ready
        rospy.sleep(1)

        # Initialize the move group for the right arm
        arm = MoveGroupCommander(GROUP_NAME_ARM)

        # Get the name of the end-effector link
        end_effector_link = arm.get_end_effector_link()

        # Allow some leeway in position (meters) and orientation (radians)
        arm.set_goal_position_tolerance(0.01)
        arm.set_goal_orientation_tolerance(0.05)

        # Allow replanning to increase the odds of a solution
        arm.allow_replanning(True)

        # Set the right arm reference frame accordingly
        arm.set_pose_reference_frame(REFERENCE_FRAME)

        # Allow 5 seconds per planning attempt
        arm.set_planning_time(5)

        # Give each of the scene objects a unique name
        table_id = 'table'
        box1_id = 'box1'
        box2_id = 'box2'

        # Remove leftover objects from a previous run
        scene.remove_world_object(table_id)
        scene.remove_world_object(box1_id)
        scene.remove_world_object(box2_id)

        # Give the scene a chance to catch up
        rospy.sleep(1)

        # Start the arm in the "resting" pose stored in the SRDF file
        arm.set_named_target('l_arm_init')
        arm.go()

        rospy.sleep(2)

        # Set the height of the table off the ground
        table_ground = 0.65

        # Set the length, width and height of the table and boxes
        table_size = [0.2, 0.7, 0.01]
        box1_size = [0.1, 0.05, 0.05]
        box2_size = [0.05, 0.05, 0.15]

        # Add a table top and two boxes to the scene
        table_pose = PoseStamped()
        table_pose.header.frame_id = REFERENCE_FRAME
        table_pose.pose.position.x = 0.35
        table_pose.pose.position.y = 0.0
        table_pose.pose.position.z = table_ground + table_size[2] / 2.0
        table_pose.pose.orientation.w = 1.0
        scene.add_box(table_id, table_pose, table_size)

        box1_pose = PoseStamped()
        box1_pose.header.frame_id = REFERENCE_FRAME
        box1_pose.pose.position.x = 0.3
        box1_pose.pose.position.y = 0
        box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
        box1_pose.pose.orientation.w = 1.0
        scene.add_box(box1_id, box1_pose, box1_size)

        box2_pose = PoseStamped()
        box2_pose.header.frame_id = REFERENCE_FRAME
        box2_pose.pose.position.x = 0.3
        box2_pose.pose.position.y = 0.25
        box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
        box2_pose.pose.orientation.w = 1.0
        scene.add_box(box2_id, box2_pose, box2_size)

        # Make the table red and the boxes orange
        self.setColor(table_id, 0.8, 0, 0, 1.0)
        self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
        self.setColor(box2_id, 0.8, 0.4, 0, 1.0)

        # Send the colors to the planning scene
        self.sendColors()

        # Set the target pose in between the boxes and above the table
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
#.........这里部分代码省略.........

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_world_object [as 别名]
        gripper_pose = arm.get_current_pose(arm.get_end_effector_link())
        # part
        p.pose.position.x = gripper_pose.pose.position.x
        p.pose.position.y = gripper_pose.pose.position.y
        p.pose.position.z = gripper_pose.pose.position.z
        # add part
        scene.add_box(PICK_OBJECT, p, (0.07, 0.07, 0.1))
        rospy.loginfo("Added object to world")

        # attach object manually
        arm.attach_object(PICK_OBJECT, arm.get_end_effector_link(), GRIPPER_JOINTS)
        rospy.sleep(1)

        #  ===== place start ==== #
        place_result = place(PLANNING_GROUP, PICK_OBJECT, generate_place_pose())

        rospy.loginfo("Place Result is:")
        rospy.loginfo("Human readable error: " + str(moveit_error_dict[place_result.error_code.val]))
        rospy.sleep(5)

        # remove part
        scene.remove_world_object(PICK_OBJECT)
        scene.remove_attached_object(arm.get_end_effector_link(), PICK_OBJECT)
        rospy.sleep(2)
        i += 1

    # end
    moveit_commander.roscpp_shutdown()
    moveit_commander.os._exit(0)

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_world_object [as 别名]
    print "============ Robot Groups:"
    print robot.get_group_names()
    print "============ Robot Links for arm:"
    print robot.get_link_names("arm")
    print "============ Robot Links for gripper:"
    print robot.get_link_names("gripper")
    print right_arm.get_end_effector_link()
    print "============ Printing robot state"
    #print robot.get_current_state()
    print "============"

    
    rospy.sleep(1)

    # clean the scene
    scene.remove_world_object("pole")
    scene.remove_world_object("table")
    #scene.remove_world_object("part")
    scene.remove_world_object("grasp_marker")
    # publish a demo scene
    p = PoseStamped()
    p.header.frame_id = robot.get_planning_frame()
    print p.header.frame_id
    p.pose.position.x = 0.4
    p.pose.position.y = -0.4
    p.pose.position.z = 0.85
    p.pose.orientation.w = 1.0
    #scene.add_box("pole", p, (0.3, 0.1, 1.0))

    p.pose.position.y = -0.2
    p.pose.position.z = 0.175

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_world_object [as 别名]
    def __init__(self):

        roscpp_initialize(sys.argv)
        rospy.init_node('moveit_py_demo', anonymous=True)
      
        scene = PlanningSceneInterface()
        robot = RobotCommander()
       
        right_arm = MoveGroupCommander("right_arm")
        right_gripper = MoveGroupCommander("right_gripper")
       
        eef = right_arm.get_end_effector_link()
       
        rospy.sleep(2)
       
        scene.remove_attached_object("right_gripper_link", "part")

   
        # clean the scene
        scene.remove_world_object("table")
        scene.remove_world_object("part")
   
        right_arm.set_named_target("start1")
        right_arm.go()
      
        right_gripper.set_named_target("open")
        right_gripper.go()
      
        rospy.sleep(1)
   
        # publish a demo scene
        p = PoseStamped()
        p.header.frame_id = robot.get_planning_frame()
   
        # add a table
        #p.pose.position.x = 0.42
        #p.pose.position.y = -0.2
        #p.pose.position.z = 0.3
        #scene.add_box("table", p, (0.5, 1.5, 0.6))

   
        # add an object to be grasped
        p.pose.position.x = 0.15

        p.pose.position.y = -0.12
        p.pose.position.z = 0.7
        scene.add_box("part", p, (0.07, 0.01, 0.2))
      
        rospy.sleep(1)
             
        g = Grasp()
        g.id = "test"
        start_pose = PoseStamped()
        start_pose.header.frame_id = FIXED_FRAME
   
        # start the gripper in a neutral pose part way to the target
        start_pose.pose.position.x = 0.0130178
        start_pose.pose.position.y = -0.125155
        start_pose.pose.position.z = 0.597653
        start_pose.pose.orientation.x = 0.0
        start_pose.pose.orientation.y = 0.388109
        start_pose.pose.orientation.z = 0.0
        start_pose.pose.orientation.w = 0.921613
          
        right_arm.set_pose_target(start_pose)
        right_arm.go()
       
        rospy.sleep(2)

        # generate a list of grasps
        #grasps = self.make_grasps(start_pose)
   
        #result = False
        #n_attempts = 0
       
        # repeat until will succeed
        #while result == False:
            #result = robot.right_arm.pick("part", grasps)      
            #n_attempts += 1
            #print "Attempts: ", n_attempts
            #rospy.sleep(0.2)
          
        rospy.spin()
        roscpp_shutdown()

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_world_object [as 别名]
if __name__ == '__main__':
    
  
    roscpp_initialize(sys.argv)
    rospy.init_node('moveit_py_demo', anonymous=True)
    print "Esperando al servicio ... grasp_service ... from hermes_grasp_service"
    rospy.wait_for_service('grasp_service')
    
    scene = PlanningSceneInterface()
    robot = RobotCommander()
    pose = PoseStamped()
    
    rospy.sleep(1)
   
    # clean the scene
    scene.remove_world_object("right_shoe")
    
    # move_group
    group_right = MoveGroupCommander("r_arm")
    group_left = MoveGroupCommander("l_arm")
   
   
    #Get Current state
    state = robot.get_current_variable_values()
    
    # Link off contact
    links_off=[]
    links_off.append("r_thumb1")
    links_off.append("r_thumb2")
    links_off.append("r_thumb3")
    links_off.append("r_index1")

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_world_object [as 别名]
    def __init__(self):
        """
        Setup ROS communication between the Kinect and Baxter
        :return:
        """

        # Initialise variables
        self.rgb_img = None
        self.depth_img = None
        self.marker_box = None
        self.marker_center_x = None
        self.marker_center_y = None
        self.marker_depth = None

        self.bridge = CvBridge()

        # First initialize moveit_commander and rospy.
        print "============ Initialising Baxter"
        moveit_commander.roscpp_initialize(sys.argv)
        rospy.init_node('can_node', anonymous=True)

        self.robot = moveit_commander.RobotCommander()
        
        self.scene = moveit_commander.PlanningSceneInterface()
        self.left_arm = moveit_commander.MoveGroupCommander("left_arm")
        self.right_arm = moveit_commander.MoveGroupCommander("right_arm")
        self.right_arm_navigator = Navigator('right')
       
        
        # Setup grippers
        self.right_gripper = baxter_interface.Gripper('right')
        
        # Setup the table in the scene
        scene = PlanningSceneInterface()
        self.scene_pub = rospy.Publisher('/move_group/monitored_planning_scene', PlanningScene)
        table_id = 'table'
        scene.remove_world_object(table_id)
        rospy.sleep(1)
        table_ground = -0.3
        table_size = [4.0, 4.6, 0.1]
        
        table_pose = PoseStamped()
        table_pose.header.frame_id = self.robot.get_planning_frame()
        table_pose.pose.position.x = 0.7
        table_pose.pose.position.y = 0.0
        table_pose.pose.position.z = table_ground + table_size[2] / 2.0
        table_pose.pose.orientation.w = 1.0
        scene.add_box(table_id, table_pose, table_size)
        
        # Create a dictionary to hold object colors
        self.colors = dict()
        self.setColor(table_id, 0.8, 0, 0, 1.0)
        
        self.sendColors()

        self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
                                                            moveit_msgs.msg.DisplayTrajectory,
                                                            queue_size=10)

        # We can get a list of all the groups in the robot
        print "============ Right Pose:"
        print self.right_arm.get_current_pose()
        
        print "============ Left Pose:"
        print self.left_arm.get_current_pose()

        # Setup the subscribers and publishers
        self.rgb_sub = rospy.Subscriber("/camera/rgb/image_rect_color", Image, self.callback_rgb)
        self.points_sub = rospy.Subscriber("/camera/depth_registered/points", PointCloud2,  self.callback_points) 
        self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
                                                           moveit_msgs.msg.DisplayTrajectory,
                                                           queue_size=5)
        self.screen_pub = rospy.Publisher('robot/xdisplay', Image, latch=True, queue_size=10)
        
        self.right_hand_range_pub = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self.callback_range, queue_size=1)
        
        self.left_cam_sub = rospy.Subscriber("/cameras/left_hand_camera/image", Image, self.callback_left_hand)

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_world_object [as 别名]
class PigeonPickAndPlace:
    #Class constructor (parecido com java, inicializa o que foi instanciado)
    def __init__(self):
      
        # Retrieve params:
        
        self._grasp_object_name = rospy.get_param('~grasp_object_name', 'lego_block')
        
        self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.016)

        self._arm_group     = rospy.get_param('~arm', 'arm_move_group')
        self._gripper_group = rospy.get_param('~gripper', 'gripper')

        self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.01)
        self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.4)

        # Create (debugging) publishers:
        self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True)
        
        # Create planning scene where we will add the objects etc.
        self._scene = PlanningSceneInterface()
        # Create robot commander: interface to comand the manipulator programmatically (get the planning_frame for exemple
        self._robot = RobotCommander()

        rospy.sleep(1.0)

        # Clean the scene (remove the old objects:
        self._scene.remove_world_object(self._grasp_object_name)

        # Add table and Coke can objects to the planning scene:
        # TODO get the position of the detected object
        self._pose_object_grasp = self._add_object_grasp(self._grasp_object_name)

        rospy.sleep(1.0)

        # Retrieve groups (arm and gripper):
        self._arm     = self._robot.get_group(self._arm_group)
        self._gripper = self._robot.get_group(self._gripper_group)

        # Create grasp generator 'generate' action client:
        self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction)
        if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)):
            rospy.logerr('Grasp generator action client not available!')
            rospy.signal_shutdown('Grasp generator action client not available!')
            return

        # Create move group 'pickup' action client:
        self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
        if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)):
            rospy.logerr('Pick up action client not available!')
            rospy.signal_shutdown('Pick up action client not available!')
            return

        # Pick object:
        while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width):
            rospy.logwarn('Pick up failed! Retrying ...')
            rospy.sleep(1.0)

        rospy.loginfo('Pick up successfully')
        
    def __del__(self):
        # Clean the scene
        self._scene.remove_world_object(self._grasp_object_name)

    def _add_object_grasp(self, name):

        p = PoseStamped()
        
        p.header.frame_id = self._robot.get_planning_frame() #pose
        p.header.stamp = rospy.Time.now()
	rospy.loginfo(self._robot.get_planning_frame())
        p.pose.position.x = 0.11 # 0.26599 # antigo = 0.75 - 0.01
        p.pose.position.y = -0.31 # -0.030892 #antigo = 0.25 - 0.01
        p.pose.position.z = 0.41339 #antigo = 1.00 + (0.3 + 0.03) / 2.0
        #p.pose.orientation.x = 0.0028925
        #p.pose.orientation.y = -0.0019311
        #p.pose.orientation.z = -0.71058
        #p.pose.orientation.w = 0.70361

        q = quaternion_from_euler(0.0, 0.0, 0.0)
        p.pose.orientation = Quaternion(*q)

        # Coke can size from ~/.gazebo/models/coke_can/meshes/,
        # using the measure tape tool from meshlab.
        # The box is the bounding box of the lego cylinder.
        # The values are taken from the cylinder base diameter and height.
        # the size is length (x),thickness(y),height(z)
        # I don't know if the detector return this values of object
        self._scene.add_box(name, p, (0.032, 0.016, 0.020))

        return p.pose

    def _generate_grasps(self, pose, width):
        """
        Generate grasps by using the grasp generator generate action; based on
        server_test.py example on moveit_simple_grasps pkg.
        """

        # Create goal:
        goal = GenerateGraspsGoal()
#.........这里部分代码省略.........

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_world_object [as 别名]
class MoveItDemo:
    def __init__(self):

        global obj_att

        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        rospy.init_node('moveit_demo')
        
        #Initialize robot
        robot = moveit_commander.RobotCommander()

        # Use the planning scene object to add or remove objects
        self.scene = PlanningSceneInterface()

        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)

        # Create a publisher for displaying gripper poses
        self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
        
        # Create a publisher for displaying object frames
        self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10)

        ### Create a publisher for visualizing direction ###
        self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10)


        # Create a dictionary to hold object colors
        self.colors = dict()

        # Initialize the MoveIt! commander for the arm
        self.right_arm = MoveGroupCommander(GROUP_NAME_ARM)

        # Initialize the MoveIt! commander for the gripper
        self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)

        # Allow 5 seconds per planning attempt
        self.right_arm.set_planning_time(5)

        # Prepare Action Controller for gripper
        self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
        self.ac.wait_for_server()

        # Give the scene a chance to catch up
        rospy.sleep(2)
        
        # Prepare Gazebo Subscriber
        self.pwh = None
        self.pwh_copy = None
        self.idx_targ = None
        self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)


        ### OPEN THE GRIPPER ###
        self.open_gripper()


        # PREPARE THE SCENE
        while self.pwh is None:
            rospy.sleep(0.05)

        ############## CLEAR THE SCENE ################


#        planning_scene.world.collision_objects.remove('target')

        # Remove leftover objects from a previous run
        self.scene.remove_world_object('target')
        self.scene.remove_world_object('table')
#        self.scene.remove_world_object(obstacle1_id)

        # Remove any attached objects from a previous session
        self.scene.remove_attached_object(GRIPPER_FRAME, 'target')

        # Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c
        timerThread = threading.Thread(target=self.scene_generator)
        timerThread.daemon = True
        timerThread.start()

        initial_pose = PoseStamped()
        initial_pose.header.frame_id = 'gazebo_world'
        initial_pose.pose = target_pose.pose
        

        print "==================== Generating Transformations ==========================="

        #################### PRE GRASPING POSE #########################
        M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
        M1[0,3] = target_pose.pose.position.x
        M1[1,3] = target_pose.pose.position.y 
        M1[2,3] = target_pose.pose.position.z

        M2 = transformations.euler_matrix(0, 1.57, 0)
        M2[0,3] = 0.0  # offset about x
        M2[1,3] = 0.0   # about y
        M2[2,3] = 0.25  # about z

        T = np.dot(M1,  M2)
#.........这里部分代码省略.........

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_world_object [as 别名]
class BTMotion:

    def __init__(self, name):
        # create messages that are used to publish feedback/result
        self._feedback = amazon_challenge_bt_actions.msg.BTFeedback()
        self._result   = amazon_challenge_bt_actions.msg.BTResult()
        self._action_name = name
        self._as = actionlib.SimpleActionServer(self._action_name, amazon_challenge_bt_actions.msg.BTAction, execute_cb=self.execute_cb, auto_start = False)
        self.pub_posed = rospy.Publisher('arm_posed', String, queue_size=10)
        self.pub_rate = rospy.Rate(30)

        self._planning_scene = PlanningSceneInterface()


        # get ROS parameters
        rospy.loginfo('Getting parameters...')
        while not rospy.is_shutdown():
            try:
                self._base_move_params = rospy.get_param('/base_move')
                self._timeout = rospy.get_param(name + '/timeout')
                self._sim = rospy.get_param(name + '/sim')
                self._base_pos_dict = rospy.get_param('/base_pos_dict')
                self._left_arm_joint_pos_dict = rospy.get_param('/left_arm_joint_pos_dict')
                self._right_arm_joint_pos_dict = rospy.get_param('/right_arm_joint_pos_dict')
                break
            except:
                rospy.sleep(random.uniform(0,2))
                continue

        self._exit = False

        while not rospy.is_shutdown():
            try:
                self._robot = moveit_commander.RobotCommander()
                self._left_arm = self._robot.get_group('left_arm')
                self._right_arm = self._robot.get_group('right_arm')
                self._arms = self._robot.get_group('arms')
                self._torso = self._robot.get_group('torso')
                self._head = self._robot.get_group('head')
                self._arms_dict = {'left_arm': self._left_arm, 'right_arm': self._right_arm}
                break
            except:
                rospy.sleep(random.uniform(0,2))
                continue


        self._tf_listener = tf.TransformListener()

        self._next_task_sub = rospy.Subscriber("/amazon_next_task", String, self.get_task)
        self._shelf_pose_sub = rospy.Subscriber("/pubShelfSep", PoseStamped, self.get_shelf_pose)
        self._got_shelf_pose = False

        self._l_gripper_pub = rospy.Publisher('/l_gripper_controller/command', Pr2GripperCommand)
        while not rospy.is_shutdown():
            try:
                self._tool_size = rospy.get_param('/tool_size', [0.16, 0.02, 0.04])
                self._contest = rospy.get_param('/contest', True)
                break
            except:
                rospy.sleep(random.uniform(0,1))
                continue

        if self._contest:
            self._length_tool = 0.18 + self._tool_size[0]
        else:
            self._length_tool = 0.216 + self._tool_size[0]

        self._as.start()
        rospy.loginfo('['+rospy.get_name()+']: ready!')


    def get_shelf_pose(self, msg):
        self._shelf_pose = msg
        self._got_shelf_pose = True

    def get_bm_srv(self):
        while not rospy.is_shutdown():
            try:
                rospy.wait_for_service('/base_move_server/move', 5.0)
                rospy.wait_for_service('/base_move_server/preempt', 5.0)
                break
            except:
                rospy.loginfo('[' + rospy.get_name() + ']: waiting for base move server')
                continue

        self._bm_move_srv = rospy.ServiceProxy('/base_move_server/move', BaseMove)
        self._bm_preempt_srv = rospy.ServiceProxy('/base_move_server/preempt', Empty)

    def timer_callback(self, event):
        self._timer_started = True
        rospy.logerr('[' + rospy.get_name() + ']: TIMED OUT!')
        self._planning_scene.remove_attached_object('l_wrist_roll_link', 'grasped_object')
        self._planning_scene.remove_world_object('grasped_object')


        # pull the base back 60 cm

        self._left_arm.stop()
        self._right_arm.stop()

#.........这里部分代码省略.........

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import remove_world_object [as 别名]
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        rospy.init_node('moveit_demo')
        
        #Initialize robot
        robot = moveit_commander.RobotCommander()

        # Use the planning scene object to add or remove objects
        scene = PlanningSceneInterface()

        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)

        # Create a publisher for displaying gripper poses
        self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
        
        # Create a publisher for displaying object frames
        self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10)

        # Create a dictionary to hold object colors
        self.colors = dict()

        # Initialize the MoveIt! commander for the arm
        right_arm = MoveGroupCommander(GROUP_NAME_ARM)

        # Initialize the MoveIt! commander for the gripper
        right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)

        # Get the name of the end-effector link
        eef = right_arm.get_end_effector_link()

        # Allow some leeway in position (meters) and orientation (radians)
#        right_arm.set_goal_position_tolerance(0.05)
#        right_arm.set_goal_orientation_tolerance(0.1)

        # Allow replanning to increase the odds of a solution
        right_arm.allow_replanning(True)

        # Set the right arm reference frame
        right_arm.set_pose_reference_frame(REFERENCE_FRAME)

        # Allow 5 seconds per planning attempt
        right_arm.set_planning_time(5)

        # Set a limit on the number of pick attempts before bailing
        max_pick_attempts = 5

        # Set a limit on the number of place attempts
        max_place_attempts = 5

        # Give the scene a chance to catch up
        rospy.sleep(2)
        
        # Prepare Gazebo Subscriber
        self.pwh = None
        self.pwh_copy = None
        self.idx_targ = None
        self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)

        # Prepare Gripper and open it
        self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
        self.ac.wait_for_server()

        g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100))
        g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.0, 1))
        self.ac.send_goal(g_open)

        rospy.sleep(2)
        
        # PREPARE THE SCENE
       	
       	while self.pwh is None:
            rospy.sleep(0.05)

        target_id = 'target'
        self.taid = self.pwh.name.index('wood_cube_5cm')
        table_id = 'table'
        self.tid = self.pwh.name.index('table') 
        #obstacle1_id = 'obstacle1'
        #self.o1id = self.pwh.name.index('wood_block_10_2_1cm')

        # Remove leftover objects from a previous run
        scene.remove_world_object(target_id)
        scene.remove_world_object(table_id)
        #scene.remove_world_object(obstacle1_id)

        # Remove any attached objects from a previous session
        scene.remove_attached_object(GRIPPER_FRAME, target_id)

        # Set the target size [l, w, h]
        target_size = [0.05, 0.05, 0.05]
        table_size = [1.5, 0.8, 0.03]
        #obstacle1_size = [0.1, 0.025, 0.01]

        ## Set the target pose on the table
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose = self.pwh.pose[self.taid]
#.........这里部分代码省略.........