00001
00002
00003 import rospy
00004
00005 from moveit_commander import RobotCommander
00006 from moveit_commander import PlanningSceneInterface
00007 from moveit_commander import MoveGroupCommander
00008 from moveit_msgs.srv import ListRobotStatesInWarehouse as ListStates
00009 from moveit_msgs.srv import GetRobotStateFromWarehouse as GetState
00010 from moveit_msgs.srv import CheckIfRobotStateExistsInWarehouse as HasState
00011 from moveit_msgs.srv import GetPositionFK
00012 from sr_robot_msgs.srv import PlanTrajectoryFromList
00013 from sr_robot_msgs.srv import PlanTrajectoryFromPrefix
00014 from sr_robot_msgs.srv import ExecutePlannedTrajectory
00015
00016 from std_msgs.msg import Header
00017 import geometry_msgs.msg
00018
00019
00020 class WarehousePlanner(object):
00021 def __init__(self):
00022 rospy.init_node('moveit_warehouse_planner', anonymous=True)
00023 self.scene = PlanningSceneInterface()
00024 self.robot = RobotCommander()
00025
00026 rospy.sleep(2)
00027 group_id = rospy.get_param("~arm_group_name")
00028 self.eef_step = rospy.get_param("~eef_step", 0.01)
00029 self.jump_threshold = rospy.get_param("~jump_threshold", 1000)
00030
00031 self.group = MoveGroupCommander(group_id)
00032 self._add_ground()
00033 self._robot_name = self.robot._r.get_robot_name()
00034 self._robot_link = self.group.get_end_effector_link()
00035 self._robot_frame = self.group.get_pose_reference_frame()
00036
00037 self._min_wp_fraction = rospy.get_param("~min_waypoint_fraction", 0.9)
00038
00039 rospy.sleep(4)
00040 rospy.loginfo("Waiting for warehouse services...")
00041 rospy.wait_for_service('moveit_warehouse_services/list_robot_states')
00042 rospy.wait_for_service('moveit_warehouse_services/get_robot_state')
00043 rospy.wait_for_service('moveit_warehouse_services/has_robot_state')
00044
00045 rospy.wait_for_service('/compute_fk')
00046 self._list_states = rospy.ServiceProxy(
00047 'moveit_warehouse_services/list_robot_states', ListStates)
00048 self._get_state = rospy.ServiceProxy(
00049 'moveit_warehouse_services/get_robot_state', GetState)
00050 self._has_state = rospy.ServiceProxy(
00051 'moveit_warehouse_services/has_robot_state', HasState)
00052 self._forward_k = rospy.ServiceProxy(
00053 'compute_fk', GetPositionFK)
00054 rospy.loginfo("Service proxies connected")
00055
00056 self._tr_frm_list_srv = rospy.Service('plan_trajectory_from_list',
00057 PlanTrajectoryFromList,
00058 self._plan_from_list_cb)
00059
00060 self._tr_frm_prfx_srv = rospy.Service('plan_trajectory_from_prefix',
00061 PlanTrajectoryFromPrefix,
00062 self._plan_from_prefix_cb)
00063
00064 self._execute_tr_srv = rospy.Service('execute_planned_trajectory',
00065 ExecutePlannedTrajectory,
00066 self._execute_plan_cb)
00067
00068 self.__plan = None
00069
00070 def get_waypoint_names_by_prefix(self, prefix):
00071 regex = "^" + str(prefix) + ".*"
00072 waypoint_names = self._list_states(regex, self._robot_name).states
00073 return waypoint_names
00074
00075 def get_pose_from_state(self, state):
00076 header = Header()
00077 fk_link_names = [self._robot_link]
00078 header.frame_id = self._robot_frame
00079 response = self._forward_k(header, fk_link_names, state)
00080 return response.pose_stamped[0].pose
00081
00082 def get_cartesian_waypoints(self, waypoint_names):
00083 waypoints = []
00084 waypoints.append(self.group.get_current_pose().pose)
00085 for name in waypoint_names:
00086 if self._has_state(name, self._robot_name).exists:
00087 robot_state = self._get_state(name, "").state
00088 waypoints.append(self.get_pose_from_state(robot_state))
00089 else:
00090 rospy.logerr("Waypoint %s doesn't exist for robot %s.", name,
00091 self._robot_name)
00092 return waypoints
00093
00094 def _add_ground(self):
00095 p = geometry_msgs.msg.PoseStamped()
00096 p.header.frame_id = self.robot.get_planning_frame()
00097
00098 p.pose.position.x = 0
00099 p.pose.position.y = 0
00100
00101 p.pose.position.z = -0.12
00102 p.pose.orientation.x = 0
00103 p.pose.orientation.y = 0
00104 p.pose.orientation.z = 0
00105 p.pose.orientation.w = 1
00106 self.scene.add_box("ground", p, (3, 3, 0.1))
00107 rospy.sleep(1)
00108
00109 def plan_from_filter(self, prefix):
00110 waypoint_names = self.get_waypoint_names_by_prefix(prefix)
00111 waypoint_names.sort()
00112
00113 if 0 == len(waypoint_names):
00114 rospy.logerr("No waypoints found for robot %s with prefix %s. " +
00115 "Can't make trajectory :(",
00116 self._robot_name, str(prefix))
00117 return False
00118 rospy.loginfo("Creating trajectory with %d waypoints selected by " +
00119 "prefix %s.", len(waypoint_names), str(prefix))
00120 return self.plan_from_list(waypoint_names)
00121
00122 def plan_from_list(self, waypoint_names):
00123 self.group.clear_pose_targets()
00124 waypoints = self.get_cartesian_waypoints(waypoint_names)
00125 if len(waypoints) != len(waypoint_names) + 1:
00126
00127 rospy.logerr("Not all waypoints existed, not executing.")
00128 return False
00129 (plan, fraction) = self.group.compute_cartesian_path(
00130 waypoints, self.eef_step, self.jump_threshold)
00131
00132 if fraction < self._min_wp_fraction:
00133 rospy.logerr("Only managed to generate trajectory through" +
00134 "%f of waypoints. Not executing", fraction)
00135 return False
00136
00137 self.__plan = plan
00138 return True
00139
00140 def _plan_from_list_cb(self, request):
00141
00142 self.__plan = None
00143 rospy.loginfo("Creating trajectory from points given: %s",
00144 ",".join(request.waypoint_names))
00145 return self.plan_from_list(request.waypoint_names)
00146
00147 def _plan_from_prefix_cb(self, request):
00148 self.__plan = None
00149 rospy.loginfo("Creating trajectory from points filtered by prefix %s",
00150 request.prefix)
00151 return self.plan_from_filter(request.prefix)
00152
00153 def _execute_plan_cb(self, request):
00154 if self.__plan is None:
00155 rospy.logerr("No plan stored!")
00156 return False
00157 rospy.loginfo("Executing stored plan")
00158 response = self.group.execute(self.__plan)
00159 self.__plan = None
00160 return response
00161
00162 if __name__ == "__main__":
00163 sf = WarehousePlanner()
00164 rospy.spin()