def go_to_waypoint(goal_x=0, goal_y=0, goal_theta=0): # Create an action client called "move_base" with action definition file "MoveBaseAction" client = actionlib.SimpleActionClient('move_base', MoveBaseAction) # Waits until the action server has started up and started listening for goals. client.wait_for_server() # Creates a new goal with the MoveBaseGoal constructor goal = MoveBaseGoal() goal.target_pose.header.frame_id = "map" goal.target_pose.header.stamp = rospy.Time.now() # Move 0.5 meters forward along the x axis of the "map" coordinate frame goal.target_pose.pose.position.x = goal_x goal.target_pose.pose.position.y = goal_y goal.target_pose.pose.position.z = 0 # No rotation of the mobile base frame w.r.t. map frame angle_in_quat = quaternion_from_euler(0, 0, goal_theta) goal.target_pose.pose.orientation.x = angle_in_quat[0] goal.target_pose.pose.orientation.y = angle_in_quat[1] goal.target_pose.pose.orientation.z = angle_in_quat[2] goal.target_pose.pose.orientation.w = angle_in_quat[3] # Sends the goal to the action server. client.send_goal(goal, feedback_cb=fb_callback) # Waits for the server to finish performing the action. wait = client.wait_for_result() # If the result doesn't arrive, assume the Server is not available if not wait: rospy.logerr("Action server not available!") rospy.signal_shutdown("Action server not available!") else: # Result of executing the action return client.get_result()
def depthAction(self, depth): client = actionlib.SimpleActionClient( "go_to_depth", riptide_controllers.msg.GoToDepthAction) client.wait_for_server() # Sends the goal to the action server. client.send_goal(riptide_controllers.msg.GoToDepthGoal(depth)) return client
def yawAction(self, angle): client = actionlib.SimpleActionClient( "go_to_yaw", riptide_controllers.msg.GoToYawAction) client.wait_for_server() # Sends the goal to the action server. client.send_goal(riptide_controllers.msg.GoToYawGoal(angle)) return client
def _accept_path_from_topic(self, path_msg): """ When receiving a path on a topic, just call our own action server! This ensures canceling/aborting/preemption etc behave as they should :return: None """ rospy.loginfo("Path received on topic, calling action to execute") client = actionlib.SimpleActionClient("follow_path", FollowPathAction) client.wait_for_server() client.send_goal(FollowPathGoal(path=path_msg))
def execute(self, userdata): rospy.loginfo('{}: Executing state: {}'.format( rospy.get_name(), self.__class__.__name__)) pose = userdata.current_pose.parameters['pose'] theta, beta, alpha = euler_from_quaternion( (pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z)) print 'TurnAround current theta: {} {} {}'.format(alpha, beta, theta) if theta < 0: new_pose = makePose(pose.position.x, pose.position.y, theta + math.pi) else: new_pose = makePose(pose.position.x, pose.position.y, theta - math.pi) answer_id = self.conversation_interface.setAutomaticAnswer( 'q_current_task', u'niekorzystne warunki pogodowe odwracam się') if self.sim_mode == 'sim': for i in range(50): if self.preempt_requested(): self.conversation_interface.removeAutomaticAnswer( answer_id) self.service_preempt() return 'preemption' rospy.sleep(0.1) self.conversation_interface.removeAutomaticAnswer(answer_id) return 'ok' else: goal = MoveBaseGoal() goal.target_pose.pose = new_pose goal.target_pose.header.frame_id = 'map' goal.target_pose.header.stamp = rospy.Time.now() client = actionlib.SimpleActionClient('move_base', MoveBaseAction) client.wait_for_server() # start moving client.send_goal(goal, self.move_base_done_cb, self.move_base_active_cb, self.move_base_feedback_cb) # action_feedback = GoActionFeedback() # action_result = GoActionResult() # action_result.result.is_goal_accomplished = False # userdata.nav_result = action_result.result start_time = rospy.Time.now() self.is_goal_achieved = False while self.is_goal_achieved == False: # action_feedback.feedback.current_pose = self.current_pose # userdata.nav_feedback = action_feedback.feedback # userdata.nav_actual_pose = self.current_pose end_time = rospy.Time.now() loop_time = end_time - start_time loop_time_s = loop_time.secs if self.__shutdown__: client.cancel_all_goals() self.conversation_interface.removeAutomaticAnswer( answer_id) self.service_preempt() return 'shutdown' if loop_time_s > NAVIGATION_MAX_TIME_S: # break the loop, end with error state self.conversation_interface.removeAutomaticAnswer( answer_id) rospy.logwarn( 'State: Navigation took too much time, returning error' ) client.cancel_all_goals() return 'stall' if self.preempt_requested(): self.conversation_interface.removeAutomaticAnswer( answer_id) client.cancel_all_goals() self.service_preempt() return 'preemption' rospy.sleep(0.1) # Manage state of the move_base action server self.conversation_interface.removeAutomaticAnswer(answer_id) # Here check move_base DONE status if self.move_base_status == GoalStatus.PENDING: # The goal has yet to be processed by the action server raise Exception('Wrong move_base action status: "PENDING"') elif self.move_base_status == GoalStatus.ACTIVE: # The goal is currently being processed by the action server raise Exception('Wrong move_base action status: "ACTIVE"') elif self.move_base_status == GoalStatus.PREEMPTED: # The goal received a cancel request after it started executing # and has since completed its execution (Terminal State) return 'preemption' elif self.move_base_status == GoalStatus.SUCCEEDED: # The goal was achieved successfully by the action server (Terminal State) return 'ok' elif self.move_base_status == GoalStatus.ABORTED: # The goal was aborted during execution by the action server due # to some failure (Terminal State) return 'stall' elif self.move_base_status == GoalStatus.REJECTED: # The goal was rejected by the action server without being processed, # because the goal was unattainable or invalid (Terminal State) return 'error' elif self.move_base_status == GoalStatus.PREEMPTING: # The goal received a cancel request after it started executing # and has not yet completed execution raise Exception('Wrong move_base action status: "PREEMPTING"') elif self.move_base_status == GoalStatus.RECALLING: # The goal received a cancel request before it started executing, # but the action server has not yet confirmed that the goal is canceled raise Exception('Wrong move_base action status: "RECALLING"') elif self.move_base_status == GoalStatus.RECALLED: # The goal received a cancel request before it started executing # and was successfully cancelled (Terminal State) return 'preemption' elif self.move_base_status == GoalStatus.LOST: # An action client can determine that a goal is LOST. This should not be # sent over the wire by an action server raise Exception('Wrong move_base action status: "LOST"') else: raise Exception('Wrong move_base action status value: "' + str(self.move_base_status) + '"')
argvs = sys.argv argc = len(argvs) if argc < 2: print('\007') print( "Usage : rosrun arno onemap_navigation.py fileName_1 filename_2 ..... filename_N" ) quit() os.chdir('/home/hokuyo/catkin_ws/src/arno/map/nide') load_file(argvs[1] + '_waypoint.json') listener = tf.TransformListener() client = actionlib.SimpleActionClient('move_base', MoveBaseAction) client.wait_for_server() listener.waitForTransform("map", "base_link", rospy.Time(), rospy.Duration(4.0)) dynamic_client = dynamic_reconfigure.client.Client("amcl", timeout=30) for i in range(argc - 1): if len(waypoints.poses) == 0: load_file(argvs[i + 1] + '_waypoint.json') rospy.sleep(2.0) for i in range(len(waypoints.poses)): pose = waypoints.poses[0] pub.publish(waypoints) goal = goal_pose(pose) client.send_goal(goal) if not change_params[0] == None:
def active_cb(): rospy.loginfo("Speaking action active now") import dynamic_reconfigure.client orig_config = None def config_callback(config): global orig_config if orig_config is None: orig_config = config rospy.loginfo("Config is {volume}, {pitch}, {rate}, {pitch_range}, {announce_punctuation}, {wordgap}".format(**config)) rospy.init_node('speech_client') client = actionlib.SimpleActionClient('speak', SpeakAction) client.wait_for_server() while not rospy.is_shutdown(): goal = SpeakGoal() goal.text = "Witaj świecie raz dwa" client.send_goal(goal, feedback_cb=feedback_cb, done_cb=done_cb, active_cb=active_cb) #interrupt immediately goal = SpeakGoal() goal.text = "Witaj świecie raz dwa trzy cztery" client.send_goal(goal, feedback_cb=feedback_cb, done_cb=done_cb, active_cb=active_cb) rospy.sleep(1) #interrupt aftet a second goal = SpeakGoal() goal.text = "Witaj ponownie świecie, raz dwa trzy cztery"