Python源码示例：move.MoveBaseGoal()
示例1
def _CreateMoveBaseGoal(self, goal):
'''
Creates an instance of MoveBaseGoal based on a simple goal in the form of a (x,y,theta) tuple
'''
x,y,theta = goal
moveBaseGoal = MoveBaseGoal()
moveBaseGoal.target_pose.header.frame_id = self._GoalFrameId
moveBaseGoal.target_pose.header.stamp = rospy.Time.now()
moveBaseGoal.target_pose.pose.position.x = x
moveBaseGoal.target_pose.pose.position.y = y
quaternionArray = tf.transformations.quaternion_about_axis(theta, (0,0,1))
# quaternion_about_axis offers a convenient way for calculating the members of a quaternion.
# In order to use it we need to convert it to a Quaternion message structure
moveBaseGoal.target_pose.pose.orientation = self.array_to_quaternion(quaternionArray)
print(moveBaseGoal)
return moveBaseGoal
示例2
def _CreateMoveBaseGoal(self, goal):
'''
Creates an instance of MoveBaseGoal based on a simple goal in the form of a (x,y,theta) tuple
'''
x,y,theta = goal
moveBaseGoal = MoveBaseGoal()
moveBaseGoal.target_pose.header.frame_id = self._GoalFrameId
moveBaseGoal.target_pose.header.stamp = rospy.Time.now()
moveBaseGoal.target_pose.pose.position.x = x
moveBaseGoal.target_pose.pose.position.y = y
quaternionArray = tf.transformations.quaternion_about_axis(theta, (0,0,1))
# quaternion_about_axis offers a convenient way for calculating the members of a quaternion.
# In order to use it we need to convert it to a Quaternion message structure
moveBaseGoal.target_pose.pose.orientation = self.array_to_quaternion(quaternionArray)
print(moveBaseGoal)
return moveBaseGoal
示例3
def _CreateMoveBaseGoal(self, goal):
'''
Creates an instance of MoveBaseGoal based on a simple goal in the form of a (x,y,theta) tuple
'''
x,y,theta = goal
moveBaseGoal = MoveBaseGoal()
moveBaseGoal.target_pose.header.frame_id = self._GoalFrameId
moveBaseGoal.target_pose.header.stamp = rospy.Time.now()
moveBaseGoal.target_pose.pose.position.x = x
moveBaseGoal.target_pose.pose.position.y = y
quaternionArray = tf.transformations.quaternion_about_axis(theta, (0,0,1))
# quaternion_about_axis offers a convenient way for calculating the members of a quaternion.
# In order to use it we need to convert it to a Quaternion message structure
moveBaseGoal.target_pose.pose.orientation = self.array_to_quaternion(quaternionArray)
print(moveBaseGoal)
return moveBaseGoal
示例4
def _send_action_goal(self, x, y, theta, frame):
"""A function to send the goal state to the move_base action server """
goal = MoveBaseGoal()
goal.target_pose = build_pose_msg(x, y, theta, frame)
goal.target_pose.header.stamp = rospy.Time.now()
rospy.loginfo("Waiting for the server")
self.move_base_sac.wait_for_server()
rospy.loginfo("Sending the goal")
self.move_base_sac.send_goal(goal)
rospy.sleep(0.1)
rospy.loginfo("Waiting for the Result")
while True:
assert (
self.execution_status is not 4
), "move_base failed to find a valid plan to goal"
if self.execution_status is 3:
rospy.loginfo("Base reached the goal state")
return
if self.base_state.should_stop:
rospy.loginfo("Base asked to stop. Cancelling goal sent to move_base.")
self.cancel_goal()
return
示例5
def __init__(self, position, orientation):
State.__init__(self, outcomes=['success'])
# Get an action client
self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
self.client.wait_for_server()
# Define the goal
self.goal = MoveBaseGoal()
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.pose.position.x = position[0]
self.goal.target_pose.pose.position.y = position[1]
self.goal.target_pose.pose.position.z = 0.0
self.goal.target_pose.pose.orientation.x = orientation[0]
self.goal.target_pose.pose.orientation.y = orientation[1]
self.goal.target_pose.pose.orientation.z = orientation[2]
self.goal.target_pose.pose.orientation.w = orientation[3]
示例6
def goal_pose(pose):
goal_pose = MoveBaseGoal()
goal_pose.target_pose.header.frame_id = 'world'
goal_pose.target_pose.pose.position.x = pose[0][0]
goal_pose.target_pose.pose.position.y = pose[0][1]
goal_pose.target_pose.pose.position.z = pose[0][2]
goal_pose.target_pose.pose.orientation.x = pose[1][0]
goal_pose.target_pose.pose.orientation.y = pose[1][1]
goal_pose.target_pose.pose.orientation.z = pose[1][2]
goal_pose.target_pose.pose.orientation.w = pose[1][3]
return goal_pose
示例7
def goto_pose(self, xya, frame_id):
trans, rot = conv.xya_to_trans_rot(xya)
pose = conv.trans_rot_to_pose(trans, rot)
ps = gm.PoseStamped()
ps.pose = pose
ps.header.frame_id = frame_id
ps.header.stamp = rospy.Time(0)
goal = mbm.MoveBaseGoal()
goal.target_pose = ps
goal.header.frame_id = frame_id
rospy.loginfo('Sending move base goal')
finished = self.action_client.send_goal_and_wait(goal)
rospy.loginfo('Move base action returned %d.' % finished)
return finished
示例8
def goal_pose(pose): # <2>
goal_pose = MoveBaseGoal()
goal_pose.target_pose.header.frame_id = 'map'
goal_pose.target_pose.pose.position.x = pose[0][0]
goal_pose.target_pose.pose.position.y = pose[0][1]
goal_pose.target_pose.pose.position.z = pose[0][2]
goal_pose.target_pose.pose.orientation.x = pose[1][0]
goal_pose.target_pose.pose.orientation.y = pose[1][1]
goal_pose.target_pose.pose.orientation.z = pose[1][2]
goal_pose.target_pose.pose.orientation.w = pose[1][3]
return goal_pose
示例9
def dock(self, target_bin):
self.target_bin = target_bin
rate = rospy.Rate(10)
while not rospy.is_shutdown():
marker_frame = "ar_marker_%d_up" % self.target_bin
try:
t = self.tf_listener.getLatestCommonTime("/base_link", marker_frame)
print "age: %.6f" % (rospy.Time.now() - t).to_sec()
if (rospy.Time.now() - t).to_sec() > 0.2:
rospy.sleep(0.1)
continue
(marker_translation, marker_orient) = self.tf_listener.lookupTransform('/base_link',marker_frame,t)
print "marker: " + str(marker_translation)
target_translation = Vector3(1.3, 0, 0.5)
if (abs(marker_translation[0]) + abs(marker_translation[1])) < 0.15:
print "close enough!"
break;
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = marker_frame
goal.target_pose.pose.position.x = 0
goal.target_pose.pose.position.y = -1.5
orient = Quaternion(*quaternion_from_euler(0, 0, 1.57))
goal.target_pose.pose.orientation = orient
self.move_base.send_goal(goal)
self.move_base.wait_for_result()
result = self.move_base.get_result()
nav_state = self.move_base.get_state()
if nav_state == 3:
print "move success! waiting to calm down before looking again..."
rospy.sleep(1) # wait for things to calm down a bit before looking
self.point_head_forwards()
rospy.sleep(0.5)
print "done waiting."
else:
print "move failure!"
except(tf.Exception, tf.LookupException,
tf.ConnectivityException, tf.ExtrapolationException):
rate.sleep() # not yet in view
