（11-4-03）基于SLAM的自主路径导航系统：路径规划(3)

11.5.3  RRT算法

RRT（Rapidly-exploring Random Tree，快速探索随机树）算法是一种用于路径规划的基于树结构的算法，通过在自由空间中随机生成点，并将这些点逐渐连接起来形成树结构，以便找到起点到目标点的可行路径。算法的基本思想是在图形结构中快速生成节点，以便尽快探索整个空间，并尽量减少搜索时间。

（1）文件rrt.py实现了快速探索随机树（RRT）算法，用于在给定地图中搜索从起点到目标点的路径。它定义了一系列函数来生成随机点、构建RRT图、搜索路径以及将路径可视化。通过调用 find_path_RRT函数，可以在给定地图中查找从起点到目标点的路径，并返回路径及相应的RRT图。

MAP_IMG = '/home/fazildgr8/catkin_ws/src/ros_autonomous_slam/media/my_map.png' # 用于地图的黑白图像文件路径
MIN_NUM_VERT = 20 # 图中顶点的最小数量
MAX_NUM_VERT = 1500 # 图中顶点的最大数量
STEP_DISTANCE = 20 # 两个顶点之间的最大距离
SEED = None # 用于随机数生成的种子

def rapidlyExploringRandomTree(img, start, goal, seed=None):
  hundreds = 100
  random.seed(seed)
  points = []
  graph = []
  points.append(start)
  graph.append((start, []))
  # print 'Generating and conecting random points'
  occupied = True
  phaseTwo = False

  # 第二阶段值（目标点周围 5 步距离的点）
  minX = max(goal[0] - 5 * STEP_DISTANCE, 0)
  maxX = min(goal[0] + 5 * STEP_DISTANCE, len(img[0]) - 1)
  minY = max(goal[1] - 5 * STEP_DISTANCE, 0)
  maxY = min(goal[1] + 5 * STEP_DISTANCE, len(img) - 1)

  i = 0
  while (goal not in points) and (len(points) < MAX_NUM_VERT):
    # if (i % 100) == 0:
    #   print i, 'points randomly generated'

    if (len(points) % hundreds) == 0:
      # print len(points), 'vertex generated'
      hundreds = hundreds + 100

    while(occupied):
      if phaseTwo and (random.random() > 0.8):
        point = [ random.randint(minX, maxX), random.randint(minY, maxY) ]
      else:
        point = [ random.randint(0, len(img[0]) - 1), random.randint(0, len(img) - 1) ]

      if(img[point[1]][point[0]][0] > 250):
        occupied = False

    occupied = True

    nearest = findNearestPoint(points, point)
    newPoints = connectPoints(point, nearest, img)
    addToGraph(graph, newPoints, point)
    newPoints.pop(0) # 第一个元素已经在 points 列表中
    points.extend(newPoints)

    i = i + 1

    if len(points) >= MIN_NUM_VERT:
      if not phaseTwo:
        print('第二阶段')
      phaseTwo = True

    if phaseTwo:
      nearest = findNearestPoint(points, goal)
      newPoints = connectPoints(goal, nearest, img)
      addToGraph(graph, newPoints, goal)
      newPoints.pop(0)
      points.extend(newPoints)


  if goal in points:
    # print 'Goal found, total vertex in graph:', len(points), 'total random points generated:', i
    path = searchPath(graph, start, [start])
    # print 'Showing resulting map'
    # print 'Final path:', path
    # print 'The final path is made from:', len(path),'connected points'
  else:
    path = None
    print('已达到最大顶点数，未找到目标')
    print('图中的总顶点数:', len(points), '总生成的随机点数:', i)
    print('显示生成的地图')


  return path,graph

def searchPath(graph, point, path):
  for i in graph:
    if point == i[0]:
      p = i

  if p[0] == graph[-1][0]:
    return path

  for link in p[1]:
    path.append(link)
    finalPath = searchPath(graph, link, path)

    if finalPath != None:
      return finalPath
    else:
      path.pop()

def addToGraph(graph, newPoints, point):
  if len(newPoints) > 1: # 如果有任何内容要添加到图中
    for p in range(len(newPoints) - 1):
      nearest = [ nearest for nearest in graph if (nearest[0] == [ newPoints[p][0], newPoints[p][1] ]) ]
      nearest[0][1].append(newPoints[p + 1])
      graph.append((newPoints[p + 1], []))

def connectPoints(a, b, img):
  newPoints = []
  newPoints.append([ b[0], b[1] ])
  step = [ (a[0] - b[0]) / float(STEP_DISTANCE), (a[1] - b[1]) / float(STEP_DISTANCE) ]

  # 设置小步长以检查墙壁
  pointsNeeded = int(math.floor(max(math.fabs(step[0]), math.fabs(step[1]))))

  if math.fabs(step[0]) > math.fabs(step[1]):
    if step[0] >= 0:
      step = [ 1, step[1] / math.fabs(step[0]) ]
    else:
      step = [ -1, step[1] / math.fabs(step[0]) ]

  else:
    if step[1] >= 0:
      step = [ step[0] / math.fabs(step[1]), 1 ]
    else:
      step = [ step[0]/math.fabs(step[1]), -1 ]

  blocked = False
  for i in range(pointsNeeded+1): # 创建两点之间的点
    for j in range(STEP_DISTANCE): # 检查两点之间是否有墙
      coordX = round(newPoints[i][0] + step[0] * j)
      coordY = round(newPoints[i][1] + step[1] * j)

      if coordX == a[0] and coordY == a[1]:
        break
      if coordY >= len(img) or coordX >= len(img[0]):
        break
      if img[int(coordY)][int(coordX)][0] < 240:
        blocked = True
      if blocked:
        break

    if blocked:
      break
    if not (coordX == a[0] and coordY == a[1]):
      newPoints.append([ newPoints[i][0]+(step[0]*STEP_DISTANCE), newPoints[i][1]+(step[1]*STEP_DISTANCE) ])

  if not blocked:
    newPoints.append([ a[0], a[1] ])
  return newPoints

def findNearestPoint(points, point):
  best = (sys.maxint, sys.maxint, sys.maxint)
  for p in points:
    if p == point:
      continue
    dist = math.sqrt((p[0] - point[0]) ** 2 + (p[1] - point[1]) ** 2)
    if dist < best[2]:
      best = (p[0], p[1], dist)
  return (best[0], best[1])

def find_path_RRT(start,goal,my_map):
  path = rapidlyExploringRandomTree(my_map, start, goal, seed=None)
  return path

def main():
  print('Loading map... with file \'', MAP_IMG,'\'')
  img = imread(MAP_IMG)
  img = np.array(img)
  print('Map is', len(img[0]), 'x', len(img))
  start, goal = ([65.0, 248.0], [326.0, 2711.0])
  print(start,goal)
  path,graph = rapidlyExploringRandomTree(img, start, goal, seed=SEED)

if len(sys.argv) > 2:
  print( 'Only one argument is needed')
elif len(sys.argv) > 1:
  if os.path.isfile(sys.argv[1]):
    MAP_IMG = sys.argv[1]
  else:
    print(sys.argv[1], 'is not a file')

（2）文件rrt_single.py实现了单个RRT（快速探索随机树）路径规划算法，包括随机树的快速生成、连接、搜索路径等功能。在指定的地图上生成随机点并逐步连接，最终找到起点到目标点的可行路径。允许在图形界面上选择起始点和目标点，通过可视化展示路径规划的过程和结果。

# 快速探索随机树（RRT）算法的实现
def rapidlyExploringRandomTree(ax, img, start, goal, seed=None):
  # 初始化随机种子和数据结构
  hundreds = 100
  random.seed(seed)
  points = []  # 存储树中的所有点
  graph = []   # 存储树的结构
  points.append(start)
  graph.append((start, []))
  print( '生成和连接随机点')
  occupied = True  # 标记是否有障碍物
  phaseTwo = False  # 标记是否进入第二阶段
  
  # 第二阶段的值（目标点周围5个步距的点）
  minX = max(goal[0] - 5 * STEP_DISTANCE, 0)
  maxX = min(goal[0] + 5 * STEP_DISTANCE, len(img[0]) - 1)
  minY = max(goal[1] - 5 * STEP_DISTANCE, 0)
  maxY = min(goal[1] + 5 * STEP_DISTANCE, len(img) - 1)

  i = 0
  # 当目标点不在树中且点数未达到最大值时，循环生成和连接随机点
  while (goal not in points) and (len(points) < MAX_NUM_VERT):
    if (i % 100) == 0:
      print(i, '个随机生成的点')

    if (len(points) % hundreds) == 0:
      print(len(points), '个顶点生成')
      hundreds = hundreds + 100

    while(occupied):
      if phaseTwo and (random.random() > 0.8):
        point = [ random.randint(minX, maxX), random.randint(minY, maxY) ]
      else:
        point = [ random.randint(0, len(img[0]) - 1), random.randint(0, len(img) - 1) ]

      if(img[point[1]][point[0]][0] > 250):
        occupied = False

    occupied = True

    nearest = findNearestPoint(points, point)
    newPoints = connectPoints(point, nearest, img)
    addToGraph(ax, graph, newPoints, point)
    newPoints.pop(0) # 第一个元素已在点列表中
    points.extend(newPoints)
    ppl.draw()
    i = i + 1

    if len(points) >= MIN_NUM_VERT:
      if not phaseTwo:
        print('第二阶段')
      phaseTwo = True

    if phaseTwo:
      nearest = findNearestPoint(points, goal)
      newPoints = connectPoints(goal, nearest, img)
      addToGraph(ax, graph, newPoints, goal)
      newPoints.pop(0)
      points.extend(newPoints)
      ppl.draw()

  # 如果目标点在树中，则搜索路径并绘制结果
  if goal in points:
    print('找到目标点，总顶点数:', len(points), '总随机生成点数:', i)
    path = searchPath(graph, start, [start])

    for i in range(len(path)-1):
      ax.plot([ path[i][0], path[i+1][0] ], [ path[i][1], path[i+1][1] ], color='g', linestyle='-', linewidth=2)
      ppl.draw()

    print('显示结果地图')
    print('最终路径:', path)
    print('最终路径由:', len(path),'个连接点组成')
  else:
    path = None
    print('已达到最大顶点数，未找到目标')
    print('总顶点数:', len(points), '总随机生成点数:', i)
    print('显示结果地图')

  ppl.show()
  return path, graph

# 递归搜索路径
def searchPath(graph, point, path):
  for i in graph:
    if point == i[0]:
      p = i

  if p[0] == graph[-1][0]:
    return path

  for link in p[1]:
    path.append(link)
    finalPath = searchPath(graph, link, path)

    if finalPath != None:
      return finalPath
    else:
      path.pop()

# 添加新的点到图中
def addToGraph(ax, graph, newPoints, point):
  if len(newPoints) > 1:
    for p in range(len(newPoints) - 1):
      nearest = [ nearest for nearest in graph if (nearest[0] == [ newPoints[p][0], newPoints[p][1] ]) ]
      nearest[0][1].append(newPoints[p + 1])
      graph.append((newPoints[p + 1], []))

      if not p==0:
        ax.plot(newPoints[p][0], newPoints[p][1], '+k') # 第一个点已绘制
      ax.plot([ newPoints[p][0], newPoints[p+1][0] ], [ newPoints[p][1], newPoints[p+1][1] ], color='k', linestyle='-', linewidth=1)

    if point in newPoints:
      ax.plot(point[0], point[1], '.g') # 最后一个点是绿色的
    else:
      ax.plot(newPoints[p + 1][0], newPoints[p + 1][1], '+k') # 最后一个点不是绿色的

# 连接两个点，生成新的点
def connectPoints(a, b, img):
  newPoints = []
  newPoints.append([ b[0], b[1] ])
  step = [ (a[0] - b[0]) / float(STEP_DISTANCE), (a[1] - b[1]) / float(STEP_DISTANCE) ]

  pointsNeeded = int(math.floor(max(math.fabs(step[0]), math.fabs(step[1]))))

  if math.fabs(step[0]) > math.fabs(step[1]):
    if step[0] >= 0:
      step = [ 1, step[1] / math.fabs(step[0]) ]
    else:
      step = [ -1, step[1] / math.fabs(step[0]) ]
  else:
    if step[1] >= 0:
      step = [ step[0] / math.fabs(step[1]), 1 ]
    else:
      step = [ step[0]/math.fabs(step[1]), -1 ]

  blocked = False
  for i in range(pointsNeeded+1):
    for j in range(STEP_DISTANCE):
      coordX = round(newPoints[i][0] + step[0] * j)
      coordY = round(newPoints[i][1] + step[1] * j)

      if coordX == a[0] and coordY == a[1]:
        break
      if coordY >= len(img) or coordX >= len(img[0]):
        break
      if img[int(coordY)][int(coordX)][0] < 240:
        blocked = True
      if blocked:
        break

    if blocked:
      break
    if not (coordX == a[0] and coordY == a[1]):
      newPoints.append([ newPoints[i][0]+(step[0]*STEP_DISTANCE), newPoints[i][1]+(step[1]*STEP_DISTANCE) ])

  if not blocked:
    newPoints.append([ a[0], a[1] ])
  return newPoints

# 查找距离给定点最近的点
def findNearestPoint(points, point):
  best = (sys.maxint, sys.maxint, sys.maxint)
  for p in points:
    if p == point:
      continue
    dist = math.sqrt((p[0] - point[0]) ** 2 + (p[1] - point[1]) ** 2)
    if dist < best[2]:
      best = (p[0], p[1], dist)
  return (best[0], best[1])

# 在地图上选择起始点和目标点
def selectStartGoalPoints(ax, img):
  print('选择一个起始点')
  ax.set_xlabel('选择一个起始点')
  occupied = True
  while(occupied):
    point = ppl.ginput(1, timeout=-1, show_clicks=False, mouse_pop=2)
    start = [ round(point[0][0]), round(point[0][1]) ]
    if(img[int(start[1])][int(start[0])][0] > 250):
      occupied = False
      ax.plot(start[0], start[1], '.r')
    else:
      print('无法在该处放置起始点')
      print(img[int(start[1])][int(start[0])][0])
      ax.set_xlabel('无法在该处放置起始点，请选择其他点')

  print('选择一个目标点')
  ax.set_xlabel('选择一个目标点')
  occupied = True
  while(occupied):
    point = ppl.ginput(1, timeout=-1, show_clicks=False, mouse_pop=2)
    goal = [ round(point[0][0]), round(point[0][1]) ]
    if(img[int(goal[1])][int(goal[0])][0] > 250):
      occupied = False
      ax.plot(goal[0], goal[1], '.b')
    else:
      print('无法在该处放置目标点')
      ax.set_xlabel('无法在该处放置目标点，请选择其他点')

  ppl.draw()
  return start, goal

def main():
  print('加载地图... 使用文件 \'', MAP_IMG,'\'')
  img = imread(MAP_IMG)
  fig = ppl.gcf()
  fig.clf()
  ax = fig.add_subplot(1, 1, 1)
  ax.imshow(img, cmap=cm.Greys_r)
  ax.axis('image')
  ppl.draw()
  print( '地图尺寸为', len(img[0]), 'x', len(img))
  start, goal = selectStartGoalPoints(ax, img)
  path,graph = rapidlyExploringRandomTree(ax, img, start, goal, seed=SEED)
  print('####################################',len(graph))
  print(graph[0],graph[1])

if len(sys.argv) > 2:
  print('只需要一个参数')
elif len(sys.argv) > 1:
  if os.path.isfile(sys.argv[1]):
    MAP_IMG = sys.argv[1]
  else:
    print(sys.argv[1], '不是一个文件')

（3）文件autonomous_rrt.py实现了一个基于 RRT 算法的自主探索器，主要功能包括：

1. 通过订阅 `/odom` 和 `/map` 主题获取机器人的里程计数据和地图信息。
2. 实现了 RRT 算法来规划机器人的路径，并将路径可视化发布到 RVIZ中。
3. 通过 `/cmd_vel` 主题控制机器人移动，使其沿着 RRT 规划的路径前进。
4. 可以选择起始点和目标点来进行路径规划。
5. 实时显示构建的地图，并将路径点发布为 ROS Marker 用于 RVIZ 可视化。

#全局变量初始化
final_goal_location = [326.0, 2711.0]  # 最终目标位置
goal_reached = False  # 目标是否达到的标志
robot_rotation = [0,0,0]  # 机器人旋转角度
robot_location = [0,0,0]  # 机器人位置
current_map = np.zeros((384,384))  # 当前地图数组初始化

# 将给定的坐标集进行平移和旋转
def convert_path(path,trans,t):
    npath = []
    for x in path:
        mat = [x[0],x[1]]
        mat = rot2d(mat,t)
        npath.append((mat[0]+trans[0],mat[1]+trans[1]))
    return npath

# 计算两个位置之间的距离
def Distance_compute(pos1,pos2,Type = 'd'):
    x1 = pos1[0]
    y1 = pos1[1]
    x2 = pos2[0]
    y2 = pos2[1]
    d = ((x1-x2)**2) + ((y1-y2)**2)
    if Type == 'd':
        return math.sqrt(d)
    if Type == 'eu':
        return d
    if Type == 'manhattan':
        return abs(x1-x2)+abs(y1-y2)

# 二维旋转点
def rot2d(v,t):
    x,y = v[0],v[1]
    xr = x*cos(t)-y*sin(t)
    yr = x*sin(t)+y*cos(t)
    return [xr,yr]

# 控制机器人前往目标位置
def go_to_goal(goal):
    global robot_location,robot_rotation
    d = Distance_compute(robot_location,goal)
    theta = robot_rotation[2]
    kl = 1
    ka = 4
    vx = 0
    va = 0
    heading = math.atan2(goal[1]-robot_location[1],goal[0]-robot_location[0])
    err_theta = heading - theta
    if(d>0.01):
        vx = kl*abs(d)
        vx = 1
    if(abs(err_theta)>0.01):
        va = ka*(err_theta)

    vel_1 = rospy.Publisher('/cmd_vel', geometry_msgs.msg.Twist,queue_size=10)  # 发布机器人控制命令到/cmd_vel
    cmd = geometry_msgs.msg.Twist()
    cmd.linear.x = vx
    cmd.angular.z = va
    vel_1.publish(cmd)

# 沿着给定路径移动
def Follow_path(path):
    global final_goal_location, goal_reached
    cpath = path
    goal_point = cpath[-1]
    print('Following Path -->',cpath)
    for loc in cpath:
        while(Distance_compute(robot_location,loc)>0.1):
            go_to_goal([loc[0]/10,loc[1]/10])  # 将路径点缩小10倍，以适应地图尺寸
            if(loc==goal_point):
                goal_reached = True

# 接收里程计读数并分配给全局变量
def callback_odom(msg):
    global robot_location, robot_rotation, robot_orientation
    location = [msg.pose.pose.position.x, msg.pose.pose.position.y]
    robot_location = location
    orientation = [msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]
    (roll, pitch, yaw) = euler_from_quaternion(orientation)
    rot = [roll, pitch, yaw]
    robot_rotation = rot
    robot_orientation = orientation

# 接收地图信息
def callback_map(msg):
    global current_map
    data = np.array(msg.data)
    map_width = msg.info.width
    map_height = msg.info.height
    current_map =  np.reshape(data,(map_height,map_width))

# 移动基地客户端
def movebase_client(x,y):
    client = actionlib.SimpleActionClient('move_base',MoveBaseAction)
    client.wait_for_server()

    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = "map"
    goal.target_pose.header.stamp = rospy.Time.now()
    goal.target_pose.pose.position.x = x
    goal.target_pose.pose.orientation.w = y

    client.send_goal(goal)
    wait = client.wait_for_result()
    if not wait:
        rospy.logerr("Action server not available!")
        rospy.signal_shutdown("Action server not available!")
    else:
        return client.get_result()

# 将地图数组转换为图像数组
def map_img(arr):
    disp_map = np.ones((384,384))*255
    for i in range(arr.shape[0]):
        for j in range(arr.shape[1]):
            if arr[i][j]==-1:
                disp_map[i][j] = 100
            if arr[i][j] == 100:
                disp_map[i][j] = 0
    im = np.array(disp_map, dtype = np.uint8)
    return im[::-1]

# 发布路径点
def points_publisher(points_list):
    marker_pub = rospy.Publisher('path_points', Marker,queue_size=1)  # 发布机器人位置到 RVIZ
    marker_data = Marker()
    marker_data.type = marker_data.POINTS
    marker_data.action = marker_data.ADD
    marker_data.header.frame_id = '/map'

    marker_data.scale.x = 0.1  # 宽度
    marker_data.scale.y = 0.1  # 高度

    marker_data.color.a = 1
    marker_data.color.r = 1
    marker_data.color.g = 0
    marker_data.color.b = 0

    for p in points_list:
        marker_data.points.append(Point(p[0],p[1],0))
    marker_pub.publish(marker_data)

# 主函数
if __name__ == '__main__':
    rospy.init_node('RRT_Explorer')
    rate = rospy.Rate(10.0)
    # 订阅 /odom 和 /map 主题
    sub_odom = rospy.Subscriber('/odom', Odometry, callback_odom)  # 接收里程计读数
    sub_map = rospy.Subscriber('/map',OccupancyGrid, callback_map)  # 接收地图

    # 读取地图
    img = imread('/home/fazildgr8/catkin_ws/src/ros_autonomous_slam/media/my_map.png')
    start, goal = ([-2+int(384/2), 0.5+int(384/2)], [5+int(384/2), -0.5+int(384/2)])
    flag = True
    while not rospy.is_shutdown():
        if flag:
            # 使用 RRT 算法找到路径
            path,graph = find_path_RRT(start,goal,cv2.cvtColor(map_img(current_map), cv2.COLOR_GRAY2BGR)[::-1])
            print(path)
            print(graph)
            flag = False
        points_publisher(convert_path(path,[-192,-192],0))  # 将路径点发布到 RVIZ 进行可视化
        cv2.imshow('Constructed Map',map_img(current_map))  # 实时显示构建的地图
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break
    cv2.destroyAllWindows()

（4）文件autonomous_move.py实现了一个ROS节点，用于实现自主移动机器人的路径规划和控制。此文件实现了多个功能函数，包括全局变量的初始化、移动基地客户端、坐标转换函数、距离计算函数、旋转函数、前往目标函数、路径跟随函数、订阅里程计和地图信息的回调函数、地图转换为图像函数以及发布路径点函数。通过ROS接收里程计和地图信息，利用RRT算法规划路径，然后控制机器人沿着路径移动。

#全局变量初始化
final_goal_location = [326.0, 2711.0]  # 最终目标位置
goal_reached = False  # 目标是否达到的标志
robot_rotation = [0,0,0]  # 机器人旋转角度
robot_location = [-2,0.5,0]  # 机器人位置
current_map = np.zeros((384,384))  # 当前地图数组初始化

# 移动基地客户端
def movebase_client(x,y):
    client = actionlib.SimpleActionClient('move_base',MoveBaseAction)
    client.wait_for_server()

    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = "map"
    goal.target_pose.header.stamp = rospy.Time.now()
    goal.target_pose.pose.position.x = x
    goal.target_pose.pose.position.y = y
    goal.target_pose.pose.orientation.w = 1

    client.send_goal(goal)
    wait = client.wait_for_result()
    if not wait:
        rospy.logerr("Action server not available!")
        rospy.signal_shutdown("Action server not available!")
    else:
        return client.get_result()

# 将给定的坐标集进行平移和旋转
def convert_path(path,trans,t):
    '''
    Translates and Rotates the given set of coordinates
    '''
    npath = []
    for x in path:
        mat = [x[0],x[1]]
        mat = rot2d(mat,t)
        npath.append((mat[0]+trans[0],mat[1]+trans[1]))
    return npath

# 将给定的坐标进行平移和旋转
def convert_point(x,trans,t):
    '''
    Translates and Rotates the given set of coordinate
    '''
    mat = [x[0],x[1]]
    mat = rot2d(mat,t)
    return (mat[0]+trans[0],mat[1]+trans[1])

# 计算两个位置之间的距离
def Distance_compute(pos1,pos2,Type = 'd'):
    '''
    Distance Compute between two positions
    '''
    x1 = pos1[0]
    y1 = pos1[1]
    x2 = pos2[0]
    y2 = pos2[1]
    d = ((x1-x2)**2) + ((y1-y2)**2)
    if Type == 'd':
        return math.sqrt(d)
    if Type == 'eu':
        return d
    if Type == 'manhattan':
        return abs(x1-x2)+abs(y1-y2)

# 二维旋转点
def rot2d(v,t):
    '''
    2D Rotation points
    '''
    x,y = v[0],v[1]
    xr = x*cos(t)-y*sin(t)
    yr = x*sin(t)+y*cos(t)
    return [xr,yr]

# 控制机器人前往目标位置
def go_to_goal(goal):
    '''
    Function to command robot in ROS stage to go to given goal wrt /odom frame
    '''
    global robot_location,robot_rotation
    d = Distance_compute(robot_location,goal)
    theta = robot_rotation[2]
    kl = 1
    ka = 4
    vx = 0
    va = 0
    heading = math.atan2(goal[1]-robot_location[1],goal[0]-robot_location[0])
    err_theta = heading - theta
    if(d>0.01):
        vx = kl*abs(d)
        vx = 1
    if(abs(err_theta)>0.01):
        va = ka*(err_theta)

    vel_1 = rospy.Publisher('/cmd_vel', geometry_msgs.msg.Twist,queue_size=10)  # 发布机器人控制命令到/cmd_vel
    cmd = geometry_msgs.msg.Twist()
    cmd.linear.x = vx
    cmd.angular.z = va
    vel_1.publish(cmd)

# 沿着给定路径移动
def Follow_path(path):
    '''
    Follows a given set of path
    - Reaches all the points in a list in consecutive order
    '''
    global final_goal_location, goal_reached
    cpath = path
    goal_point = cpath[-1]
    print('Following Path -->',cpath)
    for loc in cpath:
        while(Distance_compute(robot_location,loc)>0.1):
            go_to_goal([loc[0]/10,loc[1]/10])  # 将路径点缩小10倍，以适应地图尺寸
            if(loc==goal_point):
                goal_reached = True

# 接收里程计读数并分配给全局变量
def callback_odom(msg):
    '''
    Obtains Odometer readings and assigns to global Variables
    '''
    global robot_location, robot_rotation, robot_orientation
    location = [msg.pose.pose.position.x, msg.pose.pose.position.y]
    robot_location = location
    orientation = [msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]
    (roll, pitch, yaw) = euler_from_quaternion(orientation)
    rot = [roll, pitch, yaw]
    robot_rotation = rot
    robot_orientation = orientation

# 接收地图信息
def callback_map(msg):
    global current_map
    data = np.array(msg.data)
    map_width = msg.info.width
    map_height = msg.info.height
    current_map =  np.reshape(data,(map_height,map_width))

# 将地图数组转换为图像数组
def map_img(arr):
    disp_map = np.ones((384,384))*255
    for i in range(arr.shape[0]):
        for j in range(arr.shape[1]):
            if arr[i][j]==-1:
                disp_map[i][j] = 100
            if arr[i][j] == 100:
                disp_map[i][j] = 0
    im = np.array(disp_map, dtype = np.uint8)
    return im[::-1]

# 发布路径点
def points_publisher(points_list):
    marker_pub = rospy.Publisher('path_points', Marker,queue_size=1)  # 发布机器人位置到 RVIZ
    marker_data = Marker()
    marker_data.type = marker_data.POINTS
    marker_data.action = marker_data.ADD
    marker_data.header.frame_id = '/map'

    marker_data.scale.x = 0.1  # 宽度
    marker_data.scale.y = 0.1  # 高度

    marker_data.color.a = 1
    marker_data.color.r = 1
    marker_data.color.g = 0
    marker_data.color.b = 0

    for p in points_list:
        marker_data.points.append(Point(p[0],p[1],0))
    marker_pub.publish(marker_data)

if __name__ == '__main__':
    rospy.init_node('RRT_Explorer')
    rate = rospy.Rate(10.0)
    # 订阅 /odom
    sub_odom = rospy.Subscriber('/odom', Odometry, callback_odom)  # 接收里程计读数
    sub_map = rospy.Subscriber('/map',OccupancyGrid, callback_map)  # 接收地图
    img = imread('/home/fazildgr8/catkin_ws/src/ros_autonomous_slam/media/my_map.png')
    
    flag = True
    while not rospy.is_shutdown():
        start, goal = (convert_point(robot_location,[192,192],0), convert_point([2,0.5],[192,192],0))
        print(start,goal)
        if flag:
            # 使用 RRT 算法找到路径
            path,graph = find_path_RRT(convert_point(robot_location,[192,192],0),convert_point([-1,0.5],[192,192],0),cv2.cvtColor(map_img(current_map), cv2.COLOR_GRAY2BGR)[::-1])
            print(path)
            print(graph)
            flag = False
        print('Map Shape',current_map.shape)
        cv2.imshow('Constructed Map',map_img(current_map))
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break
    cv2.destroyAllWindows()

在本项目中，文件autonomous_move.py和autonomous_rrt.py的区别如下所示。

1. 文件autonomous_move.py：主要功能是通过ROS接收里程计和地图信息，利用移动基地客户端实现机器人的移动。使用函数movebase_client来发送目标位置给移动基地，包含了坐标转换函数、距离计算函数、旋转函数、前往目标函数等。通过订阅里程计和地图信息的回调函数来更新机器人的位置和地图状态。
2. 文件autonomous_rrt.py：主要功能是利用RRT算法规划路径，然后控制机器人沿着路径移动。使用RRT 算法规划路径，通过前往目标函数来控制机器人移动。包含了坐标转换函数、距离计算函数、旋转函数、前往目标函数等，通过订阅里程计和地图信息的回调函数来更新机器人的位置和地图状态。

总的来说，autonomous_move.py 更注重于直接控制机器人的移动，而 autonomous_rrt.py 则更注重于路径规划。

到此为止，整个项目介绍完毕。运行“launch”目录中的“.launch”文件即可运行程序。通过RViz可视化功能可以看到TurtleBot3机器人进行SLAM构建实时地图的过程。执行效果如图11-1所示。

图11-1  执行效果