def get_traversable_and_coverable_points(self):
desize = int(len(self.points) / 70000)
obstacle_points = self.get_points_that_are(OBSTACLE)[::desize, :]
coverable_points = self.get_points_that_are(COVERABLE)[::desize, :]
self.print("obst" + str(obstacle_points.shape))
self.print("coverable_points" + str(coverable_points.shape))
ground_points_idx = np.where(self.map.flatten() == COVERABLE)[0]
traversable_points_idx = ground_points_idx
full_pcd = self.map_to_full_pcd()
dist_matrix = distance_matrix(obstacle_points, coverable_points)
rows, cols = np.where(dist_matrix < 10 * self.resolution)
border_points = obstacle_points[rows]
for i, untraversable_point in enumerate(border_points):
#if i % 100 == 0:
# self.self.print("Working on border pos " + str(i) + " out of " + str(len(uncoverable_border_points)))
collision_risk_points = full_pcd.points_idx_in_radius(
untraversable_point, ROBOT_RADIUS)
traversable_points_idx = self.delete_values(
traversable_points_idx, collision_risk_points)
traversable_pcd = PointCloud(
print, points=full_pcd.points[traversable_points_idx.astype(int)])
coverable_points_idx_queue = ground_points_idx
coverable_points_idx_queue = self.delete_values(
coverable_points_idx_queue, traversable_points_idx)
false_coverable_points_idx = np.array([])
while len(coverable_points_idx_queue):
if len(coverable_points_idx_queue) % 1000 == 0:
self.print("coverable_points_idx_queue: " +
str(len(coverable_points_idx_queue)))
point_idx, coverable_points_idx_queue = coverable_points_idx_queue[
0], coverable_points_idx_queue[1:]
distance_to_nearest_traversable_point = traversable_pcd.distance_to_nearest(
full_pcd.points[point_idx])
if distance_to_nearest_traversable_point > ROBOT_RADIUS:
false_coverable_points_idx = np.append(
false_coverable_points_idx, point_idx)
real_coverable_points_idx = self.delete_values(
ground_points_idx, false_coverable_points_idx)
#self.print((len(real_coverable_points_idx), len(traversable_points_idx), len(false_coverable_points_idx), len(full_pcd.points)))
return full_pcd.points[traversable_points_idx], full_pcd.points[
real_coverable_points_idx]
class PointClassification():
''' Class for calculating points where a robot could go without collision.
'''
def __init__(self, print, pcd):
'''
Args:
print: function for printing messages
pcd: Point Cloud of the environment.
'''
self.pcd = pcd
self.print = print
def get_classified_points(self, ground_points_idx, uncoverable_border_points):
''' Given some points, find those which are traversable by the robot,
given robot size specifics. go through every point in the point cloud
that is classified as obstacle and filter out ground points that are
close.
Args:
ground_points_idx: indexes of points in the point cloud that are
classified as obstacle free.
'''
start = timeit.default_timer()
self.ground_point_cloud = PointCloud(self.print, points= self.pcd.points[ground_points_idx])
traversable_points_idx = ground_points_idx
false_uncoverable_idx = []
self.print("Starting search...")
for i, uncoverable_point in enumerate(uncoverable_border_points):
#self.print("Working on " + str(i) + " out of " + str(len(uncoverable_border_points)))
distance_to_nearest_ground_point = self.ground_point_cloud.distance_to_nearest(uncoverable_point)
if distance_to_nearest_ground_point < CELL_SIZE/2:
false_uncoverable_idx.append(i)
uncoverable_border_points = np.delete(uncoverable_border_points, false_uncoverable_idx, 0)
for i, untraversable_point in enumerate(uncoverable_border_points):
#if i % 100 == 0:
# self.print("Working on border pos " + str(i) + " out of " + str(len(uncoverable_border_points)))
collision_risk_points = self.pcd.points_idx_in_radius(untraversable_point, np.sqrt(1/2)*CELL_SIZE + 0.5*ROBOT_SIZE)
traversable_points_idx = self.delete_values(traversable_points_idx, collision_risk_points)
#Hindsight wrong classified points removal by hand:
wrong_points = np.empty((0,3))
#For pointcloud.pcd:
#wrong_points = np.append(wrong_points, [[24.395610809326172, 12.705216407775879, -5.311060428619385]], axis=0)
#wrong_points = np.append(wrong_points, [[-17.590679168701172, -3.7045161724090576, -6.118121147155762]], axis=0)
#For bridge.pcd:
wrong_points = np.append(wrong_points, [[-98.5624, 182.8, -30.83]], axis=0)
wrong_points = np.append(wrong_points, [[ 0.8125 , 93.30000305, -32.33319855]], axis=0)
wrong_points = np.append(wrong_points, [[-17.590679168701172, -3.7045161724090576, -6.118121147155762]], axis=0)
#For small bridge:
#wrong_points = np.append(wrong_points, [[ -5.11 , 15.47, -0.39]], axis=0)
#wrong_points = np.append(wrong_points, [[ -5.11 , 15.97, -0.39]], axis=0)
for wrong_point in wrong_points:
points_nearby = self.pcd.points_idx_in_radius(wrong_point, 0.5*ROBOT_SIZE)
traversable_points_idx = self.delete_values(traversable_points_idx, points_nearby)
traversable_pcd = PointCloud(self.print, points= self.pcd.points[traversable_points_idx.astype(int)])
coverable_points_idx_queue = ground_points_idx
coverable_points_idx_queue = self.delete_values(coverable_points_idx_queue, traversable_points_idx)
false_coverable_points_idx = np.array([])
while len(coverable_points_idx_queue):
#if len(coverable_points_idx_queue) % 1000 == 0:
# self.print("coverable_points_idx_queue: " + str(len(coverable_points_idx_queue)))
point_idx, coverable_points_idx_queue = coverable_points_idx_queue[0], coverable_points_idx_queue[1:]
point = self.pcd.points[point_idx]
distance_to_nearest_traversable_point = traversable_pcd.distance_to_nearest(self.pcd.points[point_idx])
if distance_to_nearest_traversable_point > ROBOT_SIZE/2:
false_coverable_points_idx = np.append(false_coverable_points_idx, point_idx)
real_coverable_points_idx = self.delete_values(ground_points_idx, false_coverable_points_idx)
stats = self.print_result(start, len(real_coverable_points_idx), len(traversable_points_idx), len(false_coverable_points_idx), len(self.pcd.points))
return self.pcd.points[traversable_points_idx], self.pcd.points[real_coverable_points_idx], self.pcd.points[false_coverable_points_idx.astype(int)], stats
def delete_values(self, array, values):
''' Removes specific values from an array
Args:
array: NumPy array to remove values from
values: NumPy array with values that should be removed.
'''
return array[ np.isin(array, values, assume_unique=True, invert=True) ]
def print_result(self, start, nbr_of_coverable, nbr_of_traversable, nbr_of_inaccessable, total_nbr_of_points):
''' Prints result data of the point classification.
'''
end = timeit.default_timer()
self.print("="*20)
self.print("POINT CLASSIFCATION")
self.print("Computational time: " + str(round(end - start, 1)) + " sec")
self.print("Number of COVERABLE points: " + str(nbr_of_coverable))
self.print("Number of TRAVERSABLE points: " + str(nbr_of_traversable))
self.print("Number of INACCESSABLE points: " + str(nbr_of_inaccessable))
self.print("Number of OBSTACLE points: " + str(total_nbr_of_points - nbr_of_inaccessable- nbr_of_coverable))
self.print("TOTAL Number of points: " + str(total_nbr_of_points))
return {
"Computational time": round(end - start, 1),
"Number of COVERABLE points": nbr_of_coverable,
"Number of TRAVERSABLE points": nbr_of_traversable,
"Number of INACCESSABLE points": nbr_of_inaccessable,
"Number of OBSTACLE points": total_nbr_of_points - nbr_of_inaccessable- nbr_of_coverable,
"TOTAL Number of points": total_nbr_of_points
}
def get_classified_points(self, ground_points_idx, uncoverable_border_points):
''' Given some points, find those which are traversable by the robot,
given robot size specifics. go through every point in the point cloud
that is classified as obstacle and filter out ground points that are
close.
Args:
ground_points_idx: indexes of points in the point cloud that are
classified as obstacle free.
'''
start = timeit.default_timer()
self.ground_point_cloud = PointCloud(self.print, points= self.pcd.points[ground_points_idx])
traversable_points_idx = ground_points_idx
false_uncoverable_idx = []
self.print("Starting search...")
for i, uncoverable_point in enumerate(uncoverable_border_points):
#self.print("Working on " + str(i) + " out of " + str(len(uncoverable_border_points)))
distance_to_nearest_ground_point = self.ground_point_cloud.distance_to_nearest(uncoverable_point)
if distance_to_nearest_ground_point < CELL_SIZE/2:
false_uncoverable_idx.append(i)
uncoverable_border_points = np.delete(uncoverable_border_points, false_uncoverable_idx, 0)
for i, untraversable_point in enumerate(uncoverable_border_points):
#if i % 100 == 0:
# self.print("Working on border pos " + str(i) + " out of " + str(len(uncoverable_border_points)))
collision_risk_points = self.pcd.points_idx_in_radius(untraversable_point, np.sqrt(1/2)*CELL_SIZE + 0.5*ROBOT_SIZE)
traversable_points_idx = self.delete_values(traversable_points_idx, collision_risk_points)
#Hindsight wrong classified points removal by hand:
wrong_points = np.empty((0,3))
#For pointcloud.pcd:
#wrong_points = np.append(wrong_points, [[24.395610809326172, 12.705216407775879, -5.311060428619385]], axis=0)
#wrong_points = np.append(wrong_points, [[-17.590679168701172, -3.7045161724090576, -6.118121147155762]], axis=0)
#For bridge.pcd:
wrong_points = np.append(wrong_points, [[-98.5624, 182.8, -30.83]], axis=0)
wrong_points = np.append(wrong_points, [[ 0.8125 , 93.30000305, -32.33319855]], axis=0)
wrong_points = np.append(wrong_points, [[-17.590679168701172, -3.7045161724090576, -6.118121147155762]], axis=0)
#For small bridge:
#wrong_points = np.append(wrong_points, [[ -5.11 , 15.47, -0.39]], axis=0)
#wrong_points = np.append(wrong_points, [[ -5.11 , 15.97, -0.39]], axis=0)
for wrong_point in wrong_points:
points_nearby = self.pcd.points_idx_in_radius(wrong_point, 0.5*ROBOT_SIZE)
traversable_points_idx = self.delete_values(traversable_points_idx, points_nearby)
traversable_pcd = PointCloud(self.print, points= self.pcd.points[traversable_points_idx.astype(int)])
coverable_points_idx_queue = ground_points_idx
coverable_points_idx_queue = self.delete_values(coverable_points_idx_queue, traversable_points_idx)
false_coverable_points_idx = np.array([])
while len(coverable_points_idx_queue):
#if len(coverable_points_idx_queue) % 1000 == 0:
# self.print("coverable_points_idx_queue: " + str(len(coverable_points_idx_queue)))
point_idx, coverable_points_idx_queue = coverable_points_idx_queue[0], coverable_points_idx_queue[1:]
point = self.pcd.points[point_idx]
distance_to_nearest_traversable_point = traversable_pcd.distance_to_nearest(self.pcd.points[point_idx])
if distance_to_nearest_traversable_point > ROBOT_SIZE/2:
false_coverable_points_idx = np.append(false_coverable_points_idx, point_idx)
real_coverable_points_idx = self.delete_values(ground_points_idx, false_coverable_points_idx)
stats = self.print_result(start, len(real_coverable_points_idx), len(traversable_points_idx), len(false_coverable_points_idx), len(self.pcd.points))
return self.pcd.points[traversable_points_idx], self.pcd.points[real_coverable_points_idx], self.pcd.points[false_coverable_points_idx.astype(int)], stats
class MainNode(Node):
def __init__(self):
super().__init__('MainNode')
#Publishers:
self.markers_pub = self.create_publisher(
visualization_msgs.MarkerArray, 'marker', 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)
#Varaiables for publishers
self.last_id = 0
timer_period = 5
animation_time_period = 0.01
self.animation_iteration = 0
self.path = []
#Subscribers:
self.rviz_sub = self.create_subscription(geometry_msgs.PointStamped,
"clicked_point",
self.clicked_point_cb, 100)
#environment = PointCloudEnvironment(self.print, "cached_coverable_points.dictionary", "pointcloud.pcd")
#environment = MapEnvironment(self.print, "simple_open.dictionary", "src/exjobb/maps/map_simple_open.png", 0.015)
#environment = MapEnvironment(self.print, "map_ipa_apartment.dictionary", "src/exjobb/maps/map_ipa_apartment.png", 0.05)
NEW_POINTCLOUD = False
if NEW_POINTCLOUD:
environment = MapEnvironment(
self.print, "simple_open.dictionary",
"src/exjobb/maps/map_simple_open.png", 0.015)
self.point_cloud = PointCloud(self.print,
file="cross.pcd",
new_point_cloud=True)
else:
#environment = PointCloudEnvironment(self.print, "pointcloud2.dictionary", "pointcloud2.pcd", False) #x,y = #351451.84, 4022966.5 Street
#environment = PointCloudEnvironment(self.print, "pointcloud3.dictionary", "pointcloud3.pcd", False) #x,y = (351609.25, 4022912.0) Same Underground garage one floor
#environment = PointCloudEnvironment(self.print, "pointcloud4.dictionary", "pointcloud4.pcd", False) #x,y = (326815.75, 4152473.25) Busy street, with cars
#environment = PointCloudEnvironment(self.print, "cached_coverable_points.dictionary", "pointcloud.pcd")
#environment = MapEnvironment(self.print, "map_ipa_apartment.dictionary", "src/exjobb/maps/map_ipa_apartment.png", 0.05)
#environment = PointCloudEnvironment(self.print, "bridge_2_small.dictionary", "bridge_2_small.pcd", False)
#environment = PointCloudEnvironment(self.print, "cross_terrain_assessment.dictionary", "cross.pcd", False)
#environment = PointCloudEnvironment(self.print, "pre_garage_terrain_assessment.dictionary", "garage.pcd", False)'
environment = PointCloudEnvironment(
self.print, "garage_terrain_assessment.dictionary",
"garage.pcd", False)
#environment = PointCloudEnvironment(self.print, "bridge_terrain_assessment.dictionary", "bridge_2.pcd", False)
#environment = MapEnvironment(self.print, "simple_open.dictionary", "src/exjobb/maps/map_simple_open.png", 0.015)
self.point_cloud = environment.full_pcd
#traversable_points, coverable_points, inaccessible_points = self.do_terrain_assessment()
#self.traversable_point_cloud = PointCloud(self.print, points= traversable_points)
#self.coverable_point_cloud = PointCloud(self.print, points= coverable_points)
#self.inaccessible_point_cloud = PointCloud(self.print, points= inaccessible_points)
#
self.point_cloud.pcd = self.point_cloud.point_cloud(
self.point_cloud.points, 'my_frame')
self.traversable_point_cloud = environment.traversable_pcd
self.traversable_point_cloud.pcd = self.traversable_point_cloud.point_cloud(
self.traversable_point_cloud.points, 'my_frame')
self.coverable_point_cloud = environment.coverable_pcd
self.coverable_point_cloud.pcd = self.coverable_point_cloud.point_cloud(
self.coverable_point_cloud.points, 'my_frame')
self.inaccessible_point_cloud = environment.inaccessible_pcd
self.inaccessible_point_cloud.pcd = self.inaccessible_point_cloud.point_cloud(
self.inaccessible_point_cloud.points, 'my_frame')
if SMALL_POINT_CLOUD:
bbox = o3d.geometry.AxisAlignedBoundingBox([10, 15, -5.3],
[15, 21, 10])
trav_points_idx = bbox.get_point_indices_within_bounding_box(
self.traversable_point_cloud.raw_pcd.points)
self.traversable_point_cloud = PointCloud(
self.print,
points=self.traversable_point_cloud.points[trav_points_idx])
cov_points_idx = bbox.get_point_indices_within_bounding_box(
self.coverable_point_cloud.raw_pcd.points)
self.coverable_point_cloud = PointCloud(
self.print,
points=self.coverable_point_cloud.points[cov_points_idx])
self.markers = []
motion_planner = MotionPlanner(self.print,
self.traversable_point_cloud)
if PUBLISH_INACCESSIBLE_PCD:
inaccessible_pcd_pub = self.create_timer(
timer_period, self.inaccessible_point_cloud_publisher)
if MOTION_PLANNER_TEST:
#start_pos = [5.0625 , 91.05000305, -32.58319855]
#end_pos = [ 0.8125 , 93.30000305, -32.33319855]
end_pos = np.array([6.05999994, -13., -5.71468687])
start_pos = np.array([28.6, -6.7, -10.3])
start_point = self.traversable_point_cloud.find_k_nearest(
start_pos, 1)[0]
end_point = self.traversable_point_cloud.find_k_nearest(
end_pos, 1)[0]
self.path = motion_planner.Astar(start_point, end_point)
self.markers.append({"point": start_point, "color": RED})
self.markers.append({"point": end_point, "color": BLUE})
if self.path is False:
self.path = []
if CPP_TEST:
random_idx = np.random.choice(len(
self.traversable_point_cloud.points),
1,
replace=False)[0]
#start_point = [-14, -16, -3.6] #
#start_point = [-23, 30, -0.9]
start_point = self.traversable_point_cloud.points[random_idx]
#SAMPLED BA*
#cost 4953, length: 3684, rotation: 1269
######real: cost: ?? lkength: 3235, rotation: 1108
sparam1 = {
'ba_exploration': 0.90756041115558,
'max_distance': 4.78202945337845,
'max_distance_part_II': 6.75513650527977,
'min_bastar_cost_per_coverage': 8192.530314616084,
'min_spiral_cost_per_coverage': 12157.969167186768,
'step_size': 0.562061544696692,
'visited_threshold': 0.279490436505789
}
#cost 4615, length: 3294, rotation: 1321
######real: cost: ??, length: 3334, rotation: 1304
sparam2 = {
'ba_exploration': 0.816319265003861,
'max_distance': 1.02476727664307,
'max_distance_part_II': 4.76356301411862,
'min_bastar_cost_per_coverage': 6616.530314616084,
'min_spiral_cost_per_coverage': 19277.969167186768,
'step_size': 0.950568870175564,
'visited_threshold': 0.484179597225153
}
#cost 4261, length: 3158, rotation: 1103
#######real: cost: ??, length: 3078, rotation: 1114
sparam3 = {
'ba_exploration': 0.853031300592955,
'max_distance': 3.89663024793223,
'max_distance_part_II': 4.80685526433465,
'min_bastar_cost_per_coverage': 9312.530314616084,
'min_spiral_cost_per_coverage': 13196.969167186768,
'step_size': 0.636195719728099,
'visited_threshold': 0.337665370485907
}
#length: 3596, rotation: 1296, 97% - annan step size (0.6..)
#real: cost: 4306, length: 3073, rotation: 1233
param4 = {
'ba_exploration': 0.8653615601139727,
'max_distance': 4.129493635268686,
'max_distance_part_II': 6.935911381739787,
'min_bastar_cost_per_coverage': 8238.530314616084,
'min_spiral_cost_per_coverage': 13644.969167186768,
'step_size': 0.54868363557903,
'visited_threshold': 0.3730115058138923
}
#cost: 5797, length: 4643, rotation: 1154, 97% - annan step size (0.6..)
#real: cost: 6422, length: 5116, rotation: 1306
param_best_brdige = {
'ba_exploration': 0.939978646944692,
'max_distance': 4.49053749147136,
'max_distance_part_II': 7.05948312639,
'min_bastar_cost_per_coverage': 12772.530314616084,
'min_spiral_cost_per_coverage': 25988.969167186768,
'step_size': 0.618705451980032,
'visited_threshold': 0.38872474480067
}
#cost: 3001, length: 2186, rotation: 815
#real: cost: 3083, length: 2281, rotation: 802
param_best_cross = {
'ba_exploration': 0.863145455156051,
'max_distance': 1.69280755868826,
'max_distance_part_II': 4.48375188984703,
'min_bastar_cost_per_coverage': 6488.530314616084,
'min_spiral_cost_per_coverage': 8141.257661974652297,
'step_size': 0.553977048496769,
'visited_threshold': 0.38872474480067
}
#BASTAR:
#cost: 16062, lenth: 10575 rotation: 5487
param1 = {
'angle_offset': 3.44800051788481,
'step_size': 0.963400677899873,
'visited_threshold': 0.257015802906527
}
#cost: 7583, lenth: 4452 rotation: 3131
param2 = {
'angle_offset': 3.78341027362029,
'step_size': 0.601687134922371,
'visited_threshold': 0.328108983656107
}
#cost: 5013, lenth: 3049 rotation: 1964
param3 = {
'angle_offset': 5.27158130667689,
'step_size': 0.517468289229711,
'visited_threshold': 0.455659073558674
}
#cost: 4238, lenth: 2896 rotation: 1342
param4 = {
'angle_offset': 4.64664343656672,
'step_size': 0.633652049936913,
'visited_threshold': 0.472819723019576
}
#cost: 3262, lenth: 2249 rotation: 1013
param_best_cross = {
'angle_offset': 4.70135588957793,
'step_size': 0.523646671416283,
'visited_threshold': 0.403681713288835
}
#cost: 6385, lenth: 4562 rotation: 1823
param_best_brdige = {
'angle_offset': 5.33881157053433,
'step_size': 0.55692737194204,
'visited_threshold': 0.453169184364576
}
#SPIRAL:
#cost: 14292, lenth: 7523 rotation: 6769
param1 = {
'step_size': 0.999314930298507,
'visited_threshold': 0.32443603324225
}
#cost: 7431, lenth: 3990 rotation: 3441
param2 = {
'step_size': 0.825030992319859,
'visited_threshold': 0.433448258850281
}
#cost: 6466, lenth: 3218 rotation: 3248
param3 = {
'step_size': 0.521396930930628,
'visited_threshold': 0.47473068968531
}
#cost: 5787, lenth: 3101 rotation: 2686
param4 = {
'step_size': 0.627870706339337,
'visited_threshold': 0.498775709725593
}
#cost: 7213, lenth: 4440 rotation: 2773
param_best_brdige = {
'step_size': 0.737114020263598,
'visited_threshold': 0.483088877473477
}
#cost: 4054, lenth: 2239 rotation: 1815
param_best_cross = {
'step_size': 0.664671825076571,
'visited_threshold': 0.499669038773602
}
#param = {'step_size': 0.5,
# 'visited_threshold': 0.4}
start_point = [-20.7, 43, -1]
#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
#start_point = np.array([15.6, -16.7, -5.3])
#start_point = np.array([0.6,0.6,0])
#start_points = {}
#for n in range(10):
# random_idx = np.random.choice(len(self.traversable_point_cloud.points), 1, replace=False)[0]
# start_points[n] = self.traversable_point_cloud.points[random_idx]
# #self.markers.append( {"point": self.traversable_point_cloud.points[random_idx], "color": RED} )
#self.print(start_points)
self.cpp = RandomBAstar3(self.print,
motion_planner,
self.coverable_point_cloud,
time_limit=300,
parameters=sparam4)
self.path = self.cpp.get_cpp_path(start_point, goal_coverage=0.97)
#self.path = self.cpp.breadth_first_search(start_point)
#self.print(self.cpp.print_results())
#self.path = self.cpp.get_cpp_node_path(start_point)
self.print(self.cpp.print_stats(self.path))
for marker in self.cpp.points_to_mark:
self.markers.append(marker)
#self.markers.append( {"point": self.path[-1], "color": RED} )
#self.points_to_mark = [self.path[-1]]
if PUBLISH_FULL_PCD:
#pcd_pub = self.create_timer(timer_period, self.point_cloud_publisher)
self.point_cloud_publisher()
if PUBLISH_GROUND_PCD:
#coverable_pcd_pub = self.create_timer(timer_period, self.coverable_point_cloud_publisher)
self.coverable_point_cloud_publisher()
if PUBLISH_TRAVERSABLE_PCD:
#traversable_pcd_pub = self.create_timer(timer_period, self.traversable_point_cloud_publisher)
self.traversable_point_cloud_publisher()
#HYPER_START_POS = np.array([-53.7, 54.2, -2.7])
#start_points = {
# 0: np.array([-43.10443115, 3.99802136, 4.46702003]),
# 1: np.array([ 21.61431885, -33.00197983, -2.77298403]),
# 2: np.array([-34.51068115, 12.49802208, -4.17298126]),
# 3: np.array([ 15.9268198 , -36.00197983, -2.6929822 ]),
# 4: np.array([38.98931885, 45.49802399, 1.19701743]),
# 5: np.array([ 3.73931861, 40.74802399, 2.83701849]),
# 6: np.array([ 15.5205698 , -31.50197792, -2.8729825 ]),
# 7: np.array([-16.44818115, -19.25197792, -3.58298159]),
# 8: np.array([10.52056885, 42.74802399, 2.46701956]),
# 9: np.array([53.89556885, 35.99802399, 0.33701676])}
#for point in start_points.values():
# self.markers.append({
# "point": point,
# "color": [0.0,0.0,1.0]
# })
#self.markers.append({
# "point": HYPER_START_POS,
# "color": [0.0,1.0,0.0]
# })
#CPP_TEST = True
if PUBLISH_MARKERS and len(self.markers):
#for marker in self.cpp.points_to_mark:
# self.markers.append(marker)
markers_pub = self.create_timer(timer_period,
self.marker_publisher)
if PUBLISH_PATH and len(self.path) > 0 and not PUBLISH_PATH_ANIMATION:
path_pub = self.create_timer(timer_period, self.path_publisher)
if PUBLISH_VISITED_PCD:
self.point_cloud.visit_path(self.path)
self.visited_points_pcd = self.point_cloud.get_covered_points_as_pcd(
)
visited_pcd_pub = self.create_timer(
timer_period, self.visited_point_cloud_publisher)
if PUBLISH_VISITED_GROUND_PCD and len(self.path):
#self.coverable_point_cloud = PointCloud(self.print, points= coverable_points)
self.coverable_point_cloud.visit_path(self.path)
self.visited_ground_points_pcd = self.coverable_point_cloud.get_covered_points_as_pcd(
)
visited_ground_pcd_pub = self.create_timer(
timer_period, self.visited_ground_point_cloud_publisher)
if PUBLISH_PATH_ANIMATION and len(self.path) > 0:
time.sleep(8)
self.coverable_point_cloud.covered_points_idx = np.array([])
path_pub = self.create_timer(animation_time_period,
self.animated_path_publisher)
if PUBLISH_SEGMENTS_ANIMATION and len(self.cpp.all_segments) > 0:
time.sleep(8)
self.coverable_point_cloud.covered_points_idx = np.array([])
self.segments = self.cpp.all_segments
path_pub = self.create_timer(animation_time_period,
self.animated_segment_publisher)
def point_cloud_publisher(self):
self.pcd_pub.publish(self.point_cloud.pcd)
def coverable_point_cloud_publisher(self):
self.coverable_pcd_pub.publish(self.coverable_point_cloud.pcd)
def traversable_point_cloud_publisher(self):
self.traversable_pcd_pub.publish(self.traversable_point_cloud.pcd)
def inaccessible_point_cloud_publisher(self):
self.inaccessible_pcd_pub.publish(self.inaccessible_point_cloud.pcd)
def visited_point_cloud_publisher(self):
self.visited_pcd_pub.publish(self.visited_points_pcd)
def visited_ground_point_cloud_publisher(self):
self.visited_ground_pcd_pub.publish(self.visited_ground_points_pcd)
def path_publisher(self):
path_msg = ROSMessage.path(self.path)
self.path_pub.publish(path_msg)
def animated_path_publisher(self):
'''Publishes the path point by point to create an animation in RViz.
'''
self.animation_iteration += 10
if self.animation_iteration >= len(self.path):
self.path_publisher()
return
path_msg = ROSMessage.path(self.path[0:self.animation_iteration])
self.path_pub.publish(path_msg)
new_path = self.path[self.animation_iteration -
10:self.animation_iteration]
self.coverable_point_cloud.visit_path(new_path)
#point = self.path[self.animation_iteration]
#self.coverable_point_cloud.visit_path_to_position(point, self.path[self.animation_iteration-1])
self.visited_ground_points_pcd = self.coverable_point_cloud.get_covered_points_as_pcd(
)
self.visited_ground_point_cloud_publisher()
def animated_segment_publisher(self):
'''Publishes the path point by point to create an animation in RViz.
'''
if self.animation_iteration >= len(self.segments):
#self.path_publisher()
self.print("DONE!")
return
current_path = np.empty((0, 3))
for idx in range(self.animation_iteration):
current_path = np.append(current_path,
self.segments[idx].path,
axis=0)
path_msg = ROSMessage.path(current_path)
self.path_pub.publish(path_msg)
latest = self.segments[self.animation_iteration].path
self.coverable_point_cloud.visit_path(latest)
self.visited_ground_points_pcd = self.coverable_point_cloud.get_covered_points_as_pcd(
)
self.visited_ground_point_cloud_publisher()
self.animation_iteration += 1
self.marker_publisher(self.animation_iteration)
def marker_publisher(self, max=None):
self.markers_msg = visualization_msgs.MarkerArray()
for idx, marker in enumerate(self.markers[0:max]):
stamp = self.get_clock().now().to_msg()
msg = ROSMessage.point_marker(self.last_id, stamp, marker["point"],
marker["color"], str(idx))
self.markers_msg.markers.append(msg)
self.last_id += 1
self.markers_pub.publish(self.markers_msg)
def clicked_point_cb(self, msg):
self.print(msg)
point = np.array([msg.point.x, msg.point.y, msg.point.z])
self.print(self.traversable_point_cloud.distance_to_nearest(point))
def print(self, object_to_print):
self.get_logger().info(str(object_to_print))
