diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index fe4f5b88a..f5263f7c0 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -420,7 +420,12 @@ void copy_booking_data( booking_json["unix_millis_request_time"] = to_millis(request_time.value().time_since_epoch()).count(); } - // TODO(MXG): Add priority and labels + const auto labels = booking.labels(); + if (labels.size() != 0) + { + booking_json["labels"] = booking.labels(); + } + // TODO(MXG): Add priority } //============================================================================== diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp index 1f279b1f9..c5f2e6515 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/agv/FleetUpdateHandle.cpp @@ -283,6 +283,11 @@ std::shared_ptr FleetUpdateHandle::Implementation::convert( requester = i_it->get(); } + std::vector labels = {}; + const auto labels_it = request_msg.find("labels"); + if (labels_it != request_msg.end()) + labels = labels_it->get>(); + rmf_task::Task::ConstBookingPtr booking = requester.has_value() ? std::make_shared( task_id, @@ -290,12 +295,14 @@ std::shared_ptr FleetUpdateHandle::Implementation::convert( priority, requester.value(), request_time, - false) : + false, + labels) : std::make_shared( task_id, earliest_start_time, priority, - false); + false, + labels); const auto new_request = std::make_shared( std::move(booking), deserialized_task.description); diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp index cd48937c2..1b397e1cd 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/phases/WaitForCharge.cpp @@ -94,6 +94,30 @@ WaitForCharge::Active::Active( .get_observable() .start_with(initial_msg); + // If the charging waypoint has a mutex group, release all other mutexes + std::unordered_set retain_mutexes; + const auto charging_waypoint = _context->dedicated_charging_wp(); + const auto& graph = _context->navigation_graph(); + retain_mutexes.insert( + graph.get_waypoint(charging_waypoint).in_mutex_group()); + _context->retain_mutex_groups(retain_mutexes); + + if (retain_mutexes.begin()->empty()) + { + RCLCPP_INFO( + _context->node()->get_logger(), + "Robot [%s] is waiting to charge. All its mutex groups will be released.", + _context->name().c_str()); + } + else + { + RCLCPP_INFO( + _context->node()->get_logger(), + "Robot [%s] is waiting to charge. It will retain only the mutex group [%s].", + _context->name().c_str(), + retain_mutexes.begin()->c_str()); + } + _context->current_mode(rmf_fleet_msgs::msg::RobotMode::MODE_CHARGING); } diff --git a/rmf_fleet_adapter_python/src/graph/graph.cpp b/rmf_fleet_adapter_python/src/graph/graph.cpp index 0aaf60172..ce17b8edd 100644 --- a/rmf_fleet_adapter_python/src/graph/graph.cpp +++ b/rmf_fleet_adapter_python/src/graph/graph.cpp @@ -195,7 +195,10 @@ void bind_graph(py::module& m) .def_property_readonly("num_lanes", &Graph::num_lanes) .def("lanes_from_waypoint", py::overload_cast(&Graph::lanes_from, py::const_), - py::arg("wp_index")); + py::arg("wp_index")) + + //Lifts + .def_property_readonly("all_known_lifts", &Graph::all_known_lifts); // PARSE GRAPH ============================================================== // Helper function to parse a graph from a yaml file