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() 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 __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)