.. _program_listing_file__tmp_ws_src_rmf_simulation_rmf_robot_sim_common_include_rmf_robot_sim_common_readonly_common.hpp:

Program Listing for File readonly_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_readonly_common.hpp>` (``/tmp/ws/src/rmf_simulation/rmf_robot_sim_common/include/rmf_robot_sim_common/readonly_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_ROBOT_SIM_COMMON__READONLY_COMMON_HPP
   #define RMF_ROBOT_SIM_COMMON__READONLY_COMMON_HPP
   
   #include <rclcpp/rclcpp.hpp>
   #include <rclcpp/logger.hpp>
   
   #include <memory>
   #include <unordered_map>
   #include <unordered_set>
   #include <mutex>
   #include <string>
   
   #include <Eigen/Geometry>
   
   #include <rmf_fleet_msgs/msg/robot_state.hpp>
   #include <rmf_building_map_msgs/msg/building_map.hpp>
   #include <rmf_building_map_msgs/msg/level.hpp>
   #include <rmf_building_map_msgs/msg/graph.hpp>
   
   namespace rmf_readonly_common {
   
   class ReadonlyCommon
   {
   public:
   
     using BuildingMap = rmf_building_map_msgs::msg::BuildingMap;
     using Level = rmf_building_map_msgs::msg::Level;
     using Graph = rmf_building_map_msgs::msg::Graph;
     using Location = rmf_fleet_msgs::msg::Location;
     using Path = std::vector<Location>;
   
     rclcpp::Node::SharedPtr ros_node;
   
     void set_name(const std::string& name);
     std::string get_name() const;
     rclcpp::Logger logger();
     template<typename SdfPtrT>
     void read_sdf(SdfPtrT& sdf);
     void init(rclcpp::Node::SharedPtr node);
     void on_update(Eigen::Isometry3d& pose, double sim_time);
   
   private:
     std::string _name = "caddy"; // Placeholder
   
     // Updated in each on_update() call
     Eigen::Isometry3d _pose;
     double _sim_time = 0.0;
   
     rclcpp::Publisher<rmf_fleet_msgs::msg::RobotState>::SharedPtr _robot_state_pub;
     rclcpp::Subscription<BuildingMap>::SharedPtr _building_map_sub;
   
     rmf_fleet_msgs::msg::RobotState _robot_state_msg;
     rmf_fleet_msgs::msg::RobotMode _current_mode;
   
     bool _found_level = false;
     bool _found_graph = false;
     bool _initialized_graph = false;
     bool _initialized_start = false;
   
     // Store cache of BuildingMap
     Path _path;
     Level _level;
     Graph _graph;
   
     std::string _level_name = "L1";
     std::size_t _nav_graph_index = 1;
     std::string _start_wp_name = "caddy";
     // The number of waypoints to add to the predicted path. Minimum is 1.
     std::size_t _lookahead = 1;
     std::size_t _start_wp;
     std::vector<std::size_t> _next_wp;
   
     std::unordered_map<std::size_t,
       std::unordered_set<std::size_t>> _neighbor_map;
   
     double _last_update_time = 0.0;
     double _update_threshold = 0.5; // Update every 0.5s
     double _waypoint_threshold = 2.0;
   
     bool _merge_lane = false;
     double _lane_threshold = 0.2; // Meters
   
     std::string _current_task_id;
   
     std::mutex _graph_update_mutex;
   
     void map_cb(const BuildingMap::SharedPtr msg);
     void initialize_graph();
     double compute_ds(
       const Eigen::Isometry3d& pose,
       const std::size_t& wp);
     void initialize_start(const Eigen::Isometry3d& pose);
     std::size_t get_next_waypoint(const std::size_t start_wp,
       const Eigen::Vector3d& heading);
     ReadonlyCommon::Path compute_path(
       const Eigen::Isometry3d& pose);
   };
   
   template<typename SdfPtrT>
   void ReadonlyCommon::read_sdf(SdfPtrT& sdf)
   {
     // Getting sdf elements
     if (sdf->HasElement("level_name"))
       _level_name = sdf->template Get<std::string>("level_name");
     RCLCPP_INFO(logger(), "Setting level name to: %s", _level_name.c_str());
   
     if (sdf->HasElement("graph_index"))
       _nav_graph_index = sdf->template Get<std::size_t>("graph_index");
     RCLCPP_INFO(logger(), "Setting nav graph index: %d", _nav_graph_index);
   
     if (sdf->HasElement("spawn_waypoint"))
       _start_wp_name = sdf->template Get<std::string>("spawn_waypoint");
     RCLCPP_INFO(logger(), "Setting start wp name: %s", _start_wp_name.c_str());
   
     if (sdf->HasElement("look_ahead"))
       _lookahead = sdf->template Get<std::size_t>("look_ahead");
     _lookahead = _lookahead < 1 ? 1 : _lookahead;
     _next_wp.resize(_lookahead);
     _path.resize(_lookahead);
     RCLCPP_INFO(logger(), "Setting lookahead: %d", _lookahead);
   
     if (sdf->HasElement("update_rate"))
       _update_threshold = 1.0 / sdf->template Get<double>("update_rate");
     RCLCPP_INFO(logger(), "Setting update threshold: %f", _update_threshold);
   
     if (sdf->HasElement("waypoint_threshold"))
       _waypoint_threshold = sdf->template Get<double>("waypoint_threshold");
     RCLCPP_INFO(logger(), "Setting waypoint threshold: %f", _waypoint_threshold);
   
     if (sdf->HasElement("merge_lane"))
       _merge_lane = sdf->template Get<bool>("merge_lane");
     RCLCPP_INFO(
       logger(),
       "Setting merge lane: %s",
       std::to_string(_merge_lane).c_str());
   
     if (sdf->HasElement("lane_threshold"))
       _lane_threshold = sdf->template Get<double>("lane_threshold");
     RCLCPP_INFO(logger(), "Setting lane threshold: %f", _lane_threshold);
   }
   
   } // namespace rmf_readonly_common
   
   #endif