Merge branch 'main' into commission_api

open-rmf · Apr 8, 2024 · 11cc303 · 11cc303
2 parents 021e692 + 0685ec2
commit 11cc303
Show file tree

Hide file tree

Showing 4 changed files with 43 additions and 4 deletions.
diff --git 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
@@ -283,19 +283,26 @@ std::shared_ptr<rmf_task::Request> FleetUpdateHandle::Implementation::convert(
     requester = i_it->get<std::string>();
   }
 
+  std::vector<std::string> labels = {};
+  const auto labels_it = request_msg.find("labels");
+  if (labels_it != request_msg.end())
+    labels = labels_it->get<std::vector<std::string>>();
+
   rmf_task::Task::ConstBookingPtr booking = requester.has_value() ?
     std::make_shared<const rmf_task::Task::Booking>(
     task_id,
     earliest_start_time,
     priority,
     requester.value(),
     request_time,
-    false) :
+    false,
+    labels) :
     std::make_shared<const rmf_task::Task::Booking>(
     task_id,
     earliest_start_time,
     priority,
-    false);
+    false,
+    labels);
   const auto new_request = std::make_shared<rmf_task::Request>(
     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
@@ -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<std::string> 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
@@ -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<std::size_t>(&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