Ejemplo n.º 1
0
    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)
Ejemplo n.º 2
0
def main():

    #### STEP 1 - Get classified pointcloud ####

    environment = PointCloudEnvironment(my_print, TERRAIN_ASSESSMENT_FILE,
                                        POINTCLOUD_FILE)
    coverable_points = environment.coverable_pcd.points
    traversable_points = environment.traversable_pcd.points
    motion_planner = MotionPlanner(my_print, environment.traversable_pcd)

    ###

    TIME_LIMIT = 400
    start_point = np.array([28.6, -6.7, -10.3])
    goal_coverage = 0.97
    paths_markers = []
    #Get CPP path
    for pub_path in ALL_PATHS:

        if not pub_path["do_calc"]:
            continue

        coverable_pcd = PointCloud(my_print, points=coverable_points)
        if pub_path["cpp"] == "BA*":
            cpp = BAstar(my_print,
                         motion_planner,
                         coverable_pcd,
                         time_limit=TIME_LIMIT,
                         parameters=pub_path["param"])
        if pub_path["cpp"] == "Inward Spiral":
            cpp = Spiral(my_print,
                         motion_planner,
                         coverable_pcd,
                         time_limit=TIME_LIMIT,
                         parameters=pub_path["param"])
        if pub_path["cpp"] == "Sampled BA*":
            cpp = RandomBAstar3(my_print,
                                motion_planner,
                                coverable_pcd,
                                time_limit=TIME_LIMIT,
                                parameters=pub_path["param"])

        ns = pub_path["ns"]
        pub_path["path"] = cpp.get_cpp_path(start_point,
                                            goal_coverage=goal_coverage)
        pub_path["markers"] = cpp.points_to_mark
        pub_path["stats"] = cpp.print_stats(pub_path["path"])
        save_data(ALL_PATHS)

    print("DONE!")
Ejemplo n.º 3
0
    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)

        #Create Environment
        environment = PointCloudEnvironment(
            self.print, PCD_DATA[PCD]["terr_assessment_file"],
            PCD_DATA[PCD]["pcd_file"], False)
        point_cloud = environment.full_pcd
        traversable_point_cloud = environment.traversable_pcd
        coverable_point_cloud = environment.coverable_pcd

        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'))
def main():

    #with open(RESULTS_FILE, 'rb') as cached_pcd_file:
    #    cache_data = pickle.load(cached_pcd_file)
    #    pprint.pprint(cache_data)
    #return

    #with open(RESULTS_FILE, 'rb') as cached_pcd_file:
    #    cache_data = pickle.load(cached_pcd_file)
    #    for alg in ALGORITHMS:
    #        if ALGORITHMS[alg]["do_hyper"]:
    #            ALGORITHMS[alg]["opt_param"] = cache_data[alg]["opt_param"]

    #### STEP 1 - Get classified pointcloud ####

    environment = PointCloudEnvironment(my_print, TERRAIN_ASSESSMENT_FILE,
                                        POINTCLOUD_FILE)
    coverable_points = environment.coverable_pcd.points
    traversable_points = environment.traversable_pcd.points
    motion_planner = MotionPlanner(my_print, environment.traversable_pcd)

    #If from terrain assessment file:
    #with open(TERRAIN_ASSESSMENT_FILE, 'rb') as cached_pcd_file:
    #    cache_data = pickle.load(cached_pcd_file)
    #    coverable_points = cache_data["coverable_points"]
    #    traversable_points = cache_data["traversable_points"]
    #traversable_pcd = PointCloud(my_print, points= traversable_points)
    #motion_planner = MotionPlanner(my_print, traversable_pcd)

    #### STEP 2 - Hyper parameters ####
    for algorithm_key, algorithm in ALGORITHMS.items():
        if algorithm["do_hyper"]:
            trials = Trials()
            hyper_optimizer = HyptoOptimizer(save_data, algorithm, my_print,
                                             HYPER_START_POS, motion_planner,
                                             coverable_points)
            if algorithm_key == "BA*":
                opt_param = fmin(hyper_optimizer.hyper_test_bastar,
                                 space=(hp.uniform('angle_offset', 0,
                                                   np.pi * 2),
                                        hp.uniform('step_size', 0.5, 1),
                                        hp.uniform('visited_threshold', 0.25,
                                                   0.5)),
                                 algo=tpe.suggest,
                                 max_evals=HYPER_MAX_EVAL,
                                 trials=trials)
            elif algorithm_key == "Inward Spiral":
                opt_param = fmin(hyper_optimizer.hyper_test_inward_spiral,
                                 space=(hp.uniform('step_size', 0.5, 1),
                                        hp.uniform('visited_threshold', 0.25,
                                                   0.5)),
                                 algo=tpe.suggest,
                                 max_evals=HYPER_MAX_EVAL,
                                 trials=trials)
            elif algorithm_key == "Sampled BA*":
                coverage_2 = algorithm["hyper_min_coverage"] / 100
                opt_param = fmin(
                    hyper_optimizer.hyper_test_sampled_bastar_param,
                    space=(hp.uniform('coverage_1', 0.25, coverage_2),
                           hp.uniform('coverage_2', coverage_2 - 0.025,
                                      coverage_2),
                           hp.uniform('max_distance', 1, 10),
                           hp.uniform('max_distance_part_II', 1,
                                      20), hp.uniform('max_iterations', 30,
                                                      150),
                           hp.uniform('min_bastar_coverage', 0.005, 0.05),
                           hp.uniform('min_spiral_length', 2, 100),
                           hp.uniform('nbr_of_angles', 0.6,
                                      8.4), hp.uniform('step_size', 0.66,
                                                       1.33),
                           hp.uniform('visited_threshold', 0.25, 0.5)),
                    algo=tpe.suggest,
                    max_evals=HYPER_MAX_EVAL,
                    trials=trials)
            print(trials.statuses())
            algorithm["opt_param"] = opt_param
            algorithm["hyper_data"] = trials.trials
            ALGORITHMS[algorithm_key] = algorithm
            save_data(ALGORITHMS)

    #### STEP 3 - Full tests ####
    for start_point_nr in range(NUMBER_OF_START_POINTS):
        #start_point = get_random_point(traversable_points)
        start_point = start_points[start_point_nr]
        print("Start point " + str(start_point_nr) + ": " + str(start_point))

        for algorithm_key, algorithm in ALGORITHMS.items():
            if algorithm["do_experiment"]:
                experimenter = Experimenter(algorithm, print)
                parameters = None
                if "opt_param" in algorithm:
                    parameters = algorithm["opt_param"]

                cpp = algorithm["cpp"](my_print, motion_planner,
                                       coverable_points,
                                       algorithm["experiment_time_limit"],
                                       parameters)

                if "sample_specific_stats" in algorithm:
                    experimenter.perform_sample_cpp(cpp, start_point,
                                                    start_point_nr)
                    algorithm["sample_specific_stats"].append(
                        experimenter.sample_specific_stats)
                else:
                    experimenter.perform_cpp(cpp, start_point, start_point_nr)

                algorithm["experiment_results"].append(experimenter.results)
                ALGORITHMS[algorithm_key] = algorithm
                save_data(ALGORITHMS)
def main():

    #with open(RESULTS_FILE, 'rb') as cached_pcd_file:
    #    cache_data = pickle.load(cached_pcd_file)
    #    pprint.pprint(cache_data)
    #return

    with open(RESULTS_FILE, 'rb') as cached_pcd_file:
        cache_data = pickle.load(cached_pcd_file)
        ALGORITHMS = deepcopy(cache_data)
        for alg in ALGORITHMS:
            ALGORITHMS[alg]["do_hyper"] = False
            ALGORITHMS[alg][
                "cpp"] = lambda print, motion_planner, cov_points, time_limit, parameters: RandomBAstar3(
                    print, motion_planner, PointCloud(
                        print, points=cov_points), time_limit, parameters)

    #### STEP 1 - Get classified pointcloud ####

    environment = PointCloudEnvironment(my_print, TERRAIN_ASSESSMENT_FILE,
                                        POINTCLOUD_FILE)
    coverable_points = environment.coverable_pcd.points
    traversable_points = environment.traversable_pcd.points
    motion_planner = MotionPlanner(my_print, environment.traversable_pcd)

    #If from terrain assessment file:
    #with open(TERRAIN_ASSESSMENT_FILE, 'rb') as cached_pcd_file:
    #    cache_data = pickle.load(cached_pcd_file)
    #    coverable_points = cache_data["coverable_points"]
    #    traversable_points = cache_data["traversable_points"]
    #traversable_pcd = PointCloud(my_print, points= traversable_points)
    #motion_planner = MotionPlanner(my_print, traversable_pcd)

    #### STEP 2 - Hyper parameters ####
    for algorithm_key, algorithm in ALGORITHMS.items():
        if algorithm["do_hyper"]:
            trials = Trials()
            hyper_optimizer = HyptoOptimizer(save_data, algorithm, my_print,
                                             HYPER_START_POS, motion_planner,
                                             coverable_points)
            opt_param = fmin(
                hyper_optimizer.hyper_test_newest_sampled_bastar_param,
                space=(hp.uniform('ba_exploration', 0.75,
                                  0.95), hp.uniform('max_distance', 1, 5),
                       hp.uniform('max_distance_part_II', 4, 10),
                       hp.uniform('min_bastar_cost_per_coverage', 5000, 10000),
                       hp.uniform('min_spiral_cost_per_coverage', 10000,
                                  20000), hp.uniform('step_size', 0.5, 1.0),
                       hp.uniform('visited_threshold', 0.25, 0.5)),
                algo=tpe.suggest,
                max_evals=HYPER_MAX_EVAL,
                trials=trials)
            print(trials.statuses())
            algorithm["opt_param"] = opt_param
            algorithm["hyper_data"] = trials.trials
            ALGORITHMS[algorithm_key] = algorithm
            save_data(ALGORITHMS)

    #### STEP 3 - Full tests ####
    for start_point_nr in range(NUMBER_OF_START_POINTS):
        #start_point = get_random_point(traversable_points)
        start_point = start_points[start_point_nr]
        print("Start point " + str(start_point_nr) + ": " + str(start_point))

        for algorithm_key, algorithm in ALGORITHMS.items():
            if algorithm["do_experiment"]:
                experimenter = Experimenter(algorithm, print)
                parameters = None
                if "opt_param" in algorithm:
                    parameters = algorithm["opt_param"]

                cpp = algorithm["cpp"](my_print, motion_planner,
                                       coverable_points,
                                       algorithm["experiment_time_limit"],
                                       parameters)

                if "sample_specific_stats" in algorithm:
                    experimenter.perform_sample_cpp(cpp, start_point,
                                                    start_point_nr)
                    algorithm["sample_specific_stats"].append(
                        experimenter.sample_specific_stats)
                else:
                    experimenter.perform_cpp(cpp, start_point, start_point_nr)

                algorithm["experiment_results"].append(experimenter.results)
                ALGORITHMS[algorithm_key] = algorithm
                save_data(ALGORITHMS)
Ejemplo n.º 6
0
    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)
Ejemplo n.º 7
0
    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)
            self.prev_file = deepcopy(cache_data)

        #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
        start_point = np.array([28.6, -6.7, -10.3])
        goal_coverage = 0.97
        paths_markers = []
        #Get CPP path
        for pub_path in ALL_PATHS:

            if not pub_path["do_calc"]:
                continue

            coverable_pcd = PointCloud(self.print,
                                       points=coverable_point_cloud.points)
            if pub_path["cpp"] == "BA*":
                cpp = BAstar(self.print,
                             motion_planner,
                             coverable_pcd,
                             time_limit=TIME_LIMIT,
                             parameters=pub_path["param"])
            if pub_path["cpp"] == "Inward Spiral":
                cpp = Spiral(self.print,
                             motion_planner,
                             coverable_pcd,
                             time_limit=TIME_LIMIT,
                             parameters=pub_path["param"])
            if pub_path["cpp"] == "Sampled BA*":
                cpp = RandomBAstar3(self.print,
                                    motion_planner,
                                    coverable_pcd,
                                    time_limit=TIME_LIMIT,
                                    parameters=pub_path["param"])

            ns = pub_path["ns"]
            pub_path["path"] = cpp.get_cpp_path(start_point,
                                                goal_coverage=goal_coverage)
            pub_path["markers"] = cpp.points_to_mark
            pub_path["stats"] = cpp.print_stats(pub_path["path"])
            save_data(ALL_PATHS)
            #self.print(path_msg)
            #paths_markers.append(path_msg)
            #path_pub = self.create_publisher(nav_msgs.Path, ns + '/path', 10)
            #self.markers_pub.publish(path_msg)
        #
        #self.markers_msg = visualization_msgs.MarkerArray()
        #
        #for idx, msg in enumerate(paths_markers):
        #    stamp = self.get_clock().now().to_msg()
        #    self.markers_msg.markers.append(msg)
        #    #self.last_id += 1
        #

        markers_pub = self.create_timer(5, self.marker_publisher)

        #self.markers_pub.publish(self.markers_msg)

        for idx, pub_path in enumerate(ALL_PATHS):
            if pub_path.get("stats", False) is False:
                pub_path["stats"] = self.prev_file[idx]["stats"]
            self.print("=" * 20)
            self.print(pub_path["ns"])
            self.print(pub_path["stats"])

        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'))
Ejemplo n.º 8
0
    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)
Ejemplo n.º 9
0
    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 = PointCloudEnvironment(
            self.print, PCD_DATA[PCD]["terr_assessment_file"],
            PCD_DATA[PCD]["pcd_file"], 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)

        parameters = list(
            filter(lambda x: x["pcd"] == PCD and x["cpp"] == ALGORITHM,
                   PARAMETER_DATA))[0]["param"]

        if ALGORITHM == "BA*":
            cpp = BAstar(self.print,
                         motion_planner,
                         coverable_point_cloud,
                         time_limit=TIME_LIMIT,
                         parameters=parameters)
        if ALGORITHM == "Inward Spiral":
            cpp = Spiral(self.print,
                         motion_planner,
                         coverable_point_cloud,
                         time_limit=TIME_LIMIT,
                         parameters=parameters)
        if ALGORITHM == "Sampled BA*":
            cpp = RandomBAstar3(self.print,
                                motion_planner,
                                coverable_point_cloud,
                                time_limit=TIME_LIMIT,
                                parameters=parameters)

        self.path = cpp.get_cpp_path(PCD_DATA[PCD]["start_point"],
                                     goal_coverage=GOAL_COVERAGE)
        self.print(cpp.print_stats(self.path))

        markers_pub = self.create_timer(5, self.marker_publisher)

        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'))