Python msg.MoveBaseGoal方法代码示例

# 需要导入模块: from move_base_msgs import msg [as 别名]
# 或者: from move_base_msgs.msg import MoveBaseGoal [as 别名]
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 

# 需要导入模块: from move_base_msgs import msg [as 别名]
# 或者: from move_base_msgs.msg import MoveBaseGoal [as 别名]
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] 

# 需要导入模块: from move_base_msgs import msg [as 别名]
# 或者: from move_base_msgs.msg import MoveBaseGoal [as 别名]
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 

# 需要导入模块: from move_base_msgs import msg [as 别名]
# 或者: from move_base_msgs.msg import MoveBaseGoal [as 别名]
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 

# 需要导入模块: from move_base_msgs import msg [as 别名]
# 或者: from move_base_msgs.msg import MoveBaseGoal [as 别名]
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 

# 需要导入模块: from move_base_msgs import msg [as 别名]
# 或者: from move_base_msgs.msg import MoveBaseGoal [as 别名]
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 

# 需要导入模块: from move_base_msgs import msg [as 别名]
# 或者: from move_base_msgs.msg import MoveBaseGoal [as 别名]
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