Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
#!/usr/bin/env python
import rospy, actionlib, tf
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
# convert x,y,radians into 3D pose and send to move_base action server
def publishgoal(str):
global move_base
s = str.split(",")
x = float(s[0])
y = float(s[1])
th = float(s[2])
goal = MoveBaseGoal()
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.header.frame_id = "map"
goal.target_pose.pose.position.x = x
goal.target_pose.pose.position.y = y
goal.target_pose.pose.position.z = 0.0
quat = tf.transformations.quaternion_from_euler(0, 0, th)
goal.target_pose.pose.orientation.x = quat[0]
goal.target_pose.pose.orientation.y = quat[1]
goal.target_pose.pose.orientation.z = quat[2]
goal.target_pose.pose.orientation.w = quat[3]
move_base.send_goal(goal)
# Main
# initialize node
rospy.init_node('single2DGoalSend', anonymous=False)
# connect to move_base action server
move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
move_base.wait_for_server()
publishgoal("4.797,2.962,-3.1241")
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.