def __init__(self,
world=None,
NGEN=1000,
n_ind=100,
n_elite=10,
fitness_thresh=0.1,
margin_on=False,
verbose=True):
self.trajectory = [world.start]
self.history = []
self.pop = []
self.best_ind = []
self.gtot = 0
self.n_ind = n_ind # The number of individuals in a population.
self.n_elite = n_elite
self.NGEN = NGEN # The number of generation loop.
self.fitness_thresh = fitness_thresh
self.verbose = verbose
# --- Generate Collision Checker
self.world = world
if margin_on:
self.world_margin = world.mcopy(rate=1.05)
else:
self.world_margin = world
self.cc = CollisionChecker(world)
self.ccm = CollisionChecker(self.world_margin)
def __init__(self):
self.world_size = GlobalVars.get_world_size()
self.bird_state = BirdState()
self.background_state = BackgroundState()
self.collision_checker = CollisionChecker(self.background_state, self.bird_state)
self.is_alive = True
self.score = 0
def __init__(self, num_paths, path_offset, circle_offsets, circle_radii,
path_select_weight, time_gap, a_max, coasting_speed,
stop_line_buffer):
self.num_paths = num_paths
self.path_offset = path_offset
self.path_optimizer = PathOptimizer()
self.collision_checker = CollisionChecker(circle_offsets, circle_radii,
path_select_weight)
self.velocity_planner = VelocityPlanner(time_gap, a_max,
coasting_speed,
stop_line_buffer)
def __init__(self, world,
branch_length=0.1, resampling_length=0.05, verbose=False):
# set RRT parameter
self.verbose = verbose
self.rrt_length = branch_length
self.resampling_length = resampling_length
self.rrt_size = 5000
self.world = world
self.path = []
self.smoothed_path = []
self.resampled_path = []
self.tree = []
self.cc = CollisionChecker(world)
def shortcut(path_list, world):
cc = CollisionChecker(world)
q0 = np.array(path_list[0])
res_path_list = [q0]
i = 1
while i < len(path_list):
q1 = np.array(path_list[i])
if cc.line_validation(q0, q1):
i += 1
else:
q0 = np.array(path_list[i - 1])
res_path_list.append(q0)
i += 1
res_path_list.append(path_list[-1])
return np.array(res_path_list)
def locate(self, scale_minmax):
frame = np.array(self.world.frame)
xmax = np.max(frame[:, 0])
xmin = np.min(frame[:, 0])
ymax = np.max(frame[:, 1])
ymin = np.min(frame[:, 1])
def collision_check(target):
res = [
cc.path_validation_new(target[i], target[i + 1])
for i in range(len(target) - 1)
]
res_start = [
cc.crossing_number_algorithm(self.world.start, target)
]
res_goal = [cc.crossing_number_algorithm(self.world.goal, target)]
return all(res + res_start + res_goal)
new_objects = []
for obj in self.world.objects:
self.world.objects = new_objects
cc = CollisionChecker(self.world, 0.05)
for iter in range(10):
# set random value
# x, y: translate distance, theta: rotate angle, scale: scale
x = np.random.rand() * (xmax - xmin) + xmin
y = np.random.rand() * (ymax - ymin) + ymin
theta = np.random.rand() * 2.0 * pi
scale = np.random.rand() * \
(scale_minmax[1] - scale_minmax[0]) + scale_minmax[0]
# Rotate, scale, translate
rot_mat = np.array([[np.cos(theta), -np.sin(theta)],
[np.sin(theta),
np.cos(theta)]])
rslt = np.dot(rot_mat, obj.T) * scale + \
np.array([x, y]).reshape(-1, 1)
# Check collisions with the other objects.
if collision_check(rslt.T):
new_objects.append(rslt.T)
break
self.world.objects = new_objects.copy()
def __init__(self, world, n, k, delta=0.01):
# --- Collision Checker
self.cc = CollisionChecker(world, delta)
# --- World
self.world = world
# --- parameter
self.n = n
self.d = 2
self.k = k
self.V = np.array([])
self.E = []
self.roadmap = []
self.resampling_length = 0.1
self.path = None
self.smoothed_path = None
self.resampled_path = None
self.path_list = []
def __init__(
self,
road: Road,
actions: Actions,
constraints: Constraints,
termination_conditions: TerminationConditions,
cost_calculator: CostCalculator,
start_state: State,
ego,
subject,
):
self.road = road
self.actions = actions
self.start_state = start_state
self.constraints = constraints
self.termination_conditions = termination_conditions
self.cost_calculator = cost_calculator
self.collision_checker = CollisionChecker(1.75, 1)
self.ego_vehicle = ego
self.subject_vehicle = subject
[pi, -pi],
[-pi, -pi]])
# --- Set start/goal point
world.start = np.array([-pi / 2, -pi / 2]) * 1.9
world.goal = np.array([pi / 2, pi / 2]) * 1.9
# --- Generate objects
og = ObjectGenerator(world)
world.objects = og.example()
# og.generate(10, 5)
# og.locate([2.0, 3.0])
# world.objects = og.world.objects
# --- Generte Collision Checker
cc = CollisionChecker(world, 0.1)
'''
Functions
'''
class Individual(np.ndarray):
"""Container of a individual."""
fitness = None
def __new__(cls, a):
return np.asarray(a).view(cls)
def create_pop_prm(m, n):
'''
World setting
'''
# --- world class
world = World()
# --- Set frame
world.frame = np.array(
[[-pi, -pi],
[-pi, pi],
[pi, pi],
[pi, -pi],
[-pi, -pi]])
# --- Set start/goal point
world.start = np.array([-pi / 2, -pi / 2]) * 1.9
world.goal = np.array([pi / 2, pi / 2]) * 1.9
# --- Generate objects
og = ObjectGenerator(world)
world.objects = og.example()
world_margin = world.mcopy(rate=1.1)
cc = CollisionChecker(world_margin)
# In[]:
# --- Visualize
gd = GraphDrawer(world_margin)
plt.show()
world = World()
# --- Set frame
world.frame = np.array([[-pi, -pi], [-pi, pi], [pi, pi], [pi, -pi], [-pi,
-pi]])
# --- Set start/goal point
world.start = np.array([-pi / 2, -pi / 2]) * 1.9
world.goal = np.array([pi / 2, pi / 2]) * 1.9
# --- Generate objects
og = ObjectGenerator(world)
world.objects = og.example()
# --- CollisionChecker
cc = CollisionChecker(world)
# In[]:
m = 6
n = 6
w = 1.0
h = 1.0
x0 = -3.0
y0 = -3.0
V = [[x0 + i * w, y0 + j * h] for j in range(n) for i in range(m)]
E = []
for i in range(m * n - 1):
if i % m != (m - 1):
E.append([(i, i + 1), 1.0])
global sensor_data
sensor_data = data
if __name__ == '__main__':
os.environ["WANDB_SILENT"] = "true" # Avoid small wandb bug
# Subscribe to essential pathfinding info
sailbot = sbot.Sailbot(nodeName='log_stats')
rospy.Subscriber("sensors", msg.Sensors, sensorsCallback)
sailbot.waitForFirstSensorDataAndGlobalPath()
# Setup subscribers
log_closest_obstacle = LogClosestObstacle(create_ros_node=False)
path_storer = PathStorer(create_ros_node=False)
collision_checker = CollisionChecker(create_ros_node=False)
sailbot_gps_data = SailbotGPSData(create_ros_node=False)
rate = rospy.Rate(UPDATE_TIME_SECONDS)
# Set wandb run name and directory path
start_datetime = datetime.now().strftime('%b_%d-%H_%M_%S')
run_name = 'stats-' + start_datetime
dir_dir = os.path.expanduser('~/.ros/stats/')
dir_path = dir_dir + start_datetime
os.makedirs(dir_dir + start_datetime)
os.symlink(dir_path, dir_dir + 'tmp')
os.rename(dir_dir + 'tmp',
dir_dir + 'latest-dir') # use replace() in Python 3
wandb.init(entity='sailbot', project='stats', dir=dir_path, name=run_name)
