Skip to content

Instantly share code, notes, and snippets.

@ymollard
Last active May 31, 2022 13:55
Show Gist options
  • Save ymollard/19b097259576f126fc3a75fa7381a6d9 to your computer and use it in GitHub Desktop.
Save ymollard/19b097259576f126fc3a75fa7381a6d9 to your computer and use it in GitHub Desktop.
english
#!/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)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment