class CheckScheduler(object): def __init__(self, name): rospy.loginfo("Starting %s ..." % name) self.topic = rospy.get_param("~topic") rospy.loginfo("Waiting for message type of '%s' ..." % self.topic) self.ttype = rostopic.get_topic_class(self.topic, True)[0] rospy.loginfo("... got topic type") self.rate = rospy.Rate(.02) self.ok = True rospy.loginfo("Starting email client ...") self.client = SimpleActionClient("strands_emails", SendEmailAction) self.client.wait_for_server() rospy.loginfo("... done") self.message = SendEmailGoal() self.message.to_address = "*****@*****.**" self.message.subject = "%s stopped working" % self.topic self.text = "The topic '"+self.topic+"' is not published any more. Last published at %s." rospy.loginfo("... all done") def spin(self): while not rospy.is_shutdown(): try: rospy.wait_for_message(self.topic, self.ttype, timeout=60.) except rospy.ROSException: if self.ok: rospy.logerr("%s is not published" % self.topic) self.message.text = self.text % str(time.asctime(time.localtime(time.time()))) # print self.message self.client.send_goal(self.message) self.ok = False else: self.ok = True self.rate.sleep()
class Terminal(object): def __init__(self, name='terminal'): self.client = SimpleActionClient(resolve_name(name), ExchangeAction) self.client.wait_for_server() def __del__(self): self.close() def close(self): del self.client def send(self, mode, text, handler=None, block=True): goal = ExchangeGoal() goal.mode = mode goal.text = text self.client.send_goal(goal, feedback_cb=handler) if block: self.client.wait_for_result() def write(self, text): self.send(PRINT, text) def prompt(self, text): self.send(LINE, text) return self.client.get_result().line def get(self, text, escape, handler, block=True): callback = lambda feedback: handler(feedback.key) self.send(KEY, escape + text, callback, block)
class NavigateToMovingGoalState(State): """ Moves the robot to a moving goal in the map frame. """ def __init__(self, goal): # type: (Callable[[], PoseStamped]) -> None State.__init__(self, outcomes=['ok', 'err']) self.goal = goal self.client = SimpleActionClient('move_base', MoveBaseAction) def execute(self, ud): from geometry_msgs.msg import Twist twist_publisher = rospy.Publisher('/cmd_vel_mux/input/teleop', Twist) for _ in range(10): t = Twist() t.linear.z = -1 twist_publisher.publish(t) rospy.sleep(0.1) self.client.wait_for_server() start_time = rospy.get_time() while True: pose = self.goal() if pose is None: return 'err' goal = MoveBaseGoal() goal.target_pose.header.frame_id = 'map' goal.target_pose.pose.position = pose.pose.position goal.target_pose.pose.orientation = pose.pose.orientation self.client.send_goal(goal) if self.client.wait_for_result(timeout=rospy.Duration(1)): break if rospy.get_time() - start_time > 30: return 'ok' return 'ok'
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 StationHold(HandlerBase): alarm_name = 'station-hold' def __init__(self): self.move_client = SimpleActionClient('/move_to', MoveAction) self.change_wrench = rospy.ServiceProxy('/wrench/select', MuxSelect) self.broadcaster = AlarmBroadcaster(self.alarm_name) def _client_cb(self, terminal_state, result): if terminal_state != 3: rospy.logwarn('Station hold goal failed (Status={}, Result={})'.format( TerminalState.to_string(terminal_state), result)) return try: self.change_wrench('autonomous') rospy.loginfo('Now station holding') self.broadcaster.clear_alarm() except rospy.ServiceException as e: rospy.logwarn('Station hold changing wrench failed: {}'.format(e)) def raised(self, alarm): rospy.loginfo("Attempting to station hold") station_hold_goal = MoveGoal() station_hold_goal.move_type = 'hold' self.move_client.send_goal(station_hold_goal, done_cb=self._client_cb) def cleared(self, alarm): # When cleared, do nothing and just wait for new goal / custom wrench pass
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'
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 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 FindInteractant(object): def __init__(self, name): rospy.loginfo("Starting %s ..." % name) self._ps = PNPSimplePluginServer( name=name, ActionSpec=FindInteractantAction, execute_cb=self.execute_cb, auto_start=False ) rospy.loginfo("Creating tracker client") self.start_client = SimpleActionClient("/start_tracking_person", TrackPersonAction) self.start_client.wait_for_server() rospy.loginfo("Tracker client connected") self._ps.start() rospy.loginfo("... done") def execute_cb(self, goal): rospy.loginfo("Finding interactant '%s'" % (goal.interactant_id,)) self.start_client.send_goal(TrackPersonGoal(id=goal.id, interactant_id=goal.interactant_id, no_turn=True)) res = FindInteractantResult() res.result.append(ActionResult(cond="found_interactant__"+goal.interactant_id+"__"+goal.id, truth_value=True)) res.result.append(ActionResult(cond="free_interactant_id__"+goal.interactant_id, truth_value=False)) if self._ps.is_preempt_requested(): self._ps.set_preempted() else: self._ps.set_succeeded(res)
class IntroduceSelf(smach.State): def __init__(self, profession=True, residence=True, date_of_birth=True, timeout=120.0, action_server='/mdr_actions/introduce_self_action_server'): smach.State.__init__(self, outcomes=['succeeded', 'failed']) self.profession = profession self.residence = residence self.date_of_birth = date_of_birth self.timeout = timeout self.introduce_self_client = SimpleActionClient( action_server, IntroduceSelfAction) self.introduce_self_client.wait_for_server() def execute(self, userdata): goal = IntroduceSelfGoal() goal.profession = self.profession goal.residence = self.residence goal.date_of_birth = self.date_of_birth rospy.loginfo('Calling introduce_self action') self.introduce_self_client.send_goal(goal) duration = rospy.Duration.from_sec(self.timeout) self.introduce_self_client.wait_for_result(duration) rospy.loginfo('introduce_self call completed') res = self.introduce_self_client.get_result() if res and res.success: return 'succeeded' else: return 'failed'
class TerminateInteraction(object): def __init__(self, name): rospy.loginfo("Starting %s ..." % name) self._ps = PNPSimplePluginServer( name=name, ActionSpec=TerminateInteractionAction, execute_cb=self.execute_cb, auto_start=False ) rospy.loginfo("Creating tracker client") self.stop_client = SimpleActionClient("/stop_tracking_person", TrackPersonAction) self.stop_client.wait_for_server() rospy.loginfo("Tracker client connected") self._ps.start() rospy.loginfo("... done") def execute_cb(self, goal): rospy.loginfo("Disengaging human '%s'" % (goal.id,)) self.stop_client.send_goal(TrackPersonGoal()) res = TerminateInteractionResult() res.result.append(ActionResult(cond="engaged__"+goal.interactant_id+"__"+goal.text, truth_value=False)) res.result.append(ActionResult(cond="said__"+goal.id+"__"+goal.text, truth_value=False)) res.result.append(ActionResult(cond="found_interactant__"+goal.interactant_id+"__"+goal.id, truth_value=False)) res.result.append(ActionResult(cond="human_exists__"+goal.id, truth_value=False)) res.result.append(ActionResult(cond="free_interactant_id__"+goal.interactant_id, truth_value=True)) if self._ps.is_preempt_requested(): self._ps.set_preempted() else: self._ps.set_succeeded(res)
class FollowTrajectoryClient(object): """ Class to make modules (torso) follow trajectory """ def __init__(self, rospy, name, joint_names): self.rospy =rospy self.client = SimpleActionClient("%s/follow_joint_trajectory" % name, FollowJointTrajectoryAction) self.rospy.loginfo("Waiting for %s..." % name) self.client.wait_for_server() self.joint_names = joint_names def move_to(self, positions, duration = 5.0): if len(self.joint_names) != len(positions): print("Invalid trajectory position") return False trajectory = JointTrajectory() trajectory.joint_names = self.joint_names trajectory.points.append(JointTrajectoryPoint()) trajectory.points[0].positions = positions trajectory.points[0].velocities = [0.0 for _ in positions] trajectory.points[0].accelerations = [0.0 for _ in positions] trajectory.points[0].time_from_start = self.rospy.Duration(duration) follow_goal = FollowJointTrajectoryGoal() follow_goal.trajectory = trajectory self.client.send_goal(follow_goal) self.client.wait_for_result()
class Server(object): def __init__(self, name): rospy.loginfo("Starting '{test}'.".format(test=name)) self.client = SimpleActionClient("/topo_nav_node", TopoNavAction) self.client.wait_for_server() with open(rospy.get_param("~goals_yaml"), "r") as f: self.goals = yaml.load(f) print self.goals self.cnt = {} self.active_id = -1 self.sub = rospy.Subscriber("/ar_pose_marker", ARMarkers, self.pose_cb) rospy.loginfo("Started '{test}'.".format(test=name)) def pose_cb(self, msg): ids = [] for m in msg.markers: ids.append(m.id) if m.id in self.cnt: self.cnt[m.id] += 1 else: self.cnt[m.id] = 1 print self.cnt for k in self.cnt.keys(): if k not in ids: self.cnt[k] = 0 if self.cnt[k] > 5: if self.active_id != k: print "Going to:", self.goals[k] self.client.send_goal(TopoNavGoal(self.goals[k])) self.active_id = k self.cnt[k] = 0
class StationHold(HandlerBase): alarm_name = 'station-hold' def __init__(self): self.move_client = SimpleActionClient('/move_to', MoveAction) self.change_wrench = rospy.ServiceProxy('/wrench/select', MuxSelect) self.broadcaster = AlarmBroadcaster(self.alarm_name) def _client_cb(self, terminal_state, result): if terminal_state != 3: rospy.logwarn( 'Station hold goal failed (Status={}, Result={})'.format( TerminalState.to_string(terminal_state), result)) return try: self.change_wrench('autonomous') rospy.loginfo('Now station holding') self.broadcaster.clear_alarm() except rospy.ServiceException as e: rospy.logwarn('Station hold changing wrench failed: {}'.format(e)) def raised(self, alarm): rospy.loginfo("Attempting to station hold") station_hold_goal = MoveGoal() station_hold_goal.move_type = 'hold' self.move_client.send_goal(station_hold_goal, done_cb=self._client_cb) def cleared(self, alarm): # When cleared, do nothing and just wait for new goal / custom wrench pass
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 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 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
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 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'
class Speak(object): def __init__(self, name): rospy.loginfo("Starting %s ..." % name) self.texts = {"hello": "Hello, I am pepper!"} self.predicate = rospy.get_param("~predicate", "said") rospy.loginfo("Starting dialogue client") self.client = SimpleActionClient("/dialogue_start", dialogueAction) rospy.loginfo("Waiting for dialogue client") self.client.wait_for_server() rospy.loginfo("Dialogue client started") self.pub = rospy.Publisher("/animated_speech", String, queue_size=10) self._ps = PNPSimplePluginServer(name=name, ActionSpec=SpeakAction, execute_cb=self.execute_cb, auto_start=False) rospy.loginfo("Creating tracker client") self.start_client = SimpleActionClient("/start_tracking_person", TrackPersonAction) self.start_client.wait_for_server() rospy.loginfo("Tracker client connected") self._ps.start() rospy.loginfo("... done") def execute_cb(self, goal): # self.pub.publish(self.texts[goal.text]) # rospy.sleep(3) self.start_client.send_goal(TrackPersonGoal(id=goal.id)) self.client.send_goal(dialogueGoal(userID=int(goal.id.split("_")[1]))) res = SpeakResult() res.result.append( ActionResult(cond=self.predicate + "__" + goal.id + "__" + goal.text, truth_value=ActionResult.TRUE)) self._ps.set_succeeded(res)
def move_base_client(): client = SimpleActionClient('move_base', MoveBaseAction) client.wait_for_server() goal = MoveBaseGoal() goal.target_pose.header.frame_id = 'map' goal.target_pose.header.stamp = rospy.Time.now() # line - 3, -1, -math.pi/2 theta = eval(sys.argv[3]) quat = quaternion_from_euler(0, 0, theta) goal.target_pose.pose.position.x = float(sys.argv[1]) goal.target_pose.pose.position.y = float(sys.argv[2]) goal.target_pose.pose.orientation.x = quat[0] goal.target_pose.pose.orientation.y = quat[1] goal.target_pose.pose.orientation.z = quat[2] goal.target_pose.pose.orientation.w = quat[3] 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_result()
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 RecognizeGenders(smach.State): def __init__(self, **kwargs): smach.State.__init__( self, outcomes=['succeeded', 'failed'], input_keys=['image', 'number_of_faces', 'bounding_boxes']) self.timeout = kwargs.get('timeout', 10) self.say_topic = kwargs.get('say_topic', '/say') self.say_pub = rospy.Publisher(self.say_topic, String, queue_size=1) self.gender_client = SimpleActionClient( 'mdr_actions/gender_recognition_server', GenderRecognitionAction) self.gender_client.wait_for_server() def execute(self, userdata): # Recognize gender goal = GenderRecognitionGoal() goal.image = userdata.image goal.number_of_faces = userdata.number_of_faces goal.bounding_boxes = userdata.bounding_boxes self.gender_client.send_goal(goal) self.gender_client.wait_for_result() result = self.gender_client.get_result() men = result.genders.count('man') women = result.genders.count('woman') # Say x men and y women msg = String() msg.data = 'There are %i men and %i women' % (men, women) rospy.loginfo(msg.data) self.say_pub.publish(msg) return 'succeeded'
def lookAtLocation(userdata): head_goal = PointHeadGoal() head_goal.target.header.frame_id = "base_link" # Iterate until we find the object we are searching for index_of_object = 0 while index_of_object < 10: if userdata.object_found.object_list[index_of_object].name == userdata.object_to_search_for: break; index_of_object += 1 head_goal.target.point.x = userdata.object_found.object_list[index_of_object].pose.position.x head_goal.target.point.y = userdata.object_found.object_list[index_of_object].pose.position.y head_goal.target.point.z = userdata.object_found.object_list[index_of_object].pose.position.z head_goal.pointing_frame = "stereo_link" head_goal.pointing_axis.x = 1.0 head_goal.pointing_axis.y = 0.0 head_goal.pointing_axis.z = 0.0 head_goal.max_velocity = 1.5 head_goal.min_duration.secs = 1.5 client = SimpleActionClient("/head_traj_controller/point_head_action", PointHeadAction) client.wait_for_server(rospy.Duration(0.5)) client.send_goal(head_goal) return succeeded
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 EnterDoor(smach.State): def __init__(self, timeout=120.0, move_forward_server='/mdr_actions/move_forward_server', movement_duration=15., speed=0.1): smach.State.__init__(self, outcomes=['succeeded', 'failed']) self.timeout = timeout self.movement_duration = movement_duration self.speed = speed self.move_forward_client = SimpleActionClient(move_forward_server, MoveForwardAction) self.move_forward_client.wait_for_server() self.entered = False def execute(self, userdata): goal = MoveForwardGoal() goal.movement_duration = self.movement_duration goal.speed = self.speed self.move_forward_client.send_goal(goal) timeout_duration = rospy.Duration.from_sec(self.timeout) self.move_forward_client.wait_for_result(timeout_duration) result = self.move_forward_client.get_result() if result and result.success: return 'succeeded' else: return 'failed'
class Enter(ScenarioStateBase): def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__( self, 'enter_door', save_sm_state=save_sm_state, outcomes=['succeeded', 'failed', 'failed_after_retrying'], output_keys=['enter_door_feedback']) self.timeout = kwargs.get('timeout', 120.) self.number_of_retries = kwargs.get('number_of_retries', 0) self.retry_count = 0 self.enter_door_server = kwargs.get('enter_action_server', 'enter_door_server') self.enter_action_client = SimpleActionClient(self.enter_door_server, EnterDoorAction) self.enter_action_client.wait_for_server(rospy.Duration(10.)) def execute(self, userdata): goal = EnterDoorGoal() rospy.loginfo('[ENTER_DOOR] Calling door entering action') self.say('Entering door') self.enter_action_client.send_goal(goal) duration = rospy.Duration.from_sec(self.timeout) success = self.enter_action_client.wait_for_result(duration) if success: rospy.loginfo('Entered successfully') return 'succeeded' else: rospy.logerr('Entering failed') self.say('Could not enter door') if self.retry_count == self.number_of_retries: self.say('Aborting operation') return 'failed_after_retrying' return 'failed'
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
class Planning: def __init__(self): self.client = SimpleActionClient('/move_base', MoveBaseAction) self.goal = MoveBaseGoal() self.current_goal = 'home' self.goals = { 'home': np.array([0.0, 0.0]), 'pos1': np.array([1.0, 0.0]), 'pos2': np.array([1.0, 1.0]), } def sendGoal(self, x_pos, y_pos, yaw): if abs(x_pos) <= 0.1: x_pos = 0.0 if abs(y_pos) <= 0.1: y_pos = 0.0 # quaternion = transformations.quaternion_from_euler(0, 0, yaw) goal = MoveBaseGoal() goal.target_pose.header.frame_id = "map" goal.target_pose.header.stamp = rospy.Time.now() goal.target_pose.pose.position.x = x_pos goal.target_pose.pose.position.y = y_pos goal.target_pose.pose.position.z = 0.0 # goal.target_pose.pose.orientation.x = quaternion[0] # goal.target_pose.pose.orientation.y = quaternion[1] # goal.target_pose.pose.orientation.z = quaternion[2] goal.target_pose.pose.orientation.w = 1.0 #quaternion[3] self.client.send_goal(goal) self.client.wait_for_result() print('Reached ' + self.current_goal) print(self.client.get_result()) def computeNewGoal(self): if self.current_goal == 'pos2': self.current_goal = 'home' elif self.current_goal == 'pos1': self.current_goal = 'pos2' elif self.current_goal == 'home': self.current_goal = 'pos1' else: self.current_goal = 'home' self.sendGoal(self.goals[self.current_goal][0], self.goals[self.current_goal][1], 0) def loop(self, rate): self.sendGoal(self.goals['home'][0], self.goals['home'][1], 0) while not rospy.is_shutdown(): self.computeNewGoal() rate.sleep()
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 MoveBase(smach.State): def __init__(self, destination_locations, timeout=120.0, action_server='/mdr_actions/move_base_safe_server'): smach.State.__init__(self, outcomes=['succeeded', 'failed']) self.destination_locations = list(destination_locations) self.timeout = timeout self.action_server = action_server self.client = SimpleActionClient(action_server, MoveBaseSafeAction) self.client.wait_for_server() def execute(self, userdata): goal = MoveBaseSafeGoal() for i, destination_location in enumerate(self.destination_locations): goal.destination_location = destination_location rospy.loginfo('Sending the base to %s' % destination_location) self.client.send_goal(goal) self.client.wait_for_result(rospy.Duration.from_sec(self.timeout)) res = self.client.get_result() if res and res.success: rospy.loginfo('Successfully reached %s' % destination_location) else: rospy.logerr('Could not reach %s' % destination_location) return 'failed' return 'succeeded'
class Gripper(): LEFT = 'l' RIGHT = 'r' def __init__(self, direction): assert (direction == Gripper.LEFT or direction == Gripper.RIGHT) self.direction = direction name_space = '/{0}_gripper_controller/gripper_action'.format( self.direction) self.gripper_client = SimpleActionClient(name_space, GripperCommandAction) self.gripper_client.wait_for_server() def open_gripper(self, wait=False): self.change_state(0.08, wait) def close_gripper(self, wait=False): self.change_state(0.0, wait) def change_state(self, value, wait): gripper_goal = GripperCommandGoal() gripper_goal.command.position = value gripper_goal.command.max_effort = 30.0 self.gripper_client.send_goal(gripper_goal) if wait: self.gripper_client.wait_for_result() if not self.gripper_client.get_result().reached_goal: time.sleep(1)
class Gripper: LEFT = "l" RIGHT = "r" def __init__(self, direction): assert direction == Gripper.LEFT or direction == Gripper.RIGHT self.direction = direction name_space = "/{0}_gripper_controller/gripper_action".format(self.direction) self.gripper_client = SimpleActionClient(name_space, GripperCommandAction) self.gripper_client.wait_for_server() def open_gripper(self, wait=False): self.change_state(0.08, wait) def close_gripper(self, wait=False): self.change_state(0.0, wait) def change_state(self, value, wait): gripper_goal = GripperCommandGoal() gripper_goal.command.position = value gripper_goal.command.max_effort = 30.0 self.gripper_client.send_goal(gripper_goal) if wait: self.gripper_client.wait_for_result() if not self.gripper_client.get_result().reached_goal: time.sleep(1)
class DropService(): def __init__(self): rospy.init_node(NAME + 'server' , anonymous=True) self.client = SimpleActionClient("smart_arm_action", SmartArmAction) self.gripper_client = SimpleActionClient("smart_arm_gripper_action", SmartArmGripperAction) self.client.wait_for_server() self.gripper_client.wait_for_server() self.service = rospy.Service('drop_service', ArmDropService, self.handle_drop) rospy.loginfo("%s: Ready for Dropping", NAME) #drop it like it's hot def handle_drop(self, req): rospy.loginfo("told to drop") success = False #this seems to be as far as it can open (both straight ahead), same vals anh uses if self.move_gripper(0.2, -0.2): success = True return success #stolen gripper code from Anh Tran's "block_stacking_actions.py" in wubble_blocks def move_gripper(self, left_finger, right_finger): goal = SmartArmGripperGoal() goal.target_joints = [left_finger, right_finger] self.gripper_client.send_goal(goal) self.gripper_client.wait_for_goal_to_finish() result = self.gripper_client.get_result() if result.success == False: rospy.loginfo("Drop failed") else: rospy.loginfo("Object Released! Hopefully it isn't sticky")
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...")
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 MotionPlayer(object): def __init__(self): rospy.loginfo("Initializing MotionPlayer...") self.ac = SimpleActionClient('/play_motion', PlayMotionAction) rospy.loginfo("Connecting to /play_motion...") # If more than 5s pass by, we crash if not self.ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr("Could not connect to /tts action server!") exit(0) rospy.loginfo("Connected!") def play_motion(self, motion_name, block=True): # To check motions: # rosparam list | grep play_motion | grep joints # for example: # open_hand, close_hand, offer_hand, wave # shake_hands, thumb_up_hand, pointing_hand, pinch_hand # head_tour, do_weights, pick_from_floor g = PlayMotionGoal() g.motion_name = motion_name if block: self.ac.send_goal_and_wait(g) else: self.ac.send_goal(g)
class execute_plan(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['success', 'failure', 'preempted'], input_keys=['already_once_executed', 'plan'], output_keys=['already_once_executed']) self.planner_executor_client = SimpleActionClient( '/task_planning/execute_plan', ExecutePlanAction) self.planner_executor_client.wait_for_server() rospy.sleep(0.2) def execute(self, userdata): userdata.already_once_executed = True success = self.execute_plan(userdata.plan) if not success: return 'failure' return 'success' def execute_plan(self, plan): goal = ExecutePlanGoal() goal.plan = plan self.planner_executor_client.send_goal(goal) finished = self.planner_executor_client.wait_for_result() if not finished: return (False) res = self.planner_executor_client.get_result() return res.success
def move_base(self, offset): client = SimpleActionClient( '/position_arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction) client.wait_for_server() goal = FollowJointTrajectoryGoal() goal.trajectory.joint_names = [ 'move_1_joint', 'move_2_joint', 'move_3_joint' ] point = JointTrajectoryPoint() point.time_from_start = rospy.Duration.from_sec(0.3) point.positions = offset.tolist() # point1 = JointTrajectoryPoint() # point1.time_from_start = rospy.Duration.from_sec(0.3) # print point1.time_from_start # point1.positions = [0, 0, 0] # point2 = JointTrajectoryPoint() # point2.time_from_start = rospy.Duration.from_sec(0.6) # point2.positions = [offset_x, offset_y, offset_z] goal.trajectory.points.append(point) # goal.trajectory.points.append(point2) client.send_goal(goal) client.wait_for_result()
class Hello: """ a class to greet people need to recognise people """ def __init__(self): self.voice = rospy.get_param("~voice", "voice_en1_mbrola") # Create the sound client object self.soundhandle = SoundClient() # 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() #set action server self.client = SimpleActionClient('face_recognition', face_recognition.msg.FaceRecognitionAction) # listening for goals. self.client.wait_for_server() rospy.sleep(2) #result will be published on this topic rospy.Subscriber("/face_recognition/result", FaceRecognitionActionFeedback, self.face_found) rospy.Subscriber("/face_recognition/feedback", FaceRecognitionActionFeedback, self.Unknown) self.look_for_face() def look_for_face(self): ''' send command look for face once ''' goal = face_recognition.msg.FaceRecognitionGoal(order_id=0, order_argument="none") self.client.send_goal(goal) #self.client.wait_for_result(rospy.Duration.from_sec(6.0)) #if self.client.get_state() == GoalStatus.FAILED: #self.unknown() def face_found(self,msg): ''' clean up feedback to extract the name check if greeted before answer accordingly ''' person = str(msg.result.names[0]) rospy.logwarn(person) self.soundhandle.say("hello "+ person ,self.voice) #self.task1 = "hello" + person #rospy.sleep(2) #return the processed value self.value = str(self.task1) def Unknown(self,msg): self.soundhandle.say("hello what is your name " ,self.voice) def __str__(self): return
class Arm: def __init__(self, arm): if arm == 'r': self.arm_client = SAC('r_arm_controller/joint_trajectory_action', JointTrajectoryAction) self.arm = arm elif arm == 'l': self.arm_client = SAC('l_arm_controller/joint_trajectory_action', JointTrajectoryAction) self.arm = arm self.arm_client.wait_for_server() rospy.loginfo('Waiting for server...') def move(self, angles, time, blocking): angles = [angles] times = [time] timeout=times[-1] + 1.0 goal = JointTrajectoryGoal() goal.trajectory.joint_names =self.get_joint_names(self.arm) for (ang, t) in zip(angles, times): point = JointTrajectoryPoint() point.positions = ang point.time_from_start = rospy.Duration(t) goal.trajectory.points.append(point) goal.trajectory.header.stamp = rospy.Time.now() self.arm_client.send_goal(goal) rospy.sleep(.1) rospy.loginfo("command sent to client") status = 0 if blocking: #XXX why isn't this perfect? end_time = rospy.Time.now() + rospy.Duration(timeout+ .1) while ( (not rospy.is_shutdown()) and\ (rospy.Time.now() < end_time) and\ (status < gs.SUCCEEDED) and\ (type(self.arm_client.action_client.last_status_msg) != type(None))): status = self.arm_client.action_client.last_status_msg.status_list[-1].status #XXX get to 80 rospy.Rate(10).sleep() if status >gs.SUCCEEDED: rospy.loginfo("goal status achieved. exiting") else: rospy.loginfo("ending due to timeout") result = self.arm_client.get_result() return result def get_joint_names(self, char): return [char+'_shoulder_pan_joint', char+'_shoulder_lift_joint', char+'_upper_arm_roll_joint', char+'_elbow_flex_joint', char+'_forearm_roll_joint', char+'_wrist_flex_joint', char+'_wrist_roll_joint' ]
class Kill(HandlerBase): alarm_name = 'kill' def __init__(self): self._killed = False self.initial_alarm = Alarm(self.alarm_name, True, node_name='alarm_server', problem_description='Initial kill') self.bag_client = SimpleActionClient("/online_bagger/bag", BagOnlineAction) self.task_client = TaskClient() self.first = True def _online_bagger_cb(self, status, result): if status == 3: rospy.loginfo('KILL BAG WRITTEN TO {}'.format(result.filename)) else: rospy.logwarn('KILL BAG {}, status: {}'.format(TerminalState.to_string(status), result.status)) def _kill_task_cb(self, status, result): if status == 3: rospy.loginfo('Killed task success!') return rospy.logwarn('Killed task failed ({}): {}'.format(TerminalState.to_string(status), result.result)) def raised(self, alarm): self._killed = True if self.first: self.first = False return if 'BAG_ALWAYS' not in os.environ or 'bag_kill' not in os.environ: rospy.logwarn('BAG_ALWAYS or BAG_KILL not set. Not making kill bag.') else: goal = BagOnlineGoal(bag_name='kill.bag') goal.topics = os.environ['BAG_ALWAYS'] + ' ' + os.environ['bag_kill'] self.bag_client.send_goal(goal, done_cb=self._online_bagger_cb) self.task_client.run_task('Killed', done_cb=self._kill_task_cb) def cleared(self, alarm): self._killed = False def meta_predicate(self, meta_alarm, alarms): ignore = [] # Don't kill on low battery, only on critical # if alarms['battery-voltage'].raised and alarms['battery-voltage'].severity < 2: # ignore.append('battery-voltage') # Raised if any alarms besides the two above are raised raised = [name for name, alarm in alarms.items() if name not in ignore and alarm.raised] if len(raised): return Alarm('kill', True, node_name=rospy.get_name(), problem_description='Killed by meta alarm(s) ' + ', '.join(raised), parameters={'Raised': raised}) return self._killed
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 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 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.')
class HeadObjectTracking(): 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.curr_tracking_point = Point(1, 0, 0.5) self.point_head(self.curr_tracking_point.x, self.curr_tracking_point.y, self.curr_tracking_point.z) rospy.Subscriber('catch_me_destination_publisher', AlvarMarker, self.new_tracking_data) def new_tracking_data(self, marker): """ Adds a new tracking data point for the head. Points the head to a point taken as a moving average over some number of previous tracking data points. """ pos = marker.pose.pose.position OLD_DATA_WEIGHT = .3 # calculate the moving average of the x, y, z positions tracking_point = self.curr_tracking_point avg_x = (self.curr_tracking_point.x * OLD_DATA_WEIGHT) + (pos.x * (1 - OLD_DATA_WEIGHT)) avg_y = (self.curr_tracking_point.y * OLD_DATA_WEIGHT) + (pos.y * (1 - OLD_DATA_WEIGHT)) avg_z = (self.curr_tracking_point.z * OLD_DATA_WEIGHT) + (pos.z * (1 - OLD_DATA_WEIGHT)) # make a new averaged point to track and point the head there self.curr_tracking_point = Point(avg_x, avg_y, avg_z) self.point_head(avg_x, avg_y, avg_z) def point_head(self, x, y, z): """ Point the head to the specified point """ head_goal = PointHeadGoal() head_goal.target.header.frame_id = '/torso_lift_link' head_goal.max_velocity = .3 # The transform seems to aim high. Move it down a little... head_goal.target.point = Point(x, y, z - .4) rospy.logdebug('Moving head to\n' + str(head_goal.target.point)) self.head_client.send_goal(head_goal)
class Kill(HandlerBase): alarm_name = 'kill' def __init__(self): self._killed = False self.initial_alarm = Alarm(self.alarm_name, True, node_name='alarm_server', problem_description='Initial kill') self.bag_client = SimpleActionClient("/online_bagger/bag", BagOnlineAction) self.first = True def _online_bagger_cb(self, status, result): if status == 3: rospy.loginfo('KILL BAG WRITTEN TO {}'.format(result.filename)) else: rospy.logwarn('KILL BAG {}, status: {}'.format(TerminalState.to_string(status), result.status)) def raised(self, alarm): self._killed = True if self.first: self.first = False return if 'BAG_ALWAYS' not in os.environ or 'bag_kill' not in os.environ: rospy.logwarn('BAG_ALWAYS or BAG_KILL not set. Not making kill bag.') return goal = BagOnlineGoal(bag_name='kill.bag') goal.topics = os.environ['BAG_ALWAYS'] + ' ' + os.environ['bag_kill'] self.bag_client.send_goal(goal, done_cb=self._online_bagger_cb) def cleared(self, alarm): self._killed = False def meta_predicate(self, meta_alarm, alarms): if self._killed: # Stay killed until manually cleared return True ignore = [] # Don't kill on low battery, only on critical # if alarms['battery-voltage'].raised and alarms['battery-voltage'].severity < 2: # ignore.append('battery-voltage') # Raised if any alarms besides the two above are raised return any([alarm.raised for name, alarm in alarms.items() if name not in ignore])
class Torso: def __init__(self): name_space = '/torso_controller/position_joint_action' self.torso_client = SimpleActionClient(name_space, SingleJointPositionAction) self.torso_client.wait_for_server() def move(self, position): assert(position >= 0.0 and position <= 0.2) self.position = position up = SingleJointPositionGoal() up.position = self.position up.min_duration = rospy.Duration(2.0) up.max_velocity = 1.0 self.torso_client.send_goal(up) self.torso_client.wait_for_result()
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())
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!")
def fixHeadPosition(userdata): head_goal = PointHeadGoal() head_goal.target.header.frame_id = "base_link" head_goal.target.point.x = 1.0 head_goal.target.point.y = 0.0 head_goal.target.point.z = 1.65 head_goal.pointing_frame = "stereo_link" head_goal.pointing_axis.x = 1.0 head_goal.pointing_axis.y = 0.0 head_goal.pointing_axis.z = 0.0 head_goal.max_velocity = 1.5 head_goal.min_duration.secs = 1.5 client = SimpleActionClient("/head_traj_controller/point_head_action", PointHeadAction) client.wait_for_server(rospy.Duration(5.0)) client.send_goal(head_goal) return succeeded
class Move_Head: """ a class to fins and pick up a object """ def __init__(self, text): #self.task = text#whole message from greeting self.head_client = SimpleActionClient('head_controller/follow_joint_trajectory', FollowJointTrajectoryAction) self.head_client.wait_for_server() ar = text.split(':') cmd = ar[0] direction = ar[5] if direction =='left': self.move_head(1.1, 0.0) if direction =='right': self.move_head(-1.1, 0.0) if direction =='front': self.move_head(0.0, 0.0) def move_head(self, pan , tilt): # Which joints define the head? head_joints = ['head_pan_joint', 'head_tilt_mod_joint'] # Create a single-point head trajectory with the head_goal as the end-point head_trajectory = JointTrajectory() head_trajectory.joint_names = head_joints head_trajectory.points.append(JointTrajectoryPoint()) head_trajectory.points[0].positions = pan , tilt head_trajectory.points[0].velocities = [0.0 for i in head_joints] head_trajectory.points[0].accelerations = [0.0 for i in head_joints] head_trajectory.points[0].time_from_start = rospy.Duration(3.0) # Send the trajectory to the head action server rospy.loginfo('Moving the head to goal position...') head_goal = FollowJointTrajectoryGoal() head_goal.trajectory = head_trajectory head_goal.goal_time_tolerance = rospy.Duration(0.0) # Send the goal self.head_client.send_goal(head_goal) # Wait for up to 5 seconds for the motion to complete self.head_client.wait_for_result(rospy.Duration(2.0))
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 gripper_action(self, gripper_type, gripper_position): name_space = '/' + gripper_type + '_gripper_controller/gripper_action' gripper_client = SimpleActionClient(name_space, GripperCommandAction) gripper_client.wait_for_server() gripper_goal = GripperCommandGoal() gripper_goal.command.position = gripper_position gripper_goal.command.max_effort = 30.0 gripper_client.send_goal(gripper_goal) # update the state of the current gripper being modified if (gripper_type == 'l'): if (gripper_position == 0.0): self.left_gripper_state = 'closed' else: self.left_gripper_state = 'open' else: if (gripper_position == 0.0): self.right_gripper_state = 'closed' else: self.right_gripper_state = 'open'
def main(): rospy.init_node("send_reference") ref_args = sys.argv[1:] if not ref_args: rospy.logwarn("Usage: send_reference JOINT_NAME_1 POSITION_1 [ JOINT_NAME_2 POSITION 2 ... ]") return 1 ac_joint_traj = SimpleActionClient("/amigo/body/joint_trajectory_action", FollowJointTrajectoryAction) while not ac_joint_traj.wait_for_server(timeout=rospy.Duration(1.0)) and not rospy.is_shutdown(): rospy.logwarn("Waiting for server...") if rospy.is_shutdown(): rospy.logwarn("No reference sent") return 1 rospy.loginfo("Connected to server") # - - - - - - - - - - - - - - - - - - - - - - - - - - - - # Even items in ref_args are joint_names joint_names = ref_args[0::2] # Odd items in ref_args are set points ref_positions = [float(i) for i in ref_args[1::2]] # - - - - - - - - - - - - - - - - - - - - - - - - - - - - points = [JointTrajectoryPoint(positions=ref_positions, time_from_start=rospy.Duration(0))] joint_trajectory = JointTrajectory(joint_names=joint_names, points=points) goal = FollowJointTrajectoryGoal(trajectory=joint_trajectory, goal_time_tolerance=rospy.Duration(1000)) ac_joint_traj.send_goal(goal) ac_joint_traj.wait_for_result(rospy.Duration(1000)) res = ac_joint_traj.get_result() if res.error_string: rospy.logerr(res.error_string)
def move_gripper(): name_space = "/{0}_gripper_controller/gripper_action".format(self.direction) gripper_client = SimpleActionClient(name_space, GripperCommandAction) gripper_client.wait_for_server() gripper_goal = GripperCommandGoal() if self.state: # The gripper is open. Close it and update its state. gripper_goal.command.position = 0.0 self.state = Gripper.CLOSED else: # The gripper is closed. Open it and update its state. gripper_goal.command.position = 0.08 self.state = Gripper.OPEN gripper_goal.command.max_effort = 30.0 gripper_client.send_goal(gripper_goal) gripper_client.wait_for_result(rospy.Duration(10.0)) self.gui.show_text_in_rviz("Gripper")
def run(self): # pub = rospy.Publisher('joy_cmd', JoyDriveStamped, queue_size=1) # r = rospy.Rate(10) simpleac = SimpleActionClient("move_base", MoveBaseAction) goal = MoveBaseGoal() goal.target_pose.header.frame_id = '/map' while not rospy.is_shutdown(): buf = self.conn.recvfrom(struct.calcsize(self.ctrl_def)) self.data = struct.unpack(self.ctrl_def, buf[0]) goal.target_pose.header.stamp = rospy.Time.from_sec(self.data[0]) goal.target_pose.pose.position.x = self.data[1] goal.target_pose.pose.position.y = self.data[2] goal.target_pose.pose.orientation.z = self.data[3] goal.target_pose.pose.orientation.w = self.data[4] print self.data simpleac.send_goal(goal)
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')