diff --git a/rmf_fleet_adapter_python/CMakeLists.txt b/rmf_fleet_adapter_python/CMakeLists.txt index a70e6a6c5..3d4070b2c 100644 --- a/rmf_fleet_adapter_python/CMakeLists.txt +++ b/rmf_fleet_adapter_python/CMakeLists.txt @@ -5,9 +5,9 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS on) find_package(ament_cmake REQUIRED) -# Default to C++14 +# Default to C++17 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/rmf_fleet_adapter_python/src/adapter.cpp b/rmf_fleet_adapter_python/src/adapter.cpp index b8c75979b..bee9ebf60 100644 --- a/rmf_fleet_adapter_python/src/adapter.cpp +++ b/rmf_fleet_adapter_python/src/adapter.cpp @@ -123,6 +123,7 @@ PYBIND11_MODULE(rmf_adapter, m) { [&](agv::RobotUpdateHandle& self){ return self.unstable().get_participant(); }, + py::return_value_policy::reference_internal, "Experimental API to access the schedule participant"); // FLEETUPDATE HANDLE ====================================================== @@ -275,7 +276,12 @@ PYBIND11_MODULE(rmf_adapter, m) { py::class_ >(m, "Node") .def("now", [](rclcpp::Node& self){ return rmf_traffic_ros2::convert(self.now()); - }); + }) + .def("use_sim_time", [](rclcpp::Node& self) + { + rclcpp::Parameter param("use_sim_time", true); + self.set_parameter(param); + }); // Python rclcpp init and spin call m.def("init_rclcpp", [](){ rclcpp::init(0, nullptr); }); diff --git a/rmf_fleet_adapter_python/src/graph/graph.cpp b/rmf_fleet_adapter_python/src/graph/graph.cpp index 2fa021715..d5394ce48 100644 --- a/rmf_fleet_adapter_python/src/graph/graph.cpp +++ b/rmf_fleet_adapter_python/src/graph/graph.cpp @@ -168,10 +168,15 @@ void bind_graph(py::module &m) { .def("get_lane", py::overload_cast(&Graph::get_lane)) .def("get_lane", py::overload_cast( &Graph::get_lane, py::const_)) - .def("lane_from", py::overload_cast( - &Graph::lane_from)) - .def("lane_from", py::overload_cast( - &Graph::lane_from, py::const_)) + .def( + "lane_from", + py::overload_cast(&Graph::lane_from), + py::return_value_policy::reference_internal) + .def( + "lane_from", + py::overload_cast( + &Graph::lane_from, py::const_), + py::return_value_policy::reference_internal) .def_property_readonly("num_lanes", &Graph::num_lanes) .def("lanes_from_waypoint", py::overload_cast(&Graph::lanes_from, py::const_), diff --git a/rmf_fleet_adapter_python/src/planner/planner.cpp b/rmf_fleet_adapter_python/src/planner/planner.cpp index 3eb3e7387..db7cddf7d 100644 --- a/rmf_fleet_adapter_python/src/planner/planner.cpp +++ b/rmf_fleet_adapter_python/src/planner/planner.cpp @@ -18,6 +18,9 @@ using Goal = Planner::Goal; using Options = Planner::Options; using Configuration = Planner::Configuration; using Result = Planner::Result; +using Graph = rmf_traffic::agv::Graph; +using VehicleTraits = rmf_traffic::agv::VehicleTraits; +using Interpolate = rmf_traffic::agv::Interpolate; using TimePoint = std::chrono::time_point; @@ -45,6 +48,12 @@ Start make_start(TimePoint initial_time, initial_lane); } +Planner make_planner(Configuration config) +{ + const auto default_options = Options{nullptr}; + return Planner(config, default_options); +} + void bind_plan(py::module &m) { auto m_plan = m.def_submodule("plan"); @@ -144,4 +153,41 @@ void bind_plan(py::module &m) { py::overload_cast<>(&Plan::Goal::orientation, py::const_), py::overload_cast(&Plan::Goal::orientation)) .def("any_orientation", &Plan::Goal::any_orientation); + + // Configuration ============================================================= + py::class_(m_plan, "Configuration") + .def(py::init(), + py::arg("graph"), + py::arg("traits")) + .def_property_readonly("graph", &Configuration::graph) + .def_property_readonly("traits", &Configuration::vehicle_traits); + + // Options ============================================================= + // TODO + + // Planner =================================================================== + py::class_(m_plan, "Planner") + .def(py::init([&](Configuration& config) + { + return new Planner(config, Planner::Options{nullptr} ); + }), + py::arg("config")) + .def("get_plan_waypoints", + [&](Planner& self, + Start start, + Goal goal) + { + std::vector waypoints; + const auto result = self.plan(start, goal); + if (result.success()) + { + waypoints = result->get_waypoints(); + } + + return waypoints; + + }, + py::arg("start"), py::arg("goal"), + py::return_value_policy::reference_internal); + } diff --git a/rmf_fleet_adapter_python/tests/unit/test_planner.py b/rmf_fleet_adapter_python/tests/unit/test_planner.py new file mode 100644 index 000000000..bcb3549e2 --- /dev/null +++ b/rmf_fleet_adapter_python/tests/unit/test_planner.py @@ -0,0 +1,77 @@ +#! /usr/bin/env python3 + +import rmf_adapter.vehicletraits as traits +import rmf_adapter.geometry as geometry +import rmf_adapter.graph as graph +import rmf_adapter.graph.lane as lane +import rmf_adapter.plan as plan + +import datetime + +""" + 10 + | + | + 8------9 + | | + | | + 3------4------5------6------7 + | | + | | + 1------2 + | + | + 0 +""" +map_name = "test_map" +nav_graph = graph.Graph() +nav_graph.add_waypoint(map_name, [0.0, -10.0]) # 0 +nav_graph.add_waypoint(map_name, [0.0, -5.0]) # 1 +nav_graph.add_waypoint(map_name, [5.0, -5.0]) # 2 +nav_graph.add_waypoint(map_name, [-10.0, 0]) # 3 +nav_graph.add_waypoint(map_name, [-5.0, 0.0]) # 4 +nav_graph.add_waypoint(map_name, [0.0, 0.0]) # 5 +nav_graph.add_waypoint(map_name, [5.0, 0.0]) # 6 +nav_graph.add_waypoint(map_name, [10.0, 0.0]) # 7 +nav_graph.add_waypoint(map_name, [0.0, 5.0]) \ + .set_holding_point(True) # 8 +nav_graph.add_waypoint(map_name, [5.0, 5.0]) \ + .set_passthrough_point(True) # 9 +nav_graph.add_waypoint(map_name, [0.0, 10.0]) \ + .set_parking_spot(True) # 10 + +lanes = [[0, 1], + [1, 2], + [1, 5], + [2, 6], + [3, 4], + [4, 5], + [5, 6], + [6, 7], + [5, 8], + [6, 9], + [8, 9], + [8, 10]] +for _lane in lanes: + nav_graph.add_bidir_lane(*_lane) +assert nav_graph.num_lanes == 24 + +profile = traits.Profile(geometry.make_final_convex_circle(1.0), + geometry.make_final_convex_circle(1.5)) +vehicle_traits = traits.VehicleTraits( + linear=traits.Limits(0.5, 0.3), + angular=traits.Limits(0.3, 0.4), + profile=profile) +vehicle_traits.differential.reversible = False + +config = plan.Configuration(nav_graph, vehicle_traits) +planner = plan.Planner(config) +assert(planner) + +start = plan.Start( + datetime.datetime.today(), + 0, + 0.0) +goal = plan.Goal(7) +waypoints = planner.get_plan_waypoints(start, goal) +assert(waypoints)