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()
if not change_params[0] == None:
dynamic_client.update_configuration(change_params[i])
while not rospy.is_shutdown():
now = rospy.Time.now()
listener.waitForTransform("map", "base_link", now,
rospy.Duration(4.0))
position, quaternion = listener.lookupTransform(
"map", "base_link", now)
euler = tf.transformations.euler_from_quaternion(
(quaternion[0], quaternion[1], quaternion[2],
quaternion[3]))
goal_euler = tf.transformations.euler_from_quaternion(
(goal.target_pose.pose.orientation.x,
goal.target_pose.pose.orientation.y,
goal.target_pose.pose.orientation.z,
goal.target_pose.pose.orientation.w))
print("dis = " +
str((position[0] - goal.target_pose.pose.position.x)**2 +
(position[1] - goal.target_pose.pose.position.y)**2))
print("rad = " + str(angle_dif(goal_euler[2], euler[2])))
client.wait_for_result(rospy.Duration(0.1))
if (math.sqrt(
(position[0] - goal.target_pose.pose.position.x)**2 +
(position[1] - goal.target_pose.pose.position.y)**2) <=
xy_tolerance) or (client.get_result()):
print("next!!")
waypoints.poses.pop(0)
break
for i in range(len(waypoints.poses)):
pose = waypoints.poses[i]
pub.publish(waypoints)
goal = goal_pose(pose)
client.send_goal(goal)
if not change_params[i] == None:
dynamic_client.update_configuration(change_params[i])
while not rospy.is_shutdown():
now = rospy.Time.now()
listener.waitForTransform("map", "base_link", now,
rospy.Duration(4.0))
position, quaternion = listener.lookupTransform(
"map", "base_link", now)
remaindXDist = position[0] - goal.target_pose.pose.position.x
remaindYDist = position[1] - goal.target_pose.pose.position.y
client.wait_for_result(rospy.Duration(0.1))
if (math.sqrt(remaindXDist**2 + remaindYDist**2) <=
xy_tolerance):
print("next!!")
break
elif (client.get_result()):
# resend goal potision
print("send Fixed goal position!!")
goal.target_pose.pose.position.x += remaindXDist
goal.target_pose.pose.position.y += remaindYDist
client.send_goal(goal)
# else:
# rospy.sleep(0.5)
position, quaternion = listener.lookupTransform(
"map", "base_link", now)
euler = tf.transformations.euler_from_quaternion(
(quaternion[0], quaternion[1], quaternion[2],
quaternion[3]))
goal_euler = tf.transformations.euler_from_quaternion(
(goal.target_pose.pose.orientation.x,
goal.target_pose.pose.orientation.y,
goal.target_pose.pose.orientation.z,
goal.target_pose.pose.orientation.w))
print("dis = " +
str((position[0] - goal.target_pose.pose.position.x)**2 +
(position[1] - goal.target_pose.pose.position.y)**2))
print("rad = " + str(angle_dif(goal_euler[2], euler[2])))
client.wait_for_result(rospy.Duration(0.1))
xy = math.sqrt(
(position[0] - goal.target_pose.pose.position.x)**2 +
(position[1] - goal.target_pose.pose.position.y)**2)
if ((len(waypoints.poses) == 1) and
((xy <= last_xy_tolerance) or (client.get_result()))):
print("next!!!!!!!!!!")
waypoints.poses.pop(0)
break
if ((xy <= xy_tolerance) or (client.get_result())):
print("next!!")
waypoints.poses.pop(0)
break
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)
while not rospy.is_shutdown():
for i in range(len(waypoints.poses)):
pose = waypoints.poses[i]
pub.publish(waypoints)
goal = goal_pose(pose)
client.send_goal(goal)
if not change_params[i] == None:
dynamic_client.update_configuration(change_params[i])
while not rospy.is_shutdown():
now = rospy.Time.now()
listener.waitForTransform("map", "base_link", now, rospy.Duration(4.0))
position, quaternion = listener.lookupTransform("map", "base_link", now)
euler = tf.transformations.euler_from_quaternion((quaternion[0], quaternion[1], quaternion[2], quaternion[3]))
goal_euler = tf.transformations.euler_from_quaternion((goal.target_pose.pose.orientation.x, goal.target_pose.pose.orientation.y, goal.target_pose.pose.orientation.z, goal.target_pose.pose.orientation.w))
print("dis = "+str((position[0]-goal.target_pose.pose.position.x)**2 + (position[1]-goal.target_pose.pose.position.y)**2 ))
print("rad = "+str(angle_dif(goal_euler[2], euler[2])))
client.wait_for_result(rospy.Duration(0.1))
if (math.sqrt((position[0]-goal.target_pose.pose.position.x)**2 + (position[1]-goal.target_pose.pose.position.y)**2 ) <= xy_tolerance) or (client.get_result()):
print("next!!")
break
# else:
# rospy.sleep(0.5)