Program Listing for File EasyFullControl.hpp

↰ Return to documentation for file (include/rmf_fleet_adapter/agv/EasyFullControl.hpp)

/*
 * Copyright (C) 2023 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/

#ifndef RMF_FLEET_ADAPTER__AGV__EASYFULLCONTROL_HPP
#define RMF_FLEET_ADAPTER__AGV__EASYFULLCONTROL_HPP

// ROS 2 headers
#include <rclcpp/rclcpp.hpp>

// rmf_fleet_adapter headers
#include <rmf_fleet_adapter/agv/FleetUpdateHandle.hpp>
#include <rmf_fleet_adapter/agv/RobotUpdateHandle.hpp>
#include <rmf_fleet_adapter/agv/Transformation.hpp>

// rmf_battery headers
#include <rmf_battery/agv/BatterySystem.hpp>
#include <rmf_battery/agv/MechanicalSystem.hpp>
#include <rmf_battery/agv/SimpleMotionPowerSink.hpp>
#include <rmf_battery/agv/SimpleDevicePowerSink.hpp>

// rmf_traffic headers
#include <rmf_traffic/agv/Graph.hpp>
#include <rmf_traffic/agv/VehicleTraits.hpp>

#include <rmf_utils/impl_ptr.hpp>

// System headers
#include <Eigen/Geometry>

namespace rmf_fleet_adapter {
namespace agv {

//==============================================================================
class EasyFullControl : public std::enable_shared_from_this<EasyFullControl>
{
public:

  // Aliases
  using Graph = rmf_traffic::agv::Graph;
  using VehicleTraits = rmf_traffic::agv::VehicleTraits;
  using ActionExecutor = RobotUpdateHandle::ActionExecutor;
  using ActivityIdentifier = RobotUpdateHandle::ActivityIdentifier;
  using ActivityIdentifierPtr = RobotUpdateHandle::ActivityIdentifierPtr;
  using ConstActivityIdentifierPtr =
    RobotUpdateHandle::ConstActivityIdentifierPtr;
  using Stubbornness = RobotUpdateHandle::Unstable::Stubbornness;
  using ConsiderRequest = FleetUpdateHandle::ConsiderRequest;

  // Nested class declarations
  class EasyRobotUpdateHandle;
  class RobotState;
  class RobotConfiguration;
  class RobotCallbacks;
  class Destination;
  class FleetConfiguration;
  class CommandExecution;

  using NavigationRequest =
    std::function<void(
        Destination destination,
        CommandExecution execution)>;

  using StopRequest = std::function<void(ConstActivityIdentifierPtr)>;

  using LocalizationRequest = std::function<void(
        Destination location_estimate,
        CommandExecution execution)>;

  std::shared_ptr<EasyRobotUpdateHandle> add_robot(
    std::string name,
    RobotState initial_state,
    RobotConfiguration configuration,
    RobotCallbacks callbacks);

  std::shared_ptr<FleetUpdateHandle> more();

  std::shared_ptr<const FleetUpdateHandle> more() const;

  class Implementation;
private:
  EasyFullControl();
  rmf_utils::unique_impl_ptr<Implementation> _pimpl;
};

using EasyFullControlPtr = std::shared_ptr<EasyFullControl>;

class EasyFullControl::EasyRobotUpdateHandle
{
public:
  void update(
    RobotState state,
    ConstActivityIdentifierPtr current_activity);

  double max_merge_waypoint_distance() const;

  void set_max_merge_waypoint_distance(double distance);

  double max_merge_lane_distance() const;

  void set_max_merge_lane_distance(double distance);

  double min_lane_length() const;

  void set_min_lane_length(double length);

  std::shared_ptr<RobotUpdateHandle> more();

  std::shared_ptr<const RobotUpdateHandle> more() const;

  class Implementation;
private:
  EasyRobotUpdateHandle();
  rmf_utils::unique_impl_ptr<Implementation> _pimpl;
};

class EasyFullControl::RobotState
{
public:
  RobotState(
    std::string map_name,
    Eigen::Vector3d position,
    double battery_soc);

  const std::string& map() const;

  void set_map(std::string value);

  Eigen::Vector3d position() const;

  void set_position(Eigen::Vector3d value);

  double battery_state_of_charge() const;

  void set_battery_state_of_charge(double value);

  class Implementation;
private:
  rmf_utils::impl_ptr<Implementation> _pimpl;
};

class EasyFullControl::RobotConfiguration
{
public:

  RobotConfiguration(
    std::vector<std::string> compatible_chargers,
    std::optional<bool> responsive_wait = std::nullopt,
    std::optional<double> max_merge_waypoint_distance = 1e-3,
    std::optional<double> max_merge_lane_distance = 0.3,
    std::optional<double> min_lane_length = 1e-8);

  const std::vector<std::string>& compatible_chargers() const;

  void set_compatible_chargers(std::vector<std::string> chargers);

  std::optional<bool> responsive_wait() const;

  void set_responsive_wait(std::optional<bool> enable);

  std::optional<double> max_merge_waypoint_distance() const;

  void set_max_merge_waypoint_distance(std::optional<double> distance);

  std::optional<double> max_merge_lane_distance() const;

  void set_max_merge_lane_distance(std::optional<double> distance);

  std::optional<double> min_lane_length() const;

  void set_min_lane_length(std::optional<double> distance);

  class Implementation;
private:
  rmf_utils::impl_ptr<Implementation> _pimpl;
};

class EasyFullControl::RobotCallbacks
{
public:

  RobotCallbacks(
    NavigationRequest navigate,
    StopRequest stop,
    ActionExecutor action_executor);

  NavigationRequest navigate() const;

  StopRequest stop() const;

  ActionExecutor action_executor() const;

  RobotCallbacks& with_localization(LocalizationRequest localization);

  LocalizationRequest localize() const;

  class Implementation;
private:
  rmf_utils::impl_ptr<Implementation> _pimpl;
};

class EasyFullControl::CommandExecution
{
public:

  void finished();

  bool okay() const;

  Stubbornness override_schedule(
    std::string map,
    std::vector<Eigen::Vector3d> path,
    rmf_traffic::Duration hold = rmf_traffic::Duration(0));

  ConstActivityIdentifierPtr identifier() const;

  class Implementation;
private:
  CommandExecution();
  rmf_utils::impl_ptr<Implementation> _pimpl;
};

class EasyFullControl::Destination
{
public:
  const std::string& map() const;

  Eigen::Vector3d position() const;

  Eigen::Vector2d xy() const;

  double yaw() const;

  std::optional<std::size_t> graph_index() const;

  std::string name() const;

  std::optional<double> speed_limit() const;

  std::optional<std::string> dock() const;

  rmf_traffic::agv::Graph::LiftPropertiesPtr inside_lift() const;

  class Implementation;
private:
  Destination();
  rmf_utils::impl_ptr<Implementation> _pimpl;
};

class EasyFullControl::FleetConfiguration
{
public:

  FleetConfiguration(
    const std::string& fleet_name,
    std::optional<std::unordered_map<std::string, Transformation>>
    transformations_to_robot_coordinates,
    std::unordered_map<std::string, RobotConfiguration>
    known_robot_configurations,
    std::shared_ptr<const rmf_traffic::agv::VehicleTraits> traits,
    std::shared_ptr<const rmf_traffic::agv::Graph> graph,
    rmf_battery::agv::ConstBatterySystemPtr battery_system,
    rmf_battery::ConstMotionPowerSinkPtr motion_sink,
    rmf_battery::ConstDevicePowerSinkPtr ambient_sink,
    rmf_battery::ConstDevicePowerSinkPtr tool_sink,
    double recharge_threshold,
    double recharge_soc,
    bool account_for_battery_drain,
    std::unordered_map<std::string, ConsiderRequest> task_consideration,
    std::unordered_map<std::string, ConsiderRequest> action_consideration,
    rmf_task::ConstRequestFactoryPtr finishing_request = nullptr,
    bool skip_rotation_commands = true,
    std::optional<std::string> server_uri = std::nullopt,
    rmf_traffic::Duration max_delay = rmf_traffic::time::from_seconds(10.0),
    rmf_traffic::Duration update_interval = rmf_traffic::time::from_seconds(
      0.5),
    bool default_responsive_wait = false,
    double default_max_merge_waypoint_distance = 1e-3,
    double default_max_merge_lane_distance = 0.3,
    double min_lane_length = 1e-8
  );

  static std::optional<FleetConfiguration> from_config_files(
    const std::string& config_file,
    const std::string& nav_graph_path,
    std::optional<std::string> server_uri = std::nullopt);

  const std::string& fleet_name() const;

  void set_fleet_name(std::string value);

  const std::optional<std::unordered_map<std::string, Transformation>>&
  transformations_to_robot_coordinates() const;

  void add_robot_coordinate_transformation(
    std::string map,
    Transformation transformation);

  const std::unordered_map<std::string, RobotConfiguration>&
  known_robot_configurations() const;

  std::vector<std::string> known_robots() const;

  void add_known_robot_configuration(
    std::string robot_name,
    RobotConfiguration configuration);

  std::optional<RobotConfiguration> get_known_robot_configuration(
    const std::string& robot_name) const;

  const std::shared_ptr<const VehicleTraits>& vehicle_traits() const;

  void set_vehicle_traits(std::shared_ptr<const VehicleTraits> value);

  const std::shared_ptr<const Graph>& graph() const;

  void set_graph(std::shared_ptr<const Graph> value);

  rmf_battery::agv::ConstBatterySystemPtr battery_system() const;

  void set_battery_system(rmf_battery::agv::ConstBatterySystemPtr value);

  rmf_battery::ConstMotionPowerSinkPtr motion_sink() const;

  void set_motion_sink(rmf_battery::ConstMotionPowerSinkPtr value);

  rmf_battery::ConstDevicePowerSinkPtr ambient_sink() const;

  void set_ambient_sink(rmf_battery::ConstDevicePowerSinkPtr value);

  rmf_battery::ConstDevicePowerSinkPtr tool_sink() const;

  void set_tool_sink(rmf_battery::ConstDevicePowerSinkPtr value);

  double recharge_threshold() const;

  void set_recharge_threshold(double value);

  double recharge_soc() const;

  void set_recharge_soc(double value);

  bool account_for_battery_drain() const;

  void set_account_for_battery_drain(bool value);

  const std::unordered_map<std::string, ConsiderRequest>&
  task_consideration() const;

  std::unordered_map<std::string, ConsiderRequest>& task_consideration();

  const std::unordered_map<std::string, ConsiderRequest>&
  action_consideration() const;

  std::unordered_map<std::string, ConsiderRequest>& action_consideration();

  rmf_task::ConstRequestFactoryPtr finishing_request() const;

  void set_finishing_request(rmf_task::ConstRequestFactoryPtr value);

  bool skip_rotation_commands() const;

  void set_skip_rotation_commands(bool value);

  std::optional<std::string> server_uri() const;

  void set_server_uri(std::optional<std::string> value);

  rmf_traffic::Duration max_delay() const;

  void set_max_delay(rmf_traffic::Duration value);

  rmf_traffic::Duration update_interval() const;

  void set_update_interval(rmf_traffic::Duration value);

  bool default_responsive_wait() const;

  void set_default_responsive_wait(bool enable);

  double default_max_merge_waypoint_distance() const;

  void set_default_max_merge_waypoint_distance(double distance);

  double default_max_merge_lane_distance() const;

  void set_default_max_merge_lane_distance(double distance);

  double default_min_lane_length() const;

  void set_default_min_lane_length(double distance);

  void set_lift_emergency_level(
    std::string lift_name,
    std::string emergency_level_name);

  std::unordered_map<std::string, std::string>& change_lift_emergency_levels();

  const std::unordered_map<std::string, std::string>&
  lift_emergency_levels() const;

  class Implementation;
private:
  rmf_utils::impl_ptr<Implementation> _pimpl;
};

} // namespace agv
} // namespace rmf_fleet_adapter

#endif // RMF_FLEET_ADAPTER__AGV__EASYFULLCONTROL_HPP