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()
stamp = self.get_clock().now().to_msg()
msg = ROSMessage.line_marker(0, stamp, self.path, [0.1, 1.0, 1.0],
"path")
self.markers_msg.markers = [msg]
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)
all_results = deepcopy(cache_data)
path = list(filter(lambda x: x["ns"] == PATH_NS,
all_results))[0]["path"]
#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)
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'))
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 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 marker_publisher(self, max=None):
self.markers_msg = visualization_msgs.MarkerArray()
self.last_id = 0
for idx, path in enumerate([]):
stamp = self.get_clock().now().to_msg()
if path.get("path", False) is False:
path["path"] = self.prev_file[idx]["path"]
msg = ROSMessage.line_marker(self.last_id, stamp, path["path"], [0.1,1.0,1.0], path["ns"])
self.markers_msg.markers.append(msg)
self.last_id += 1
self.markers_pub.publish(self.markers_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 __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 path_publisher(self):
path_msg = ROSMessage.path(self.path)
self.path_pub.publish(path_msg)