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