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])))
