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 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 path_publisher(self):
path_msg = ROSMessage.path(self.path)
self.path_pub.publish(path_msg)