diff --git a/src/applications/CMakeLists.txt b/src/applications/CMakeLists.txt index bc85c9f..8c345a4 100644 --- a/src/applications/CMakeLists.txt +++ b/src/applications/CMakeLists.txt @@ -30,7 +30,9 @@ add_executable(maliput_query target_link_libraries(maliput_query gflags maliput::common + maliput::base maliput::plugin + maliput::routing maliput::utility maliput_malidrive::loader maliput_integration::integration diff --git a/src/applications/maliput_query.cc b/src/applications/maliput_query.cc index e462c2d..630fc40 100644 --- a/src/applications/maliput_query.cc +++ b/src/applications/maliput_query.cc @@ -55,12 +55,16 @@ #include #include +#include #include #include #include #include #include #include +#include +#include +#include #include #include #include @@ -256,6 +260,13 @@ const std::map CommandsUsage() { "to a bounding box of size [box_length_2, box_width_2, box_height_2] ", "and pose [x_2, y_2, z_2, roll_2, pitch_2, yaw_2]"}, 19}}, + {"FindRoutes", + {"FindRoutes", + "FindRoutes start_lane_id start_s end_lane_id end_s allow_lane_switch max_phase_cost max_route_cost", + {"Find Routes from ", "RoadPosition(LaneId(start_lane_id), LanePosition(start_s, 0, 0)) ", + "to RoadPosition(LaneId(end_lane_id), LanePosition(end_s, 0, 0)) ", + "with RoutingConstraints(allow_lane_switch, max_phase_cost, max_route_cost)."}, + 8}}, }; } @@ -319,7 +330,7 @@ std::ostream& operator<<(std::ostream& out, const maliput::api::LaneSRange& lane // Serializes `lane_s_route` into `out`. std::ostream& operator<<(std::ostream& out, const maliput::api::LaneSRoute& lane_s_route) { - out << "Route(ranges: ["; + out << "LaneSRoute(ranges: ["; for (const auto& range : lane_s_route.ranges()) { out << range << ", "; } @@ -342,6 +353,38 @@ std::ostream& operator<<(std::ostream& out, const maliput::api::rules::RightOfWa return out; } +// Serializes `phase` into `out`. +std::ostream& operator<<(std::ostream& out, const maliput::routing::Phase& phase) { + out << "Phase(index: " << phase.index() << ", "; + out << "lane_s_range_tolerance: " << phase.lane_s_range_tolerance() << ", "; + out << "start_positions: ["; + for (const maliput::api::RoadPosition& pos : phase.start_positions()) { + out << pos << ", "; + } + out << "], "; + out << "end_positions: ["; + for (const maliput::api::RoadPosition& pos : phase.end_positions()) { + out << pos << ", "; + } + out << "], "; + out << "lane_s_ranges: ["; + for (const maliput::api::LaneSRange& lane_s_range : phase.lane_s_ranges()) { + out << lane_s_range << ", "; + } + out << "])"; + return out; +} + +// Serializes `route` into `out`. +std::ostream& operator<<(std::ostream& out, const maliput::routing::Route& route) { + out << "Route(phases: ["; + for (int i = 0; i < route.size(); ++i) { + out << route.Get(i) << ", "; + } + out << "])"; + return out; +} + // Returns a string with the usage message. std::string GetUsageMessage() { std::stringstream ss; @@ -860,6 +903,31 @@ class RoadNetworkQuery { PrintQueryTime(duration.count()); } + /// Finds all the Routes from start to end given the constraints. + void FindRoutes(const maliput::api::LaneId& start_lane_id, const maliput::api::LanePosition& start_lane_pos, + const maliput::api::LaneId& end_lane_id, const maliput::api::LanePosition& end_lane_pos, + const maliput::DistanceRouter& router, + const maliput::routing::RoutingConstraints& constraints) const { + const maliput::api::Lane* start_lane = rn_->road_geometry()->ById().GetLane(start_lane_id); + const maliput::api::Lane* end_lane = rn_->road_geometry()->ById().GetLane(end_lane_id); + MALIPUT_THROW_UNLESS(start_lane != nullptr); + MALIPUT_THROW_UNLESS(end_lane != nullptr); + const maliput::api::RoadPosition start_pos(start_lane, start_lane_pos); + const maliput::api::RoadPosition end_pos(end_lane, start_lane_pos); + + const auto start = std::chrono::high_resolution_clock::now(); + const std::vector routes = router.ComputeRoutes(start_pos, end_pos, constraints); + const auto end = std::chrono::high_resolution_clock::now(); + + (*out_) << "The Routes from " << start_pos << " to " << end_pos << " are: " << std::endl; + for (const auto& route : routes) { + (*out_) << "\t- " << route << std::endl; + } + + const std::chrono::duration duration = (end - start); + PrintQueryTime(duration.count()); + } + /// @return the object_book_ variable. maliput::object::ManualObjectBook* GetManualObjectBook() { return object_book_.get(); } @@ -1062,6 +1130,38 @@ double SFromCLI(char** argv) { return s; } +/// @return A bool represented by `*argv`. It must be either "true" or "false". +/// @pre `argv` is not nullptr. +/// @pre `argv` points to a string containing "true" or "false". +/// @throws maliput::common::assertion_error When preconditions are not met. +bool BoolFromCLI(char** argv) { + MALIPUT_THROW_UNLESS(argv != nullptr); + static const std::string kTrueStr("true"); + static const std::string kFalseStr("false"); + const std::string token(argv[0]); + + if (kTrueStr.compare(token) == 0) { + return true; + } + if (kFalseStr.compare(token) == 0) { + return false; + } + MALIPUT_THROW_MESSAGE("Cannot convert token into bool, try with true or false."); +} + +/// @return A maliput::routing::RoutingConstraints represented by @p argv. +/// @p It must point to at least 3 different char sequences. +/// @pre `argv` is not nullptr. +/// @pre `argv` points to a string containing "true" or "false". +/// @throws maliput::common::assertion_error When preconditions are not met. +maliput::routing::RoutingConstraints RoutingConstraintsFromCLI(char** argv) { + MALIPUT_THROW_UNLESS(argv != nullptr); + const bool allow_lane_switch = BoolFromCLI(&(argv[0])); + const double max_phase_cost = std::strtod(argv[1], nullptr); + const double max_route_cost = std::strtod(argv[2], nullptr); + return maliput::routing::RoutingConstraints{allow_lane_switch, max_phase_cost, max_route_cost}; +} + int Main(int argc, char* argv[]) { gflags::SetUsageMessage(GetUsageMessage()); gflags::ParseCommandLineFlags(&argc, &argv, true); @@ -1212,6 +1312,14 @@ int Main(int argc, char* argv[]) { query.GetManualObjectBook()->AddObject(std::move(bounding_object_1)); query.GetManualObjectBook()->AddObject(std::move(bounding_object_2)); query.Route(bounding_object_ptr_1, bounding_object_ptr_2); + } else if (command.name.compare("FindRoutes") == 0) { + const maliput::api::LaneId start_lane_id = LaneIdFromCLI(&(argv[2])); + const maliput::api::LanePosition start_lane_pos(SFromCLI(&(argv[3])), 0., 0.); + const maliput::api::LaneId end_lane_id = LaneIdFromCLI(&(argv[4])); + const maliput::api::LanePosition end_lane_pos(SFromCLI(&(argv[5])), 0., 0.); + const maliput::routing::RoutingConstraints constraints = RoutingConstraintsFromCLI(&(argv[6])); + const maliput::DistanceRouter router(*rn_ptr, rn_ptr->road_geometry()->linear_tolerance()); + query.FindRoutes(start_lane_id, start_lane_pos, end_lane_id, end_lane_pos, router, constraints); } return 0;