Код: Выделить всё
#!/usr/bin/env python3
import matplotlib.pyplot as plt
import random
import math
import copy
import rospy
import time
import numpy as np
from numpy import genfromtxt
from numpy import array
from sensor_msgs.msg import NavSatFix
from sensor_msgs.msg import Imu
from std_msgs.msg import String
from geometry_msgs.msg import Vector3Stamped
from geometry_msgs.msg import Twist
from geometry_msgs.msg import Pose2D
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped
from std_msgs.msg import Float32MultiArray
show_animation = True
class RRT():
"""
Class for RRT Planning
"""
def __init__(self, start, goal, obstacles,
randArea, expandDis=1.0, goalSampleRate=5, maxIter = 500):
"""
Setting Parameter
start: Start Position [x, y]
goal: Goal Position [x, y]
obstacles: Obstacle Positions [[x, y, size], ...]
randArea: Random Sampling Area [min, max]
"""
self.start = Node(start[0], start[1])
self.end = Node(goal[0], goal[1])
self.minrand = randArea[0]
self.maxrand = randArea[1]
self.expandDis = expandDis
self.goalSampleRate = goalSampleRate
self.maxIter = maxIter
self.obstacles = obstacles
def Planning(self, animation=True):
"""
Path planning
animation: flag for animation on or off
"""
self.nodeList = [self.start]
while True:
# Random Sampling
if random.randint(0, 100) > self.goalSampleRate:
rnd = [random.uniform(self.minrand, self.maxrand), random.uniform(self.minrand, self.maxrand)]
else:
rnd = [self.end.x, self.end.y]
# Find nearest node
nind = self.GetNearestListIndex(self.nodeList, rnd)
# Expand tree
nearestNode = self.nodeList[nind]
theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)
newNode = copy.deepcopy(nearestNode)
newNode.x += self.expandDis * math.cos(theta)
newNode.y += self.expandDis * math.sin(theta)
newNode.parent = nind
if not self.__CollisionCheck(newNode, self.obstacles):
continue
self.nodeList.append(newNode)
print("Node list length:", len(self.nodeList))
# Check goal
dx = newNode.x - self.end.x
dy = newNode.y - self.end.y
d = math.sqrt(dx * dx + dy * dy)
if d
Подробнее здесь: [url]https://stackoverflow.com/questions/79367558/cant-publish-to-cmd-vel-in-ros-gazebo[/url]