Python PlanningSceneInterface.add_box方法代码示例

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import add_box [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 add_box [as 别名]

#.........这里部分代码省略.........
        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]
        target_pose.pose.position.z += 0.025
        # Add the target object to the scene
        scene.add_box(target_id, target_pose, target_size)

        table_pose = PoseStamped()
        table_pose.header.frame_id = REFERENCE_FRAME
        table_pose.pose = self.pwh.pose[self.tid]
        table_pose.pose.position.z += 1
        scene.add_box(table_id, table_pose, table_size)
        
        #obstacle1_pose = PoseStamped()
        #obstacle1_pose.header.frame_id = REFERENCE_FRAME
        #obstacle1_pose.pose = self.pwh.pose[self.o1id]
        ## Add the target object to the scene
        #scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)

        # Specify a pose to place the target after being picked up
        place_pose = PoseStamped()
        place_pose.header.frame_id = REFERENCE_FRAME
        place_pose.pose.position.x = 0.50
        place_pose.pose.position.y = -0.30
        place_pose.pose.orientation.w = 1.0

        # Add the target object to the scene
        scene.add_box(target_id, target_pose, target_size)
                
        ### Make the target purple ###
        self.setColor(target_id, 0.6, 0, 1, 1.0)

        # Send the colors to the planning scene
        self.sendColors()
        
        #print target_pose
        self.object_frames_pub.publish(target_pose)
        rospy.sleep(2)

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

#.........这里部分代码省略.........
        # Pick Coke can 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')

        # Place Coke can object on another place on the support surface (table):
        while not self._place(self._arm_group, self._grasp_object_name, self._pose_place):
            rospy.logwarn('Place failed! Retrying ...')
            rospy.sleep(1.0)

        rospy.loginfo('Place successfully')

    def __del__(self):
        # Clean the scene:
        self._scene.remove_world_object(self._grasp_object_name)
        self._scene.remove_world_object(self._table_object_name)

    def _add_table(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.5
        p.pose.position.y = 0.0
        p.pose.position.z = 0.22

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

        # Table size from ~/.gazebo/models/table/model.sdf, using the values
        # for the surface link.
        self._scene.add_box(name, p, (0.5, 0.4, 0.02))

        return p.pose

    def _add_coke_can(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.25   
        p.pose.position.y = 0
        p.pose.position.z = 0.32

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

        # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
        # using the measure tape tool from meshlab.
        # The box is the bounding box of the coke cylinder.
        # The values are taken from the cylinder base diameter and height.
        self._scene.add_box(name, p, (0.01, 0.01, 0.009))

        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 add_box [as 别名]
 
 rospy.sleep(1)  
 
 # create a pose
 p = PoseStamped()
 p.header.frame_id = "world" #robot.get_planning_frame()
 p.pose.position.x = 0
 p.pose.position.y = 0
 p.pose.position.z = -0.05
 p.pose.orientation.x = 0
 p.pose.orientation.y = 0
 p.pose.orientation.z = 0
 p.pose.orientation.w = 1
 
 # add a box there
 scene.add_box("ground", p, (3, 3, 0.02))
 
 # access some meshes
 rospack = rospkg.RosPack()
 resourcepath = rospack.get_path('regrasping_app')+"/../resources/"
 
 # modify the pose
 p.pose.position.x = 0.7
 p.pose.position.y = 0.7
 p.pose.position.z = 0.0
 # add the cup
 scene.add_mesh("cup",p,resourcepath+'objects/cup.dae')
 
 # modify the pose
 p.pose.position.x = 0.72
 p.pose.position.z = 0.05

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

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

            rospy.logdebug("in AS: Closest cluster id is: " + str(closest_cluster_id))
            #TODO visualize bbox
            #TODO publish filtered pointcloud?
            rospy.logdebug("BBOX: " + str(obj_bbox_dims))
            ########
            self.update_feedback("Check reachability")
            ########
            self.update_feedback("Generating grasps")
            rospy.logdebug("Object pose before tf thing is: " + str(object_pose))
            #transform pose to base_link, IS THIS NECESSARY?? should be a function in any case
            self.tf_listener.waitForTransform("base_link", object_pose.header.frame_id, object_pose.header.stamp, rospy.Duration(15))
            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
            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():

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import add_box [as 别名]
    # 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))

    p.pose.position.x = 0
    p.pose.position.y = 0.8
    p.pose.position.z = 0.1
    p.pose.orientation.w = 1.0
    scene.add_sphere("ball", p, 0.1)

    p.pose.position.x = 0
    p.pose.position.y = 0
    p.pose.position.z = -0.1
    scene.add_box("table", p, (1.0, 2.6, 0.2))

    p.pose.position.x = 0
    p.pose.position.y = 0
    p.pose.position.z = -0.4

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import add_box [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 add_box [as 别名]
if __name__ == '__main__':
	rospy.init_node('scripting_example')
	while rospy.get_time() == 0.0: pass
	
	### Create a handle for the Planning Scene Interface
	psi = PlanningSceneInterface()
	rospy.sleep(1.0)
	
	### Create a handle for the Move Group Commander
	mgc = MoveGroupCommander("arm")
	rospy.sleep(1.0)
	
	
	### Add virtual obstacle
	pose = gen_pose(pos=[-0.2, -0.1, 1.2])
	psi.add_box("box", pose, size=(0.15, 0.15, 0.6))
	rospy.sleep(1.0)
	
	### Move to stored joint position
	mgc.set_named_target("left")
	mgc.go()
	
	### Move to Cartesian position
	goal_pose = gen_pose(pos=[0.123, -0.417, 1.361], euler=[3.1415, 0.0, 1.5707])
	mgc.go(goal_pose.pose)
	
	### Move Cartesian linear
	goal_pose.pose.position.z -= 0.1
	(traj,frac) = mgc.compute_cartesian_path([goal_pose.pose], 0.01, 4, True)
	mgc.execute(traj)
	

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import add_box [as 别名]
    scene = PlanningSceneInterface()
    rospy.sleep(1)

    # publish a demo scene
    p = PoseStamped()
    p.header.frame_id = REFERENCE_LINK
    p.header.stamp = rospy.Time.now()

    quat = quaternion_from_euler(0.0, 0.0, 0.0)  # roll, pitch, yaw
    p.pose.orientation = Quaternion(*quat)

    p.pose.position.x = 0.6
    p.pose.position.y = 0.0
    p.pose.position.z = 0.0
    # add table
    scene.add_box(TABLE_OBJECT, p, (0.5, 0.5, 0.2))

    i = 1
    while i <= 10:

        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)

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import add_box [as 别名]
 
 rospy.loginfo("Cleaning world objects")
 # clean the scene
 scene.remove_world_object("table")
 scene.remove_world_object("part")
 
 # publish a demo scene
 p = PoseStamped()
 p.header.frame_id = '/base_link'
 p.header.stamp = rospy.Time.now()
 
 p.pose.position.x = 0.6
 p.pose.position.y = 0.0    
 p.pose.position.z = 0.65
 p.pose.orientation.w = 1.0
 scene.add_box("table", p, (0.5, 1.5, 0.9))
 p.pose.position.x = 0.4
 p.pose.position.y = -0.3
 p.pose.position.z = 1.15
 
 angle = radians(80) # angles are expressed in radians
 quat = quaternion_from_euler(0.0, 0.0, angle) # roll, pitch, yaw
 p.pose.orientation = Quaternion(*quat.tolist())
 
 
 #scene.add_box("part", p, (0.04, 0.04, 0.1))
 scene.add_box("part", p, (0.03, 0.03, 0.1))
 rospy.loginfo("Added object to world")
 
    
 rospy.sleep(1)    

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import add_box [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 add_box [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 add_box [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 add_box [as 别名]

#.........这里部分代码省略.........
    def open_gripper(self):

        g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100))
        self.ac.send_goal(g_open)
        self.ac.wait_for_result()
        rospy.sleep(5) # And up to 20 to detach it


    def scene_generator(self):
#        print obj_att
        global target_pose
        global target_id
        global target_size
        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')


        # 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]
        target_pose.pose.position.z += 0.025
        # Add the target object to the scene
        if obj_att is None:
            self.scene.add_box(target_id, target_pose, target_size)

            table_pose = PoseStamped()
            table_pose.header.frame_id = REFERENCE_FRAME
            table_pose.pose = self.pwh.pose[self.tid]
            table_pose.pose.position.z += 1
            self.scene.add_box(table_id, table_pose, table_size)
            
            #obstacle1_pose = PoseStamped()
            #obstacle1_pose.header.frame_id = REFERENCE_FRAME
            #obstacle1_pose.pose = self.pwh.pose[self.o1id]
            ## Add the target object to the scene
            #scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)

            # Specify a pose to place the target after being picked up
            place_pose = PoseStamped()
            place_pose.header.frame_id = REFERENCE_FRAME
            place_pose.pose.position.x = 0.50
            place_pose.pose.position.y = -0.30
            place_pose.pose.orientation.w = 1.0

            # Add the target object to the scene
            self.scene.add_box(target_id, target_pose, target_size)
                    
            ### Make the target purple ###
            self.setColor(target_id, 0.6, 0, 1, 1.0)

            # Send the colors to the planning scene
            self.sendColors()
        else: 
            self.scene.remove_world_object('target')
        # Publish targe's frame
        #self.object_frames_pub.publish(target_pose)

# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import add_box [as 别名]
    # p.pose.position.x = 0.8
    # p.pose.position.y = -0.15
    # p.pose.position.z = 0.85
    # p.pose.orientation.w = 1.0
    #scene.add_box("pole", p, (0.3, 0.05, 1.0))

    # p.pose.position.x = 0.79
    # p.pose.position.y = 0.15
    # p.pose.position.z = 0.70
    # add_result = scene.add_box("table", p, (0.6, 0.6, 0.15))
    # print add_result
    
    p.pose.position.x = 0.6
    p.pose.position.y = 0.18
    p.pose.position.z = 0.7
    scene.add_box("part1", p, (0.05, 0.05, 0.1 ))

    robot.right_arm.set_named_target("right_pregrasp") 
    robot.right_arm.go()
    rospy.sleep(3)

    gripper.gripper_open()
    rospy.sleep(3)
    # pick an object
    robot.right_arm.pick("part1")
    rospy.sleep(1)

    gripper.gripper_close()
    rospy.sleep(1)

    robot.right_arm.set_named_target("right_normal")