def main():
#### STEP 1 - Get classified pointcloud ####
environment = PointCloudEnvironment(my_print, TERRAIN_ASSESSMENT_FILE,
POINTCLOUD_FILE)
coverable_points = environment.coverable_pcd.points
traversable_points = environment.traversable_pcd.points
motion_planner = MotionPlanner(my_print, environment.traversable_pcd)
###
TIME_LIMIT = 400
start_point = np.array([28.6, -6.7, -10.3])
goal_coverage = 0.97
paths_markers = []
#Get CPP path
for pub_path in ALL_PATHS:
if not pub_path["do_calc"]:
continue
coverable_pcd = PointCloud(my_print, points=coverable_points)
if pub_path["cpp"] == "BA*":
cpp = BAstar(my_print,
motion_planner,
coverable_pcd,
time_limit=TIME_LIMIT,
parameters=pub_path["param"])
if pub_path["cpp"] == "Inward Spiral":
cpp = Spiral(my_print,
motion_planner,
coverable_pcd,
time_limit=TIME_LIMIT,
parameters=pub_path["param"])
if pub_path["cpp"] == "Sampled BA*":
cpp = RandomBAstar3(my_print,
motion_planner,
coverable_pcd,
time_limit=TIME_LIMIT,
parameters=pub_path["param"])
ns = pub_path["ns"]
pub_path["path"] = cpp.get_cpp_path(start_point,
goal_coverage=goal_coverage)
pub_path["markers"] = cpp.points_to_mark
pub_path["stats"] = cpp.print_stats(pub_path["path"])
save_data(ALL_PATHS)
print("DONE!")
"hyper_test":
"step_param",
"hyper_time_limit":
250,
"hyper_min_coverage":
95,
"do_experiment":
True,
"experiment_time_limit":
400,
"experiment_results": [],
"hyper_data": [],
"formatted_hyper_data": [],
"cpp":
lambda print, motion_planner, cov_points, time_limit, parameters:
BAstar(print, motion_planner, PointCloud(print, points=cov_points),
time_limit, parameters),
'line':
'r',
'confidence_color': (1.0, 0.0, 0.0, 0.3)
}
}
###################
def my_print(text):
if PRINT:
return print(text)
else:
return
def main():
#with open(RESULTS_FILE, 'rb') as cached_pcd_file:
# cache_data = pickle.load(cached_pcd_file)
# pprint.pprint(cache_data)
#return
#with open(RESULTS_FILE, 'rb') as cached_pcd_file:
# cache_data = pickle.load(cached_pcd_file)
# for alg in ALGORITHMS:
# if ALGORITHMS[alg]["do_hyper"]:
# ALGORITHMS[alg]["opt_param"] = cache_data[alg]["opt_param"]
with open(PREVIOUS_FILE, 'rb') as cached_pcd_file:
cache_data = pickle.load(cached_pcd_file)
ALGORITHMS = deepcopy(cache_data)
for alg in ALGORITHMS:
ALGORITHMS[alg]["do_hyper"] = False
ALGORITHMS[alg][
"cpp"] = lambda print, motion_planner, cov_points, time_limit, parameters: BAstar(
print, motion_planner, PointCloud(
print, points=cov_points), time_limit, parameters)
ALGORITHMS[alg]["experiment_results"] = []
#### STEP 1 - Get classified pointcloud ####
environment = PointCloudEnvironment(my_print, TERRAIN_ASSESSMENT_FILE,
POINTCLOUD_FILE)
coverable_points = environment.coverable_pcd.points
traversable_points = environment.traversable_pcd.points
motion_planner = MotionPlanner(my_print, environment.traversable_pcd)
#If from terrain assessment file:
#with open(TERRAIN_ASSESSMENT_FILE, 'rb') as cached_pcd_file:
# cache_data = pickle.load(cached_pcd_file)
# coverable_points = cache_data["coverable_points"]
# traversable_points = cache_data["traversable_points"]
#traversable_pcd = PointCloud(my_print, points= traversable_points)
#motion_planner = MotionPlanner(my_print, traversable_pcd)
#### STEP 2 - Hyper parameters ####
for algorithm_key, algorithm in ALGORITHMS.items():
if algorithm["do_hyper"]:
trials = Trials()
hyper_optimizer = HyptoOptimizer(save_data, algorithm, my_print,
HYPER_START_POS, motion_planner,
coverable_points)
if algorithm_key == "BA*":
opt_param = fmin(hyper_optimizer.hyper_test_bastar,
space=(hp.uniform('angle_offset', 0,
np.pi * 2),
hp.uniform('step_size', 0.5, 1),
hp.uniform('visited_threshold', 0.25,
0.5)),
algo=tpe.suggest,
max_evals=HYPER_MAX_EVAL,
trials=trials)
elif algorithm_key == "Inward Spiral":
opt_param = fmin(hyper_optimizer.hyper_test_inward_spiral,
space=(hp.uniform('step_size', 0.5, 1),
hp.uniform('visited_threshold', 0.5,
1)),
algo=tpe.suggest,
max_evals=HYPER_MAX_EVAL,
trials=trials)
elif algorithm_key == "Sampled BA*":
coverage_2 = algorithm["hyper_min_coverage"] / 100
opt_param = fmin(
hyper_optimizer.hyper_test_sampled_bastar_param,
space=(hp.uniform('coverage_1', 0.25, coverage_2),
hp.uniform('coverage_2', coverage_2 - 0.025,
coverage_2),
hp.uniform('max_distance', 1, 10),
hp.uniform('max_distance_part_II', 1,
20), hp.uniform('max_iterations', 30,
150),
hp.uniform('min_bastar_coverage', 0.005, 0.05),
hp.uniform('min_spiral_length', 2, 100),
hp.uniform('nbr_of_angles', 0.6,
8.4), hp.uniform('step_size', 0.66,
1.33),
hp.uniform('visited_threshold', 0.5, 1)),
algo=tpe.suggest,
max_evals=HYPER_MAX_EVAL,
trials=trials)
print(trials.statuses())
algorithm["opt_param"] = opt_param
algorithm["hyper_data"] = trials.trials
ALGORITHMS[algorithm_key] = algorithm
save_data(ALGORITHMS)
#### STEP 3 - Full tests ####
for start_point_nr in range(NUMBER_OF_START_POINTS):
#start_point = get_random_point(traversable_points)
start_point = start_points[start_point_nr]
print("Start point " + str(start_point_nr) + ": " + str(start_point))
for algorithm_key, algorithm in ALGORITHMS.items():
if algorithm["do_experiment"]:
experimenter = Experimenter(algorithm, print)
parameters = None
if "opt_param" in algorithm:
parameters = algorithm["opt_param"]
cpp = algorithm["cpp"](my_print, motion_planner,
coverable_points,
algorithm["experiment_time_limit"],
parameters)
if "sample_specific_stats" in algorithm:
experimenter.perform_sample_cpp(cpp, start_point,
start_point_nr)
algorithm["sample_specific_stats"].append(
experimenter.sample_specific_stats)
else:
experimenter.perform_cpp(cpp, start_point, start_point_nr)
algorithm["experiment_results"].append(experimenter.results)
ALGORITHMS[algorithm_key] = algorithm
save_data(ALGORITHMS)
def __init__(self):
super().__init__('MainNode')
#Publishers:
self.markers_pub = self.create_publisher(
visualization_msgs.MarkerArray, 'marker', 3000)
self.markers_path_pub = self.create_publisher(
visualization_msgs.Marker, 'path_markers', 3000)
self.pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'pcd',
10)
self.coverable_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2,
'coverable_pcd', 100)
self.visited_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2,
'visited_pcd', 100)
self.visited_ground_pcd_pub = self.create_publisher(
sensor_msgs.PointCloud2, 'visited_ground_pcd', 100)
self.traversable_pcd_pub = self.create_publisher(
sensor_msgs.PointCloud2, 'traversable_pcd', 100)
self.inaccessible_pcd_pub = self.create_publisher(
sensor_msgs.PointCloud2, 'inaccessible_pcd', 100)
self.path_pub = self.create_publisher(nav_msgs.Path, 'path', 10)
#Create Environment
environment = MapEnvironment(self.print, "simple_open.dictionary",
"src/exjobb/maps/map_simple_open.png",
0.015)
point_cloud = environment.full_pcd
traversable_point_cloud = environment.traversable_pcd
coverable_point_cloud = environment.coverable_pcd
motion_planner = MotionPlanner(self.print, traversable_point_cloud)
TIME_LIMIT = 400
#start_point = np.array([5,0.55,0]) #spiral
start_point = np.array([0.55, 0.7, 0])
goal_coverage = 1
paths_markers = []
#Get CPP path
current = "BA*"
current_param = {
'angle_offset': 0,
'step_size': 0.5,
'visited_threshold': 0.375
}
if current == "BA*":
cpp = BAstar(self.print,
motion_planner,
coverable_point_cloud,
time_limit=TIME_LIMIT,
parameters=current_param)
if current == "Inward Spiral":
cpp = Spiral(self.print,
motion_planner,
coverable_point_cloud,
time_limit=TIME_LIMIT,
parameters=current_param)
self.pcd_pub.publish(
point_cloud.point_cloud(point_cloud.points, 'my_frame'))
self.coverable_pcd_pub.publish(
coverable_point_cloud.point_cloud(coverable_point_cloud.points,
'my_frame'))
self.traversable_pcd_pub.publish(
traversable_point_cloud.point_cloud(traversable_point_cloud.points,
'my_frame'))
path = cpp.get_cpp_path(start_point, goal_coverage=goal_coverage)
self.print(cpp.print_stats(path))
current_max = 0
coverable_point_cloud.covered_points_idx = np.array([])
while current_max < len(path):
self.markers_msg = visualization_msgs.MarkerArray()
stamp = self.get_clock().now().to_msg()
msg = ROSMessage.line_marker(0, stamp, path[0:current_max],
[0.1, 1.0, 1.0], "path")
self.markers_msg.markers = [msg]
current_max += 2
new_path = path[current_max - 2:current_max]
self.markers_pub.publish(self.markers_msg)
#if current_max > 2:
coverable_point_cloud.visit_path(new_path)
visited_ground_points_pcd = coverable_point_cloud.get_covered_points_as_pcd(
)
self.visited_ground_pcd_pub.publish(visited_ground_points_pcd)
def __init__(self):
super().__init__('MainNode')
#Publishers:
self.markers_pub = self.create_publisher(visualization_msgs.MarkerArray, 'marker', 3000)
self.markers_path_pub = self.create_publisher(visualization_msgs.Marker, 'path_markers', 3000)
self.pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'pcd', 10)
self.coverable_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'coverable_pcd', 100)
self.visited_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'visited_pcd', 100)
self.visited_ground_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'visited_ground_pcd', 100)
self.traversable_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'traversable_pcd', 100)
self.inaccessible_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'inaccessible_pcd', 100)
self.path_pub = self.create_publisher(nav_msgs.Path, 'path', 10)
with open(FILE, 'rb') as cached_pcd_file:
cache_data = pickle.load(cached_pcd_file)
all_results = deepcopy(cache_data)
param = list(filter(lambda x: x["ns"] == PATH_NS, all_results))[0]["param"]
cpp_name = list(filter(lambda x: x["ns"] == PATH_NS, all_results))[0]["cpp"]
#Create Environment
environment = PointCloudEnvironment(self.print, "garage_terrain_assessment.dictionary", "garage.pcd", False)
point_cloud = environment.full_pcd
traversable_point_cloud = environment.traversable_pcd
coverable_point_cloud = environment.coverable_pcd
motion_planner = MotionPlanner(self.print, traversable_point_cloud)
TIME_LIMIT = 400
goal_coverage = 0.97
start_point = np.array([28.6, -6.7, -10.3]) #garage
#start_point = np.array([-53.7, 54.2, -2.7]) #bridge
#start_point = np.array([-20.7, 43, -1]) #cross
if cpp_name == "BA*":
cpp = BAstar(self.print, motion_planner, coverable_point_cloud, time_limit=TIME_LIMIT, parameters = param)
if cpp_name == "Inward Spiral":
cpp = Spiral(self.print, motion_planner, coverable_point_cloud, time_limit=TIME_LIMIT, parameters = param)
if cpp_name == "Sampled BA*":
cpp = RandomBAstar3(self.print, motion_planner, coverable_point_cloud, time_limit=TIME_LIMIT, parameters = param)
path = cpp.get_cpp_path(start_point, goal_coverage=goal_coverage)
all_segments = cpp.all_segments
all_movements = cpp.all_movements
markers = cpp.points_to_mark
self.pcd_pub.publish(point_cloud.point_cloud(point_cloud.points, 'my_frame'))
self.coverable_pcd_pub.publish(coverable_point_cloud.point_cloud(coverable_point_cloud.points, 'my_frame'))
self.traversable_pcd_pub.publish(traversable_point_cloud.point_cloud(traversable_point_cloud.points, 'my_frame'))
self.markers_msg = visualization_msgs.MarkerArray()
self.last_id = 0
ANIMATION = True
if not ANIMATION:
#Only markers
for idx, marker in enumerate(markers):
stamp = self.get_clock().now().to_msg()
msg = ROSMessage.point_marker(self.last_id, stamp, marker["point"], marker["color"], "markers")
self.markers_msg.markers.append(msg)
self.last_id += 1
#Add segments
coverable_point_cloud.covered_points_idx = np.array([])
for idx, segment in enumerate(all_segments):
if len(segment.path) == 0:
#all_movements.pop(idx)
continue
stamp = self.get_clock().now().to_msg()
msg = ROSMessage.line_marker(self.last_id, stamp, segment.path, [0.1,1.0,1.0], "segment"+str(idx))
self.markers_msg.markers.append(msg)
self.last_id += 1
coverable_point_cloud.visit_path(segment.path)
if ANIMATION:
visited_ground_points_pcd = coverable_point_cloud.get_covered_points_as_pcd()
self.visited_ground_pcd_pub.publish(visited_ground_points_pcd)
self.markers_pub.publish(self.markers_msg)
time.sleep(1)
#Add Movements
for idx, move in enumerate(all_movements):
stamp = self.get_clock().now().to_msg()
msg = ROSMessage.line_marker(self.last_id, stamp, move.path, [1.0,0.5,0.5], "move"+str(idx))
self.markers_msg.markers.append(msg)
self.last_id += 1
visited_ground_points_pcd = coverable_point_cloud.get_covered_points_as_pcd()
self.visited_ground_pcd_pub.publish(visited_ground_points_pcd)
self.markers_pub.publish(self.markers_msg)
return
current_max = 0
while current_max < len(path):
self.markers_msg = visualization_msgs.MarkerArray()
stamp = self.get_clock().now().to_msg()
msg = ROSMessage.line_marker(0, stamp, path[0:current_max], [0.1,1.0,1.0], PATH_NS)
self.markers_msg.markers = [msg]
current_max += 10
self.markers_pub.publish(self.markers_msg)
def __init__(self):
super().__init__('MainNode')
#Publishers:
self.markers_pub = self.create_publisher(
visualization_msgs.MarkerArray, 'marker', 3000)
self.markers_path_pub = self.create_publisher(
visualization_msgs.Marker, 'path_markers', 3000)
self.pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'pcd',
10)
self.coverable_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2,
'coverable_pcd', 100)
self.visited_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2,
'visited_pcd', 100)
self.visited_ground_pcd_pub = self.create_publisher(
sensor_msgs.PointCloud2, 'visited_ground_pcd', 100)
self.traversable_pcd_pub = self.create_publisher(
sensor_msgs.PointCloud2, 'traversable_pcd', 100)
self.inaccessible_pcd_pub = self.create_publisher(
sensor_msgs.PointCloud2, 'inaccessible_pcd', 100)
self.path_pub = self.create_publisher(nav_msgs.Path, 'path', 10)
with open(FILE, 'rb') as cached_pcd_file:
cache_data = pickle.load(cached_pcd_file)
self.prev_file = deepcopy(cache_data)
#Create Environment
environment = PointCloudEnvironment(
self.print, "garage_terrain_assessment.dictionary", "garage.pcd",
False)
point_cloud = environment.full_pcd
traversable_point_cloud = environment.traversable_pcd
coverable_point_cloud = environment.coverable_pcd
motion_planner = MotionPlanner(self.print, traversable_point_cloud)
TIME_LIMIT = 400
start_point = np.array([28.6, -6.7, -10.3])
goal_coverage = 0.97
paths_markers = []
#Get CPP path
for pub_path in ALL_PATHS:
if not pub_path["do_calc"]:
continue
coverable_pcd = PointCloud(self.print,
points=coverable_point_cloud.points)
if pub_path["cpp"] == "BA*":
cpp = BAstar(self.print,
motion_planner,
coverable_pcd,
time_limit=TIME_LIMIT,
parameters=pub_path["param"])
if pub_path["cpp"] == "Inward Spiral":
cpp = Spiral(self.print,
motion_planner,
coverable_pcd,
time_limit=TIME_LIMIT,
parameters=pub_path["param"])
if pub_path["cpp"] == "Sampled BA*":
cpp = RandomBAstar3(self.print,
motion_planner,
coverable_pcd,
time_limit=TIME_LIMIT,
parameters=pub_path["param"])
ns = pub_path["ns"]
pub_path["path"] = cpp.get_cpp_path(start_point,
goal_coverage=goal_coverage)
pub_path["markers"] = cpp.points_to_mark
pub_path["stats"] = cpp.print_stats(pub_path["path"])
save_data(ALL_PATHS)
#self.print(path_msg)
#paths_markers.append(path_msg)
#path_pub = self.create_publisher(nav_msgs.Path, ns + '/path', 10)
#self.markers_pub.publish(path_msg)
#
#self.markers_msg = visualization_msgs.MarkerArray()
#
#for idx, msg in enumerate(paths_markers):
# stamp = self.get_clock().now().to_msg()
# self.markers_msg.markers.append(msg)
# #self.last_id += 1
#
markers_pub = self.create_timer(5, self.marker_publisher)
#self.markers_pub.publish(self.markers_msg)
for idx, pub_path in enumerate(ALL_PATHS):
if pub_path.get("stats", False) is False:
pub_path["stats"] = self.prev_file[idx]["stats"]
self.print("=" * 20)
self.print(pub_path["ns"])
self.print(pub_path["stats"])
self.pcd_pub.publish(
point_cloud.point_cloud(point_cloud.points, 'my_frame'))
self.coverable_pcd_pub.publish(
coverable_point_cloud.point_cloud(coverable_point_cloud.points,
'my_frame'))
self.traversable_pcd_pub.publish(
traversable_point_cloud.point_cloud(traversable_point_cloud.points,
'my_frame'))
"cpp": lambda print, motion_planner, cov_points, time_limit, parameters: RandomBAstar(print, motion_planner, PointCloud(print, points= cov_points), time_limit, parameters),
'line': 'm',
'confidence_color': (1.0, 0.0, 1.0, 0.3)
},
"BA*": {
"name": "BA*",
"do_hyper": True,
"hyper_test": "step_param",
"hyper_time_limit": 250,
"hyper_min_coverage": 95,
"do_experiment": True,
"experiment_time_limit": 400,
"experiment_results": [],
"hyper_data": [],
"formatted_hyper_data": [],
"cpp": lambda print, motion_planner, cov_points, time_limit, parameters: BAstar(print, motion_planner, PointCloud(print, points= cov_points), time_limit, parameters) ,
'line': 'r',
'confidence_color': (1.0, 0.0, 0.0, 0.3)
},
"Curved BA*": {
"name": "Curved BA*",
"do_hyper": False,
"hyper_test": "step_param",
"hyper_time_limit": 60,
"hyper_min_coverage": 95,
"do_experiment": False,
"experiment_time_limit": 400,
"experiment_no_of_angles": 1,
"experiment_results": [],
"hyper_data": [],
"formatted_hyper_data": [],
def __init__(self):
super().__init__('MainNode')
#Publishers:
self.markers_pub = self.create_publisher(
visualization_msgs.MarkerArray, 'marker', 3000)
self.markers_path_pub = self.create_publisher(
visualization_msgs.Marker, 'path_markers', 3000)
self.pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'pcd',
10)
self.coverable_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2,
'coverable_pcd', 100)
self.visited_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2,
'visited_pcd', 100)
self.visited_ground_pcd_pub = self.create_publisher(
sensor_msgs.PointCloud2, 'visited_ground_pcd', 100)
self.traversable_pcd_pub = self.create_publisher(
sensor_msgs.PointCloud2, 'traversable_pcd', 100)
self.inaccessible_pcd_pub = self.create_publisher(
sensor_msgs.PointCloud2, 'inaccessible_pcd', 100)
self.path_pub = self.create_publisher(nav_msgs.Path, 'path', 10)
#Create Environment
environment = PointCloudEnvironment(
self.print, PCD_DATA[PCD]["terr_assessment_file"],
PCD_DATA[PCD]["pcd_file"], False)
point_cloud = environment.full_pcd
traversable_point_cloud = environment.traversable_pcd
coverable_point_cloud = environment.coverable_pcd
motion_planner = MotionPlanner(self.print, traversable_point_cloud)
parameters = list(
filter(lambda x: x["pcd"] == PCD and x["cpp"] == ALGORITHM,
PARAMETER_DATA))[0]["param"]
if ALGORITHM == "BA*":
cpp = BAstar(self.print,
motion_planner,
coverable_point_cloud,
time_limit=TIME_LIMIT,
parameters=parameters)
if ALGORITHM == "Inward Spiral":
cpp = Spiral(self.print,
motion_planner,
coverable_point_cloud,
time_limit=TIME_LIMIT,
parameters=parameters)
if ALGORITHM == "Sampled BA*":
cpp = RandomBAstar3(self.print,
motion_planner,
coverable_point_cloud,
time_limit=TIME_LIMIT,
parameters=parameters)
self.path = cpp.get_cpp_path(PCD_DATA[PCD]["start_point"],
goal_coverage=GOAL_COVERAGE)
self.print(cpp.print_stats(self.path))
markers_pub = self.create_timer(5, self.marker_publisher)
self.pcd_pub.publish(
point_cloud.point_cloud(point_cloud.points, 'my_frame'))
self.coverable_pcd_pub.publish(
coverable_point_cloud.point_cloud(coverable_point_cloud.points,
'my_frame'))
self.traversable_pcd_pub.publish(
traversable_point_cloud.point_cloud(traversable_point_cloud.points,
'my_frame'))