.. _program_listing_file__tmp_ws_src_rmf_simulation_rmf_robot_sim_common_include_rmf_robot_sim_common_slotcar_common.hpp:

Program Listing for File slotcar_common.hpp
===========================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_rmf_simulation_rmf_robot_sim_common_include_rmf_robot_sim_common_slotcar_common.hpp>` (``/tmp/ws/src/rmf_simulation/rmf_robot_sim_common/include/rmf_robot_sim_common/slotcar_common.hpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   /*
    * Copyright (C) 2020 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_BUILDING_SIM_COMMON__SLOTCAR_COMMON_HPP
   #define RMF_BUILDING_SIM_COMMON__SLOTCAR_COMMON_HPP
   
   #include <sstream>
   
   #include <rclcpp/rclcpp.hpp>
   #include <Eigen/Geometry>
   #include <tf2_ros/transform_broadcaster.h>
   
   #include <geometry_msgs/msg/transform_stamped.hpp>
   #include <rmf_fleet_msgs/msg/robot_mode.hpp>
   #include <rmf_fleet_msgs/msg/robot_state.hpp>
   #include <rmf_fleet_msgs/msg/path_request.hpp>
   #include <rmf_fleet_msgs/msg/pause_request.hpp>
   #include <rmf_fleet_msgs/msg/mode_request.hpp>
   #include <rmf_building_map_msgs/msg/building_map.hpp>
   
   namespace rmf_robot_sim_common {
   
   struct SlotcarTrajectory
   {
     SlotcarTrajectory(const Eigen::Isometry3d& _pose)
     : pose(_pose)
     {}
     SlotcarTrajectory() {}
     // positions
     Eigen::Isometry3d pose;
     // Maximum speed for the lane approaching this waypoint
     std::optional<double> approach_speed_limit;
   };
   
   // steering type constants
   enum class SteeringType
   {
     DIFF_DRIVE,
     ACKERMANN
   };
   
   class SlotcarCommon
   {
   public:
     struct UpdateResult
     {
       double v = 0.0; // Target displacement in X (forward)
       double w = 0.0; // Target displacement in yaw
       double target_linear_speed_now = 0.0;     // Target linear speed now
       double target_linear_speed_destination = 0.0; // Target linear speed at destination
       std::optional<double> max_speed = std::nullopt; // Maximum speed allowed while navigating
     };
   
     SlotcarCommon();
   
     rclcpp::Logger logger() const;
   
     template<typename SdfPtrT>
     void read_sdf(SdfPtrT& sdf);
   
     void set_model_name(const std::string& model_name);
   
     std::string model_name() const;
   
     void init_ros_node(const rclcpp::Node::SharedPtr node);
   
     UpdateResult update(const Eigen::Isometry3d& pose,
       const std::vector<Eigen::Vector3d>& obstacle_positions,
       const double time);
   
     bool emergency_stop(const std::vector<Eigen::Vector3d>& obstacle_positions,
       const Eigen::Vector3d& current_heading);
   
     std::array<double, 2> calculate_control_signals(const std::array<double,
       2>& curr_velocities,
       const std::pair<double, double>& displacements,
       const double dt,
       const double target_velocity_now = 0.0,
       const double target_velocity_at_dest = 0.0,
       const std::optional<double>& linear_speed_limit = std::nullopt) const;
   
     std::array<double, 2> calculate_joint_control_signals(
       const std::array<double, 2>& w_tire,
       const std::pair<double, double>& displacements,
       const double dt,
       const double target_linear_speed_now = 0.0,
       const double target_linear_speed_destination = 0.0,
       const std::optional<double>& linear_speed_limit = std::nullopt) const;
   
     void charge_state_cb(const std::string& name, bool selected);
   
     void publish_robot_state(const double time);
   
     Eigen::Vector3d get_lookahead_point() const;
   
     bool display_markers = false; // Ignition only: toggles display of waypoint and lookahead markers
   
     using PathRequestCallback =
       std::function<void(const rmf_fleet_msgs::msg::PathRequest::SharedPtr)>;
     void set_path_request_callback(PathRequestCallback cb)
     { _path_request_callback = cb; }
   
   private:
     // Parameters needed for power dissipation and charging calculations
     // Default values may be overriden using values from sdf file
     struct PowerParams
     {
       double nominal_voltage = 12; // V
       double nominal_capacity = 24; // Ah
       double charging_current = 2; // A
       double mass = 20;
       double inertia = 10;
       double friction_coefficient = 0.3;
       double nominal_power = 10;
     };
   
     struct ChargerWaypoint
     {
       double x;
       double y;
       ChargerWaypoint(double x, double y)
       : x(x), y(y)
       {
       }
     };
   
     // Constants for update rate of tf2 and robot_state topic
     static constexpr float TF2_RATE = 100.0;
     static constexpr float STATE_TOPIC_RATE = 2.0;
   
     // Initial distance threshold over which a fleet adapter error is reported
     static constexpr float INITIAL_DISTANCE_THRESHOLD = 1.0;
   
     rclcpp::Node::SharedPtr _ros_node;
   
     double _last_update_time = 0.0;
     double last_tf2_pub = 0.0;
     double last_topic_pub = 0.0;
     std::size_t _sequence = 0;
   
     std::vector<SlotcarTrajectory> trajectory;
     std::size_t _traj_wp_idx = 0;
   
     rmf_fleet_msgs::msg::PauseRequest pause_request;
   
     std::mutex _mutex;
   
     std::string _model_name;
     bool _emergency_stop = false;
     bool _adapter_error = false;
   
     bool _initialized_pose = false; // True if at least 1 call to update() has been made
     Eigen::Isometry3d _old_pose; // Pose at previous time step
     // Assumes robot is stationary upon initialization
     Eigen::Vector3d _old_lin_vel = Eigen::Vector3d::Zero(); // Linear velocity at previous time step
     double _old_ang_vel = 0.0; // Angular velocity at previous time step
     bool _was_rotating; // Whether robot was rotating towards its next target in previous time step
     Eigen::Isometry3d _pose; // Pose at current time step
     int _rot_dir = 1; // Current direction of rotation
   
     std::unordered_map<std::string, double> _level_to_elevation;
     bool _initialized_levels = false;
     std::string _last_known_level = "";
   
     std::shared_ptr<tf2_ros::TransformBroadcaster> _tf2_broadcaster;
     rclcpp::Publisher<rmf_fleet_msgs::msg::RobotState>::SharedPtr _robot_state_pub;
   
     rclcpp::Subscription<rmf_fleet_msgs::msg::PathRequest>::SharedPtr _traj_sub;
     rclcpp::Subscription<rmf_fleet_msgs::msg::PauseRequest>::SharedPtr _pause_sub;
     rclcpp::Subscription<rmf_fleet_msgs::msg::ModeRequest>::SharedPtr _mode_sub;
     rclcpp::Subscription<rmf_building_map_msgs::msg::BuildingMap>::SharedPtr
       _building_map_sub;
   
     rmf_fleet_msgs::msg::RobotMode _current_mode;
   
     SteeringType _steering_type = SteeringType::DIFF_DRIVE;
   
     std::string _current_task_id;
     std::vector<rmf_fleet_msgs::msg::Location> _remaining_path;
   
     // Vehicle dynamic constants
     // TODO(MXG): Consider fetching these values from model data
     // Radius of a tire
     double _tire_radius = 0.1;
     // Distance of a tire from the origin
     double _base_width = 0.52;
   
     double _nominal_drive_speed = 0.5;         // nominal robot velocity (m/s)
     double _nominal_drive_acceleration = 0.05; // nominal robot forward acceleration (m/s^2)
     double _max_drive_acceleration = 0.1;      // maximum robot forward acceleration (m/s^2)
   
     double _nominal_turn_speed = M_PI / 8.0;         // nominal robot turning speed (half a rotation per 8 seconds)
     double _nominal_turn_acceleration = M_PI / 10.0; // nominal robot turning acceleration (rad/s^2)
   
     double _max_turn_acceleration = M_PI; // maximum robot turning acceleration (rad/s^2)
   
     double _stop_distance = 1.0;
     double _stop_radius = 1.0;
   
     double _min_turning_radius = -1.0; // minimum turning radius, will use a formula if negative
     double _turning_right_angle_mul_offset = 1.0; // if _min_turning_radius is computed, this value multiplies it
   
     bool _reversible = true; // true if the robot can drive backwards
   
     PowerParams _params;
     bool _enable_charge = true;
     bool _enable_instant_charge = false;
     bool _enable_drain = true;
     // Used for comparing with name argument of charge_state_cb to identify button selected
     const std::string _enable_charge_str = "_enable_charge";
     const std::string _enable_instant_charge_str = "_enable_instant_charge";
     const std::string _enable_drain_str = "_enable_drain";
     const double _soc_max = 1.0;
     double _soc = _soc_max;
     std::unordered_map<std::string, std::vector<ChargerWaypoint>>
     _charger_waypoints;
     // Straight line distance to charging waypoint within which charging can occur
     static constexpr double _charger_dist_thres = 0.3;
   
     bool _docking = false;
   
     Eigen::Vector3d _lookahead_point;
     double _lookahead_distance = 8.0;
   
     PathRequestCallback _path_request_callback = nullptr;
   
     std::string get_level_name(const double z);
   
     double compute_change_in_rotation(
       const Eigen::Vector3d& heading_vec,
       const Eigen::Vector3d& dpos,
       const Eigen::Vector3d* traj_vec = nullptr,
       double* const dir = nullptr) const;
   
     void publish_tf2(const rclcpp::Time& t);
   
     void publish_state_topic(const rclcpp::Time& t);
   
     bool path_request_valid(
       const rmf_fleet_msgs::msg::PathRequest::SharedPtr msg);
   
     void path_request_cb(const rmf_fleet_msgs::msg::PathRequest::SharedPtr msg);
   
     void pause_request_cb(const rmf_fleet_msgs::msg::PauseRequest::SharedPtr msg);
   
     void mode_request_cb(const rmf_fleet_msgs::msg::ModeRequest::SharedPtr msg);
   
     void map_cb(const rmf_building_map_msgs::msg::BuildingMap::SharedPtr msg);
   
     UpdateResult update_diff_drive(
       const std::vector<Eigen::Vector3d>& obstacle_positions,
       const double time);
   
     UpdateResult update_ackermann(
       const std::vector<Eigen::Vector3d>& obstacle_positions,
       const double time);
   
     bool near_charger(const Eigen::Isometry3d& pose) const;
   
     double compute_charge(const double run_time) const;
   
     double compute_discharge(
       const Eigen::Vector3d lin_vel, const double ang_vel,
       const Eigen::Vector3d lin_acc, const double ang_acc,
       const double run_time) const;
   };
   
   template<typename SdfPtrT, typename valueT>
   bool get_element_val_if_present(
     SdfPtrT& _sdf,
     const std::string& _element_name,
     valueT& _val)
   {
     if (!_sdf->HasElement(_element_name))
     {
       return false;
     }
     _val = _sdf->template Get<valueT>(_element_name);
     return true;
   }
   
   template<typename SdfPtrT>
   void SlotcarCommon::read_sdf(SdfPtrT& sdf)
   {
     std::string steering_type;
     get_element_val_if_present<SdfPtrT, std::string>(sdf, "steering",
       steering_type);
   
     if (steering_type == "ackermann")
     {
       _steering_type = SteeringType::ACKERMANN;
       _reversible = false;
     }
     else if (steering_type == "diff_drive")
     {
       _steering_type = SteeringType::DIFF_DRIVE;
     }
   
     RCLCPP_INFO(
       logger(),
       "Vehicle uses %s steering", steering_type.c_str());
   
     get_element_val_if_present<SdfPtrT, double>(sdf, "nominal_drive_speed",
       this->_nominal_drive_speed);
     RCLCPP_INFO(
       logger(),
       "Setting nominal drive speed to: %f",
       _nominal_drive_speed);
   
     get_element_val_if_present<SdfPtrT, double>(sdf,
       "nominal_drive_acceleration", this->_nominal_drive_acceleration);
     RCLCPP_INFO(
       logger(),
       "Setting nominal drive acceleration to: %f",
       _nominal_drive_acceleration);
   
     get_element_val_if_present<SdfPtrT, double>(sdf,
       "max_drive_acceleration", this->_max_drive_acceleration);
     RCLCPP_INFO(
       logger(),
       "Setting max drive acceleration to: %f",
       _max_drive_acceleration);
   
     get_element_val_if_present<SdfPtrT, double>(sdf,
       "nominal_turn_speed", this->_nominal_turn_speed);
     RCLCPP_INFO(
       logger(),
       "Setting nominal turn speed to: %f",
       _nominal_turn_speed);
   
     get_element_val_if_present<SdfPtrT, double>(sdf,
       "nominal_turn_acceleration", this->_nominal_turn_acceleration);
     RCLCPP_INFO(
       logger(),
       "Setting nominal turn acceleration to: %f",
       _nominal_turn_acceleration);
   
     get_element_val_if_present<SdfPtrT, double>(sdf,
       "max_turn_acceleration", this->_max_turn_acceleration);
     RCLCPP_INFO(
       logger(),
       "Setting max turn acceleration to: %f",
       _max_turn_acceleration);
   
     get_element_val_if_present<SdfPtrT, double>(sdf,
       "min_turning_radius", this->_min_turning_radius);
     RCLCPP_INFO(
       logger(),
       "Setting minimum turning radius to: %f",
       _min_turning_radius);
   
     get_element_val_if_present<SdfPtrT, double>(sdf,
       "turning_right_angle_mul_offset", this->_turning_right_angle_mul_offset);
     RCLCPP_INFO(
       logger(),
       "Setting turning right angle multiplier offset to: %f",
       _turning_right_angle_mul_offset);
   
     get_element_val_if_present<SdfPtrT, double>(sdf,
       "stop_distance", this->_stop_distance);
     RCLCPP_INFO(logger(), "Setting stop distance to: %f", _stop_distance);
   
     get_element_val_if_present<SdfPtrT, double>(sdf,
       "stop_radius", this->_stop_radius);
     RCLCPP_INFO(logger(), "Setting stop radius to: %f", _stop_radius);
   
     get_element_val_if_present<SdfPtrT, double>(sdf,
       "tire_radius", this->_tire_radius);
     RCLCPP_INFO(logger(), "Setting tire radius to: %f", _tire_radius);
   
     get_element_val_if_present<SdfPtrT, double>(sdf,
       "base_width", this->_base_width);
     RCLCPP_INFO(logger(), "Setting base width to: %f", _base_width);
   
     get_element_val_if_present<SdfPtrT, bool>(sdf,
       "reversible", this->_reversible);
     RCLCPP_INFO(logger(), "Setting reversible to: %d", _reversible);
   
     get_element_val_if_present<SdfPtrT, double>(sdf,
       "nominal_voltage", this->_params.nominal_voltage);
     RCLCPP_INFO(
       logger(),
       "Setting nominal voltage to: %f",
       _params.nominal_voltage);
   
     get_element_val_if_present<SdfPtrT, double>(sdf,
       "nominal_capacity", this->_params.nominal_capacity);
     RCLCPP_INFO(
       logger(),
       "Setting nominal capacity to: %f",
       _params.nominal_capacity);
   
     get_element_val_if_present<SdfPtrT, double>(sdf,
       "charging_current", this->_params.charging_current);
     RCLCPP_INFO(
       logger(),
       "Setting charging current to: %f",
       _params.charging_current);
   
     get_element_val_if_present<SdfPtrT, double>(sdf,
       "mass", this->_params.mass);
     RCLCPP_INFO(logger(), "Setting mass to: %f", _params.mass);
   
     get_element_val_if_present<SdfPtrT, double>(sdf,
       "inertia", this->_params.inertia);
     RCLCPP_INFO(
       logger(),
       "Setting inertia to: %f",
       _params.inertia);
   
     get_element_val_if_present<SdfPtrT, double>(sdf,
       "friction_coefficient", this->_params.friction_coefficient);
     RCLCPP_INFO(
       logger(),
       "Setting friction coefficient to: %f",
       _params.friction_coefficient);
   
     get_element_val_if_present<SdfPtrT, double>(sdf,
       "nominal_power", this->_params.nominal_power);
     RCLCPP_INFO(
       logger(),
       "Setting nominal power to: %f",
       _params.nominal_power);
   
     get_element_val_if_present<SdfPtrT, double>(sdf,
       "lookahead_distance", this->_lookahead_distance);
     RCLCPP_INFO(
       logger(),
       "Setting lookahead distance to: %f",
       _lookahead_distance);
   
     get_element_val_if_present<SdfPtrT, bool>(sdf,
       "display_markers", this->display_markers);
     RCLCPP_INFO(logger(), "Setting display_markers to: %d", display_markers);
   
     // Charger Waypoint coordinates are in child element of top level world element
     if (sdf->GetParent() && sdf->GetParent()->GetParent())
     {
       auto parent = sdf->GetParent()->GetParent();
       if (parent->HasElement("rmf_charger_waypoints"))
       {
         auto waypoints = parent->GetElement("rmf_charger_waypoints");
         if (waypoints->HasElement("rmf_vertex"))
         {
           auto waypoint = waypoints->GetElement("rmf_vertex");
           while (waypoint)
           {
             if (waypoint->HasAttribute("x") && waypoint->HasAttribute("y") &&
               waypoint->HasAttribute("level"))
             {
               std::string lvl_name;
               double x, y;
               waypoint->GetAttribute("x")->Get(x);
               waypoint->GetAttribute("y")->Get(y);
               waypoint->GetAttribute("level")->Get(lvl_name);
               _charger_waypoints[lvl_name].push_back(ChargerWaypoint(x, y));
             }
             waypoint = waypoint->GetNextElement("rmf_vertex");
           }
         }
       }
       else
       {
         RCLCPP_INFO(logger(), "No charger waypoints found.");
       }
     }
   
     RCLCPP_INFO(logger(), "Setting name to: %s", _model_name.c_str());
   }
   } // namespace rmf_robot_sim_common
   
   #endif // RMF_BUILDING_SIM_COMMON__SLOTCAR_COMMON_HPP