|
#!/usr/bin/env python3 |
|
import rospy, actionlib, tf |
|
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal |
|
from actionlib_msgs.msg import GoalStatus |
|
from geometry_msgs.msg import Pose, Point, Quaternion |
|
|
|
def goto(x, y, theta, timeout=120): |
|
goal = MoveBaseGoal() |
|
goal.target_pose.header.frame_id = 'map' |
|
goal.target_pose.header.stamp = rospy.Time.now() |
|
goal.target_pose.pose = Pose(Point(x, y, 0), Quaternion(*tf.transformations.quaternion_from_euler(0, 0, theta))) |
|
move_base.send_goal(goal) |
|
rospy.loginfo("Going to x={} y={} theta={}...".format(x, y, theta)) |
|
if move_base.wait_for_result(rospy.Duration(timeout)) and move_base.get_state() == GoalStatus.SUCCEEDED: |
|
rospy.loginfo("The target is reached") |
|
else: |
|
rospy.logwarn("We could NOT reach the target after " + str(timeout) + "secs") |
|
move_base.cancel_goal() |
|
|
|
# Now, start the navigation node |
|
rospy.init_node('navigation') |
|
move_base = actionlib.SimpleActionClient("/move_base", MoveBaseAction) |
|
rospy.loginfo("Waiting for the connection to /move_base...") |
|
move_base.wait_for_server(rospy.Duration(20)) |
|
|
|
# Just call this function to reach a point (x, y, theta) on the map |
|
goto(x=1, y=0.5, theta=3.14) |
