class HeadClient(): def __init__(self): name_space = '/head_traj_controller/point_head_action' self.head_client = SimpleActionClient(name_space, PointHeadAction) self.head_client.wait_for_server() self.hor_pos = 0.0 self.vert_pos = 0.0 def move_head_vert (self, vert_val): head_goal = PointHeadGoal() head_goal.target.header.frame_id = 'torso_lift_link' head_goal.max_velocity = .3 self.vert_pos = vert_val/50.0 - 1 head_goal.target.point = Point(1.5, self.hor_pos, self.vert_pos) self.head_client.send_goal(head_goal) if (self.head_client.get_state() != GoalStatus.SUCCEEDED): rospy.logwarn('Head action unsuccessful.') def move_head_hor (self, hor_val): head_goal = PointHeadGoal() head_goal.target.header.frame_id = 'torso_lift_link' head_goal.max_velocity = .3 self.hor_pos = -(hor_val/20.0 - 2.5) head_goal.target.point = Point(1.5, self.hor_pos, self.vert_pos) self.head_client.send_goal(head_goal) if (self.head_client.get_state() != GoalStatus.SUCCEEDED): rospy.logwarn('Head action unsuccessful.')
class ActionTasks: def __init__(self, script_path): rospy.init_node('action_testr') rospy.on_shutdown(self.cleanup) self.fridge = (Pose(Point(0, 0, 0.0), Quaternion(0.0, 0.0, 0, 1))) self.client = SimpleActionClient("move_base", MoveBaseAction) self.client.wait_for_server() self.move_to(self.fridge) def move_to(self, location): goal = MoveBaseGoal() goal.target_pose.pose = location goal.target_pose.header.frame_id = 'map' goal.target_pose.header.stamp = rospy.Time.now() self.client.send_goal(goal) #self.client.wait_for_result() self.client.wait_for_result(rospy.Duration.from_sec(40.0)) if self.client.get_state() == GoalStatus.SUCCEEDED: result = self.client.get_result() print "Result: SUCCEEDED " elif self.client.get_state() == GoalStatus.PREEMPTED: print "Action pre-empted" else: print "Action failed" def cleanup(self): rospy.loginfo("Shutting down talk node...")
def test_grasp_action(self): """Test sending a grasp.""" goal = GraspGoal() goal.grasp = self.mk_grasp({ 'LFJ3': 1.4, 'RFJ3': 1.4, 'MFJ3': 1.4, 'FFJ3': 1.4, 'LFJ0': 3.0, 'RFJ0': 3.0, 'MFJ0': 3.0, 'FFJ0': 3.0, }) # Get a action client client = SimpleActionClient('grasp', GraspAction) client.wait_for_server() # Send pre-grasp goal.pre_grasp = True client.send_goal(goal) client.wait_for_result(rospy.Duration.from_sec(20.0)) self.assertEqual(client.get_state(), GoalStatus.SUCCEEDED, "Action did not return in SUCCEEDED state.") rospy.sleep(2) # Send grasp goal.pre_grasp = False client.send_goal(goal) client.wait_for_result(rospy.Duration.from_sec(20.0)) self.assertEqual(client.get_state(), GoalStatus.SUCCEEDED, "Action did not return in SUCCEEDED state.")
class ActionTasks: def __init__(self, script_path): rospy.init_node('action_testr') rospy.on_shutdown(self.cleanup) self.fridge = (Pose(Point(0.295, -2.304, 0.0), Quaternion(0.0, 0.0, -0.715, 0.699))) self.client = SimpleActionClient("move_base", MoveBaseAction) self.client.wait_for_server() self.move_to(self.fridge) def move_to(self, location): goal = MoveBaseGoal() goal.target_pose.pose = location goal.target_pose.header.frame_id = 'map' goal.target_pose.header.stamp = rospy.Time.now() self.client.send_goal(goal) #self.client.wait_for_result() self.client.wait_for_result(rospy.Duration.from_sec(40.0)) if self.client.get_state() == GoalStatus.SUCCEEDED: result = self.client.get_result() print "Result: SUCCEEDED " elif self.client.get_state() == GoalStatus.PREEMPTED: print "Action pre-empted" else: print "Action failed" def cleanup(self): rospy.loginfo("Shutting down talk node...")
class Tiago: def __init__(self): rospy.init_node('run_motion_python') rospy.loginfo("Starting run_motion_python application...") wait_for_valid_time(10.0) self.client = SimpleActionClient('/play_motion', PlayMotionAction) rospy.loginfo("Waiting for Action Server...") self.client.wait_for_server() def act(self, action): goal = PlayMotionGoal() goal.motion_name = action goal.skip_planning = False goal.priority = 0 # Optional rospy.loginfo("Sending goal with motion: " + sys.argv[1]) self.client.send_goal(goal) rospy.loginfo("Waiting for result...") action_ok = self.client.wait_for_result(rospy.Duration(30.0)) state = self.client.get_state() if action_ok: rospy.loginfo("Action finished succesfully with state: " + str(get_status_string(state))) else: rospy.logwarn("Action failed with state: " + str(get_status_string(state))) def reset(self): goal = PlayMotionGoal() goal.motion_name = 'home' goal.skip_planning = False goal.priority = 0 # Optional rospy.loginfo("Sending goal with motion: " + sys.argv[1]) self.client.send_goal(goal) rospy.loginfo("Waiting for result...") action_ok = self.client.wait_for_result(rospy.Duration(30.0)) state = self.client.get_state() if action_ok: rospy.loginfo("Action finished succesfully with state: " + str(get_status_string(state))) else: rospy.logwarn("Action failed with state: " + str(get_status_string(state)))
class TestClientsAndServer(unittest.TestCase): def setUp(self): rospy.init_node('test_move_end_effector_server_node') self.effector_client = Client(move_end_effector_controller_topic, MoveEndEffectorAction) self.server = MoveEndEffectorServer() self.goal_maker = EffectorGoalMaker() self.sensor_pub = Publisher('mock/sensor', String, queue_size=1) self.linear_actuator_pub = Publisher('mock/linear_actuator', String, queue_size=1) self.head_pub = Publisher('mock/head', String, queue_size=1) rospy.sleep(1) def test_execute_cb(self): ''' Tests if server is set correctly (triggered by callback!) ''' self.sensor_pub.publish('success:1') self.linear_actuator_pub.publish('success:1') self.head_pub.publish('success:1') self.goal = MoveEndEffectorGoal() self.goal = self.goal_maker.create_goal() self.server.wait_for_servers() self.effector_client.send_goal(self.goal) self.effector_client.wait_for_result() self.assertEqual(self.effector_client.get_state(), GoalStatus.SUCCEEDED) self.sensor_pub.publish('abort:1') self.linear_actuator_pub.publish('success:1') self.head_pub.publish('success:1') self.goal = MoveEndEffectorGoal() self.goal = self.goal_maker.create_goal() self.effector_client.send_goal(self.goal) self.effector_client.wait_for_result() self.assertEqual(self.effector_client.get_state(), GoalStatus.ABORTED) self.sensor_pub.publish('preempt:1') self.linear_actuator_pub.publish('success:1') self.head_pub.publish('preempt:1') self.goal = MoveEndEffectorGoal() self.goal = self.goal_maker.create_goal() self.effector_client.send_goal(self.goal) self.effector_client.wait_for_result() self.assertEqual(self.effector_client.get_state(), GoalStatus.PREEMPTED) self.sensor_pub.publish('abort:1') self.linear_actuator_pub.publish('success:1') self.head_pub.publish('preempt:1') self.goal = MoveEndEffectorGoal() self.goal = self.goal_maker.create_goal() self.effector_client.send_goal(self.goal) self.effector_client.wait_for_result() self.assertEqual(self.effector_client.get_state(), GoalStatus.ABORTED)
class plan_task(smach.State): def __init__(self, mode=PlanGoal.NORMAL): smach.State.__init__( self, outcomes=['success', 'failure'], input_keys=['domain_file', 'problem_file', 'planner'], output_keys=['plan']) self.mode = mode self.planner_client = SimpleActionClient( '/mir_task_planning/task_planner_server/plan_task', PlanAction) self.planner_client.wait_for_server() rospy.sleep(0.2) def execute(self, userdata): goal = PlanGoal(domain_file=userdata.domain_file, problem_file=userdata.problem_file, planner=userdata.planner, mode=self.mode) self.planner_client.send_goal(goal) self.planner_client.wait_for_result() state = self.planner_client.get_state() result = self.planner_client.get_result() if state == GoalStatus.SUCCEEDED: rospy.loginfo("Found a plan with %i actions", len(result.plan.plan)) userdata.plan = result.plan return 'success' elif state == GoalStatus.ABORTED: rospy.logerr("Failed to plan") rospy.sleep(2.0) return 'failure'
def move_arm_to_grasping_joint_pose(joint_names, joint_positions, allowed_contacts=[], link_padding=[], collision_operations=OrderedCollisionOperations()): move_arm_client = SimpleActionClient('move_left_arm', MoveArmAction) move_arm_client.wait_for_server() rospy.loginfo('connected to move_left_arm action server') goal = MoveArmGoal() goal.planner_service_name = 'ompl_planning/plan_kinematic_path' goal.motion_plan_request.planner_id = '' goal.motion_plan_request.group_name = 'left_arm' goal.motion_plan_request.num_planning_attempts = 1 goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.0) goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint(j, p, 0.1, 0.1, 0.0) for (j,p) in zip(joint_names,joint_positions)] goal.motion_plan_request.allowed_contacts = allowed_contacts goal.motion_plan_request.link_padding = link_padding goal.motion_plan_request.ordered_collision_operations = collision_operations move_arm_client.send_goal(goal) finished_within_time = move_arm_client.wait_for_result(rospy.Duration(200.0)) if not finished_within_time: move_arm_client.cancel_goal() rospy.loginfo('timed out trying to achieve a joint goal') else: state = move_arm_client.get_state() if state == GoalStatus.SUCCEEDED: rospy.loginfo('action finished: %s' % str(state)) return True else: rospy.loginfo('action failed: %s' % str(state)) return False
def head_action(self, x, y, z, wait=False): name_space = '/head_traj_controller/point_head_action' head_client = SimpleActionClient(name_space, PointHeadAction) head_goal = PointHeadGoal() head_goal.target.header.frame_id = 'base_link' head_goal.min_duration = rospy.Duration(1.0) head_goal.target.point = Point(x, y, z) head_client.send_goal(head_goal) if wait: # wait for the head movement to finish before we try to detect and pickup an object finished_within_time = head_client.wait_for_result( rospy.Duration(5)) # Check for success or failure if not finished_within_time: head_client.cancel_goal() rospy.loginfo("Timed out achieving head movement goal") else: state = head_client.get_state() if state == GoalStatus.SUCCEEDED: rospy.loginfo("Head movement goal succeeded!") rospy.loginfo("State:" + str(state)) else: rospy.loginfo( "Head movement goal failed with error code: " + str(self.goal_states[state]))
class NavServer(object): def __init__(self, name): rospy.loginfo("Starting '%s'." % name) self._ps = PNPSimplePluginServer( name=name, ActionSpec=StringAction, execute_cb=self.execute_cb, auto_start=False ) self._ps.register_preempt_callback(self.preempt_cb) self.client = SimpleActionClient('topological_navigation', GotoNodeAction) self.client.wait_for_server() self._ps.start() rospy.loginfo("Done") def execute_cb(self, goal): self.client.send_goal_and_wait(GotoNodeGoal(target=goal.value)) result = self.client.get_state() if result == GoalStatus.PREEMPTED: return if result == GoalStatus.SUCCEEDED: self._ps.set_succeeded() else: self._ps.set_aborted() def preempt_cb(self): self.client.cancel_all_goals() self._ps.set_preempted()
class TrajectoryExecution: def __init__(self, side_prefix): traj_controller_name = '/' + side_prefix + '_arm_controller/joint_trajectory_action' self.traj_action_client = SimpleActionClient(traj_controller_name, JointTrajectoryAction) rospy.loginfo('Waiting for a response from the trajectory action server arm: ' + side_prefix) self.traj_action_client.wait_for_server() rospy.loginfo('Trajectory executor is ready for arm: ' + side_prefix) motion_request_name = '/' + side_prefix + '_arm_motion_request/joint_trajectory_action' self.action_server = SimpleActionServer(motion_request_name, JointTrajectoryAction, execute_cb=self.move_to_joints) self.action_server.start() self.has_goal = False def move_to_joints(self, traj_goal): rospy.loginfo('Receieved a trajectory execution request.') traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1)) self.traj_action_client.send_goal(traj_goal) rospy.sleep(1) is_terminated = False while(not is_terminated): action_state = self.traj_action_client.get_state() if (action_state == GoalStatus.SUCCEEDED): self.action_server.set_succeeded() is_terminated = True elif (action_state == GoalStatus.ABORTED): self.action_server.set_aborted() is_terminated = True elif (action_state == GoalStatus.PREEMPTED): self.action_server.set_preempted() is_terminated = True rospy.loginfo('Trajectory completed.')
class InitMove(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['succeeded', 'supervise']) self.move_base_client = SimpleActionClient('move_base', MoveBaseAction) def execute(self, ud): self.move_base_client.wait_for_server() goal = MoveBaseGoal() pose = PoseStamped() pose.header.frame_id = 'map' pose.header.stamp = rospy.Time.now() pose.pose.position.x = 2.0 pose.pose.position.y = 0.0 pose.pose.orientation.x = 0.0 pose.pose.orientation.y = 0.0 pose.pose.orientation.z = 0.0 pose.pose.orientation.w = 1.0 goal.target_pose = pose goal.target_pose.header.stamp = rospy.Time.now() self.move_base_client.send_goal(goal) self.move_base_client.wait_for_result(rospy.Duration(30)) status = self.move_base_client.get_state() if status == GoalStatus.SUCCEEDED: return 'succeeded' else: return 'supervise'
class unstage_object(smach.State): def __init__(self, platform=None): smach.State.__init__(self, outcomes=['success', 'failed'], input_keys=['goal']) self.client = SimpleActionClient('unstage_object_server', GenericExecuteAction) self.client.wait_for_server() self.goal = GenericExecuteGoal() if platform is not None: self.goal.parameters.append( KeyValue(key='platform', value=str(platform).upper())) def execute(self, userdata): # initialise platform in goal if not already initialised current_platform = Utils.get_value_of(self.goal.parameters, 'platform') if current_platform is None: platform = Utils.get_value_of(userdata.goal.parameters, 'platform') if platform is None: rospy.logwarn('Platform not provided. Using default') platform = 'platform_middle' self.goal.parameters.append( KeyValue(key='platform', value=str(platform).upper())) self.client.send_goal(self.goal) self.client.wait_for_result(rospy.Duration.from_sec(30.0)) state = self.client.get_state() if state == GoalStatus.SUCCEEDED: return 'success' else: return 'failed'
class ArmController: def __init__(self, arm): self.client = SimpleActionClient( "%s_arm_controller/joint_trajectory_action" % arm, JointTrajectoryAction) #wait for the action servers to come up rospy.loginfo("[ARM] Waiting for %s controller" % arm) self.client.wait_for_server() rospy.loginfo("[ARM] Got %s controller" % arm) def start_trajectory(self, trajectory, set_time_stamp=True, wait=True): """Creates an action from the trajectory and sends it to the server""" goal = JointTrajectoryGoal() goal.trajectory = trajectory if set_time_stamp: goal.trajectory.header.stamp = rospy.Time.now() self.client.send_goal(goal) if wait: self.wait() def wait(self): self.client.wait_for_result() def is_done(self): return self.client.get_state() > SimpleGoalState.ACTIVE
class DoReset(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'], input_keys=['reset_plan']) self.eef_pub = rospy.Publisher('/CModelRobotOutput', eef_cmd, queue_size=1, latch=True) self.move_arm_client = SimpleActionClient('/wm_arm_driver_node/execute_plan', executePlanAction) def execute(self, ud): rospy.logdebug("Entered 'RO_RESET' state.") hand_cmd = eef_cmd() hand_cmd.rACT = 1 # activate gripper hand_cmd.rGTO = 1 # request to go to position hand_cmd.rSP = 200 # set activation speed (0[slowest]-255[fastest]) hand_cmd.rFR = 0 # set force limit (0[min] - 255[max]) hand_cmd.rPR = 0 # request to open self.eef_pub.publish(hand_cmd) self.move_arm_client.wait_for_server() goal = executePlanGoal() goal.trajectory = ud.reset_plan self.move_arm_client.send_goal(goal) self.move_arm_client.wait_for_result() status = self.move_arm_client.get_state() if status == GoalStatus.SUCCEEDED: return 'succeeded' elif status == GoalStatus.PREEMPTED: return 'preempted' else: return 'aborted'
class ExplorationController: def __init__(self): self._plan_service = rospy.ServiceProxy('get_exploration_path', GetRobotTrajectory) self._move_base = SimpleActionClient('move_base', MoveBaseAction) def run(self): r = rospy.Rate(1 / 7.0) while not rospy.is_shutdown(): self.run_once() r.sleep() def run_once(self): path = self._plan_service().trajectory poses = path.poses if not path.poses: rospy.loginfo('No frontiers left.') return rospy.loginfo('Moving to frontier...') self.move_to_pose(poses[-1]) def move_to_pose(self, pose_stamped, timeout=20.0): goal = MoveBaseGoal() goal.target_pose = pose_stamped self._move_base.send_goal(goal) self._move_base.wait_for_result(rospy.Duration(timeout)) return self._move_base.get_state() == GoalStatus.SUCCEEDED
class ExplorationController: def __init__(self): rospy.wait_for_service('get_exploration_path', timeout=2) self._plan_service = rospy.ServiceProxy('get_exploration_path', GetRobotTrajectory) self._move_base = SimpleActionClient('move_base', MoveBaseAction) def run(self): r = rospy.Rate(10) while not rospy.is_shutdown(): self.run_once() r.sleep() def run_once(self): global count if (count >= 20 or self._move_base.get_state() == GoalStatus.SUCCEEDED): path = self._plan_service().trajectory poses = path.poses if not path.poses: rospy.loginfo('No frontiers left.') return rospy.loginfo('Moving to frontier...') self.move_to_pose(poses[-1]) count = 0 else: count += 1 def move_to_pose(self, pose_stamped, timeout=20.0): goal = MoveBaseGoal() goal.target_pose = pose_stamped self._move_base.send_goal(goal)
class Move(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['wp_reached', 'wp_not_reached', 'test_finished'], input_keys=['move_waypoints', 'move_target_wp', 'move_wp_str'], output_keys=['move_target_wp']) self.move_base_client = SimpleActionClient('move_base', MoveBaseAction) self.tts_pub = rospy.Publisher('sara_tts', String, queue_size=1, latch=True) def execute(self, ud): self.move_base_client.wait_for_server() goal = MoveBaseGoal() goal.target_pose = ud.move_waypoints[ud.move_target_wp] goal.target_pose.header.stamp = rospy.Time.now() self.move_base_client.send_goal(goal) self.move_base_client.wait_for_result() status = self.move_base_client.get_state() tts_msg = String() if status == GoalStatus.SUCCEEDED: tts_msg.data = "I have reached " + ud.move_wp_str[ud.move_target_wp] + "." self.tts_pub.publish(tts_msg) if ud.move_target_wp == 0: return 'test_finished' else: ud.move_target_wp -= 1 return 'succeeded' else: return 'wp_not_reached'
def _exec_action(action_client: SimpleActionClient, goal, stopping_event: EventBase = None): """Executes goal with specified action client and optional event that stops action execution when triggered. This is a private method. :param action_client: Action client to use. :param goal: Action goal to execute. :param stopping_event: Optional event that stops goal execution when triggered. :return: None if stopping_event is None. True if goal execution is stopped because of triggered stopping_event, False otherwise. """ if stopping_event is None: action_client.send_goal(goal) action_client.wait_for_result() return stopping_event.start_listening() action_client.send_goal(goal, active_cb=None, feedback_cb=None, done_cb=None) check_rate = rospy.Rate(100) while True: goal_state = action_client.get_state() if goal_state == SimpleGoalState.DONE: stopping_event.stop_listening() return False if stopping_event.is_triggered(): action_client.cancel_goal() return True check_rate.sleep()
class NavServer(object): def __init__(self, name): rospy.loginfo("Starting '%s'." % name) self._ps = PNPSimplePluginServer(name=name, ActionSpec=StringAction, execute_cb=self.execute_cb, auto_start=False) self._ps.register_preempt_callback(self.preempt_cb) self.client = SimpleActionClient('topological_navigation', GotoNodeAction) self.client.wait_for_server() self._ps.start() rospy.loginfo("Done") def execute_cb(self, goal): self.client.send_goal_and_wait(GotoNodeGoal(target=goal.value)) result = self.client.get_state() if result == GoalStatus.PREEMPTED: return if result == GoalStatus.SUCCEEDED: self._ps.set_succeeded() else: self._ps.set_aborted() def preempt_cb(self): self.client.cancel_all_goals() self._ps.set_preempted()
class RecordPoseStampedFromPlayMotion(): """Manage the learning from a play motion gesture""" def __init__(self): rospy.loginfo("Initializing RecordFromPlayMotion") rospy.loginfo("Connecting to AS: '" + PLAY_MOTION_AS + "'") self.play_motion_as = SimpleActionClient(PLAY_MOTION_AS, PlayMotionAction) self.play_motion_as.wait_for_server() rospy.loginfo("Connected.") self.current_rosbag_name = "uninitialized_rosbag_name" self.lfee = LearnFromEndEffector(['/tf_to_ps'], ['right_arm']) def play_and_record(self, motion_name, groups=['right_arm'], bag_name="no_bag_name_set"): """Play the specified motion and start recording poses. Try to get the joints to record from the metadata of the play_motion gesture or, optionally, specify the joints to track""" # Check if motion exists in param server PLAYMOTIONPATH = '/play_motion/motions/' if not rospy.has_param(PLAYMOTIONPATH + motion_name): rospy.logerr("Motion named: " + motion_name + " does not exist in param server at " + PLAYMOTIONPATH + motion_name) return else: rospy.loginfo("Found motion " + motion_name + " in param server at " + PLAYMOTIONPATH + motion_name) # Get it's info motion_info = rospy.get_param(PLAYMOTIONPATH + motion_name) # check if joints was specified, if not, get the joints to actually save if len(groups) > 0: joints_to_record = groups else: joints_to_record = motion_info['groups'] rospy.loginfo("Got groups: " + str(joints_to_record)) # play motion rospy.loginfo("Playing motion!") pm_goal = PlayMotionGoal(motion_name, False, 0) self.play_motion_as.send_goal(pm_goal) self.lfee.start_learn(motion_name, bag_name) done_with_motion = False while not done_with_motion: state = self.play_motion_as.get_state() #rospy.loginfo("State is: " + str(state) + " which is: " + goal_status_dict[state]) if state == GoalStatus.ABORTED or state == GoalStatus.SUCCEEDED: done_with_motion = True elif state != GoalStatus.PENDING and state != GoalStatus.ACTIVE: rospy.logerr("We got state " + str(state) + " unexpectedly, motion failed. Aborting.") return None rospy.sleep(0.1) # data is written to rosbag in here motion_data = self.lfee.stop_learn() return motion_data
def testsimple(self): client = SimpleActionClient('reference_action', TestAction) self.assert_(client.wait_for_server(rospy.Duration(10.0)), 'Could not connect to the action server') goal = TestGoal(1) client.send_goal(goal) self.assert_(client.wait_for_result(rospy.Duration(10.0)), "Goal didn't finish") self.assertEqual(GoalStatus.SUCCEEDED, client.get_state()) self.assertEqual("The ref server has succeeded", client.get_goal_status_text()) goal = TestGoal(2) client.send_goal(goal) self.assert_(client.wait_for_result(rospy.Duration(10.0)), "Goal didn't finish") self.assertEqual(GoalStatus.ABORTED, client.get_state()) self.assertEqual("The ref server has aborted", client.get_goal_status_text())
class LinearActuatorClient(object): """docstring for LinearActuatorClient""" def __init__(self): self._name = "LinearActuatorClient" self.goal = linear_actuator_client['goal'] self.topic = linear_actuator_client['topic'] self.action = linear_actuator_client['action'] self.command_dict = translate_command['to_linear_actuator'] self.client = Client(self.topic, self.action) def get_name(self): return self._name def fill_goal(self, effector_goal): self.goal.command = self.command_dict[effector_goal.command] self.goal.point_of_interest = effector_goal.point_of_interest self.goal.center_point = effector_goal.center_point def send_goal(self): self.client.send_goal(self.goal) def wait_server(self): self.client.wait_for_server() def wait_result(self): self.client.wait_for_result() def has_succeeded(self): return self.client.get_state() == GoalStatus.SUCCEEDED def has_aborted(self): return self.client.get_state() == GoalStatus.ABORTED def has_preempted(self): return self.client.get_state() == GoalStatus.PREEMPTED def has_been_recalled(self): return self.client.get_state() == GoalStatus.RECALLED def preempt_if_active(self): if self.client.get_state() == GoalStatus.ACTIVE: self.client.cancel_all_goals()
def run_grasp(grasp, pre=False): goal = GraspGoal() goal.grasp = grasp goal.pre_grasp = pre client = SimpleActionClient('grasp', GraspAction) client.wait_for_server() client.send_goal(goal) client.wait_for_result(rospy.Duration.from_sec(20.0)) if client.get_state() == GoalStatus.SUCCEEDED: rospy.loginfo("Success!") else: rospy.logerr("Fail!")
class AsyncSimpleActionClient: def __init__(self, ns, action_spec): self._client = SimpleActionClient(ns, action_spec) self._client.wait_for_server() async def send_goal(self, goal): self._client.send_goal(goal) loop = asyncio.get_event_loop() await loop.run_in_executor(None, self._client.wait_for_result) return self._client.get_state(), self._client.get_result()
class safe_land(object): ''' Allows a human to teleoperate the robot through a joystick. ''' def __init__(self, name): self.robot_name = name self.state = 'IDLE' # Landing service self._land_client = SimpleActionClient( "safe_land_server", ExecuteLandAction) # Create a client to the v_search server self._land_client.wait_for_server() # Wait server to be ready self._land_goal = ExecuteLandGoal() # Message to send the goal region self.pub = rospy.Publisher("/{}/maneuvers/out".format(name), events_message, queue_size=10) # Publisher object self.msg = events_message() # Message object def execute(self): rospy.loginfo("Starting Safe Land!") self.state = 'EXE' # Start safe_land self._land_client.send_goal(self._land_goal) self._land_client.wait_for_result() state = self._land_client.get_state() # Get the state of the action print(state) if state == GoalStatus.SUCCEEDED: # safe_land successfully executed self.state = 'IDLE' # Set IDLE state self.msg.event = 'end_safe_land' self.pub.publish( self.msg ) # Send the message signaling that the safe_land is complete else: # The server aborted the motion self.state = 'ERROR' # Set ERROR state self.msg.event = 'safe_land_error' self.pub.publish(self.msg) # Send message signaling the error def reset(self): rospy.loginfo("Reseting Safe Land!") self.state = 'IDLE' def erro(self): rospy.loginfo("Safe Land Erro!") self.state = 'ERROR' # Set ERROR state self._land_client.cancel_goal() # Cancel the motion of the robot
def test_one(self): client = SimpleActionClient('reference_simple_action', TestAction) self.assert_(client.wait_for_server(rospy.Duration(2.0)), 'Could not connect to the action server') goal = TestGoal(1) client.send_goal(goal) self.assert_(client.wait_for_result(rospy.Duration(2.0)), "Goal didn't finish") self.assertEqual(GoalStatus.SUCCEEDED, client.get_state()) goal = TestGoal(2) client.send_goal(goal) self.assert_(client.wait_for_result(rospy.Duration(10.0)), "Goal didn't finish") self.assertEqual(GoalStatus.ABORTED, client.get_state()) goal = TestGoal(3) client.send_goal(goal) self.assert_(client.wait_for_result(rospy.Duration(10.0)), "Goal didn't finish") #The simple server can't reject goals self.assertEqual(GoalStatus.ABORTED, client.get_state()) goal = TestGoal(9) saved_feedback={}; def on_feedback(fb): rospy.loginfo("Got feedback") saved_feedback[0]=fb client.send_goal(goal,feedback_cb=on_feedback) self.assert_(client.wait_for_result(rospy.Duration(10.0)), "Goal didn't finish") self.assertEqual(GoalStatus.SUCCEEDED, client.get_state()) self.assertEqual(saved_feedback[0].feedback,9)
def test_one(self): client = SimpleActionClient('reference_simple_action', TestAction) self.assert_(client.wait_for_server(rospy.Duration(2.0)), 'Could not connect to the action server') goal = TestGoal(1) client.send_goal(goal) self.assert_(client.wait_for_result(rospy.Duration(2.0)), "Goal didn't finish") self.assertEqual(GoalStatus.SUCCEEDED, client.get_state()) goal = TestGoal(2) client.send_goal(goal) self.assert_(client.wait_for_result(rospy.Duration(10.0)), "Goal didn't finish") self.assertEqual(GoalStatus.ABORTED, client.get_state()) goal = TestGoal(3) client.send_goal(goal) self.assert_(client.wait_for_result(rospy.Duration(10.0)), "Goal didn't finish") #The simple server can't reject goals self.assertEqual(GoalStatus.ABORTED, client.get_state()) goal = TestGoal(9) saved_feedback = {} def on_feedback(fb): rospy.loginfo("Got feedback") saved_feedback[0] = fb client.send_goal(goal, feedback_cb=on_feedback) self.assert_(client.wait_for_result(rospy.Duration(10.0)), "Goal didn't finish") self.assertEqual(GoalStatus.SUCCEEDED, client.get_state()) self.assertEqual(saved_feedback[0].feedback, 9)
class perceive_cavity(smach.State): def __init__(self): smach.State.__init__(self, outcomes=["success", "failed"]) self.client = SimpleActionClient("perceive_cavity_server", GenericExecuteAction) self.client.wait_for_server() self.goal = GenericExecuteGoal() def execute(self, userdata): self.client.send_goal(self.goal) self.client.wait_for_result(rospy.Duration.from_sec(30.0)) state = self.client.get_state() if state == GoalStatus.SUCCEEDED: return "success" else: return "failed"
def look_at_r_gripper(self): name_space = '/head_traj_controller/point_head_action' head_client = SimpleActionClient(name_space, PointHeadAction) head_client.wait_for_server() head_goal = PointHeadGoal() head_goal.target.header.frame_id = "r_gripper_tool_frame" head_goal.min_duration = rospy.Duration(0.3) head_goal.target.point = Point(0, 0, 0) head_goal.max_velocity = 1.0 head_client.send_goal(head_goal) head_client.wait_for_result(rospy.Duration(5.0)) if (head_client.get_state() != GoalStatus.SUCCEEDED): rospy.logwarn('Head action unsuccessful.')
class pick_object(smach.State): def __init__(self): smach.State.__init__(self, outcomes=["success", "failed"]) self.client = SimpleActionClient("wbc_pick_object_server", GenericExecuteAction) self.client.wait_for_server() self.goal = GenericExecuteGoal() self.goal.parameters.append(KeyValue(key="object", value="any")) def execute(self, userdata): self.client.send_goal(self.goal) self.client.wait_for_result(rospy.Duration.from_sec(30.0)) state = self.client.get_state() if state == GoalStatus.SUCCEEDED: return "success" else: return "failed"
class perceive_location(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['success', 'failed']) self.client = SimpleActionClient('perceive_location_server', GenericExecuteAction) self.client.wait_for_server() self.goal = GenericExecuteGoal() def execute(self, userdata): self.client.send_goal(self.goal) self.client.wait_for_result(rospy.Duration.from_sec(30.0)) state = self.client.get_state() if state == GoalStatus.SUCCEEDED: return 'success' else: return 'failed'
class GUI(object): """ Communication with the operator.""" def __init__(self, verbose=False): self.verbose = verbose self.gui_result = None self.client = Client(topics.gui_validation, ValidateVictimGUIAction) def cancel_all_goals(self): log.debug('Waiting for the GUI action server...') self.client.wait_for_server() log.info('Canceling all goals on GUI.') self.client.cancel_all_goals() sleep(3) def send_request(self, target): """ Sends a validation request to the robot operator. :param :target A target to be validated. """ goal = ValidateVictimGUIGoal() goal.victimId = target.id goal.victimFoundx = target.victimPose.pose.position.x goal.victimFoundy = target.victimPose.pose.position.y goal.probability = target.probability goal.sensorIDsFound = target.sensors log.debug('Waiting for the GUI action server.') self.client.wait_for_server() log.info('Sending validation request.') if self.verbose: log.debug(target) self.client.send_goal(goal) log.info('Waiting for response.') self.client.wait_for_result() status = self.client.get_state() verbose_status = ACTION_STATES[status] if status == GoalStatus.SUCCEEDED: log.info('Validation request succeded!') self.gui_result = self.client.get_result() return True else: log.error('Validation request failed with %s.', verbose_status) return False def result(self): return self.gui_result
class HeadNode(): def __init__(self): name_space = '/head_traj_controller/point_head_action' self.head_client = SimpleActionClient(name_space, PointHeadAction) self.head_client.wait_for_server() def moveHead(self, theta, phi): head_goal = PointHeadGoal() head_goal.target.header.frame_id = 'base_link' head_goal.min_duration = rospy.Duration(1.0) head_goal.target.point = Point(math.cos(theta), math.sin(theta), phi) self.head_client.send_goal(head_goal) self.head_client.wait_for_result(rospy.Duration(10.0)) if (self.head_client.get_state() != GoalStatus.SUCCEEDED): rospy.logwarn('Head action unsuccessful.')
def movebase_client(self, x, y, qz, qw): client = SimpleActionClient("move_base", MoveBaseAction) client.wait_for_server(rospy.Duration(3)) goal = MoveBaseGoal() goal.target_pose.header.frame_id = "map" goal.target_pose.header.stamp = rospy.Time.now() goal.target_pose.pose.position = Point(x, y, 0) goal.target_pose.pose.orientation = Quaternion(0, 0, qz, qw) client.send_goal(goal) wait = client.wait_for_result() if not wait: rospy.logerr("Action Server NOT Available!") rospy.signal_shutdown("Action Server NOT Available!") else: return client.get_state()
class move_base(smach.State): def __init__(self, destination_location): smach.State.__init__(self, outcomes=["success", "failed"], input_keys=["goal"]) self.client = SimpleActionClient("move_base_safe_server", GenericExecuteAction) self.client.wait_for_server() self.goal = GenericExecuteGoal() self.destination_location = destination_location def execute(self, userdata): self.goal.parameters = [] self.goal.parameters.append( KeyValue(key="arm_safe_position", value="barrier_tape")) # if location is sent as an argument to this state, set it here if self.destination_location is not None: self.goal.parameters.append( KeyValue(key="destination_location", value=str(self.destination_location).upper())) current_destination_location = Utils.get_value_of( self.goal.parameters, "destination_location") # if location has not been set read it from userdata (either # destination_location or location) if current_destination_location is None: location = Utils.get_value_of(userdata.goal.parameters, "destination_location") if location is None: location = Utils.get_value_of(userdata.goal.parameters, "location") if location is None: rospy.logerr( "Location not specified. Not calling move_base") return 'failed' self.goal.parameters.append( KeyValue(key="destination_location", value=str(location).upper())) self.client.send_goal(self.goal) self.client.wait_for_result(rospy.Duration.from_sec(int(15.0))) state = self.client.get_state() if state == GoalStatus.SUCCEEDED: return "success" else: return "failed"
def move_head(): name_space = '/head_traj_controller/point_head_action' head_client = SimpleActionClient(name_space, PointHeadAction) head_client.wait_for_server() head_goal = PointHeadGoal() head_goal.target.header.frame_id = self.get_frame() head_goal.min_duration = rospy.Duration(0.3) head_goal.target.point = self.get_target() head_goal.max_velocity = 10.0 head_client.send_goal(head_goal) head_client.wait_for_result(rospy.Duration(2.0)) if (head_client.get_state() != GoalStatus.SUCCEEDED): rospy.logwarn('Head action unsuccessful.') self.gui.show_text_in_rviz("Head!")
class perceive_cavity(smach.State): def __init__(self, perception_mode='three_view'): smach.State.__init__(self, outcomes=["success", "failed"]) self.client = SimpleActionClient("perceive_cavity_server", GenericExecuteAction) self.client.wait_for_server() self.goal = GenericExecuteGoal() self.goal.parameters.append( KeyValue(key='perception_mode', value=perception_mode)) def execute(self, userdata): self.client.send_goal(self.goal) self.client.wait_for_result(rospy.Duration.from_sec(30.0)) state = self.client.get_state() if state == GoalStatus.SUCCEEDED: return "success" else: return "failed"
class place_object(smach.State): def __init__(self, platform_name): smach.State.__init__(self, outcomes=['success', 'failed']) self.client = SimpleActionClient('place_object_server', GenericExecuteAction) self.client.wait_for_server() self.goal = GenericExecuteGoal() self.goal.parameters.append( KeyValue(key='location', value=str(platform_name).upper())) def execute(self, userdata): self.client.send_goal(self.goal) self.client.wait_for_result(rospy.Duration.from_sec(15.0)) state = self.client.get_state() if state == GoalStatus.SUCCEEDED: return 'success' else: return 'failed'
def tilt_head(self, down=True): name_space = '/head_traj_controller/point_head_action' head_client = SimpleActionClient(name_space, PointHeadAction) head_client.wait_for_server() head_goal = PointHeadGoal() head_goal.target.header.frame_id = self.get_frame() head_goal.min_duration = rospy.Duration(0.3) if down: head_goal.target.point = Point(1, 0, Head.speed * -0.1) else: head_goal.target.point = Point(1, 0, Head.speed * 0.1) head_goal.max_velocity = 10.0 head_client.send_goal(head_goal) head_client.wait_for_result(rospy.Duration(2.0)) if (head_client.get_state() != GoalStatus.SUCCEEDED): rospy.logwarn('Head action unsuccessful.')
def main(): rospy.init_node("simple_navigation_goals") move_base_client = SimpleActionClient('move_base', MoveBaseAction) rospy.loginfo('Connecting to server') move_base_client.wait_for_server() goal = MoveBaseGoal() goal.target_pose.header.frame_id = 'komodo_1/base_link' goal.target_pose.header.stamp = rospy.Time.now() goal.target_pose.pose.position.x = -1.0 goal.target_pose.pose.orientation.w = 1.0 rospy.loginfo('Sending goal') move_base_client.send_goal(goal) move_base_client.wait_for_result() if move_base_client.get_state() == GoalStatus.SUCCEEDED: rospy.loginfo('Hooray, the base moved 1 meter forward') else: rospy.loginfo('The base failed to move forward 1 meter for some reason')
def head_action(self, x, y, z, wait = False): name_space = '/head_traj_controller/point_head_action' head_client = SimpleActionClient(name_space, PointHeadAction) head_goal = PointHeadGoal() head_goal.target.header.frame_id = 'base_link' head_goal.min_duration = rospy.Duration(1.0) head_goal.target.point = Point(x, y, z) head_client.send_goal(head_goal) if wait: # wait for the head movement to finish before we try to detect and pickup an object finished_within_time = head_client.wait_for_result(rospy.Duration(5)) # Check for success or failure if not finished_within_time: head_client.cancel_goal() rospy.loginfo("Timed out achieving head movement goal") else: state = head_client.get_state() if state == GoalStatus.SUCCEEDED: rospy.loginfo("Head movement goal succeeded!") rospy.loginfo("State:" + str(state)) else: rospy.loginfo("Head movement goal failed with error code: " + str(self.goal_states[state]))
class SocialGaze: def __init__(self): self.defaultLookatPoint = Point(1,0,1.35) self.downLookatPoint = Point(0.5,0,0.5) self.targetLookatPoint = Point(1,0,1.35) self.currentLookatPoint = Point(1,0,1.35) self.currentGazeAction = GazeGoal.LOOK_FORWARD; self.prevGazeAction = self.currentGazeAction self.prevTargetLookatPoint = array(self.defaultLookatPoint); # Some constants self.doNotInterrupt = [GazeGoal.GLANCE_RIGHT_EE, GazeGoal.GLANCE_LEFT_EE, GazeGoal.NOD, GazeGoal.SHAKE]; self.nodPositions = [Point(1,0,1.05), Point(1,0,1.55)] self.shakePositions = [Point(1,0.2,1.35), Point(1,-0.2,1.35)] self.nNods = 5 self.nShakes = 5 self.nodCounter = 5 self.shakeCounter = 5 self.facePos = None ## Action client for sending commands to the head. self.headActionClient = SimpleActionClient('/head_traj_controller/point_head_action', PointHeadAction) self.headActionClient.wait_for_server() rospy.loginfo('Head action client has responded.') self.headGoal = PointHeadGoal() self.headGoal.target.header.frame_id = 'base_link' self.headGoal.min_duration = rospy.Duration(1.0) self.headGoal.target.point = Point(1,0,1) ## Client for receiving detected faces #self.faceClient = SimpleActionClient('face_detector_action', FaceDetectorAction) ## Service client for arm states self.tfListener = TransformListener() ## Server for gaze requested gaze actions self.gazeActionServer = actionlib.SimpleActionServer('gaze_action', GazeAction, self.executeGazeAction, False) self.gazeActionServer.start() ## Callback function to receive ee states and face location def getEEPos(self, armIndex): fromFrame = '/base_link' if (armIndex == 0): toFrame = '/r_wrist_roll_link' else: toFrame = '/l_wrist_roll_link' try: t = self.tfListener.getLatestCommonTime(fromFrame, toFrame) (position, rotation) = self.tfListener.lookupTransform(fromFrame, toFrame, t) except: rospy.logerr('Could not get the end-effector pose.') #objPoseStamped = PoseStamped() #objPoseStamped.header.stamp = t #objPoseStamped.header.frame_id = '/base_link' #objPoseStamped.pose = Pose() #relEEPose = self.tfListener.transformPose(toFrame, objPoseStamped) return Point(position[0], position[1], position[2]) def getFaceLocation(self): connected = self.faceClient.wait_for_server(rospy.Duration(1.0)) if connected: fgoal = FaceDetectorGoal() self.faceClient.send_goal(fgoal) self.faceClient.wait_for_result() f = self.faceClient.get_result() ## If there is one face follow that one, if there are more than one, follow the closest one closest = -1 if len(f.face_positions) == 1: closest = 0 elif len(f.face_positions) > 0: closest_dist = 1000 for i in range(len(f.face_positions)): dist = f.face_positions[i].pos.x*f.face_positions[i].pos.x + f.face_positions[i].pos.y*f.face_positions[i].pos.y + f.face_positions[i].pos.z*f.face_positions[i].pos.z if dist < closest_dist: closest = i closest_dist = dist if closest > -1: self.facePos = array([f.face_positions[closest].pos.x, f.face_positions[closest].pos.y, f.face_positions[closest].pos.z]) else: rospy.logwarn('No faces were detected.') self.facePos = self.defaultLookatPoint else: rospy.logwarn('Not connected to the face server, cannot follow faces.') self.facePos = self.defaultLookatPoint ## Callback function for receiving gaze commands def executeGazeAction(self, goal): command = goal.action; if (self.doNotInterrupt.count(self.currentGazeAction) == 0): if (self.currentGazeAction != command or command == GazeGoal.LOOK_AT_POINT): self.isActionComplete = False if (command == GazeGoal.LOOK_FORWARD): self.targetLookatPoint = self.defaultLookatPoint elif (command == GazeGoal.LOOK_DOWN): self.targetLookatPoint = self.downLookatPoint elif (command == GazeGoal.NOD): self.nNods = 5 self.startNod() elif (command == GazeGoal.SHAKE): self.nShakes = 5 self.startShake() elif (command == GazeGoal.NOD_ONCE): self.nNods = 5 self.startNod() elif (command == GazeGoal.SHAKE_ONCE): self.nShakes = 5 self.startShake() elif (command == GazeGoal.GLANCE_RIGHT_EE): self.startGlance(0) elif (command == GazeGoal.GLANCE_LEFT_EE): self.startGlance(1) elif (command == GazeGoal.LOOK_AT_POINT): self.targetLookatPoint = goal.point rospy.loginfo('Setting gaze action to:' + str(command)) self.currentGazeAction = command while (not self.isActionComplete): time.sleep(0.1) self.gazeActionServer.set_succeeded() else: self.gazeActionServer.set_aborted() else: self.gazeActionServer.set_aborted() def isTheSame(self, current, target): diff = target - current dist = linalg.norm(diff) return (dist<0.0001) def filterLookatPosition(self, current, target): speed = 0.02 diff = self.point2array(target) - self.point2array(current) dist = linalg.norm(diff) if (dist>speed): step = dist/speed return self.array2point(self.point2array(current) + diff/step) else: return target def startNod(self): self.prevTargetLookatPoint = self.targetLookatPoint self.prevGazeAction = str(self.currentGazeAction) self.nodCounter = 0 self.targetLookatPoint = self.nodPositions[0] def startGlance(self, armIndex): self.prevTargetLookatPoint = self.targetLookatPoint self.prevGazeAction = str(self.currentGazeAction) self.glanceCounter = 0 self.targetLookatPoint = self.getEEPos(armIndex) def startShake(self): self.prevTargetLookatPoint = self.targetLookatPoint self.prevGazeAction = str(self.currentGazeAction) self.shakeCounter = 0 self.targetLookatPoint = self.shakePositions[0] def point2array(self, p): return array((p.x, p.y, p.z)) def array2point(self, a): return Point(a[0], a[1], a[2]) def getNextNodPoint(self, current, target): if (self.isTheSame(self.point2array(current), self.point2array(target))): self.nodCounter += 1 if (self.nodCounter == self.nNods): self.currentGazeAction = self.prevGazeAction return self.prevTargetLookatPoint else: return self.nodPositions[self.nodCounter%2] else: return target def getNextGlancePoint(self, current, target): if (self.isTheSame(self.point2array(current), self.point2array(target))): self.glanceCounter = 1 self.currentGazeAction = self.prevGazeAction return self.prevTargetLookatPoint else: return target def getNextShakePoint(self, current, target): if (self.isTheSame(self.point2array(current), self.point2array(target))): self.shakeCounter += 1 if (self.shakeCounter == self.nNods): self.currentGazeAction = self.prevGazeAction return self.prevTargetLookatPoint else: return self.shakePositions[self.shakeCounter%2] else: return target def update(self): isActionPossiblyComplete = True if (self.currentGazeAction == GazeGoal.FOLLOW_RIGHT_EE): self.targetLookatPoint = self.getEEPos(0) elif (self.currentGazeAction == GazeGoal.FOLLOW_LEFT_EE): self.targetLookatPoint = self.getEEPos(1) elif (self.currentGazeAction == GazeGoal.FOLLOW_FACE): self.getFaceLocation() self.targetLookatPoint = self.facePos elif (self.currentGazeAction == GazeGoal.NOD): self.targetLookatPoint = self.getNextNodPoint(self.currentLookatPoint, self.targetLookatPoint) self.headGoal.min_duration = rospy.Duration(0.5) isActionPossiblyComplete = False; elif (self.currentGazeAction == GazeGoal.SHAKE): self.targetLookatPoint = self.getNextShakePoint(self.currentLookatPoint, self.targetLookatPoint) self.headGoal.min_duration = rospy.Duration(0.5) isActionPossiblyComplete = False; elif (self.currentGazeAction == GazeGoal.GLANCE_RIGHT_EE or self.currentGazeAction == GazeGoal.GLANCE_LEFT_EE): self.targetLookatPoint = self.getNextGlancePoint(self.currentLookatPoint, self.targetLookatPoint) isActionPossiblyComplete = False; self.currentLookatPoint = self.filterLookatPosition(self.currentLookatPoint, self.targetLookatPoint) if (self.isTheSame(self.point2array(self.headGoal.target.point), self.point2array(self.currentLookatPoint))): if (isActionPossiblyComplete): if (self.headActionClient.get_state() == GoalStatus.SUCCEEDED): self.isActionComplete = True else: self.headGoal.target.point = self.currentLookatPoint self.headActionClient.send_goal(self.headGoal) time.sleep(0.02)
class ActionTasks: def __init__(self, script_path): rospy.init_node('task_cordinator') rospy.on_shutdown(self.cleanup) # Initialize a number of parameters and variables for nav locations setup_task_environment(self) # Set the default TTS voice to use self.voice = rospy.get_param("~voice", "voice_en1_mbrola") self.robot = rospy.get_param("~robot", "robbie") #self.NavPublisher = rospy.Publisher("goto", String) # Publisher to manually control the robot (e.g. to stop it) self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5) self.client = SimpleActionClient("move_base", MoveBaseAction) self.client.wait_for_server() # Create the sound client object self.soundhandle = SoundClient() # Set the wave file path if used self.wavepath = rospy.get_param("~wavepath", script_path + "/../sounds") #define afternoon and morning self.noon = strftime("%p:", localtime()) if self.noon == "AM:": self.noon1 = "Goood Morning " else: self.noon1 ="Good After Noon " self.local = strftime("%H:%M:", localtime()) #local time self.local = strftime("%H:%M:", localtime()) # Wait a moment to let the client connect to the # sound_play server rospy.sleep(1) # Make sure any lingering sound_play processes are stopped. self.soundhandle.stopAll() self.soundhandle.say(self.noon1 + self.robot +" is on line" + " the time is " + self.local, self.voice) rospy.sleep(2) # Subscribe to the recognizer output and set the callback function rospy.Subscriber('/nltk_interpret', String, self.commands) self.pub = rospy.Publisher('auto_dock', String, queue_size=1) # here we do our main text prosessing def commands(self,text): task = text.data ar = task.split(':') rospy.logwarn(ar) driver = Driver() if ar[0] == "stop": self.soundhandle.say("stopping", self.voice) self.move_base.cancel_all_goals() self.cmd_vel_pub.publish(Twist()) if ar[0] == "go": loc = ar[6]+ar[9] self.soundhandle.say(loc, self.voice) self.move_to(loc) if ar[0] == "pick": self.soundhandle.say(str(PickUp(task)), self.voice) if ar[0] == "run"and ar[5] == "demo": self.soundhandle.say("ok running show time", self.voice) ShowTime() if ar[0] == "get" and ar[9] == "beer": self.soundhandle.say("ok get a beer for you", self.voice) if ar[0] == "turn" and ar[5] == "left": self.soundhandle.say("ok turning left", self.voice) driver.turn(angle = -0.5 * math.pi, angularSpeed = 0.5);#90 degrees left turn if ar[0] == "turn" and ar[5] == "right": driver.turn(angle = 0.5 * math.pi, angularSpeed = 0.5);#90 degrees right turn self.soundhandle.say("ok turning right", self.voice) if ar[0] == "move" and ar[5] == "forward": driver.driveX(distance = 1.0, speed = 0.15) self.soundhandle.say("ok, moving forward.", self.voice) if ar[0] == "look": self.soundhandle.say("ok, looking "+ str(ar[5]), self.voice) Move_Head(task) if ar[1] == "what"and ar[5] == "this": self.soundhandle.say(" I have no idea. My vision system is not on line.", self.voice) if ar[1] == "what"and ar[5] == "time": local = strftime("%H:%M:", localtime()) self.soundhandle.say(" the time is " + local , self.voice) if ar[1] == "what"and ar[5] == "weather": self.soundhandle.say(str(Weather()) , self.voice) if ar[5] == "charge": self.soundhandle.say("Starting auto dock sequence", self.voice) self.move_base.cancel_all_goals() self.cmd_vel_pub.publish(Twist()) self.pub.publish("dock") if ar[11] != "": #self.soundhandle.say(str(Hello()), self.voice) self.soundhandle.say("hello", self.voice) def move_to(self, location): goal = MoveBaseGoal() goal.target_pose.pose = self.room_locations[location] goal.target_pose.header.frame_id = 'map' goal.target_pose.header.stamp = rospy.Time.now() self.client.send_goal(goal) self.client.wait_for_result(rospy.Duration.from_sec(40.0)) if self.client.get_state() == GoalStatus.SUCCEEDED: result = self.client.get_result() print "Result: SUCCEEDED " elif self.client.get_state() == GoalStatus.PREEMPTED: print "Action pre-empted" else: print "Action failed" def cleanup(self): self.soundhandle.stopAll() self.cmd_vel_pub.publish(Twist()) rospy.loginfo("Shutting down talk node...")
import roslib roslib.load_manifest('rospy') roslib.load_manifest('actionlib') roslib.load_manifest('actionlib_msgs') roslib.load_manifest('control_msgs') roslib.load_manifest('geometry_msgs') import rospy from control_msgs.msg import PointHeadAction from control_msgs.msg import PointHeadGoal from actionlib import SimpleActionClient from actionlib_msgs.msg import GoalStatus from geometry_msgs.msg import Point if __name__ == "__main__": rospy.init_node('head_test_node', anonymous=True) name_space = '/head_traj_controller/point_head_action' head_client = SimpleActionClient(name_space, PointHeadAction) head_client.wait_for_server() head_goal = PointHeadGoal() head_goal.target.header.frame_id = 'base_link' head_goal.min_duration = rospy.Duration(1.0) # head_goal.target.point = Point(1, 0, 1) # look forward! head_goal.target.point = Point(1, 0, 0.5) # look forward! head_client.send_goal(head_goal) head_client.wait_for_result(rospy.Duration(10.0)) if (head_client.get_state() != GoalStatus.SUCCEEDED): rospy.logwarn('Head action unsuccessful.')
class Arm: ''' Interfacing with one arm for controlling mode and action execution''' _is_autorelease_on = False def __init__(self, arm_index): self.arm_index = arm_index self.arm_mode = ArmMode.HOLD self.gripper_state = None self.gripper_joint_name = self._side_prefix() + '_gripper_joint' self.ee_name = self._side_prefix() + '_wrist_roll_link' self.joint_names = [self._side_prefix() + '_shoulder_pan_joint', self._side_prefix() + '_shoulder_lift_joint', self._side_prefix() + '_upper_arm_roll_joint', self._side_prefix() + '_elbow_flex_joint', self._side_prefix() + '_forearm_roll_joint', self._side_prefix() + '_wrist_flex_joint', self._side_prefix() + '_wrist_roll_joint'] self.all_joint_names = [] self.all_joint_poses = [] self.last_ee_pose = None self.movement_buffer_size = 40 self.last_unstable_time = rospy.Time.now() self.arm_movement = [] rospy.loginfo('Initializing ' + self._side() + ' arm.') switch_controller = 'pr2_controller_manager/switch_controller' rospy.wait_for_service(switch_controller) self.switch_service = rospy.ServiceProxy(switch_controller, SwitchController) rospy.loginfo('Got response form the switch controller for ' + self._side() + ' arm.') # Create a trajectory action client traj_controller_name = (self._side_prefix() + '_arm_controller/joint_trajectory_action') self.traj_action_client = SimpleActionClient( traj_controller_name, JointTrajectoryAction) self.traj_action_client.wait_for_server() rospy.loginfo('Got response form trajectory action server for ' + self._side() + ' arm.') # Set up Inversse Kinematics self.ik_srv = None self.ik_request = None self.ik_joints = None self.ik_limits = None self._setup_ik() gripper_name = (self._side_prefix() + '_gripper_controller/gripper_action') self.gripper_client = SimpleActionClient(gripper_name, Pr2GripperCommandAction) self.gripper_client.wait_for_server() rospy.loginfo('Got response form gripper server for ' + self._side() + ' arm.') filter_srv_name = '/trajectory_filter/filter_trajectory' rospy.wait_for_service(filter_srv_name) self.filter_service = rospy.ServiceProxy(filter_srv_name, FilterJointTrajectory) rospy.loginfo('Filtering service has responded for ' + self._side() + ' arm.') self.lock = threading.Lock() rospy.Subscriber('joint_states', JointState, self.joint_states_cb) def _setup_ik(self): '''Sets up services for inverse kinematics''' ik_info_srv_name = ('pr2_' + self._side() + '_arm_kinematics_simple/get_ik_solver_info') ik_srv_name = 'pr2_' + self._side() + '_arm_kinematics_simple/get_ik' rospy.wait_for_service(ik_info_srv_name) ik_info_srv = rospy.ServiceProxy(ik_info_srv_name, GetKinematicSolverInfo) solver_info = ik_info_srv() rospy.loginfo('IK info service has responded for ' + self._side() + ' arm.') rospy.wait_for_service(ik_srv_name) self.ik_srv = rospy.ServiceProxy(ik_srv_name, GetPositionIK, persistent=True) rospy.loginfo('IK service has responded for ' + self._side() + ' arm.') # Set up common parts of an IK request self.ik_request = GetPositionIKRequest() self.ik_request.timeout = rospy.Duration(4.0) self.ik_joints = solver_info.kinematic_solver_info.joint_names self.ik_limits = solver_info.kinematic_solver_info.limits ik_links = solver_info.kinematic_solver_info.link_names request = self.ik_request.ik_request request.ik_link_name = ik_links[0] request.pose_stamped.header.frame_id = 'base_link' request.ik_seed_state.joint_state.name = self.ik_joints request.ik_seed_state.joint_state.position = [0] * len(self.ik_joints) def _side(self): '''Returns the word right or left depending on arm side''' if (self.arm_index == Side.RIGHT): return 'right' elif (self.arm_index == Side.LEFT): return 'left' def _side_prefix(self): ''' Returns the letter r or l depending on arm side''' side = self._side() return side[0] def get_ee_state(self, ref_frame='base_link'): ''' Returns end effector pose for the arm''' try: time = World.tf_listener.getLatestCommonTime(ref_frame, self.ee_name) (position, orientation) = World.tf_listener.lookupTransform( ref_frame, self.ee_name, time) ee_pose = Pose() ee_pose.position = Point(position[0], position[1], position[2]) ee_pose.orientation = Quaternion(orientation[0], orientation[1], orientation[2], orientation[3]) return ee_pose except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logwarn('Something wrong with transform request.') return None def joint_states_cb(self, msg): '''Callback function that saves the joint positions when a joint_states message is received''' self.lock.acquire() self.all_joint_names = msg.name self.all_joint_poses = msg.position self.lock.release() def get_joint_state(self, joint_names=None): '''Returns position for the requested or all arm joints''' if joint_names == None: joint_names = self.joint_names if self.all_joint_names == []: rospy.logerr("No robot_state messages received!\n") return [] positions = [] self.lock.acquire() for joint_name in joint_names: if joint_name in self.all_joint_names: index = self.all_joint_names.index(joint_name) position = self.all_joint_poses[index] positions.append(position) else: rospy.logerr("Joint %s not found!", joint_name) self.lock.release() return positions def _solve_ik(self, ee_pose, seed=None): '''Gets the IK solution for end effector pose''' self.ik_request.ik_request.pose_stamped.pose = ee_pose if seed == None: # If no see is specified for IK search, start search at midpoint seed = [] for i in range(0, len(self.ik_joints)): seed.append((self.ik_limits[i].min_position + self.ik_limits[i].max_position) / 2.0) self.ik_request.ik_request.ik_seed_state.joint_state.position = seed try: rospy.loginfo('Sending IK request.') response = self.ik_srv(self.ik_request) if(response.error_code.val == response.error_code.SUCCESS): return response.solution.joint_state.position else: return None except rospy.ServiceException: rospy.logerr('Exception while getting the IK solution.') return None def set_mode(self, mode): '''Releases or holds the arm by turning the controller on/off''' controller_name = self._side_prefix() + '_arm_controller' if mode == ArmMode.RELEASE: start_controllers = [] stop_controllers = [controller_name] rospy.loginfo('Switching ' + str(self._side()) + ' arm to the kinesthetic mode') elif mode == ArmMode.HOLD: start_controllers = [controller_name] stop_controllers = [] rospy.loginfo('Switching ' + str(self._side()) + ' to the Joint-control mode') else: rospy.logwarn('Unknown mode ' + str(mode) + '. Keeping the current mode.') return try: self.switch_service(start_controllers, stop_controllers, 1) self.arm_mode = mode except rospy.ServiceException: rospy.logerr("Service did not process request") def get_mode(self): '''Returns the current arm mode (released/holding)''' return self.arm_mode def _send_gripper_command(self, pos=0.08, eff=30.0, wait=False): '''Sets the position of the gripper''' command = Pr2GripperCommandGoal() command.command.position = pos command.command.max_effort = eff self.gripper_client.send_goal(command) if wait: self.gripper_client.wait_for_result(rospy.Duration(10.0)) def is_gripper_moving(self): ''' Whether or not the gripper is in the process of opening/closing''' return (self.gripper_client.get_state() == GoalStatus.ACTIVE or self.gripper_client.get_state() == GoalStatus.PENDING) def is_gripper_at_goal(self): ''' Whether or not the gripper has reached its goal''' return (self.gripper_client.get_state() == GoalStatus.SUCCEEDED) def get_gripper_state(self): '''Returns current gripper state''' return self.gripper_state def check_gripper_state(self, joint_name=None): '''Checks gripper state at the hardware level''' if (joint_name == None): joint_name = self.gripper_joint_name gripper_pos = self.get_joint_state([joint_name]) if gripper_pos != []: if gripper_pos[0] > 0.078: self.gripper_state = GripperState.OPEN else: self.gripper_state = GripperState.CLOSED else: rospy.logwarn('Could not update the gripper state.') def open_gripper(self, pos=0.08, eff=30.0, wait=False): '''Opens gripper''' self._send_gripper_command(pos, eff, wait) self.gripper_state = GripperState.OPEN def close_gripper(self, pos=0.0, eff=30.0, wait=False): '''Closes gripper''' self._send_gripper_command(pos, eff, wait) self.gripper_state = GripperState.CLOSED def set_gripper(self, gripper_state): '''Sets gripper to the desired state''' if (gripper_state == GripperState.CLOSED): self.close_gripper() elif (gripper_state == GripperState.OPEN): self.open_gripper() def exectute_joint_traj(self, joint_trajectory, timing): '''Moves the arm through the joint sequence''' # First, do filtering on the trajectory to fix the velocities trajectory = JointTrajectory() # Initialize the server # When to start the trajectory: 0.1 seconds from now trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.1) trajectory.joint_names = self.joint_names ## Add all frames of the trajectory as way points for i in range(len(timing)): positions = joint_trajectory[i].joint_pose velocities = [0] * len(positions) # Add frames to the trajectory trajectory.points.append(JointTrajectoryPoint(positions=positions, velocities=velocities, time_from_start=timing[i])) output = self.filter_service(trajectory=trajectory, allowed_time=rospy.Duration.from_sec(20)) rospy.loginfo('Trajectory for arm ' + str(self.arm_index) + ' has been filtered.') traj_goal = JointTrajectoryGoal() # TO-DO: check output.error_code traj_goal.trajectory = output.trajectory traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1)) traj_goal.trajectory.joint_names = self.joint_names # Sends the goal to the trajectory server self.traj_action_client.send_goal(traj_goal) def move_to_joints(self, joints, time_to_joint): '''Moves the arm to the desired joints''' # Setup the goal traj_goal = JointTrajectoryGoal() traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1)) traj_goal.trajectory.joint_names = self.joint_names velocities = [0] * len(joints) traj_goal.trajectory.points.append(JointTrajectoryPoint( positions=joints, velocities=velocities, time_from_start=rospy.Duration(time_to_joint))) # Client sends the goal to the Server self.traj_action_client.send_goal(traj_goal) def is_executing(self): '''Whether or not there is an ongoing action execution on the arm''' return (self.traj_action_client.get_state() == GoalStatus.ACTIVE or self.traj_action_client.get_state() == GoalStatus.PENDING) def is_successful(self): '''Whetehr the execution succeeded''' return (self.traj_action_client.get_state() == GoalStatus.SUCCEEDED) def get_ik_for_ee(self, ee_pose, seed): ''' Finds the IK solution for given end effector pose''' joints = self._solve_ik(ee_pose, seed) ## If our seed did not work, try once again with the default seed if joints == None: rospy.logwarn('Could not find IK solution with preferred seed,' + 'will try default seed.') joints = self._solve_ik(ee_pose) if joints == None: rospy.logwarn('IK out of bounds, will use the seed directly.') else: rollover = array((array(joints) - array(seed)) / pi, int) joints -= ((rollover + (sign(rollover) + 1) / 2) / 2) * 2 * pi return joints @staticmethod def get_distance_bw_poses(pose0, pose1): '''Returns the dissimilarity between two end-effector poses''' w_pos = 1.0 w_rot = 0.2 pos0 = array((pose0.position.x, pose0.position.y, pose0.position.z)) pos1 = array((pose1.position.x, pose1.position.y, pose1.position.z)) rot0 = array((pose0.orientation.x, pose0.orientation.y, pose0.orientation.z, pose0.orientation.w)) rot1 = array((pose1.orientation.x, pose1.orientation.y, pose1.orientation.z, pose1.orientation.w)) pos_dist = w_pos * norm(pos0 - pos1) rot_dist = w_rot * (1 - dot(rot0, rot1)) if (pos_dist > rot_dist): dist = pos_dist else: dist = rot_dist return dist def reset_movement_history(self): ''' Clears the saved history of arm movements''' self.last_unstable_time = rospy.Time.now() self.arm_movement = [] def get_movement(self): '''Returns cumulative movement in recent history''' return sum(self.arm_movement) def _record_arm_movement(self, reading): '''Records the sensed arm movement''' self.arm_movement = [reading] + self.arm_movement if (len(self.arm_movement) > self.movement_buffer_size): self.arm_movement = self.arm_movement[0:self.movement_buffer_size] def _is_arm_moved_while_holding(self): '''Checks if user is trying to move the arm while it is stiff''' threshold = 0.02 if (self.get_mode() == ArmMode.HOLD and (len(self.arm_movement) == self.movement_buffer_size) and (self.get_movement() > threshold)): return True return False def _is_arm_stable_while_released(self): '''Checks if the arm has been stable while being released''' movement_threshold = 0.02 time_threshold = rospy.Duration(5.0) is_arm_stable = (self.get_movement() < movement_threshold) if (not is_arm_stable or self.get_mode() == ArmMode.HOLD): self.last_unstable_time = rospy.Time.now() return False else: if (rospy.Time.now() - self.last_unstable_time) > time_threshold: return True else: return False def update(self, is_executing): ''' Periodical update for one arm''' ee_pose = self.get_ee_state() if (ee_pose != None and self.last_ee_pose != None): self._record_arm_movement(Arm.get_distance_bw_poses(ee_pose, self.last_ee_pose)) self.last_ee_pose = ee_pose if (not is_executing and Arm._is_autorelease_on): if (self._is_arm_moved_while_holding()): rospy.loginfo('Automatically releasing arm.') self.set_mode(ArmMode.RELEASE) if (self._is_arm_stable_while_released()): rospy.loginfo('Automatically holding arm.') self.set_mode(ArmMode.HOLD)
class ShowTime: def __init__(self, script_path): rospy.init_node('show_time') rospy.on_shutdown(self.cleanup) # Initialize a number of parameters and variables for nav locations # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) # Initialize the move group for the arms right_arm = MoveGroupCommander('right_arm') left_arm = MoveGroupCommander('left_arm') right_gripper = MoveGroupCommander('right_gripper') left_gripper = MoveGroupCommander('left_gripper') # Set the default TTS voice to use self.voice = rospy.get_param("~voice", "voice_don_diphone") self.robot = rospy.get_param("~robot", "robbie") self.start =(Pose(Point(1.7, 0, 0.0), Quaternion(0.0, 0.0, 1.000, 0.018))) self.left =(Pose(Point(1.960, -1.854, 0.0), Quaternion(0.0, 0.0, 0.916, 0.401))) self.right =(Pose(Point(2.123, 1.592, 0.0), Quaternion(0.0, 0.0, 0.877, -0.480))) self.auto_dock =(Pose(Point(0.5, 0, 0.0), Quaternion(0.0, 0.0, 0.002, 1.000))) self.client = SimpleActionClient("move_base", MoveBaseAction) self.client.wait_for_server() # Create the sound client object self.soundhandle = SoundClient() # Wait a moment to let the client connect to the # sound_play server rospy.sleep(2) # Make sure any lingering sound_play processes are stopped. self.soundhandle.stopAll() # Set the wave file path if used self.wavepath = rospy.get_param("~wavepath", script_path + "/../sounds") #define afternoon and morning self.noon = strftime("%p:", localtime()) if self.noon == "AM:": self.noon1 = "Goood Morning " else: self.noon1 ="Good After Noon " self.local = strftime("%H:%M:", localtime()) #local time self.local = strftime("%H:%M:", localtime()) self.soundhandle.say(self.noon1 + self.robot +" is on line" + " the time is " + self.local, self.voice) right_arm.set_named_target('r_travel') right_arm.go() left_arm.set_named_target('l_travel') left_arm.go() rospy.sleep(5) self.move_to(self.start) self.soundhandle.say("Welcome to SHOW time. This is where I get to demonstrate my capabilities" , self.voice) rospy.sleep(6) self.soundhandle.say("I can move to my left", self.voice) rospy.sleep(2) self.move_to(self.left) self.soundhandle.say("now back to the start position", self.voice) rospy.sleep(2) self.move_to(self.start) self.soundhandle.say("I can move to my right", self.voice) rospy.sleep(2) self.move_to(self.right) self.soundhandle.say("And again back to the start position", self.voice) rospy.sleep(2) self.move_to(self.start) self.soundhandle.say("I once caught a fish this big", self.voice) right_arm.set_named_target('r_fish') right_arm.go() left_arm.set_named_target('l_fish') left_arm.go() #rospy.sleep(2) self.soundhandle.say("Thank you for your time ", self.voice) right_arm.set_named_target('right_start') right_arm.go() left_arm.set_named_target('left_start') left_arm.go() #rospy.sleep(2) #rospy.sleep(2) self.move_to(self.auto_dock) # add auto dock sequence here # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0) def move_to(self, location): goal = MoveBaseGoal() goal.target_pose.pose = location goal.target_pose.header.frame_id = 'map' goal.target_pose.header.stamp = rospy.Time.now() self.client.send_goal(goal) self.client.wait_for_result(rospy.Duration.from_sec(40.0)) if self.client.get_state() == GoalStatus.SUCCEEDED: result = self.client.get_result() print "Result: SUCCEEDED " elif self.client.get_state() == GoalStatus.PREEMPTED: print "Action pre-empted" else: print "Action failed" def cleanup(self): self.soundhandle.stopAll() rospy.loginfo("Shutting down show time node...")
class SimpleExerciser(unittest.TestCase): def setUp(self): self.default_wait = rospy.Duration(60.0) self.client = SimpleActionClient('test_request_action', TestRequestAction) self.assert_(self.client.wait_for_server(self.default_wait)) def test_just_succeed(self): goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_SUCCESS, the_result = 42) self.client.send_goal(goal) self.client.wait_for_result(self.default_wait) self.assertEqual(GoalStatus.SUCCEEDED, self.client.get_state()) self.assertEqual(42, self.client.get_result().the_result) def test_just_abort(self): goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_ABORTED, the_result = 42) self.client.send_goal(goal) self.client.wait_for_result(self.default_wait) self.assertEqual(GoalStatus.ABORTED, self.client.get_state()) self.assertEqual(42, self.client.get_result().the_result) def test_just_preempt(self): goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_SUCCESS, delay_terminate = rospy.Duration(100000), the_result = 42) self.client.send_goal(goal) # Ensure that the action server got the goal, before continuing timeout_time = rospy.Time.now() + self.default_wait while rospy.Time.now() < timeout_time: if (self.client.get_state() != GoalStatus.PENDING): break self.client.cancel_goal() self.client.wait_for_result(self.default_wait) self.assertEqual(GoalStatus.PREEMPTED, self.client.get_state()) self.assertEqual(42, self.client.get_result().the_result) # Should print out errors about not setting a terminal status in the action server. def test_drop(self): goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_DROP, the_result = 42) self.client.send_goal(goal) self.client.wait_for_result(self.default_wait) self.assertEqual(GoalStatus.ABORTED, self.client.get_state()) self.assertEqual(0, self.client.get_result().the_result) # Should print out errors about throwing an exception def test_exception(self): goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_EXCEPTION, the_result = 42) self.client.send_goal(goal) self.client.wait_for_result(self.default_wait) self.assertEqual(GoalStatus.ABORTED, self.client.get_state()) self.assertEqual(0, self.client.get_result().the_result) def test_ignore_cancel_and_succeed(self): goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_SUCCESS, delay_terminate = rospy.Duration(2.0), ignore_cancel = True, the_result = 42) self.client.send_goal(goal) # Ensure that the action server got the goal, before continuing timeout_time = rospy.Time.now() + self.default_wait while rospy.Time.now() < timeout_time: if (self.client.get_state() != GoalStatus.PENDING): break self.client.cancel_goal() self.client.wait_for_result(self.default_wait) self.assertEqual(GoalStatus.SUCCEEDED, self.client.get_state()) self.assertEqual(42, self.client.get_result().the_result) def test_lose(self): goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_LOSE, the_result = 42) self.client.send_goal(goal) self.client.wait_for_result(self.default_wait) self.assertEqual(GoalStatus.LOST, self.client.get_state())
class map_bot_base(): ########### # __init__: Class instantiator. ########## def __init__(self): # global self.speed = Twist() self.state = FSM_STATES.FSM_AUTONOMY self.goal = MoveBaseGoal() self.base_pose = MoveBaseGoal() self.serv_state = String() # move_base Action Client self._move_base = SimpleActionClient('move_base', MoveBaseAction) # Subscribtions rospy.Subscriber('/map_bot_base/goal', PoseStamped, self.goal_callback) rospy.Subscriber('/client_node/serv_state', String, self.serv_state_callback) # Publications self.state_pub = rospy.Publisher('/map_bot_base/state', String, queue_size=10) # /navigation_velocity_smoother/raw_cmd_vel # /mobile_base/commands/velocity # Kill start up node to save processing power rospy.loginfo('Killing start up node......') subprocess.call('rosnode kill /start_up_node', shell = True) # Set up delay to allow for hector to startup rospy.loginfo('Starting up processes......') for i in range(9): # 10 second delay rospy.loginfo('Initializing behaviour in %d' % (10-i)) rospy.sleep(rospy.Duration(1)) ################# # goal_callback : Run everytime '\bin_bot_base\goal' topic receives message # # Sends a goal to the Action Server upon callback ################# def goal_callback(self, pose_stamped): self.goal.target_pose = pose_stamped if(self.goal): self._move_base.send_goal(self.goal) rospy.loginfo("Just passed a goal") ###################### # serv_state_callback : Run everytime the bin updates it's state. ###################### def serv_state_callback(self, serv_state): self.serv_state = serv_state ################# # spin_callback : Handles state transitions ################# def spin_callback(self): # Print state print ("\n") rospy.loginfo("STATE : %s" % FSM_STATES.__geitem__(self.state)) # FSM_AUTONOMY STATE # =============> if self.state == FSM_STATES.FSM_AUTONOMY: # Let hector handle navigation # Next state transition if (self._move_base.simple_state != 2): # if a goal is detected rospy.loginfo('Killing hector navigation nodes.......') subprocess.call('rosnode kill /hector_exploration_node', shell = True) self.state = FSM_STATES.FSM_MANUAL else: self.state = FSM_STATES.FSM_AUTONOMY # FSM_MANUAL STATE # =============> if self.state == FSM_STATES.FSM_MANUAL: # Let move base handle navigation # Next state transition # if-else statement to ensure error-less view of goal status if (self.serv_state.data == 'MAP_COMPLETE'): # if a goal is detected self.state = FSM_STATES.FSM_MOVE_TO_BASE else: self.state = FSM_STATES.FSM_MANUAL # FSM_MOVE_TO_BASE STATE # =============> if self.state == FSM_STATES.FSM_MOVE_TO_BASE: # Send a move base goal to the reset position # and go to FSM_IDLE state self.base_pose.target_pose.header.frame_id = 'map' self.base_pose.target_pose.header.stamp = rospy.Time.now() self.base_pose.target_pose.pose.position.x = 0.5 self.base_pose.target_pose.pose.position.y = 0.5 self.base_pose.target_pose.pose.position.z = 0.0 self.base_pose.target_pose.pose.orientation.x = 0.0 self.base_pose.target_pose.pose.orientation.y = 0.0 self.base_pose.target_pose.pose.orientation.z = 0.0 self.base_pose.target_pose.pose.orientation.w = 0.0 self._move_base.send_goal(self.base_pose) self.goal_state = self._move_base.get_state() rospy.loginfo("Map complete!. Moving back to base...") rospy.loginfo("Goal Status: %f" % self.goal_state) self._move_base.wait_for_result() if self._move_base.get_state() == GoalStatus.SUCCEEDED: rospy.loginfo('Goal Reached!') else: rospy.loginfo('Goal aborted!') self.state = FSM_STATES.FSM_IDLE # FSM_IDLE STATE # =============> if self.state == FSM_STATES.FSM_IDLE: # Spin self.speed.linear.x = 0.0 self.speed.angular.z = 0.0 # Next state transition # if-else statement to ensure error-less view of goal status if (self._move_base.simple_state == 1): # if goal is detected self.state = FSM_STATES.FSM_NAVIGATE else: self.state = FSM_STATES.FSM_IDLE # Print general info self.printINFO() # Publish /bin_bot_base/state self.state_pub.publish(String(FSM_STATES.__geitem__(self.state))) ############## # printINFO(): Prints general debug info ############# def printINFO(self): # Print debug info about linear and angular speeds rospy.loginfo("speedPID: %f" %(self.speed.linear.x)) rospy.loginfo("anglePID: %f" %(self.speed.angular.z)) # Print goal status info if (self._move_base.simple_state == 2): self.goal_state = 'No goals received' rospy.loginfo("Goal Status: %s" %self.goal_state) else: # if a goal is detected self.goal_state = self._move_base.get_state() rospy.loginfo("Goal Status: %f" %self.goal_state) rospy.loginfo(self.serv_state.data) ######### # spin(): Provides desired rate for speed_callback() and ensures termination ######## def spin(self): rate = rospy.Rate(RATE) while not rospy.is_shutdown(): self.spin_callback() rate.sleep()
class LocalSearch(): VISUAL_FIELD_SIZE = 40 MIN_HEAD_ANGLE = -140 MAX_HEAD_ANGLE = 140 _feedback = LocalSearchFeedback() _result = LocalSearchResult() def __init__(self): self._action_name = 'local_search' self.found_marker = False self.tracking_started = False #initialize head mover name_space = '/head_traj_controller/point_head_action' self.head_client = SimpleActionClient(name_space, PointHeadAction) self.head_client.wait_for_server() rospy.loginfo('%s: Action client for PointHeadAction running' % self._action_name) #initialize tracker mark rospy.Subscriber('catch_me_destination_publisher', AlvarMarker, self.marker_tracker) rospy.loginfo('%s: subscribed to AlvarMarkers' % self._action_name) #initialize this client self._as = SimpleActionServer(self._action_name, LocalSearchAction, execute_cb=self.run, auto_start=False) self._as.start() rospy.loginfo('%s: started' % self._action_name) def marker_tracker(self, marker): if self.tracking_started: self.found_marker = True rospy.loginfo('%s: marker found' % self._action_name) def run(self, goal): success = False self.tracking_started = True self.found_marker = False rospy.loginfo('%s: Executing search for AR marker' % self._action_name) # define a set of ranges to search search_ranges = [ # first search in front of the robot (0, self.VISUAL_FIELD_SIZE), (self.VISUAL_FIELD_SIZE, -self.VISUAL_FIELD_SIZE), # then search all directions (-self.VISUAL_FIELD_SIZE, self.MAX_HEAD_ANGLE), (self.MAX_HEAD_ANGLE, self.MIN_HEAD_ANGLE), (self.MIN_HEAD_ANGLE, 0) ] range_index = 0 #success = self.search_range(*(search_ranges[range_index])) while (not success) and range_index < len(search_ranges) - 1: if self._as.is_preempt_requested(): rospy.loginfo('%s: Premepted' % self._action_name) self._as.set_preempted() break range_index = range_index + 1 success = self.search_range(*(search_ranges[range_index])) if success: rospy.loginfo('%s: Succeeded' % self._action_name) self._as.set_succeeded() else: self._as.set_aborted() self.tracking_started = False def search_range(self, start_angle, end_angle): rospy.loginfo("{}: searching range {} {}".format(self._action_name, start_angle, end_angle)) angle_tick = self.VISUAL_FIELD_SIZE if (start_angle < end_angle) else -self.VISUAL_FIELD_SIZE for cur_angle in xrange(start_angle, end_angle, angle_tick): if self._as.is_preempt_requested(): return False head_goal = self.lookat_goal(cur_angle) rospy.loginfo('%s: Head move goal for %s: %s produced' % (self._action_name, str(cur_angle), str(head_goal))) self.head_client.send_goal(head_goal) self.head_client.wait_for_result(rospy.Duration.from_sec(5.0)) if (self.head_client.get_state() != GoalStatus.SUCCEEDED): rospy.logwarn('Head could not move to specified location') break # pause at each tick rospy.sleep(0.3) if (self.found_marker): # found a marker! return True # no marker found return False def lookat_goal(self, angle): head_goal = PointHeadGoal() head_goal.target.header.frame_id = '/torso_lift_link' head_goal.max_velocity = 0.8 angle_in_radians = math.radians(angle) x = math.cos(angle_in_radians) * 5 y = math.sin(angle_in_radians) * 5 z = -0.5 head_goal.target.point = Point(x, y, z) return head_goal
class arm_bot_base(): ########### # __init__: Class instantiator. ########## def __init__(self, KpRot, KdRot, tol_z, tol_ang, maxRotSpeed, max_z, min_z): # global self.speed = Twist() self.tracked_blob = Twist() self.old_tracked_blob = Twist() self.state = FSM_STATES.FSM_LOCALIZE self.obj_pose = MoveBaseGoal() self.goal = MoveBaseGoal() self.base_pose = MoveBaseGoal() self.filtered_blobs_3d = Blobs3d() self.blobs_detected = False self.miss_counter = 0 self.correction_start = 0.0 self.serv_state = String() self.arm_state = String() self.blob_count = 0 self.avg_z = 0.0 self.yaw = 0.0 self.init_yaw = 0.0 self.target_yaw = 0.0 self.RotDerivator = 0.0 self.yawDerivator = 0.0 # params self.KpRot = KpRot self.KdRot = KdRot self.tol_z = tol_z self.tol_ang = tol_ang self.maxRotSpeed = maxRotSpeed self.max_z = max_z self.min_z = min_z # move_base Action Client self._move_base = SimpleActionClient('move_base', MoveBaseAction) # Subscribtions rospy.Subscriber('/blobs_3d', Blobs3d, self.blob_callback) rospy.Subscriber('/arm_bot_base/goal', PoseStamped, self.goal_callback) rospy.Subscriber('/client_node/serv_state', String, self.serv_state_callback) rospy.Subscriber('/uarm/state', String, self.arm_state_callback) rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, self.amcl_callback) # Publications self.speed_pub = rospy.Publisher('/navigation_velocity_smoother/raw_cmd_vel', Twist) #/navigation_velocity_smoother/raw_cmd_vel # /mobile_base/commands/velocity self.target_pose_pub = rospy.Publisher('/arm_bot_base/target_pose', PoseStamped) self.state_pub = rospy.Publisher('/arm_bot_base/state', String, queue_size=10) #/navigation_velocity_smoother/raw_cmd_vel # /mobile_base/commands/velocity # Kill start up node to save performance rospy.loginfo('Killing start up node......') subprocess.call('rosnode kill /start_up_node', shell = True) # Set up delay to allow for amcl to startup rospy.loginfo('Starting up processes......') for i in range (9): # 10 second delay rospy.loginfo('Initializing behaviour in %d' %(10-i)) rospy.sleep(rospy.Duration(1)) self.loc_start = rospy.Time.now() ################# # goal_callback : Run everytime '\arm_bot_base\goal' topic receives message # # Sends a goal to the Action Server upon callback ################# def goal_callback(self, pose_stamped): self.goal.target_pose = pose_stamped if(self.goal): self._move_base.send_goal(self.goal) rospy.loginfo("Just passed a goal") ################# # blob_callback : Run everytime \blobs_3d receives message. Filters blobs depending on their position to exclude blobs outside # # the arena and a certain range. The largest blob is selected and recorded for processing. ################# def blob_callback(self, blobs_3d): self.old_tracked_blob = self.tracked_blob self.filtered_blobs_3d = Blobs3d() # Initialize empty filtered blobs list self.max_blob_area = 0 # reset maximum blob area self.sum_x = 0.0 self.blob_count = 0 # Filter blobs if(len(blobs_3d.blobs)): for blob in blobs_3d.blobs: if (blob.center.y >= 0.000 and blob.center.z <= 0.0012): # if blob is closer than 1.2m and below sensor level self.filtered_blobs_3d.blobs.append(blob) # if filtered list contains blobs if(len(self.filtered_blobs_3d.blobs)): self.blobs_detected = True # signal that a blobs have been detected self.miss_counter = 0 # reset miss counter # Find largest blob and track it for blob in self.filtered_blobs_3d.blobs: self.blob_count = self.blob_count + 1 self.sum_x = self.sum_x + blob.center.x if(blob.area > self.max_blob_area): self.max_blob_area = blob.area self.tracked_blob.linear = self.getPose(blob) self.avg_x = float(self.sum_x / self.blob_count) self.update_avg_z() self.tracked_blob.linear.x = self.avg_x self.tracked_blob.linear.z = self.avg_z self.pub_tracked_blob_tf(self.tracked_blob.linear) # else if filtered list does not contain any blobs else: self.miss_counter += 1 # increment miss count if(self.miss_counter >= 10): # Allow for 10 callbacks before signaling non-detection self.blobs_detected = False self.avg_z = 0 self.tracked_blob = Twist() ###################### # arm_state_callback : Run everytime the arm updates it's state. ###################### def arm_state_callback(self, arm_state): self.arm_state = arm_state ###################### # serv_state_callback : Run everytime the server updates it's state. ###################### def serv_state_callback(self, serv_state): self.serv_state = serv_state if self.serv_state.data == "RESET": self.state = FSM_STATES.FSM_RESET self.reset_time = rospy.Time.now() # Used during initial wait ################# # amcl_callback : Debug callback to compute yaw ################# def amcl_callback(self, robot_pose): self.robot_pose = robot_pose (roll,pitch,self.yaw) = euler_from_quaternion([self.robot_pose.pose.pose.orientation.x, \ self.robot_pose.pose.pose.orientation.y, self.robot_pose.pose.pose.orientation.z, self.robot_pose.pose.pose.orientation.w]) ################# # speed_callback : Pubishes velocity and handles state transitions ################# def speed_callback(self): self.zStable = False # flag to signify desired z satisfied self.angStable = False # flag to signify desired angle satisfied self.sleepDuration = rospy.Duration(3) # duration of forward movement # Print state print ("\n") rospy.loginfo("STATE : %s" % FSM_STATES.__geitem__(self.state)) # FSM_LOCALIZE STATE # =============> if self.state == FSM_STATES.FSM_LOCALIZE: # Spin self.speed.linear.x = 0.0 self.speed.angular.z = 0.5 # Next state transition # if-else statement to ensure error-less view of goal status if(rospy.Time.now() < self.loc_start + rospy.Duration(20)): self.state = FSM_STATES.FSM_LOCALIZE else: self.state = FSM_STATES.FSM_IDLE # FSM_IDLE STATE # =============> if self.state == FSM_STATES.FSM_IDLE: # Stay still self.speed.linear.x = 0.0 self.speed.angular.z = 0.0 # Next state transition # if-else statement to ensure error-less view of goal status if (self._move_base.simple_state == 1): # if goal is detected self.state = FSM_STATES.FSM_NAVIGATE else: self.state = FSM_STATES.FSM_IDLE # FSM_RESET STATE # =============> if self.state == FSM_STATES.FSM_RESET: self.reset_goal = MoveBaseGoal() self.reset_goal.target_pose.header.frame_id = 'map' self.reset_goal.target_pose.header.stamp = rospy.Time.now() self.reset_goal.target_pose.pose.position.x = 0.0 self.reset_goal.target_pose.pose.position.y = -2.0 self.reset_goal.target_pose.pose.position.z = 0.0 self.reset_goal.target_pose.pose.orientation.x = 0.0 self.reset_goal.target_pose.pose.orientation.y = 0.0 self.reset_goal.target_pose.pose.orientation.z = 0.5 self.reset_goal.target_pose.pose.orientation.w = 1.00 self._move_base.send_goal(self.reset_goal) self.goal_state = self._move_base.get_state() rospy.loginfo("Reset signal received by server. Moving back to base...") rospy.loginfo("Goal Status: %f" %self.goal_state) self._move_base.wait_for_result() if self._move_base.get_state() == GoalStatus.SUCCEEDED: rospy.loginfo('Goal Reached!') else: rospy.loginfo('Goal aborted!') rospy.wait_for_service('/move_base/clear_costmaps') self.clear_costmaps = rospy.ServiceProxy('/move_base/clear_costmaps', Empty) try: rospy.wait_for_service('/move_base/clear_costmaps') self.clear_costmaps() except rospy.ServiceException as exc: print("Service did not process request: " + str(exc)) self.state = FSM_STATES.FSM_IDLE # FSM_NAVIGATE STATE # =============> elif self.state == FSM_STATES.FSM_NAVIGATE: # No action required, simply allow path planner to navigate self.speed.linear.x = 0.0 self.speed.angular.z = 0.0 # Print debug info if (self._move_base.get_state() == GoalStatus.SUCCEEDED): rospy.loginfo('Search for object failed!') # Next state transitions if(self.blobs_detected): # if a blob is seen rospy.loginfo("Goal Status: %f" %self._move_base.get_state()) rospy.loginfo("Object detected! Aborting goal and approaching object...") self.state = FSM_STATES.FSM_OPTIMIZE else: if(self._move_base.get_state() != 3.0): rospy.loginfo("Navigating to object at pose (%f, %f, %f)..." %(self.goal.target_pose.pose.position.x, self.goal.target_pose.pose.position.y, self.goal.target_pose.pose.position.z)) else: rospy.loginfo("Waiting for goal...") rospy.loginfo("Goal Status: %f" %self._move_base.get_state()) self.state = FSM_STATES.FSM_NAVIGATE # FSM_SEARCH STATE # ===============> elif self.state == FSM_STATES.FSM_SEARCH: # Spin to search for object self.speed.linear.x = 0.0 self.speed.angular.z = 1.0 # Next state transitions if (self.blobs_detected): self.state = FSM_STATES.FSM_OPTIMIZE elif (self._move_base.simple_state == 1): # if goal is detected self.state = FSM_STATES.FSM_NAVIGATE else: self.state = FSM_STATES.FSM_SEARCH # FSM_OPTIMIZE STATE (PID/PD controller) # =================> elif self.state == FSM_STATES.FSM_OPTIMIZE : self._move_base.cancel_all_goals() rospy.loginfo("Blob detected at x:%f, y:%f, z:%f "%(self.tracked_blob.linear.x, self.tracked_blob.linear.y, self.tracked_blob.linear.z)) self.RotDerivator = 0.0 # if statement to check object is with range (TO BE DELETED/AMENDED!) if (self.tracked_blob.linear.z > self.max_z or self.tracked_blob.linear.z < self.min_z): self.speed.linear.x = 0.0 self.speed.angular.z = 0.0 rospy.loginfo("Distance = %f" %(self.tracked_blob.linear.z)) rospy.loginfo("Detected object is either too close or too far away.") else: # Set linear speed to 0 self.speed.linear.x = 0.0 # Compute PD angular error self.angError = -0.1*math.atan2(self.tracked_blob.linear.x, self.tracked_blob.linear.y); self.pRotError = self.KpRot*self.angError self.dRotError = self.KdRot*(self.angError-self.RotDerivator) self.RotDerivator = self.angError self.anglePID = self.pRotError + self.dRotError; # Cap maximum rotation speed if(math.fabs(self.anglePID)>self.maxRotSpeed): self.anglePID=self.maxRotSpeed*math.fabs(self.anglePID)/self.anglePID # Set tolerance in angle to signify stable orientation if(math.fabs(self.angError) < self.tol_ang): self.speed.angular.z = 0.0 self.RotDerivator = 0.0 self.angStable = True else: self.speed.angular.z = self.anglePID self.angStable = False # Debug messages rospy.loginfo("distance: %f" %(self.tracked_blob.linear.z)) rospy.loginfo("angError: %f, pRotError: %f, dRotError: %f" %(self.angError, self.pRotError, self.dRotError)) # Next state transitions if (self.angStable): # if stable position and orientation have been achieved self.goal = MoveBaseGoal() # reset goal if (self.tracked_blob.linear.z > 0.001): # if object is cloaser than 1 m self.state = FSM_STATES.FSM_APPROACH_OBJ else: self.correction_start = rospy.Time.now() self.state = FSM_STATES.FSM_STABLE # move to stable state elif(self.blobs_detected): # else if (while) blobs are being detected self.state = FSM_STATES.FSM_OPTIMIZE # loop in same state else: #self._move_base.send_goal(self.goal) #rospy.sleep(self.sleepDuration) #self.state = FSM_STATES.FSM_LOCALIZE # else move to nav state self.state = FSM_STATES.FSM_SEARCH # FSM_APPROACH_OBJ STATE # =============> elif self.state == FSM_STATES.FSM_APPROACH_OBJ: # Set constants and time in goal pose self.obj_pose.target_pose.header.frame_id = 'camera_depth_frame' self.obj_pose.target_pose.header.stamp = rospy.Time.now() self.obj_pose.target_pose.pose.orientation.w = 1.0 # if-else statements to perform corrections in target pose # if distance is less than 2m and more than 1.50m #if (self.tracked_blob.linear.z > 0.0015): # self.obj_pose.target_pose.pose.position.x = 1000*(self.tracked_blob.linear.z - 0.001) # set goal to (distance - 1m) # self.target_pose_pub.publish(self.obj_pose.target_pose) # self._move_base.send_goal(self.obj_pose) # send goal # rospy.loginfo('Performing approach to 1m. Pose x: %f, w: %f.' %(self.obj_pose.target_pose.pose.position.x, self.obj_pose.target_pose.pose.orientation.w)) # rospy.loginfo('Waiting for result...') # self._move_base.wait_for_result() # if self._move_base.get_state() == GoalStatus.SUCCEEDED: # rospy.loginfo('Goal Reached!') # else: # rospy.loginfo('Goal aborted!') # else if distance is less than 1.50m #else: self.obj_pose.target_pose.pose.position.x = 1000*(self.tracked_blob.linear.z) - 0.7 # set goal to final target self.target_pose_pub.publish(self.obj_pose.target_pose) self._move_base.send_goal(self.obj_pose) # send goal rospy.loginfo('Performing final approach. Pose x: %f, w: %f.' %(self.obj_pose.target_pose.pose.position.x, self.obj_pose.target_pose.pose.orientation.w)) rospy.loginfo('Waiting for result...') self._move_base.wait_for_result() if self._move_base.get_state() == GoalStatus.SUCCEEDED: rospy.loginfo('Goal Reached!') else: rospy.loginfo('Goal aborted!') # Next state transitions if (self.blobs_detected): self.state = FSM_STATES.FSM_OPTIMIZE # loop back to FSM_OPTIMIZE to correct angle else: self.state = FSM_STATES.FSM_SEARCH # FSM_STABLE STATE # ===============> elif (self.state == FSM_STATES.FSM_STABLE): # Allow 2 seconds for blob distance to saturate and then compute forward movement duration if (rospy.Time.now() < self.correction_start + rospy.Duration(2)): rospy.loginfo('Correction of detection errors...') self.state = FSM_STATES.FSM_STABLE else: self.moveDuration = rospy.Duration((self.tracked_blob.linear.z - 0.00015)/0.0001) rospy.loginfo((self.tracked_blob.linear.z - 0.0002)/0.0001) self.moveStartTime = rospy.Time.now() self.state = FSM_STATES.FSM_FINAL_APPROACH # FSM_FINAL_APPROACH # ==================> elif (self.state == FSM_STATES.FSM_FINAL_APPROACH): # Move forward for desired duration to approach target object if (rospy.Time.now() < self.moveStartTime + self.moveDuration): self.speed.linear.x = 0.1 self.speed.angular.z = 0.0 self.state = FSM_STATES.FSM_FINAL_APPROACH # Once duration has elapsed move to spin state else: self.speed.linear.x = 0.0 self.speed.angular.z = 0.0 self.state = FSM_STATES.FSM_WAIT_FOR_ACTION # FSM_SPIN STATE # =============> elif (self.state == FSM_STATES.FSM_SPIN): # Spin self.speed.linear.x = 0.0 self.speed.angular.z = -1.0 # Next state transitions # -Until arm sees object-! (Current content is temporary) if (self.arm_state.data == 'ALIGN_CAMERA' or self.arm_state.data =='ALIGN_BIN'): # if arm signifies that object has been dropped self.state = FSM_STATES.FSM_WAIT_FOR_ACTION # change to WAIT_FOR_ACTION state elif (self._move_base.simple_state != 2): # if a goal is detected self.state = FSM_STATES.FSM_MOVE_TO_BASE else: self.state = FSM_STATES.FSM_SPIN # FSM_WAIT_FOR_ACTION STATE # =============> elif (self.state == FSM_STATES.FSM_WAIT_FOR_ACTION): # Stop motors self.speed.linear.x = 0.0 self.speed.angular.z = 0.0 # Print arm state for debug rospy.loginfo(self.arm_state.data) # Next state transitions if ((self.arm_state.data == 'LOCATE_BIN') or (self.arm_state.data == 'SEARCH_OBJ')): self.state = FSM_STATES.FSM_SPIN elif (self.arm_state.data == 'IDLE'): # if arm signifies that object has been dropped self.state = FSM_STATES.FSM_IDLE # change to MOVE_TO_BASE state else: # if object not dropped self.state = FSM_STATES.FSM_WAIT_FOR_ACTION # loop back # FSM_MOVE_TO_BASE STATE # =============> elif (self.state == FSM_STATES.FSM_MOVE_TO_BASE): self.goal_state = self._move_base.get_state() rospy.loginfo("Moving back to base") rospy.loginfo("Goal Status: %f" %self.goal_state) self._move_base.wait_for_result() if self._move_base.get_state() == GoalStatus.SUCCEEDED: rospy.loginfo('Goal Reached!') else: rospy.loginfo('Goal aborted!') self.state = FSM_STATES.FSM_NAVIGATE rospy.wait_for_service('/move_base/clear_costmaps') self.clear_costmaps = rospy.ServiceProxy('/move_base/clear_costmaps', Empty) try: rospy.wait_for_service('/move_base/clear_costmaps') self.clear_costmaps() except rospy.ServiceException as exc: print("Service did not process request: " + str(exc)) # Print general info self.printINFO() # Publish /arm_bot_base/state self.state_pub.publish(String(FSM_STATES.__geitem__(self.state))) # Only publish velocity when no goal is being pursued or expected if (self.state != FSM_STATES.FSM_NAVIGATE): self.speed_pub.publish(self.speed) ############ # getPose(): Returns pose of received blob relative to the camera. ############ def getPose(self, blob): self.pose = Vector3() self.pose.x = blob.center.x self.pose.y = blob.center.y self.pose.z = blob.center.z return self.pose ################# # update_avg_z(): Updates the avg_value of distance z from blob. # # Higher weight is given to blobs received with lower distance than the average, # # and vice versa, such that the closest distance is ultimately tracked. ################# def update_avg_z(self): if (self.old_tracked_blob.linear.z == 0): self.avg_z = self.tracked_blob.z else: if(self.tracked_blob.linear.z < self.avg_z): # Give heigher weight to closer blobs self.avg_z = (self.avg_z + 10*self.tracked_blob.linear.z)/(11) else: self.avg_z = (10*self.avg_z + self.tracked_blob.linear.z)/(11) ######################## # pub_tracked_blob_tf(): Publishes a transform frame for the tracked blob. ####################### def pub_tracked_blob_tf(self, pose): br = tf.TransformBroadcaster() br.sendTransform((1000*pose.z, -1000*pose.x, -1000*pose.y), tf.transformations.quaternion_from_euler(0, 0, 1), rospy.Time.now(), 'tracked_blob', 'camera_depth_frame') ############## # printINFO(): Prints general debug info ############# def printINFO(self): # Print debug info about linear and angular speeds rospy.loginfo("speedPID: %f" %(self.speed.linear.x)) rospy.loginfo("anglePID: %f" %(self.speed.angular.z)) # Print goal status info if (self._move_base.simple_state == 2): self.goal_state = 'No goals received' rospy.loginfo("Goal Status: %s" %self.goal_state) else: # if a goal is detected self.goal_state = self._move_base.get_state() rospy.loginfo("Goal Status: %f" %self.goal_state) # Print blob info if(self.blobs_detected): rospy.loginfo("Blob distance: %f" %(self.tracked_blob.linear.z)) rospy.loginfo("miss_counter = %f" %(self.miss_counter)) else: rospy.loginfo("No blobs detected") rospy.loginfo("miss_counter = %f" %(self.miss_counter)) # Print yaw (**DEBUG**) rospy.loginfo(self.yaw) rospy.loginfo(self.arm_state.data) ######### # spin(): Provides desired rate for speed_callback() and ensures termination ######## def spin(self): rate = rospy.Rate(RATE) while not rospy.is_shutdown(): self.speed_callback() rate.sleep()
class Gripper(): """ Wrapper for all the action client stuff """ def __init__(self): """ Instantiate action clients and wait for communication with servers """ self.client_gripper = SimpleActionClient("r_gripper_sensor_controller/gripper_action", Pr2GripperCommandAction) self.client_contact = SimpleActionClient("r_gripper_sensor_controller/find_contact", PR2GripperFindContactAction) self.client_force = SimpleActionClient("r_gripper_sensor_controller/force_servo", PR2GripperForceServoAction) self.client_gripper.wait_for_server(rospy.Duration(10.0)) self.client_contact.wait_for_server(rospy.Duration(10.0)) self.client_force.wait_for_server(rospy.Duration(10.0)) rospy.loginfo('All servers ready!') rospy.Subscriber('r_gripper_pressure_balance', String, self.store_balance) def store_balance(self, balance): """ Stores gripper balance. """ self.balance = balance.data def open(self): """ Open the gripper """ goal_open = Pr2GripperCommandGoal() goal_open.command.position = 0.09 goal_open.command.max_effort = -1.0 rospy.loginfo("Sending goal: OPEN") self.client_gripper.send_goal_and_wait(goal_open) if (self.client_gripper.get_state() == GoalStatus.SUCCEEDED): rospy.loginfo('SUCCESS. The gripper opened!') else: rospy.loginfo('FAILURE. The gripper failed to open.') def open_manual(self, position): """ Open the gripper """ goal_open = Pr2GripperCommandGoal() goal_open.command.position = position goal_open.command.max_effort = -1.0 rospy.loginfo("Sending goal: OPEN") self.client_gripper.send_goal_and_wait(goal_open) if (self.client_gripper.get_state() == GoalStatus.SUCCEEDED): rospy.loginfo('SUCCESS. The gripper opened!') else: rospy.loginfo('FAILURE. The gripper failed to open.') def close(self): """ Close the gripper """ goal_close = Pr2GripperCommandGoal() goal_close.command.position = 0.00 goal_close.command.max_effort = 20.0 rospy.loginfo("Sending goal: CLOSE") self.client_gripper.send_goal_and_wait(goal_close) if (self.client_gripper.get_state() == GoalStatus.SUCCEEDED): rospy.loginfo('SUCCESS. The gripper closed!') else: rospy.loginfo('FAILURE. The gripper failed to close.') def hold(self, holdForce): """ Hold something with a specified grip force """ goal_squeeze = PR2GripperForceServoGoal() goal_squeeze.command.fingertip_force = holdForce rospy.loginfo("Sending goal: HOLD with {0}N of force.".format(holdForce)) self.client_force.send_goal_and_wait( goal_squeeze ) if (self.client_force.get_state() == GoalStatus.SUCCEEDED): rospy.loginfo('SUCCESS. Stable force achieved!') else: rospy.loginfo('FAILURE. Stable force was NOT achieved.') def findTwoContacts(self): """ Find two contacts and go into force control mode """ goal_contact = PR2GripperFindContactGoal() goal_contact.command.contact_conditions = goal_contact.command.BOTH # close until both fingers contact goal_contact.command.zero_fingertip_sensors = True # zero fingertip sensors before moving rospy.loginfo("Sending goal: FIND TWO CONTACTS") self.client_contact.send_goal_and_wait(goal_contact) if (self.client_contact.get_state() == GoalStatus.SUCCEEDED): rospy.loginfo('SUCCESS. Contact found at left: {0} and right: {1}'.format(self.client_contact.get_result().data.left_fingertip_pad_contact, self.client_contact.get_result().data.right_fingertip_pad_contact)) rospy.loginfo('Contact force for left: {0} and right: {1}'.format(self.client_contact.get_result().data.left_fingertip_pad_force, self.client_contact.get_result().data.right_fingertip_pad_force)) else: rospy.loginfo('FAILURE. No contact found or force not able to be maintained')
class arm_bot_base(): def __init__(self, desired_z, KpLin, KdLin, KpRot, KdRot, tol_z, tol_ang, maxLinSpeed, maxRotSpeed, max_z, min_z): #global self.speed = Twist() self.tracked_blob = Twist() self.state = FSM_STATES.FSM_LOCALIZE self.obj_pose = MoveBaseGoal() self.goal = MoveBaseGoal() self.blobs_3d = Blobs3d() #params self.desired_z = desired_z #0.001 #0.8 self.KpLin = KpLin #1000.0 self.KdLin = KdLin self.KpRot = KpRot #0.1 self.KdRot = KdRot self.tol_z = tol_z #0.00005 #0.05 self.tol_ang= tol_ang #0.5 self.maxLinSpeed = maxLinSpeed #0.2 self.maxRotSpeed = maxRotSpeed self.max_z = max_z #3.0 self.min_z= min_z #0.0005 #0.2 # move_base Action Client self._move_base = SimpleActionClient('move_base', MoveBaseAction) #subscribtions rospy.Subscriber('/blobs_3d', Blobs3d, self.blob_callback) rospy.Subscriber('/arm_bot_base/goal', PoseStamped, self.goal_callback) # publications self.speed_pub = rospy.Publisher('/navigation_velocity_smoother/raw_cmd_vel', Twist) #/navigation_velocity_smoother/raw_cmd_vel # /mobile_base/commands/velocity self.state_pub = rospy.Publisher('/arm_bot_base/state', String, queue_size=10) #/navigation_velocity_smoother/raw_cmd_vel # /mobile_base/commands/velocity ################# # goal_callback : Run everytime '\arm_bot_base\goal' topic receives message. # # Send a goal to the Action Server upon callback ################# def goal_callback(self, pose_stamped): self.goal.target_pose = pose_stamped if(self.goal): self._move_base.send_goal(self.goal) rospy.loginfo("Just passed a goal") ################# # blob_callback : Run everytime \blobs_3d receives message. # # Used to set rate of node and calls speed_callback() on every run ################# def blob_callback(self, blobs_3d): self.blobs_3d = blobs_3d self.blob_area = 0 if(len(blobs_3d.blobs)): for blob in blobs_3d.blobs: if (blob.area>self.blob_area): self.blob_area = blob.area self.tracked_blob.linear = self.getPose(blob) rospy.loginfo("Blob distance: %f" %(self.tracked_blob.linear.z)) #self.state = FSM_STATES.FSM_OPTIMIZE else: rospy.loginfo("No blobs detected") #self.state = FSM_STATES.FSM_SEARCH ################# # speed_callback : Pubishes velocity and handles state transitions ################# def speed_callback(self): self.zStable = False # flag to signify desired z satisfied self.angStable = False # flag to signify desired angle satisfied self.moveDuration = rospy.Duration(3) # duration of forward movement print ("\n") rospy.loginfo("STATE : %s" % FSM_STATES.__geitem__(self.state)) # FSM_LOCALIZE STATE # =============> if self.state == FSM_STATES.FSM_LOCALIZE: # Spin self.speed.linear.x = 0.0 self.speed.angular.z = 0.5 # Next state transition # if-else statement to ensure error-less view of goal status if (self._move_base.simple_state == 2): self.goal_state = 'No goals received' rospy.loginfo("Goal Status: %s" %self.goal_state) self.state = FSM_STATES.FSM_LOCALIZE elif(len(self.blobs_3d.blobs)): # if a blob is seen rospy.loginfo("Object detected! Approaching object...") self.state = FSM_STATES.FSM_OPTIMIZE else: # if a goal is detected self.goal_state = self._move_base.get_state() rospy.loginfo("Goal Status: %f" %self.goal_state) self.state = FSM_STATES.FSM_NAVIGATE # FSM_NAVIGATE STATE # =============> elif self.state == FSM_STATES.FSM_NAVIGATE: # No action required, simply allow path planner to navigate self.speed.linear.x = 0.0 self.speed.angular.z = 0.0 # Next state transitions if(len(self.blobs_3d.blobs) ): # if a blob is seen self._move_base.cancel_all_goals() # cancell all nav goals rospy.loginfo("Goal Status: %f" %self._move_base.get_state()) rospy.loginfo("Object detected! Aborting goal and approaching object...") self.state = FSM_STATES.FSM_OPTIMIZE else: self.state = FSM_STATES.FSM_NAVIGATE # FSM_SEARCH STATE # ===============> elif self.state == FSM_STATES.FSM_SEARCH: # Spin to search for object self.speed.linear.x = 0.0 self.speed.angular.z = 0.2 # Next state transitions if (len(self.blobs_3d.blobs)): self.state = FSM_STATES.FSM_OPTIMIZE else: self.state = FSM_STATES.FSM_SEARCH # FSM_OPTIMIZE STATE (PID/PD controller) # =================> elif self.state == FSM_STATES.FSM_OPTIMIZE : rospy.loginfo("Blob detected at x:%f, y:%f, z:%f "%(self.tracked_blob.linear.x, self.tracked_blob.linear.y, self.tracked_blob.linear.z)) #self.LinDerivator = 0.0 self.RotDerivator = 0.0 # if statement to check object is with range (TO BE DELETED/AMENDED!) if (self.tracked_blob.linear.z > self.max_z or self.tracked_blob.linear.z < self.min_z): self.speed.linear.x = 0.0 self.speed.angular.z = 0.0 rospy.loginfo("Detected object is either too close or too far away.") else: # Compute distance PD error #self.zError = self.tracked_blob.linear.z - self.desired_z #self.pLinError = self.zError*self.KpLin #self.dLinError = self.KdLin*(self.zError-self.LinDerivator) #self.LinDerivator = self.zError #self.speedPID = self.pLinError + self.dLinError # Cap maximum linear speed #if(math.fabs(self.speedPID)>self.maxLinSpeed): # self.speedPID=self.maxLinSpeed*math.fabs(self.speedPID)/self.speedPID; # Set tolerance in z to signify stable position #if(math.fabs(self.zError)<self.tol_z): # self.speed.linear.x = 0.0 # LinDerivator = 0.0 # self.zStable = True #else: # self.speed.linear.x = self.speedPID # self.zStable = False # Set linear speed to 0 self.speed.linear.x = 0.0 # Compute PD angular error self.angError = -math.atan2(self.tracked_blob.linear.x, self.tracked_blob.linear.y); self.pRotError = self.KpRot*self.angError self.dRotError = self.KdRot*(self.angError-self.RotDerivator) self.RotDerivator = self.angError self.anglePID = self.pRotError + self.dRotError; # Cap maximum rotation speed if(math.fabs(self.anglePID)>self.maxRotSpeed): self.anglePID=self.maxRotSpeed*math.fabs(self.anglePID)/self.anglePID; # Set tolerance in angle to signify stable orientation if(math.fabs(self.angError) < self.tol_ang): self.speed.angular.z = 0.0; RotDerivator = 0.0 self.angStable = True else: self.speed.angular.z = self.anglePID; self.angStable = False # Debug messages rospy.loginfo("distance: %f" %(self.tracked_blob.linear.z)) rospy.loginfo("angError: %f, pRotError: %f, dRotError: %f" %(self.angError, self.pRotError, self.dRotError)) # Next state transitions if (self.angStable): # if stable position and orientation have been achieved self.state = FSM_STATES.FSM_APPROACH_OBJ # move to stable state self.moveStartTime = rospy.Time.now() elif(len(self.blobs_3d.blobs)): # else if (while) blobs are being detected self.state = FSM_STATES.FSM_OPTIMIZE # loop in same state else: self.state = FSM_STATES.FSM_LOCALIZE # else move to nav state # FSM_APPROACH_OBJ STATE # =============> elif self.state == FSM_STATES.FSM_APPROACH_OBJ: self.obj_pose.target_pose.header.frame_id = 'camera_depth_frame' self.obj_pose.target_pose.header.stamp = rospy.Time.now() self.obj_pose.target_pose.pose.orientation.w = 1.0 # if-else statements to perform corrections in target pose # if distance is higher than 2.20m (4.5 m gazebo) if ( self.tracked_blob.linear.z > 0.0045): self.obj_pose.target_pose.pose.position.x = 1000*(self.tracked_blob.linear.z - 0.004) # set goal to (distance - 2m) self._move_base.send_goal(self.obj_pose) rospy.loginfo('Moving to pose x: %f, w: %f. Waiting for result' %(self.obj_pose.target_pose.pose.position.x, self.obj_pose.target_pose.pose.orientation.w)) self._move_base.wait_for_result() rospy.loginfo(self._move_base.get_result()) self.state = FSM_STATES.FSM_OPTIMIZE # loop back to FSM_OPTIMIZE to correct angle # else if distance is less than 2m and more than 1.20m (2.5 m gazebo) elif (self.tracked_blob.linear.z > 0.0025): self.obj_pose.target_pose.pose.position.x = 1000*(self.tracked_blob.linear.z - 0.002) # set goal to (distance - 1m) self._move_base.send_goal(self.obj_pose) rospy.loginfo('Moving to pose x: %f, w: %f. Waiting for result' %(self.obj_pose.target_pose.pose.position.x, self.obj_pose.target_pose.pose.orientation.w)) self._move_base.wait_for_result() rospy.loginfo(self._move_base.get_result()) self.state = FSM_STATES.FSM_OPTIMIZE # loop back to FSM_OPTIMIZE to correct angle # else if distance is less than 1m else: self.obj_pose.target_pose.pose.position.x = 1000*(self.tracked_blob.linear.z) - 0.4 # set goal to final target self._move_base.send_goal(self.obj_pose) rospy.loginfo('Moving to pose x: %f, w: %f. Waiting for result' %(self.obj_pose.target_pose.pose.position.x, self.obj_pose.target_pose.pose.orientation.w)) self._move_base.wait_for_result() rospy.loginfo(self._move_base.get_result()) self.state = FSM_STATES.FSM_SPIN # move to FSM_SPIN # if ( self.tracked_blob.linear.z > 0.004): # while(self.tracked_blob.linear.z > 0.003): # pass # self._move_base.cancel_all_goals() # cancell all nav goals # self.state = FSM_STATES.FSM_OPTIMIZE # elif ( self.tracked_blob.linear.z > 0.001): # while(self.tracked_blob.linear.z > 0.001): # pass # self._move_base.cancel_all_goals() # cancell all nav goals # self.state = FSM_STATES.FSM_OPTIMIZE # else: # self.obj_pose.target_pose.header.frame_id = 'camera_depth_frame' # self.obj_pose.target_pose.header.stamp = rospy.Time.now() # self.obj_pose.target_pose.pose.position.x = 1000*(self.tracked_blob.linear.z) - 0.4 # self.obj_pose.target_pose.pose.orientation.w = 1.0 # self._move_base.send_goal(self.obj_pose) # rospy.loginfo('Moving to pose x: %f, w: %f. Waiting for result' %(self.obj_pose.target_pose.pose.position.x, self.obj_pose.target_pose.pose.orientation.w)) # self._move_base.wait_for_result() # rospy.loginfo(self._move_base.get_result()) # self.state= FSM_STATES.FSM_SPIN # FSM_STABLE STATE # ===============> elif (self.state == FSM_STATES.FSM_STABLE): # Move forward for desired duration to approach target object if (rospy.Time.now() < self.moveStartTime + self.moveDuration): self.speed.linear.x = 0.2 self.state = FSM_STATES.FSM_STABLE # Once duration has elapsed move to spin state else: self.speed.linear.x = 0.0 self.state = FSM_STATES.FSM_SPIN # FSM_SPIN STATE # =============> elif (self.state == FSM_STATES.FSM_SPIN): # Spin self.speed.linear.x = 0.0 self.speed.angular.z = 0.5 # Next state transitions # -Until arm sees object-! self.state_pub.publish(String(FSM_STATES.__geitem__(self.state))) # Print debug info about linear and angular speeds rospy.loginfo("speedPID: %f" %(self.speed.linear.x)) rospy.loginfo("anglePID: %f" %(self.speed.angular.z)) #Only publish velocity when no goal is being pursued or expected if (self.state != FSM_STATES.FSM_NAVIGATE): self.speed_pub.publish(self.speed) ############ # getPose(): Returns pose of received blob relative to the camera. ############ def getPose(self, blob): self.pose = Vector3() self.pose.x = blob.center.x self.pose.y = blob.center.y self.pose.z = blob.center.z return self.pose def spin(self): rate = rospy.Rate(RATE) while not rospy.is_shutdown(): self.speed_callback() rate.sleep()
class ConstructStaticCollisionMapServer: def __init__(self, node_name = 'construct_static_collision_map_server', action_name = 'construct_static_collision_map_action', head_topic = '/head_traj_controller/point_head_action', static_collision_map_topic = '/make_static_collision_map' ): rospy.init_node(node_name) rospy.loginfo ( 'waiting for SimpleActionClient: %s'%( head_topic ) ) self.head_client = SimpleActionClient(head_topic,PointHeadAction ) self.head_client.wait_for_server( ) self.take_static_collision_map_client = SimpleActionClient(static_collision_map_topic, MakeStaticCollisionMapAction) self._action_server = SimpleActionServer(action_name, ConstructStaticCollisionMapAction, execute_cb=self.execute_cb) rospy.loginfo('ready') def execute_cb(self, goal): self.scan_table() goal = MakeStaticCollisionMapGoal() self.take_static_collision_map_client.send_goal(goal) self._action_server.set_succeeded() def move_head(self, x, y, z, min_dur = 0.0, max_velocity = 1.0, frame_id = 'base_link', timeout = 5.0): point = PointStamped() point.header.frame_id = frame_id point.header.stamp = rospy.Time.now() point.point.x, point.point.y, point.point.z = x, y, z goal = PointHeadGoal() goal.pointing_frame = 'head_plate_frame' goal.max_velocity = max_velocity goal.min_duration = rospy.Duration.from_sec( min_dur ) goal.target = point self.head_client.send_goal( goal ) self.head_client.wait_for_result( timeout = rospy.Duration.from_sec( timeout ) ) if not self.head_client.get_state() == GoalStatus.SUCCEEDED: rospy.logerr( 'can not move head to:\n %s'%( goal ) ) return False return True def scan_table(self, x = 0.5, y_min = -1.0, y_max = 1.0, z = 0.7, steps = 10, min_dur = 1.0, max_wait_time = 10.0 ): rospy.loginfo( 'scanning table with x = %5.3f, y_min = %5.3f, y_max = %5.3f, z = %5.3f, steps = %d'%( x, y_min, y_max, z, steps ) ) Y = np.linspace( y_min, y_max, steps ) for y in Y: if not self.move_head( x, y, z, min_dur ): return rospy.loginfo( 'done scanning table!' )
class bin_bot_base(): def __init__(self, desired_z, KpLin, KdLin, KpRot, KdRot, tol_z, tol_ang, maxLinSpeed, maxRotSpeed, max_z, min_z): #global self.speed = Twist() # variable to store velocity commands self.tracked_blob = Twist() # variable to store pose of tracked blob self.state = FSM_STATES.FSM_LOCALIZE # set initial state self.obj_pose = MoveBaseGoal() # variable self.goal = MoveBaseGoal() self.blobs_3d = Blobs3d() self.blobs_detected = False self.miss_counter = 0 self.zStable = False # flag to signify desired z satisfied self.angStable = False # flag to signify desired angle satisfied self.arm_state = String #params self.desired_z = desired_z #0.001 #0.8 self.KpLin = KpLin #1000.0 self.KdLin = KdLin self.KpRot = KpRot #0.1 self.KdRot = KdRot self.tol_z = tol_z #0.00005 #0.05 self.tol_ang= tol_ang #0.5 self.maxLinSpeed = maxLinSpeed #0.2 self.maxRotSpeed = maxRotSpeed self.max_z = max_z #3.0 self.min_z= min_z #0.0005 #0.2 # move_base Action Client self._move_base = SimpleActionClient('move_base', MoveBaseAction) self._move_base.wait_for_server() #subscribtions rospy.Subscriber('/blobs_3d', Blobs3d, self.blob_callback) rospy.Subscriber('/bin_bot_base/goal', PoseStamped, self.goal_callback) rospy.Subscriber('/arm_node/state', String, self.arm_state_callback) # publications self.speed_pub = rospy.Publisher('/navigation_velocity_smoother/raw_cmd_vel', Twist) #/navigation_velocity_smoother/raw_cmd_vel # /mobile_base/commands/velocity self.state_pub = rospy.Publisher('/bin_bot_base/state', String, queue_size=10) #/navigation_velocity_smoother/raw_cmd_vel # /mobile_base/commands/velocity ################# # goal_callback : Run everytime '\arm_bot_base\goal' topic receives message. # # Send a goal to the Action Server upon callback ################# def goal_callback(self, pose_stamped): self.goal.target_pose = pose_stamped if(self.goal): self._move_base.send_goal(self.goal) rospy.loginfo("Just passed a goal") ################# # blob_callback : Run everytime \blobs_3d receives message. ################# def blob_callback(self, blobs_3d): self.blobs_3d = blobs_3d self.blob_area = 0 if(len(self.blobs_3d.blobs)): self.blobs_detected = True self.miss_counter = 0 for blob in blobs_3d.blobs: if (blob.area>self.blob_area): self.blob_area = blob.area self.tracked_blob.linear = self.getPose(blob) else: if(self.miss_counter>=5): # Allow for 5 callbacks before signaling non-detection self.blobs_detected = False self.miss_counter += 1 ###################### # arm_state_callback : Run everytime the arm updates it's state. ###################### def arm_state_callback(self, arm_state): self.arm_state = arm_state ################# # speed_callback : Pubishes velocity and handles state transitions ################# def speed_callback(self): self.moveDuration = rospy.Duration(3) # duration of forward movement # Print state print ("\n") rospy.loginfo("STATE : %s" % FSM_STATES.__geitem__(self.state)) # FSM_LOCALIZE STATE # =============> if self.state == FSM_STATES.FSM_LOCALIZE: # Spin self.speed.linear.x = 0.0 self.speed.angular.z = 0.5 # Next state transition # if-else statement to ensure error-less view of goal status if(self.blobs_detected): # if a blob is seen rospy.loginfo("Arm Bot detected! Approaching robot...") self.state = FSM_STATES.FSM_OPTIMIZE elif (self._move_base.simple_state == 2): self.goal_state = 'No goals received' rospy.loginfo("Goal Status: %s" %self.goal_state) self.state = FSM_STATES.FSM_LOCALIZE else: # if a goal is detected self.goal_state = self._move_base.get_state() rospy.loginfo("Goal Status: %f" %self.goal_state) self.state = FSM_STATES.FSM_NAVIGATE # FSM_NAVIGATE STATE # =============> elif self.state == FSM_STATES.FSM_NAVIGATE: # No action required, simply allow path planner to navigate self.speed.linear.x = 0.0 self.speed.angular.z = 0.0 # Next state transitions if(self.blobs_detected): # and (self.tracked_blob.linear.z < 3.0): # if a blob is seen (and closer than 3 meters) self._move_base.cancel_all_goals() # cancell all nav goals rospy.loginfo("Goal Status: %f" %self._move_base.get_state()) rospy.loginfo("Arm Bot detected! Aborting goal and approaching robot...") self.state = FSM_STATES.FSM_OPTIMIZE else: if(self._move_base.get_state() != 3.0): rospy.loginfo("Navigating to Arm Bot at pose (%f, %f, %f)..." %(self.goal.target_pose.pose.position.x, self.goal.target_pose.pose.position.y, self.goal.target_pose.pose.position.z)) else: rospy.loginfo("Waiting for goal...") rospy.loginfo("Goal Status: %f" %self._move_base.get_state()) self.state = FSM_STATES.FSM_NAVIGATE # FSM_OPTIMIZE STATE (PID/PD controller) # =================> elif self.state == FSM_STATES.FSM_OPTIMIZE : rospy.loginfo("Blob detected at x:%f, y:%f, z:%f "%(self.tracked_blob.linear.x, self.tracked_blob.linear.y, self.tracked_blob.linear.z)) #self.LinDerivator = 0.0 self.RotDerivator = 0.0 # if statement to check Arm Bot is with range (TO BE DELETED/AMENDED!) if (self.tracked_blob.linear.z > self.max_z or self.tracked_blob.linear.z < self.min_z): self.speed.linear.x = 0.0 self.speed.angular.z = 0.0 rospy.loginfo("Detected object is either too close or too far away.") else: # Set linear speed to 0 self.speed.linear.x = 0.0 # Compute PD angular error self.angError = -math.atan2(self.tracked_blob.linear.x, self.tracked_blob.linear.y); self.pRotError = self.KpRot*self.angError self.dRotError = self.KdRot*(self.angError-self.RotDerivator) self.RotDerivator = self.angError self.anglePID = self.pRotError + self.dRotError; # Cap maximum rotation speed if(math.fabs(self.anglePID)>self.maxRotSpeed): self.anglePID=self.maxRotSpeed*math.fabs(self.anglePID)/self.anglePID; # Set tolerance in angle to signify stable orientation if(math.fabs(self.angError) < self.tol_ang): self.speed.angular.z = 0.0; RotDerivator = 0.0 self.angStable = True else: self.speed.angular.z = self.anglePID; self.angStable = False # Debug messages rospy.loginfo("distance: %f" %(self.tracked_blob.linear.z)) rospy.loginfo("angError: %f, pRotError: %f, dRotError: %f" %(self.angError, self.pRotError, self.dRotError)) # Next state transitions if (self.angStable): # if stable position and orientation have been achieved self.state = FSM_STATES.FSM_APPROACH_OBJ # move to stable state self.moveStartTime = rospy.Time.now() elif(self.blobs_detected): # else if (while) blobs are being detected self.state = FSM_STATES.FSM_OPTIMIZE # loop in same state else: self.state = FSM_STATES.FSM_LOCALIZE # else move to nav state # FSM_APPROACH_OBJ STATE # =============> elif self.state == FSM_STATES.FSM_APPROACH_OBJ: self.obj_pose.target_pose.header.frame_id = 'camera_depth_frame' self.obj_pose.target_pose.header.stamp = rospy.Time.now() self.obj_pose.target_pose.pose.orientation.w = 1.0 # if-else statements to perform corrections in target pose # if distance is higher than 2.20m if ( self.tracked_blob.linear.z > 0.0022): self.obj_pose.target_pose.pose.position.x = 1000*(self.tracked_blob.linear.z - 0.002) # set goal to (distance - 2m) self._move_base.send_goal(self.obj_pose) # send goal rospy.loginfo('Performing approach to 2m. Pose x: %f, w: %f.' %(self.obj_pose.target_pose.pose.position.x, self.obj_pose.target_pose.pose.orientation.w)) rospy.loginfo('Waiting for result...') self._move_base.wait_for_result() if self._move_base.get_state() == GoalStatus.SUCCEEDED: rospy.loginfo('Goal Reached!') else: rospy.loginfo('Goal aborted!') self.state = FSM_STATES.FSM_OPTIMIZE # loop back to FSM_OPTIMIZE to correct angle # else if distance is less than 2m and more than 1.20m elif (self.tracked_blob.linear.z > 0.0012): self.obj_pose.target_pose.pose.position.x = 1000*(self.tracked_blob.linear.z - 0.001) # set goal to (distance - 1m) self._move_base.send_goal(self.obj_pose) # send goal rospy.loginfo('Performing approach to 1m. Pose x: %f, w: %f.' %(self.obj_pose.target_pose.pose.position.x, self.obj_pose.target_pose.pose.orientation.w)) rospy.loginfo('Waiting for result...') self._move_base.wait_for_result() if self._move_base.get_state() == GoalStatus.SUCCEEDED: rospy.loginfo('Goal Reached!') else: rospy.loginfo('Goal aborted!') rospy.loginfo(self._move_base.get_result()) self.state = FSM_STATES.FSM_OPTIMIZE # loop back to FSM_OPTIMIZE to correct angle # else if distance is less than 1m else: self.obj_pose.target_pose.pose.position.x = 1000*(self.tracked_blob.linear.z) - 0.4 # set goal to final target self._move_base.send_goal(self.obj_pose) # send goal rospy.loginfo('Performing final approach. Pose x: %f, w: %f.' %(self.obj_pose.target_pose.pose.position.x, self.obj_pose.target_pose.pose.orientation.w)) rospy.loginfo('Waiting for result...') self._move_base.wait_for_result() if self._move_base.get_state() == GoalStatus.SUCCEEDED: rospy.loginfo('Goal Reached!') else: rospy.loginfo('Goal aborted!') self.moveStartTime = rospy.Time.now() self.state = FSM_STATES.FSM_SPIN # move to FSM_SPIN # FSM_SPIN STATE # =============> elif (self.state == FSM_STATES.FSM_SPIN): # Spin if (rospy.Time.now() < self.moveStartTime + self.moveDuration): self.speed.linear.x = 0.0 self.speed.angular.z = 0.5 self.state = FSM_STATES.FSM_SPIN else: self.state = FSM_STATES.FSM_WAIT_FOR_DROP # FSM_WAIT_FOR_DROP STATE # =============> elif (self.state == FSM_STATES.FSM_WAIT_FOR_DROP): self.speed.linear.x = 0.0 self.speed.angular.z = 0.0 # Next state transitions if (self.arm_state.data == 'OBJ_DROPPED'): # if arm signifies that object has been dropped self.state = FSM_STATES.FSM_MOVE_TO_BASE # change to MOVE_TO_BASE state else: # if object not dropped self.state = FSM_STATES.FSM_WAIT_FOR_DROP # loop back # FSM_MOVE_TO_BASE STATE # =============> elif (self.state == FSM_STATES.FSM_MOVE_TO_BASE): # **TEST** wait for coordinates of base to be passed and then move to base if (self._move_base.simple_state == 2): rospy.loginfo("Waiting for base coordinates...") self.state = FSM_STATES.FSM_MOVE_TO_BASE else: # if a goal is detected rospy.loginfo("Moving back to base") rospy.loginfo("Goal Status: %f" %self._move_base.get_state()) self._move_base.wait_for_result() if self._move_base.get_state() == GoalStatus.SUCCEEDED: rospy.loginfo('Goal Reached!') else: rospy.loginfo('Goal aborted!') self.state = FSM_STATES.FSM_NAVIGATE # Print general info self.printINFO() # Publish /arm_bot_base/state self.state_pub.publish(String(FSM_STATES.__geitem__(self.state))) # Only publish velocity when no goal is being pursued or expected if (self.state != FSM_STATES.FSM_NAVIGATE and self.state != FSM_STATES.FSM_MOVE_TO_BASE and self.state != FSM_STATES.FSM_APPROACH_OBJ): self.speed_pub.publish(self.speed) ############ # getPose(): Returns pose of received blob relative to the camera. ############ def getPose(self, blob): self.pose = Vector3() self.pose.x = blob.center.x self.pose.y = blob.center.y self.pose.z = blob.center.z return self.pose def printINFO(self): # Print debug info about linear and angular speeds rospy.loginfo("speedPID: %f" %(self.speed.linear.x)) rospy.loginfo("anglePID: %f" %(self.speed.angular.z)) if(self.blobs_detected): rospy.loginfo("Blob distance: %f" %(self.tracked_blob.linear.z)) rospy.loginfo("miss_counter = %f" %(self.miss_counter)) else: rospy.loginfo("No blobs detected") rospy.loginfo("miss_counter = %f" %(self.miss_counter)) def spin(self): rate = rospy.Rate(RATE) while not rospy.is_shutdown(): self.speed_callback() rate.sleep()