def __init__(self):
        self.current_diff = SetPlanningSceneDiffRequest()
        self.set_scene = rospy.ServiceProxy('/environment_server/set_planning_scene_diff', SetPlanningSceneDiff)
        rospy.loginfo('Waiting for planning scene service')
        self.set_scene.wait_for_service()
        self._get_world_robot = rospy.ServiceProxy('/environment_server/get_robot_state', GetRobotState)
        rospy.loginfo('Waiting for get robot state service')
        self._get_world_robot.wait_for_service()
        
        self.transformer = TransformState()

        arms = ['arm'] #could somehow get these off the parameter server I guess
        self.hands = {}
        #this is unfortunately necessary for dealing with attached objects
        for arm in arms:
            self.hands[arm] = HandDescription(arm)

        self.send_diff()
        self.world_frame = '/odom_combined'
        self.robot_frame = '/base_footprint'
        rs = self.get_robot_state()

        if rs.multi_dof_joint_state.frame_ids:
            self.world_frame = rs.multi_dof_joint_state.frame_ids[0]
            self.robot_frame = rs.multi_dof_joint_state.child_frame_ids[0]
        rospy.loginfo('Frame '+str(self.world_frame)+' is world frame and '+
                      str(self.robot_frame)+' is robot root frame')
        rospy.loginfo('Planning scene interface created')
Beispiel #2
0
    def __init__(self, arm_name):
        '''
        Constructor for ArmPlanner.

        **Args:**
            
            **arm_name (string):** The arm ('left_arm' or 'right_arm') for which to plan
        '''
        rospy.loginfo("Creating arm planner...")

        #public variables
        self.arm_name = arm_name
        self.hand = HandDescription(arm_name)

        self.kinematics_info_service = rospy.ServiceProxy\
            ('/pr2_'+self.arm_name+ '_kinematics/get_ik_solver_info', GetKinematicSolverInfo)
        rospy.loginfo('Waiting for kinematics solver info service')
        self.kinematics_info_service.wait_for_service()

        self.get_state_validity_service = rospy.ServiceProxy(
            '/planning_scene_validity_server/get_state_validity',
            GetStateValidity)
        rospy.loginfo('Waiting for state validity service.')
        self.get_state_validity_service.wait_for_service()

        #find the joint names
        info = self.kinematics_info_service()
        self.joint_names = info.kinematic_solver_info.joint_names
        self.joint_limits = info.kinematic_solver_info.limits
        for l in range(len(self.joint_limits)):
            #for some reason this isn't filled in on return
            self.joint_limits[l].joint_name = self.joint_names[l]

        #private variables
        self._psi = get_planning_scene_interface()
        self._transformer = TransformState()

        #services (private)
        self._move_arm_planner = rospy.ServiceProxy(PLANNER_NAME,
                                                    GetMotionPlan)
        rospy.loginfo('Waiting for move arm planner')
        self._move_arm_planner.wait_for_service()
        self._interpolated_ik_planning_service = rospy.ServiceProxy\
            ('/'+self.arm_name[0]+'_interpolated_ik_motion_plan', GetMotionPlan)
        rospy.loginfo('Waiting for interpolated IK planning service')
        self._interpolated_ik_planning_service.wait_for_service()
        self._interpolated_ik_parameter_service =rospy.ServiceProxy\
            ('/'+self.arm_name[0]+'_interpolated_ik_motion_plan_set_params', SetInterpolatedIKMotionPlanParams)
        rospy.loginfo('Waiting for interpolated IK parameter setting service')
        self._interpolated_ik_parameter_service.wait_for_service()
        self._filter_trajectory_with_constraints_service = rospy.ServiceProxy\
            ('/trajectory_filter_server/filter_trajectory_with_constraints', FilterJointTrajectoryWithConstraints)
        rospy.loginfo('Waiting for trajectory filter with constraints service')
        self._filter_trajectory_with_constraints_service.wait_for_service()
        self._collision_aware_ik_service = rospy.ServiceProxy\
            ('/pr2_'+self.arm_name+'_kinematics/get_constraint_aware_ik', GetConstraintAwarePositionIK)
        rospy.loginfo('Waiting for collision aware IK service')
        self._collision_aware_ik_service.wait_for_service()
        self._ik_service = rospy.ServiceProxy(
            '/pr2_' + self.arm_name + '_kinematics/get_ik', GetPositionIK)
        rospy.loginfo('Waiting for IK service')
        self._ik_service.wait_for_service()
        self._fk_service = rospy.ServiceProxy(
            '/pr2_' + self.arm_name + '_kinematics/get_fk', GetPositionFK)
        rospy.loginfo('Waiting for FK service')
        self._fk_service.wait_for_service()

        database_grasp_planner_name = rospy.get_param(
            '/object_manipulator_1/default_database_planner',
            DEFAULT_DATABASE_GRASP_PLANNER)
        print 'default database grasp planner'
        print database_grasp_planner_name
        self.database_grasp_planner = rospy.ServiceProxy(
            database_grasp_planner_name, GraspPlanning)
        rospy.loginfo('Waiting for database grasp planner')
        self.database_grasp_planner.wait_for_service()

        self._grasps_pub = rospy.Publisher(GRASPS_VISUALIZATION_PUB,
                                           MarkerArray,
                                           latch=True)

        rospy.loginfo("Arm planner created")