def store_precise_outlet_result(ud, result_state, result): if result_state == GoalStatus.SUCCEEDED: y = rospy.get_param('plugs_calibration_offset/y') z = rospy.get_param('plugs_calibration_offset/z') outlet_pose_corrected = PoseStamped() outlet_pose_corrected.pose = toMsg(fromMsg(result.outlet_pose.pose) * PyKDL.Frame(PyKDL.Vector(0, y, z))) outlet_pose_corrected.header = result.outlet_pose.header rospy.loginfo("Using calibration offset y: %f and z: %f"%(y,z)) ud.base_to_outlet = TFUtil.wait_and_transform("base_link", outlet_pose_corrected).pose print 'outlet not corrected' print TFUtil.wait_and_transform("base_link", result.outlet_pose).pose print 'outlet corrected' print TFUtil.wait_and_transform("base_link", outlet_pose_corrected).pose
def execute(self,ud): """Iterate across a set of offsets to find the outlet""" preempt_timeout = rospy.Duration(10.0) align_base_goal = ud.align_base_goal vision_detect_outlet_goal = ud.vision_detect_outlet_goal # Iterate across move_base offsets for offset in self.offsets: # align base rospy.loginfo("Search base alignment, offset %f..."%offset) align_base_goal.offset = offset align_base_goal.desired_distance = self.desired_distance if self.align_base_client.send_goal_and_wait(align_base_goal, rospy.Duration(40.0), preempt_timeout) != GoalStatus.SUCCEEDED: rospy.logerr('Aligning base failed') return 'aborted' # call vision outlet detection rospy.loginfo("Detecting outlet with the forearm camera...") vision_detect_outlet_goal.wall_normal = self.align_base_client.get_result().wall_norm vision_detect_outlet_goal.wall_normal.header.stamp = rospy.Time.now() vision_detect_outlet_goal.prior.header.stamp = rospy.Time.now() if self.vision_detect_outlet_client.send_goal_and_wait(vision_detect_outlet_goal, rospy.Duration(5.0), preempt_timeout) == GoalStatus.SUCCEEDED: # Store the rough outlet position in the state machiine user data structure ud.outlet_rough_pose = TFUtil.wait_and_transform('r_forearm_cam_optical_frame', self.vision_detect_outlet_client.get_result().outlet_pose) # Set succeeded, and return return 'succeeded' rospy.logerr("Could not find outlet in search.") return 'aborted'
def store_detect_plug_result(ud, result_state, result): if result_state == GoalStatus.SUCCEEDED: gripper_to_plug = TFUtil.wait_and_transform('r_gripper_tool_frame',result.plug_pose).pose ud.gripper_to_plug = gripper_to_plug print 'gripper_to_plug' print gripper_to_plug
def store_detect_plug_result(ud, result_state, result): if result_state == actionlib.GoalStatus.SUCCEEDED: ud.plug_on_base_pose = TFUtil.wait_and_transform('base_link', result.plug_pose).pose
def execute_cb(goal): rospy.loginfo("Action server received goal") preempt_timeout = rospy.Duration(5.0) # move spine up rospy.loginfo("Moving up spine...") spine_goal.position = 0.16 if spine_client.send_goal_and_wait(spine_goal, rospy.Duration(20.0), preempt_timeout) != GoalStatus.SUCCEEDED: rospy.logerr('Moving up spine failed') server.set_aborted() return # we have 5 different plug detection poses to_init_position() for i in range(1,5): rospy.loginfo('Detecting plug on base from position %i'%i) # move to joint space position rospy.loginfo("Move in joint space...") if joint_space_client.send_goal_and_wait(get_action_goal('pr2_plugs_configuration/detect_plug_on_base%i'%i), rospy.Duration(20.0), preempt_timeout) != GoalStatus.SUCCEEDED: rospy.logerr('Move retract in joint space failed') server.set_aborted() return if goal.record_image: rospy.loginfo("Recording images of plug on base") image_client = actionlib.SimpleActionClient('image_snapshot', ImageSnapshotAction) if not image_client.wait_for_server(rospy.Duration(20.0)): rospy.logerr("Imagesnapshot server is down.") else: image_goal = ImageSnapshotGoal() image_goal.topic_name = 'r_forearm_cam/image_raw' image_goal.num_images = 5 image_goal.output_file_name = '/removable/continuous_operation/stow_plug_failure_images_%s.bag'%str(rospy.Time.now()) image_client.send_goal_and_wait(image_goal, rospy.Duration(20.0), preempt_timeout) # call vision plug detection rospy.loginfo("Detecting plug...") detect_plug_goal = VisionPlugDetectionGoal() detect_plug_goal.camera_name = "/r_forearm_cam" detect_plug_goal.prior.pose = toMsg(PyKDL.Frame(PyKDL.Rotation.RPY(pi/2, 0, pi/2), PyKDL.Vector(0.080, 0.026, 0.23))) detect_plug_goal.prior.header.frame_id = "base_link" detect_plug_goal.origin_on_right = False detect_plug_goal.prior.header.stamp = rospy.Time.now() if detect_plug_client.send_goal_and_wait(detect_plug_goal, rospy.Duration(5.0), preempt_timeout) == GoalStatus.SUCCEEDED: pose_detect_plug = detect_plug_client.get_result().plug_pose try: pose_base_plug = fromMsg(TFUtil.wait_and_transform('base_link', pose_detect_plug).pose) except rospy.ServiceException, e: rospy.logerr('Could not transform between base_link and %s' %pose_detect_plug.header.frame_id) server.set_aborted() return error = (pose_base_plug.Inverse() * fromMsg(detect_plug_goal.prior.pose)) (r, p, y) = error.M.GetRPY() if (math.fabs(r) < 0.8) and (math.fabs(p) < 0.8) and (math.fabs(y) < 0.8) and (math.fabs(error.p[0]) < 0.05) and (math.fabs(error.p[1]) < 0.05) and (math.fabs(error.p[2]) < 0.05): to_init_position() server.set_succeeded(DetectPlugOnBaseResult(detect_plug_client.get_result().plug_pose)) rospy.loginfo("Action server goal finished") return else: rospy.loginfo('Found the plug, but it is in a different location than I expected. Error: %f, %f, %f'%(math.fabs(error.p[0]), math.fabs(error.p[1]), math.fabs(error.p[2])))