走多边形导航

#!/usr/bin/env python
# -*- coding: utf-8 -*-
""" move_base_square.py - Version 1.1 2013-12-20

使用move_base动作命令机器人在正方形中移动。
为pi机器人项目创建:http://www.pirobot.org
版权所有(c) 2012帕特里克·戈贝尔保留所有权利。
这个程序是自由软件;您可以重新分发它和/或修改它
它是根据GNU通用公共许可证的条款发布的
自由软件基金会;或许可的版本2
(由你选择)任何后期版本
这个程序的分发是希望它将是有用的，
但没有任何保证;甚至没有默示的保证
适销性或适合于特定用途。看到
有关更多详情，请浏览GNU通用公共许可证:

    http://www.gnu.org/licenses/gpl.htmlPoint

"""

import rospy
import actionlib
from actionlib_msgs.msg import *
from geometry_msgs.msg import Pose, Point, Quaternion, Twist
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from tf.transformations import quaternion_from_euler
from visualization_msgs.msg import Marker
from math import radians, pi

class MoveBaseTrangle():
    def __init__(self):
        #初始化节点
        rospy.init_node('nav_trangle', anonymous=False)
        #声明结束处理函数
        rospy.on_shutdown(self.shutdown)
        
        # 单段导航的距离
        square_size = rospy.get_param("~trangle_size", 1.0) # meters
        
        # 创造一个列表去保存目标点的方向
        quaternions = list()
        
        # 定义三个目标点的方向
        euler_angles = (2*pi/3, 4*pi/3, 0)
        
        # 把角度转换成四元数
        for angle in euler_angles:
            #先把角度转化成欧拉角
            q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz')
            #再把欧拉角转换成四元数
            q = Quaternion(*q_angle)
            #把四元素加入到四元素列表
            quaternions.append(q)
        
        # 创建一个列表去保存航路点
        waypoints = list()
        
        # 组合三个目标点的坐标和方向，创建了三个目标方位
        waypoints.append(Pose(Point(square_size, 0.0, 0.0), quaternions[0]))
        waypoints.append(Pose(Point(0.5, square_size, 0.0), quaternions[1]))
        waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[2]))
        
        #在Rviz中设置可视的标识
        # Initialize the visualization markers for RViz
        self.init_markers()
        
        # Set a visualization marker at each waypoint        
        for waypoint in waypoints:           
            p = Point()
            p = waypoint.position
            self.markers.points.append(p)
            
        # Publisher to manually control the robot (e.g. to stop it, queue_size=5)
        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
        
        # 定义SimpleActionClient，它将会发送目标到move_base行为服务器
        self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
        
        rospy.loginfo("Waiting for move_base action server...")
        
        # 发送目标之前，等待行为服务器可用，等待超过60秒则判定为超时
        self.move_base.wait_for_server(rospy.Duration(60))
        
        rospy.loginfo("Connected to move base server")
        rospy.loginfo("Starting navigation test")
        
        # Initialize a counter to track waypoints
        i = 0
        
        # 进入主循环，循环到三个目标点
        while i < 3 and not rospy.is_shutdown():
            # 发布所有目标方位的标识
            self.marker_pub.publish(self.markers)
            
            # 初始化行为类型
            goal = MoveBaseGoal()
            
            # 把frame_id设为map框架
            goal.target_pose.header.frame_id = 'map'
            
            # 时间截设为当前时间
            goal.target_pose.header.stamp = rospy.Time.now()
            
            # 设置目标位置
            goal.target_pose.pose = waypoints[i]
            
            # 发送目标到行为服务器
            self.move(goal)
            
            i += 1
        
    def move(self, goal):
            # Send the goal pose to the MoveBaseAction server
            self.move_base.send_goal(goal)
            
            # Allow 1 minute to get there
            finished_within_time = self.move_base.wait_for_result(rospy.Duration(60)) 
            
            # If we don't get there in time, abort the goal
            if not finished_within_time:
                self.move_base.cancel_goal()
                rospy.loginfo("Timed out achieving goal")
            else:
                # We made it!
                state = self.move_base.get_state()
                if state == GoalStatus.SUCCEEDED:
                    rospy.loginfo("Goal succeeded!")

    def init_markers(self):
        # Set up our waypoint markers
        marker_scale = 0.2
        marker_lifetime = 0 # 0 is forever
        marker_ns = 'waypoints'
        marker_id = 0
        marker_color = {'r': 1.0, 'g': 0.7, 'b': 1.0, 'a': 1.0}
    
        # Define a marker publisher.
        self.marker_pub = rospy.Publisher('waypoint_markers', Marker, queue_size=5)

        # Initialize the marker points list.
        self.markers = Marker()
        self.markers.ns = marker_ns
        self.markers.id = marker_id
        self.markers.type = Marker.CUBE_LIST
        self.markers.action = Marker.ADD
        self.markers.lifetime = rospy.Duration(marker_lifetime)
        self.markers.scale.x = marker_scale
        self.markers.scale.y = marker_scale
        self.markers.color.r = marker_color['r']
        self.markers.color.g = marker_color['g']
        self.markers.color.b = marker_color['b']
        self.markers.color.a = marker_color['a']

        self.markers.header.frame_id = 'odom'
        self.markers.header.stamp = rospy.Time.now()
        self.markers.points = list()

    def shutdown(self):
        rospy.loginfo("Stopping the robot...")
        # Cancel any active goals
        self.move_base.cancel_goal()
        rospy.sleep(2)
        # Stop the robot
        self.cmd_vel_pub.publish(Twist())
        rospy.sleep(1)

if __name__ == '__main__':
    try:
        MoveBaseTrangle()
    except rospy.ROSInterruptException:
        rospy.loginfo("Navigation test finished.")