Develop/task planner integration

rmf_battery/CMakeLists.txt

@@ -15,8 +15,8 @@ if(NOT CMAKE_BUILD_TYPE)
   set(CMAKE_BUILD_TYPE Release)
 endif()
 
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fsanitize=address")
-set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -fsanitize=address")
+# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fsanitize=address")
+# set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -fsanitize=address")
 
 include(GNUInstallDirs)
 

rmf_battery/src/rmf_battery/agv/SimpleMotionPowerSink.cpp

@@ -105,11 +105,11 @@ double SimpleMotionPowerSink::compute_change_in_charge(
   {
     const Eigen::Vector3d velocity = motion->compute_velocity(sim_time);
     const double v = sqrt(pow(velocity[0], 2) + pow(velocity[1], 2));
-    const double w = velocity[2];
+    const double w = std::abs(velocity[2]);
 
     const Eigen::Vector3d acceleration = motion->compute_acceleration(sim_time);
     const double a = sqrt(pow(acceleration[0], 2) + pow(acceleration[1], 2));
-    const double alpha = acceleration[2];
+    const double alpha = std::abs(acceleration[2]);
 
     // Loss through acceleration
     const double EA = ((mass * a * v) + (inertia * alpha * w)) * sim_step;

rmf_battery/test/unit/agv/test_battery_drain.cpp

@@ -241,3 +241,56 @@ SCENARIO("Test SimpleBatteryEstimator with RobotB")
     CHECK(ok);
   }
 }
+
+SCENARIO("Testing Cleaning Request")
+{
+  using BatterySystem = rmf_battery::agv::BatterySystem;
+  using MechanicalSystem = rmf_battery::agv::MechanicalSystem;
+  using PowerSystem = rmf_battery::agv::PowerSystem;
+  using SimpleMotionPowerSink = rmf_battery::agv::SimpleMotionPowerSink;
+  using SimpleDevicePowerSink = rmf_battery::agv::SimpleDevicePowerSink;
+  using namespace std::chrono_literals;
+
+  // Initializing system traits
+  BatterySystem battery_system{24, 40, 8.8};
+  REQUIRE(battery_system.valid());
+  MechanicalSystem mechanical_system{70, 40, 0.22};
+  REQUIRE(mechanical_system.valid());
+  PowerSystem power_system_1{"processor", 20};
+  REQUIRE(power_system_1.valid());
+  SimpleMotionPowerSink motion_power_sink{battery_system, mechanical_system};
+  SimpleDevicePowerSink device_power_sink{battery_system, power_system_1};
+
+  // Initializing vehicle traits
+  const rmf_traffic::agv::VehicleTraits traits(
+    {0.7, 0.5}, {0.4, 1.0}, {nullptr, nullptr});
+
+  const double initial_soc = 1.0;
+
+  WHEN("Computing invariant drain for zone_3")
+  {
+    const auto start_time = std::chrono::steady_clock::now();
+    const std::vector<Eigen::Vector3d> positions = {
+      Eigen::Vector3d{104.0, -46.92, -M_PI/2.0},
+      Eigen::Vector3d{104.0, -48.55, 0.0},
+      Eigen::Vector3d{159.8, -48.38, M_PI/2.0},
+      Eigen::Vector3d{159.8, -46.73, M_PI},
+      Eigen::Vector3d{105.4, -47.04, M_PI/2.0},
+      Eigen::Vector3d{105.5, -45.37, 0.0},
+      Eigen::Vector3d{159.8, -45.25, 0.0},
+      Eigen::Vector3d{155.0, -46.79, -M_PI/2.0},
+    };
+    rmf_traffic::Trajectory trajectory =
+      rmf_traffic::agv::Interpolate::positions(traits, start_time, positions);
+
+    const double dSOC_motion = motion_power_sink.compute_change_in_charge(
+      trajectory);
+    const double dSOC_device = device_power_sink.compute_change_in_charge(
+      rmf_traffic::time::to_seconds(trajectory.duration()));
+
+    // std::cout << "Motion: " << dSOC_motion << "Device: " << dSOC_device << std::endl;
+
+    const double remaining_soc = initial_soc - dSOC_motion - dSOC_device;
+    REQUIRE(remaining_soc <= 1.0);
+  }
+}

rmf_fleet_adapter/CMakeLists.txt

@@ -37,6 +37,8 @@ set(dep_pkgs
   rmf_task_msgs
   rmf_traffic
   rmf_traffic_ros2
+  rmf_battery
+  rmf_task
   std_msgs
 )
 foreach(pkg ${dep_pkgs})
@@ -58,6 +60,8 @@ add_library(rmf_fleet_adapter SHARED
 target_link_libraries(rmf_fleet_adapter
   PUBLIC
     rmf_traffic_ros2::rmf_traffic_ros2
+    rmf_battery::rmf_battery
+    rmf_task::rmf_task
     yaml-cpp
     ${rmf_fleet_msgs_LIBRARIES}
     ${rclcpp_LIBRARIES}
@@ -78,6 +82,8 @@ target_include_directories(rmf_fleet_adapter
     ${rmf_fleet_msgs_INCLUDE_DIRS}
     ${rclcpp_INCLUDE_DIRS}
     ${rmf_task_msgs_INCLUDE_DIRS}
+    ${rmf_battery_INCLUDE_DIRS}
+    ${rmf_task_INCLUDE_DIRS}
   PRIVATE
     ${rmf_door_msgs_INCLUDE_DIRS}
     ${rmf_lift_msgs_INCLUDE_DIRS}
@@ -158,11 +164,13 @@ target_link_libraries(full_control
   PRIVATE
     rmf_fleet_adapter
     ${rmf_fleet_msgs_LIBRARIES}
+    ${rmf_task_msgs_LIBRARIES}
 )
 
 target_include_directories(full_control
   PRIVATE
     ${rmf_fleet_msgs_INCLUDE_DIRS}
+    ${rmf_task_msgs_INCLUDE_DIRS}
 )
 
 # -----------------------------------------------------------------------------

rmf_fleet_adapter/include/rmf_fleet_adapter/StandardNames.hpp

@@ -49,6 +49,12 @@ const std::string DeliveryTopicName = "delivery_requests";
 const std::string LoopRequestTopicName = "loop_requests";
 const std::string TaskSummaryTopicName = "task_summaries";
 
+const std::string BidNoticeTopicName = "rmf_task/bid_notice";
+const std::string BidProposalTopicName = "rmf_task/bid_proposal";
+const std::string DispatchRequestTopicName = "rmf_task/dispatch_request";
+
+const std::string DockSummaryTopicName = "dock_summary";
+
 } // namespace rmf_fleet_adapter
 
 #endif // RMF_FLEET_ADAPTER__STANDARDNAMES_HPP

rmf_fleet_adapter/include/rmf_fleet_adapter/agv/FleetUpdateHandle.hpp

@@ -22,6 +22,11 @@
 #include <rmf_fleet_adapter/agv/RobotCommandHandle.hpp>
 
 #include <rmf_task_msgs/msg/delivery.hpp>
+#include <rmf_task_msgs/msg/task_profile.hpp>
+
+#include <rmf_battery/agv/BatterySystem.hpp>
+#include <rmf_battery/DevicePowerSink.hpp>
+#include <rmf_battery/MotionPowerSink.hpp>
 
 namespace rmf_fleet_adapter {
 namespace agv {
@@ -63,6 +68,64 @@ class FleetUpdateHandle : public std::enable_shared_from_this<FleetUpdateHandle>
       rmf_traffic::agv::Plan::StartSet start,
       std::function<void(std::shared_ptr<RobotUpdateHandle> handle)> handle_cb);
 
+  /// Set the parameters required for task planning
+  ///
+  /// \param[in] battery_system
+  ///   Specify the battery system used by the vehicles in this fleet.
+  ///
+  /// \param[in] motion_sink
+  ///   Specify the motion sink that describes the vehicles in this fleet.
+  ///
+  /// \param[in] ambient_sink
+  ///   Specify the device sink for ambient sensors used by the vehicles in this fleet.
+  ///
+  /// \param[in] tool_sink
+  ///   Specify the device sink for special tools used by the vehicles in this fleet.
+  ///
+  /// \param[in] drain_battery
+  ///   If false, battery drain will not be considered when planning for tasks.
+  ///   As a consequence, charging tasks will not be automatically assigned to
+  ///   vehicles in this fleet when battery levels fall below their thresholds.
+  ///
+  /// \return true if task planner parameters were successfully updated.
+  bool set_task_planner_params(
+    std::shared_ptr<rmf_battery::agv::BatterySystem> battery_system,
+    std::shared_ptr<rmf_battery::MotionPowerSink> motion_sink,
+    std::shared_ptr<rmf_battery::DevicePowerSink> ambient_sink,
+    std::shared_ptr<rmf_battery::DevicePowerSink> tool_sink,
+    const bool drain_battery);
+
+
+  /// Set the threshold state of charge below which the robot should 
+  /// automatically head back to its charging dock. The user is responsible to 
+  /// set this value such that the robot is capable of reaching its nearest
+  /// charging station from anywhere on the map. Default value is 0.2.
+  ///
+  /// \param[in] threshold
+  ///   The fraction of the total battery capacity
+  FleetUpdateHandle& set_recharge_threshold(const double threshold);
+
+  /// A callback function that evaluates whether a fleet will accept a task
+  /// request
+  ///
+  /// \param[in] request
+  ///   Information about the task request that is being considered.
+  ///
+  /// \return true to indicate that this fleet should accept the request, false
+  /// to reject the request.
+  using AcceptTaskRequest =
+      std::function<bool(const rmf_task_msgs::msg::TaskProfile& profile)>;
+
+  /// Provide a callback that indicates whether this fleet will accept a
+  /// BidNotice request. By default all requests will be rejected.
+  ///
+  /// \note The callback function that you give should ideally be non-blocking
+  /// and return quickly. It's meant to check whether this fleet's vehicles are
+  /// compatible with the requested payload, pickup, and dropoff behavior
+  /// settings. The path planning feasibility will be taken care of by the
+  /// adapter internally.
+  FleetUpdateHandle& accept_task_requests(AcceptTaskRequest check);
+
   /// A callback function that evaluates whether a fleet will accept a delivery
   /// request.
   ///
@@ -71,6 +134,9 @@ class FleetUpdateHandle : public std::enable_shared_from_this<FleetUpdateHandle>
   ///
   /// \return true to indicate that this fleet should accept the request, false
   /// to reject the request.
+  ///
+  /// \note This interface will be deprecated. Use the more general
+  ///   AcceptTaskRequest callback
   using AcceptDeliveryRequest =
       std::function<bool(const rmf_task_msgs::msg::Delivery& request)>;
 

rmf_fleet_adapter/include/rmf_fleet_adapter/agv/RobotUpdateHandle.hpp

@@ -88,6 +88,10 @@ class RobotUpdateHandle
       const double max_merge_lane_distance = 1.0,
       const double min_lane_length = 1e-8);
 
+  /// Update the current battery level of the robot by specifying its state of
+  /// charge as a fraction of its total charge capacity
+  void update_battery_soc(const double battery_soc);
+
   /// Specify how high the delay of the current itinerary can become before it
   /// gets interrupted and replanned. A nullopt value will allow for an
   /// arbitrarily long delay to build up without being interrupted.

rmf_fleet_adapter/launch/fleet_adapter.launch.xml

@@ -24,6 +24,20 @@
   <arg name="reversible" default="true" description="Can the robot drive backwards"/>
   <arg name="output" default="screen"/>
 
+  <arg name="perform_loop" default="false" description="Whether this fleet adapter can perform loops"/>
+  <arg name="perform_cleaning" default="false" description="Whether this fleet adapter can perform cleaning"/>
+  <arg name="battery_voltage" description="The nominal voltage(V) of the battery in the vehicles in this fleet"/>
+  <arg name="battery_capacity" description="The nominal capacity(Ah) of the battery in the vehicles in this fleet"/>
+  <arg name="battery_charging_current" description="The nominal charging current(A) of the battery in the vehicles in this fleet"/>
+  <arg name="mass" description="The mass(kg) of the vehicles in this fleet"/>
+  <arg name="inertia" description="The inertia(kg.m^2) of the vehicles in this fleet"/>
+  <arg name="friction_coefficient" description="The friction coefficient of the vehicles in this fleet"/>
+  <arg name="ambient_power_drain" description="The power rating(W) of ambient devices (processors, sensors, etc.) of the vehicles in this fleet"/>
+  <arg name="tool_power_drain" description="The power rating(W) of special tools (vaccuum, cleaning systems, etc.) of the vehicles in this fleet"/>
+  <arg name="drain_battery" default="false" description="Whether battery drain should be considered while assigning tasks to vechiles in this fleet"/>
+  <arg name="recharge_threshold" default="0.2" description="The fraction of total battery capacity below which the robot must return to its charger"/>
+
+
   <node pkg="rmf_fleet_adapter"
         exec="$(var control_type)"
         name="$(var fleet_name)_fleet_adapter"
@@ -43,13 +57,26 @@
     <param name="vicinity_radius" value="$(var vicinity_radius)"/>
 
     <param name="perform_deliveries" value="$(var perform_deliveries)"/>
+    <param name="perform_loop" value="$(var perform_loop)"/>
+    <param name="perform_cleaning" value="$(var perform_cleaning)"/>
 
     <param name="delay_threshold" value="$(var delay_threshold)"/>
     <param name="disable_delay_threshold" value="$(var disable_delay_threshold)"/>
     <param name="retry_wait" value="$(var retry_wait)"/>
     <param name="discovery_timeout" value="$(var discovery_timeout)"/>
     <param name="reversible" value="$(var reversible)"/>
 
+    <param name="battery_voltage" value="$(var battery_voltage)"/>
+    <param name="battery_capacity" value="$(var battery_capacity)"/>
+    <param name="battery_charging_current" value="$(var battery_charging_current)"/>
+    <param name="mass" value="$(var mass)"/>
+    <param name="inertia" value="$(var inertia)"/>
+    <param name="friction_coefficient" value="$(var friction_coefficient)"/>
+    <param name="ambient_power_drain" value="$(var ambient_power_drain)"/>
+    <param name="tool_power_drain" value="$(var tool_power_drain)"/>
+    <param name="drain_battery" value="$(var drain_battery)"/>
+    <param name="recharge_threshold" value="$(var recharge_threshold)"/> 
+
     <param name="use_sim_time" value="$(var use_sim_time)"/>
   </node>
 

rmf_fleet_adapter/package.xml

@@ -21,6 +21,8 @@
   <depend>rmf_traffic</depend>
   <depend>rmf_traffic_ros2</depend>
   <depend>rmf_traffic_msgs</depend>
+  <depend>rmf_battery</depend>
+  <depend>rmf_task</depend>
 
   <build_depend>eigen</build_depend>
   <build_depend>yaml-cpp</build_depend>