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"
