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')
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")
