english

navigate.py

#!/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)