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 get_outlet_to_plug(pose_base_outlet, pose_plug_gripper):
"""Get the pose from the outlet to the plug."""
pose_base_gripper = fromMsg(TFUtil.wait_and_lookup("base_link","r_gripper_tool_frame",
rospy.Time.now(), rospy.Duration(2.0)).pose)
outlet_to_plug = pose_base_outlet.Inverse() * pose_base_gripper * pose_plug_gripper.Inverse()
return outlet_to_plug
def outlet_to_plug_error(goal):
time = rospy.Time.now()
try:
pose_base_gripper = fromMsg(TFUtil.wait_and_lookup("base_link","r_gripper_tool_frame", time).pose)
except rospy.ServiceException, e:
rospy.logerr('Could not transform between gripper and wrist at time %f' %time.to_sec())
server.set_aborted()
return
def execute_cb(goal):
rospy.loginfo("Action server received goal")
preempt_timeout = rospy.Duration(10.0)
cart_space_goal.ik_seed = get_action_seed('pr2_plugs_configuration/approach_outlet_seed')
try:
pose_gripper_wrist= fromMsg(TFUtil.wait_and_lookup("r_gripper_tool_frame", "r_wrist_roll_link").pose)
except rospy.ServiceException, e:
rospy.logerr('Could not transform between gripper and wrist at time %f' %time.to_sec())
server.set_aborted()
return
def plug_in_contact(ud):
"""Returns true if the plug is in contact with something."""
pose_base_gripper = fromMsg(TFUtil.wait_and_lookup('base_link', 'r_gripper_tool_frame').pose)
pose_outlet_plug = fromMsg(ud.base_to_outlet).Inverse() * pose_base_gripper * fromMsg(ud.gripper_to_plug)
# check if difference between desired and measured outlet-plug along x-axis is more than 1 cm
ud.outlet_to_plug_contact = toMsg(pose_outlet_plug)
if math.fabs(pose_outlet_plug.p[0] - ud.approach_offset) > 0.01:
return True
return False
def get_outlet_to_plug_ik_goal(ud, pose):
"""Get an IK goal for a pose in the frame of the outlet"""
pose_gripper_wrist = fromMsg(TFUtil.wait_and_lookup("r_gripper_tool_frame", "r_wrist_roll_link",
rospy.Time.now(), rospy.Duration(2.0)).pose)
goal = ArmMoveIKGoal()
goal.ik_seed = get_action_seed('pr2_plugs_configuration/approach_outlet_seed')
goal.pose.pose = toMsg(fromMsg(ud.base_to_outlet) * pose * fromMsg(ud.gripper_to_plug).Inverse() * pose_gripper_wrist)
goal.pose.header.stamp = rospy.Time.now()
goal.pose.header.frame_id = 'base_link'
return goal
def get_straighten_goal(ud, goal):
"""Generate an ik goal to straighten the plug in the outlet."""
pose_gripper_wrist = fromMsg(TFUtil.wait_and_lookup('r_gripper_tool_frame', 'r_wrist_roll_link').pose)
pose_base_wrist = fromMsg(ud.base_to_outlet) * fromMsg(ud.outlet_to_plug_contact) * fromMsg(ud.gripper_to_plug).Inverse() * pose_gripper_wrist
goal = ArmMoveIKGoal()
goal.pose.pose = toMsg(pose_base_wrist)
goal.pose.header.stamp = rospy.Time.now()
goal.pose.header.frame_id = 'base_link'
goal.ik_seed = get_action_seed('pr2_plugs_configuration/approach_outlet_seed')
goal.move_duration = rospy.Duration(0.5)
return goal
def get_pull_back_goal(ud, goal):
"""Generate an ik goal to move along the local x axis of the outlet."""
pose_outlet_plug = PyKDL.Frame(PyKDL.Vector(-0.10, 0, 0))
pose_gripper_wrist = fromMsg(TFUtil.wait_and_lookup('r_gripper_tool_frame', 'r_wrist_roll_link').pose)
pose_base_wrist = fromMsg(ud.base_to_outlet) * pose_outlet_plug * fromMsg(ud.gripper_to_plug).Inverse() * pose_gripper_wrist
goal = ArmMoveIKGoal()
goal.pose.pose = toMsg(pose_base_wrist)
goal.pose.header.stamp = rospy.Time.now()
goal.pose.header.frame_id = 'base_link'
goal.ik_seed = get_action_seed('pr2_plugs_configuration/approach_outlet_seed')
goal.move_duration = rospy.Duration(3.0)
return goal
def get_twist_goal(ud, goal):
"""Generate an ik goal to rotate the plug"""
pose_contact_plug = PyKDL.Frame(PyKDL.Rotation.RPY(ud.twist_angle, 0, 0))
pose_gripper_wrist = fromMsg(TFUtil.wait_and_lookup('r_gripper_tool_frame', 'r_wrist_roll_link').pose)
pose_base_wrist = fromMsg(ud.base_to_outlet) * fromMsg(ud.outlet_to_plug_contact) * pose_contact_plug * fromMsg(ud.gripper_to_plug).Inverse() * pose_gripper_wrist
goal = ArmMoveIKGoal()
goal.pose.pose = toMsg(pose_base_wrist)
goal.pose.header.stamp = rospy.Time.now()
goal.pose.header.frame_id = 'base_link'
goal.ik_seed = get_action_seed('pr2_plugs_configuration/approach_outlet_seed')
goal.move_duration = rospy.Duration(1.0)
return goal
def get_grasp_plug_goal(ud, goal):
"""Get the ik goal for grasping the plug."""
pose_gripper_wrist = fromMsg(TFUtil.wait_and_lookup('r_gripper_tool_frame', 'r_wrist_roll_link').pose)
goal = ArmMoveIKGoal()
goal.pose.pose = toMsg(fromMsg(ud.plug_on_base_pose)
* fromMsg(ud.gripper_plug_grasp_pose).Inverse()
* pose_gripper_wrist)
goal.pose.header.stamp = rospy.Time.now()
goal.pose.header.frame_id = 'base_link'
goal.ik_seed = get_action_seed('pr2_plugs_configuration/grasp_plug_seed')
goal.move_duration = rospy.Duration(3.0)
return goal
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 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 main():
rospy.init_node("recharge_toplevel")
TFUtil()
# Construct state machine
recharge_sm = StateMachine(
outcomes=['plugged_in', 'unplugged', 'aborted', 'preempted'],
input_keys=['recharge_command'],
output_keys=['recharge_state'])
# Set the initial state explicitly
recharge_sm.set_initial_state(['PROCESS_RECHARGE_COMMAND'])
recharge_sm.userdata.recharge_state = RechargeState(
state=RechargeState.UNPLUGGED)
recharge_sm.userdata.base_to_outlet = Pose(
position=Point(-0.0178, -0.7474, 0.2824),
orientation=Quaternion(0.0002, -0.0002, -0.7061, 0.7081))
recharge_sm.userdata.gripper_to_plug_grasp = Pose(
position=Point(0.0282, -0.0050, -0.0103),
orientation=Quaternion(-0.6964, 0.1228, 0.1228, 0.6964))
recharge_sm.userdata.base_to_plug_on_base = Pose(
position=Point(0.0783, 0.0244, 0.2426),
orientation=Quaternion(0.4986, 0.4962, 0.4963, 0.5088))
recharge_sm.userdata.gripper_to_plug = Pose(
position=Point(0.0275, -0.0046, -0.0094),
orientation=Quaternion(-0.6876, 0.1293, 0.1226, 0.7039))
with recharge_sm:
### PLUGGING IN ###
@smach.cb_interface(input_keys=['recharge_command'])
def plug_in_cond(ud):
command = ud.recharge_command.command
if command is RechargeCommand.PLUG_IN:
return True
elif command is RechargeCommand.UNPLUG:
return False
StateMachine.add('PROCESS_RECHARGE_COMMAND',
ConditionState(cond_cb=plug_in_cond), {
'true': 'NAVIGATE_TO_OUTLET',
'false': 'UNPLUG'
})
sm_nav = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['recharge_command'])
StateMachine.add('NAVIGATE_TO_OUTLET', sm_nav, {
'succeeded': 'DETECT_OUTLET',
'aborted': 'FAIL_STILL_UNPLUGGED'
})
with sm_nav:
StateMachine.add(
'GOAL_IS_LOCAL',
ConditionState(
cond_cb=lambda ud: ud.recharge_command.plug_id == 'local',
input_keys=['recharge_command']), {
'true': 'UNTUCK_AT_OUTLET',
'false': 'SAFETY_TUCK'
})
StateMachine.add(
'SAFETY_TUCK',
SimpleActionState('tuck_arms',
TuckArmsAction,
goal=TuckArmsGoal(True, True)), {
'succeeded': 'GET_OUTLET_LOCATIONS',
'aborted': 'SAFETY_TUCK'
})
StateMachine.add('GET_OUTLET_LOCATIONS',
ServiceState('outlet_locations',
GetOutlets,
response_slots=['poses']),
{'succeeded': 'NAVIGATE'},
remapping={'poses': 'approach_poses'})
@smach.cb_interface(
input_keys=['approach_poses', 'recharge_command'])
def get_outlet_approach_goal(ud, goal):
"""Get the approach pose from the outlet approach poses list"""
# Get id from command
plug_id = ud.recharge_command.plug_id
# Grab the relevant outlet approach pose
for outlet in ud.approach_poses:
if outlet.name == plug_id or outlet.id == plug_id:
target_pose = PoseStamped(pose=outlet.approach_pose)
# Create goal for move base
move_base_goal = MoveBaseGoal()
move_base_goal.target_pose = target_pose
move_base_goal.target_pose.header.stamp = rospy.Time.now()
move_base_goal.target_pose.header.frame_id = "map"
return move_base_goal
StateMachine.add(
'NAVIGATE',
SimpleActionState('pr2_move_base',
MoveBaseAction,
goal_cb=get_outlet_approach_goal,
exec_timeout=rospy.Duration(20 * 60.0)),
{'succeeded': 'UNTUCK_AT_OUTLET'})
StateMachine.add(
'UNTUCK_AT_OUTLET',
SimpleActionState('tuck_arms',
TuckArmsAction,
goal=TuckArmsGoal(False, False)))
StateMachine.add('DETECT_OUTLET',
SimpleActionState(
'detect_outlet',
DetectOutletAction,
result_slots=['base_to_outlet_pose']), {
'succeeded': 'FETCH_PLUG',
'aborted': 'FAIL_STILL_UNPLUGGED'
},
remapping={'base_to_outlet_pose': 'base_to_outlet'})
StateMachine.add('FETCH_PLUG',
SimpleActionState('fetch_plug',
FetchPlugAction,
result_slots=[
'plug_on_base_pose',
'gripper_plug_grasp_pose'
]), {
'succeeded': 'PLUG_IN',
'aborted': 'FAIL_OPEN_GRIPPER'
},
remapping={
'plug_on_base_pose': 'base_to_plug_on_base',
'gripper_plug_grasp_pose': 'gripper_to_plug_grasp'
})
@smach.cb_interface(input_keys=['recharge_state'],
output_keys=['recharge_state'])
def set_plug_in_result(ud, result_status, result):
if result_status == GoalStatus.SUCCEEDED:
ud.recharge_state.state = RechargeState.PLUGGED_IN
StateMachine.add(
'PLUG_IN',
SimpleActionState('plug_in',
PlugInAction,
goal_slots=['base_to_outlet'],
result_slots=['gripper_to_plug'],
result_cb=set_plug_in_result), {
'succeeded': 'STOW_LEFT_ARM',
'aborted': 'RECOVER_STOW_PLUG'
})
# Move L arm out of the way
StateMachine.add(
'STOW_LEFT_ARM',
SimpleActionState('l_arm_controller/joint_trajectory_action',
JointTrajectoryAction,
goal=stow_l_arm_goal),
{'succeeded': 'plugged_in'})
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])))
def execute_cb(goal):
rospy.loginfo("Action server received goal")
preempt_timeout = rospy.Duration(5.0)
# move the 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
# return plug
error = 1.0
free_cord = False
iter_idx = 0
while error > ERROR_TOL and not rospy.is_shutdown() and iter_idx < MAX_ITERS:
iter_idx += 1
# appraoch in joint space when re-trying
if free_cord:
rospy.loginfo("Free plug cord by doing approach...")
if joint_space_client.send_goal_and_wait(get_action_goal('pr2_plugs_configuration/free_plug_cord'), rospy.Duration(20.0), preempt_timeout) != GoalStatus.SUCCEEDED:
rospy.logerr('Free plug cord failed')
server.set_aborted()
return
free_cord = True
# 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/stow_plug'), rospy.Duration(20.0), preempt_timeout) != GoalStatus.SUCCEEDED:
rospy.logerr('Move approach in joint space failed')
server.set_aborted()
return
# return plug to original location
cart_space_goal.ik_seed = get_action_seed('pr2_plugs_configuration/return_plug_seed')
pose_plug_gripper = fromMsg(goal.gripper_to_plug_grasp).Inverse()
pose_base_plug = fromMsg(goal.base_to_plug)
pose_gripper_wrist = fromMsg(TFUtil.wait_and_lookup('r_gripper_tool_frame', 'r_wrist_roll_link').pose)
cart_space_goal.pose.pose = toMsg(pose_base_plug * pose_plug_gripper * pose_gripper_wrist)
cart_space_goal.pose.header.stamp = rospy.Time.now()
cart_space_goal.pose.header.frame_id = "base_link"
cart_space_goal.move_duration = rospy.Duration(3.0)
if cart_space_client.send_goal_and_wait(cart_space_goal, rospy.Duration(20.0), preempt_timeout) != GoalStatus.SUCCEEDED:
rospy.logerr('Failed to return plug to location where the plug was found when fetching it.')
server.set_aborted()
return
# check if plug in correct location
pose_base_gripper_measured = fromMsg(TFUtil.wait_and_lookup('base_link', 'r_gripper_tool_frame', rospy.Time.now()).pose)
pose_base_gripper_desired = pose_base_plug * pose_plug_gripper
diff = pose_base_gripper_measured.Inverse() * pose_base_gripper_desired
error = diff.p.Norm()
(r, p, y) = diff.M.GetRPY()
error += (r + p + y)/10.0
rospy.loginfo("Diff between desired and actual plug storing poses = %f"%error)
# open gripper
rospy.loginfo("Open gripper...")
gripper_goal.command.position = 0.07
gripper_goal.command.max_effort = 99999
if gripper_client.send_goal_and_wait(gripper_goal, rospy.Duration(20.0), preempt_timeout) != GoalStatus.SUCCEEDED:
rospy.logerr('Open gripper failed')
server.set_aborted()
return
# retract arm
rospy.loginfo("Releasing plug...")
pose_plug_approach = PyKDL.Frame(PyKDL.Vector(0, 0.05, 0))
cart_space_goal.pose.pose = toMsg(pose_base_plug * pose_plug_approach * pose_plug_gripper * pose_gripper_wrist)
cart_space_goal.pose.header.stamp = rospy.Time.now()
cart_space_goal.pose.header.frame_id = "base_link"
cart_space_goal.move_duration = rospy.Duration(3.0)
if cart_space_client.send_goal_and_wait(cart_space_goal, rospy.Duration(20.0), preempt_timeout) != GoalStatus.SUCCEEDED:
rospy.logerr('Failed to move arm away from where the plug got released')
server.set_aborted()
return
# detect plug on base
rospy.loginfo("Detect plug on base...")
start_time = rospy.Time.now()
while detect_plug_on_base_client.send_goal_and_wait(DetectPlugOnBaseGoal(), rospy.Duration(120.0), preempt_timeout) != GoalStatus.SUCCEEDED:
if rospy.Time.now() > start_time + rospy.Duration(60):
rospy.logerr("Can't detect plug on base. It is soo dark in here... Giving up, driving away, living on the edge! But let's take an image to make sure.")
detect_plug_on_base_client.send_goal_and_wait(DetectPlugOnBaseGoal(record_image=True), rospy.Duration(120.0), preempt_timeout)
break
rospy.logerr('Detecting plug on base failed, trying again...')
# move the spine down
rospy.loginfo("Moving down spine...")
spine_goal.position = 0.01
spine_client.send_goal(spine_goal)
# return result
server.set_succeeded(StowPlugResult())
rospy.loginfo("Action server goal finished")
