class RandomSpiralSegment:
"""A class to generate a segment of Spiral path. Used in Sample-Based BAstar
CPP Algorithm.
"""
def __init__(self, print, motion_planner, starting_point,
visited_waypoints):
"""
Args:
print: function for printing messages
motion_planner: Motion Planner of the robot wihch also has the Point Cloud
starting_point: A [x,y,z] np.array of the start position of the robot
visited_waypoints: A Nx3 array with points that has been visited and should be avoided
"""
self.visited_waypoints = visited_waypoints
self.start = starting_point
self.print = print
self.pcd = PointCloud(print, points=motion_planner.traversable_points)
self.motion_planner = motion_planner
current_angle = 0
self.path = np.empty((0, 3))
next_starting_point = starting_point
while True:
local_spiral_path, current_position = self.get_path_until_dead_zone(
next_starting_point, current_angle)
self.path = np.append(self.path, local_spiral_path, axis=0)
self.visited_waypoints = np.append(self.visited_waypoints,
local_spiral_path,
axis=0)
next_starting_point = self.wavefront_algorithm(current_position)
if next_starting_point is False:
break
if np.linalg.norm(next_starting_point - current_position
) > RANDOM_BASTAR_VARIANT_DISTANCE:
break
path_to_next_starting_point = self.motion_planner.Astar(
current_position, next_starting_point)
if path_to_next_starting_point is False:
break
self.path = np.append(self.path,
path_to_next_starting_point,
axis=0)
current_position = next_starting_point
if len(self.path) >= 2:
current_angle = self.get_angle(self.path[-2], current_position)
self.end = current_position
self.print("Length of path: " + str(len(self.path)))
if len(self.path) > 1:
self.pcd.visit_path(self.path)
self.covered_points_idx = self.pcd.covered_points_idx
self.coverage = self.pcd.get_coverage_efficiency()
self.pcd = None
self.motion_planner = None
def wavefront_algorithm(self, start_position):
"""Using Wavefront algorithm to find the closest obstacle free uncovered position.
Args:
start_position: A [x,y,z] np.array of the start position of the search
Returns:
An obstacle free uncovered position.
"""
last_layer = np.array([start_position])
visited = np.array([start_position])
visited_points = np.append(self.visited_waypoints, self.path, axis=0)
while len(last_layer):
new_layer = np.empty((0, 3))
for pos in last_layer:
neighbours = self.get_neighbours(pos)
for neighbour in neighbours:
if self.has_been_visited(neighbour, visited):
continue
if not self.motion_planner.is_valid_step(pos, neighbour):
continue
if not self.has_been_visited(neighbour, visited_points):
return neighbour
visited = np.append(visited, [neighbour], axis=0)
new_layer = np.append(new_layer, [neighbour], axis=0)
last_layer = new_layer
self.print(
"FAIL. No new uncovered obstacle free positions could be found.")
return False
def get_path_until_dead_zone(self, current_position, current_angle):
"""Covers the area in an inward spiral motion until a dead zone is reached.
Args:
current_position: A [x,y,z] np.array of the start position of the search
current_angle: A float value representing the starting angle in radians
Returns:
New part of the path with waypoints and the position of the robot at the
end of the path.
"""
local_path = np.array([current_position])
dead_zone_reached = False
visited_points = np.append(self.visited_waypoints, self.path, axis=0)
while not dead_zone_reached:
dead_zone_reached = True
neighbours = self.get_neighbours_for_spiral(
current_position, current_angle)
for neighbour in neighbours:
if self.is_blocked(current_position, neighbour,
visited_points):
continue
local_path = np.append(local_path, [neighbour], axis=0)
visited_points = np.append(visited_points, [neighbour], axis=0)
current_angle = self.get_angle(current_position, neighbour)
current_position = neighbour
dead_zone_reached = False
break
return local_path, current_position
def get_angle(self, from_pos, to_pos):
"""Calculates the angle of the robot after making a step
Args:
from_pos: A [x,y,z] np.array of the start position
to_pos: A [x,y,z] np.array of the end position
Returns:
An angle in radians
"""
vec = to_pos[0:2] - from_pos[0:2]
return np.angle(vec[0] + vec[1] * 1j)
def is_in_list(self, list, array):
"""Checks if an array is in a list by checking if it has
values close to it.
Args:
list: list with arrays
array: array to check
Returns:
True if it finds the array in the list
"""
diffs = np.linalg.norm(list - array, axis=1)
return np.any(diffs < 0.05)
def get_neighbours_for_spiral(self, current_position, current_angle):
"""Finds neighbours of a given position. And return them in the order
to create the inward spiral motion.
Args:
current_position: A [x,y,z] np.array of the start position
current_angle: A float value representing the starting angle in radians
Returns:
List of neighbours in specific order to get the inward spiral motion,
"""
directions = []
for direction_idx in range(8):
angle = direction_idx / 8 * np.pi * 2 + current_angle
x = current_position[0] + np.cos(angle) * SPIRAL_STEP_SIZE
y = current_position[1] + np.sin(angle) * SPIRAL_STEP_SIZE
z = current_position[2]
pos = np.array([x, y, z])
directions.append(self.pcd.find_k_nearest(pos, 1)[0])
right, forwardright, forward, forwardleft, left, backleft, back, backright = directions
return [
backright, right, forwardright, forward, forwardleft, left,
backleft
]
def get_neighbours(self, current_position):
"""Finds all neighbours of a given position.
Args:
current_position: A [x,y,z] np.array of the start position
Returns:
All 8 neighbours of the given position
"""
directions = []
for direction_idx in range(8):
angle = direction_idx / 8 * np.pi * 2
x = current_position[0] + np.cos(angle) * SPIRAL_STEP_SIZE
y = current_position[1] + np.sin(angle) * SPIRAL_STEP_SIZE
z = current_position[2]
pos = np.array([x, y, z])
directions.append(self.pcd.find_k_nearest(pos, 1)[0])
return directions
def has_been_visited(self, point, path=None):
"""Checks if a point has been visited. Looks if the distance to a point in the
path is smaller than RANDOM_BASTAR_VISITED_TRESHOLD.
Args:
point: A [x,y,z] np.array of the point that should be checked.
path (optional): Specific path. Defaults to None.
Returns:
True if the point has been classified as visited
"""
if path is None:
path = self.path
distances = np.linalg.norm(path - point, axis=1)
return np.any(distances <= RANDOM_BASTAR_VISITED_TRESHOLD)
def is_blocked(self, from_point, to_point, path=None):
"""Checks if a step is valid by looking if the end point has been visited
or is an obstacle.
Args:
from_point: A [x,y,z] np.array of the start position
to_point: A [x,y,z] np.array of the end position
path (optional): Specific path. Defaults to None.
Returns:
True if the point has been classified as blocked
"""
if path is None:
path = self.path
if self.has_been_visited(to_point, path):
return True
if not self.motion_planner.is_valid_step(from_point, to_point):
return True
return False
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))