.. _program_listing_file__tmp_ws_src_rmf_simulation_rmf_building_sim_common_include_rmf_building_sim_common_crowd_simulator_common.hpp:

Program Listing for File crowd_simulator_common.hpp
===================================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_rmf_simulation_rmf_building_sim_common_include_rmf_building_sim_common_crowd_simulator_common.hpp>` (``/tmp/ws/src/rmf_simulation/rmf_building_sim_common/include/rmf_building_sim_common/crowd_simulator_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__CROWD_SIMULATOR_COMMON_HPP
   #define RMF_BUILDING_SIM_COMMON__CROWD_SIMULATOR_COMMON_HPP
   
   #include <functional>
   #include <list>
   #include <queue>
   #include <memory>
   #include <regex> //for parsing initial_pose
   
   #include <MengeCore/Runtime/SimulatorDB.h>
   #include <MengeCore/Orca/ORCADBEntry.h>
   #include <MengeCore/Orca/ORCASimulator.h>
   #include <MengeCore/PluginEngine/CorePluginEngine.h>
   
   #include <rclcpp/rclcpp.hpp>
   
   namespace crowd_simulator {
   
   using AgentPtr = std::shared_ptr<Menge::Agents::BaseAgent>;
   
   class AgentPose3d
   {
   public:
     AgentPose3d()
     : _x(0), _y(0), _z(0), _roll(0), _pitch(0), _yaw(0)
     {}
     AgentPose3d(double x, double y, double z, double roll, double pitch,
       double yaw)
     : _x(x), _y(y), _z(z), _roll(roll), _pitch(pitch), _yaw(yaw)
     {}
   
     double x() const {return _x;}
     double y() const {return _y;}
     double z() const {return _z;}
     double roll() const {return _roll;}
     double pitch() const {return _pitch;}
     double yaw() const {return _yaw;}
   
     void x(double x) {_x = x;}
     void y(double y) {_y = y;}
     void z(double z) {_z = z;}
     void roll(double roll) {_roll = roll;}
     void pitch(double pitch) {_pitch = pitch;}
     void yaw(double yaw) {_yaw = yaw;}
   
     template<typename IgnMathPose3d>
     IgnMathPose3d convert_to_ign_math_pose_3d()
     {
       return IgnMathPose3d(_x, _y, _z, _roll, _pitch, _yaw);
     }
   
   private:
     double _x, _y, _z, _roll, _pitch, _yaw;
   };
   
   //================================================================
   /*
   * class MengeHandle, provides a wrap-up class handle for actual menge lib
   * only exposing several menge function interface
   */
   class MengeHandle : public std::enable_shared_from_this<MengeHandle>
   {
   public:
   
     static std::shared_ptr<MengeHandle> init_and_make(
       const std::string& resource_path,
       const std::string& behavior_file,
       const std::string& scene_file,
       const float sim_time_step
     );
   
     MengeHandle(const std::string& resource_path,
       const std::string& behavior_file,
       const std::string& scene_file,
       const float sim_time_step = 0.0
     )
     : _resource_path(resource_path),
       _behavior_file(behavior_file),
       _scene_file(scene_file),
       _sim_time_step(sim_time_step),
       _agent_count(0)
     {
       _behavior_file = this->_resource_file_path(_behavior_file);
       _scene_file = this->_resource_file_path(_scene_file);
     }
   
     void set_sim_time_step(float sim_time_step);
     float get_sim_time_step() const;
     size_t get_agent_count();
     void sim_step() const; //proceed one-time simulation step in _sim
     AgentPtr get_agent(size_t id) const;
   
   private:
     std::string _resource_path;
     std::string _behavior_file;
     std::string _scene_file;
     float _sim_time_step;
     size_t _agent_count;
     std::shared_ptr<Menge::Agents::SimulatorInterface> _sim;
   
     std::string _resource_file_path(const std::string& relative_path) const;
     bool _load_simulation(); //initialize simulatorinterface
   };
   
   //================================================================
   /*
   * class ModelTypeDatabase
   */
   class ModelTypeDatabase
   {
   public:
     struct Record
     {
       std::string type_name;
       std::string file_name;
       AgentPose3d pose;
       std::string animation;
       std::string idle_animation;
       double animation_speed;
     };
   
     using RecordPtr = std::shared_ptr<Record>;
   
     //Create a new record and returns a reference to the record
     RecordPtr emplace(std::string type_name, RecordPtr record_ptr);
     size_t size() const;
     RecordPtr get(const std::string& type_name) const;
   
   private:
     std::unordered_map<std::string, RecordPtr> _records;
   };
   
   //================================================================
   /*
   * class CrowdSimInterface
   * provides the relationship between menge agents and gazebo models
   * provides the interface to set position between gazebo models and menge agents
   */
   class CrowdSimInterface
   {
   public:
     enum class AnimState
     {
       WALK,
       IDLE,
     };
   
     struct Object
     {
       AgentPtr agent_ptr;
       std::string model_name;
       std::string type_name;
       bool is_external = false;
       AnimState current_state;
       AnimState get_next_state(bool condition);
     };
     using ObjectPtr = std::shared_ptr<Object>;
   
     CrowdSimInterface()
     : _model_type_db_ptr(std::make_shared<crowd_simulator::ModelTypeDatabase>()),
       _sdf_loaded(false),
       _switch_anim_distance_th(0.01),
       _switch_anim_name({"idle", "stand"})
     {}
   
     std::shared_ptr<ModelTypeDatabase> _model_type_db_ptr;
     rclcpp::Logger logger() const;
     void init_ros_node(const rclcpp::Node::SharedPtr node);
   
     bool init_crowd_sim();
     double get_sim_time_step() const;
     size_t get_num_objects() const;
     ObjectPtr get_object_by_id(size_t id) const;
     void one_step_sim(double time_step) const;
     double get_switch_anim_distance_th() const;
     std::vector<std::string> get_switch_anim_name() const;
     bool enabled() const;
   
     template<typename SdfPtrT>
     bool read_sdf(SdfPtrT& sdf);
   
     template<typename IgnMathPose3d>
     void update_external_agent(
       size_t id, const IgnMathPose3d& model_pose);
   
     template<typename IgnMathPose3d>
     void update_external_agent(
       const AgentPtr agent_ptr, const IgnMathPose3d& model_pose);
   
     template<typename IgnMathPose3d>
     IgnMathPose3d get_agent_pose(
       size_t id, double delta_sim_time);
   
     template<typename IgnMathPose3d>
     IgnMathPose3d get_agent_pose(
       const AgentPtr agent_ptr, double delta_sim_time);
   
   private:
     bool _sdf_loaded;
     double _switch_anim_distance_th;
     std::vector<std::string> _switch_anim_name;
     std::vector<ObjectPtr> _objects; //Database, use id to access ObjectPtr
     std::shared_ptr<MengeHandle> _menge_handle;
     float _sim_time_step;
     std::string _resource_path;
     std::string _behavior_file;
     std::string _scene_file;
     std::vector<std::string> _external_agents;
     rclcpp::Node::SharedPtr _ros_node;
     bool _enabled = false;
   
     template<typename SdfPtrT>
     bool _load_model_init_pose(
       SdfPtrT& model_type_element, AgentPose3d& result) const;
   
     bool _spawn_object();
     void _add_object(
       AgentPtr agent_ptr, const std::string& model_name,
       const std::string& type_name, bool is_external);
   };
   
   template<typename SdfPtrT>
   bool CrowdSimInterface::read_sdf(
     SdfPtrT& sdf)
   {
     char* menge_resource_path = getenv("MENGE_RESOURCE_PATH");
   
     if (menge_resource_path == nullptr ||
       strcmp(menge_resource_path, "") == 0)
     {
       RCLCPP_WARN(logger(),
         "MENGE_RESOURCE_PATH env is empty. Crowd simulation is disabled.");
       return true;
     }
   
     _enabled = true;
     _resource_path = std::string(menge_resource_path);
     RCLCPP_INFO(logger(),
       "Crowd Sim is enabled! <env MENGE_RESOURCE_PATH> is : %s",
       _resource_path.c_str());
   
     if (!sdf->HasElement("behavior_file"))
     {
       RCLCPP_ERROR(logger(),
         "No behavior file found! <behavior_file> Required!");
       return false;
     }
     _behavior_file =
       sdf->GetElementImpl("behavior_file")->template Get<std::string>();
   
     if (!sdf->HasElement("scene_file"))
     {
       RCLCPP_ERROR(logger(),
         "No scene file found! <scene_file> Required!");
       return false;
     }
     _scene_file =
       sdf->GetElementImpl("scene_file")->template Get<std::string>();
   
     if (!sdf->HasElement("update_time_step"))
     {
       RCLCPP_ERROR(logger(),
         "No update_time_step found! <update_time_step> Required!");
       return false;
     }
     _sim_time_step =
       sdf->GetElementImpl("update_time_step")->template Get<float>();
   
     if (!sdf->HasElement("model_type"))
     {
       RCLCPP_ERROR(logger(),
         "No model type for agents found! <model_type> element Required!");
       return false;
     }
     auto model_type_element = sdf->GetElementImpl("model_type");
     while (model_type_element)
     {
       std::string s;
       if (!model_type_element->template Get<std::string>("typename", s, ""))
       {
         RCLCPP_ERROR(logger(),
           "No model type name configured in <model_type>! <typename> Required");
         return false;
       }
   
       auto model_type_ptr = this->_model_type_db_ptr->emplace(s,
           std::make_shared<ModelTypeDatabase::Record>() ); //unordered_map
       model_type_ptr->type_name = s;
   
       if (!model_type_element->template Get<std::string>("filename",
         model_type_ptr->file_name, ""))
       {
         RCLCPP_ERROR(logger(),
           "No actor skin configured in <model_type>! <filename> Required");
         return false;
       }
   
       if (!model_type_element->template Get<std::string>("animation",
         model_type_ptr->animation, ""))
       {
         RCLCPP_ERROR(logger(),
           "No animation configured in <model_type>! <animation> Required");
         return false;
       }
   
       if (!model_type_element->template Get<double>("animation_speed",
         model_type_ptr->animation_speed, 0.0))
       {
         RCLCPP_ERROR(
           logger(),
           "No animation speed configured in <model_type>! <animation_speed> Required");
         return false;
       }
   
       if (!model_type_element->HasElement("initial_pose"))
       {
         RCLCPP_ERROR(
           logger(),
           "No model initial pose configured in <model_type>! <initial_pose> Required [%s]",
           s.c_str());
         return false;
       }
       if (!_load_model_init_pose(model_type_element, model_type_ptr->pose))
       {
         RCLCPP_ERROR(
           logger(),
           "Error loading model initial pose in <model_type>! Check <initial_pose> in [%s]",
           s.c_str());
         return false;
       }
   
       model_type_element = model_type_element->GetNextElement(
         "model_type");
     }
   
     if (!sdf->HasElement("external_agent"))
     {
       RCLCPP_ERROR(
         logger(),
         "No external agent provided. <external_agent> is needed with a unique name defined above.");
     }
     auto external_agent_element = sdf->GetElementImpl("external_agent");
     while (external_agent_element)
     {
       auto ex_agent_name = external_agent_element->template Get<std::string>();
       RCLCPP_INFO(logger(),
         "Added external agent: [%s].", ex_agent_name.c_str());
       _external_agents.emplace_back(ex_agent_name); //just store the name
       external_agent_element = external_agent_element->GetNextElement(
         "external_agent");
     }
   
     _sdf_loaded = true;
     return true;
   }
   
   template<typename SdfPtrT>
   bool CrowdSimInterface::_load_model_init_pose(
     SdfPtrT& model_type_element, AgentPose3d& result) const
   {
     std::string pose_str;
     if (model_type_element->template Get<std::string>(
         "initial_pose", pose_str, ""))
     {
       std::regex ws_re("\\s+"); //whitespace
       std::vector<std::string> parts(
         std::sregex_token_iterator(pose_str.begin(), pose_str.end(), ws_re, -1),
         std::sregex_token_iterator());
   
       if (parts.size() != 6)
       {
         RCLCPP_ERROR(
           logger(),
           "Error loading <initial_pose> in <model_type>, 6 floats (x, y, z, pitch, roll, yaw) expected.");
         return false;
       }
   
       result.x(std::stod(parts[0]) );
       result.y(std::stod(parts[1]) );
       result.z(std::stod(parts[2]) );
       result.pitch(std::stod(parts[3]) );
       result.roll(std::stod(parts[4]) );
       result.yaw(std::stod(parts[5]) );
     }
     return true;
   }
   
   template<typename IgnMathPose3d>
   IgnMathPose3d CrowdSimInterface::get_agent_pose(
     size_t id, double delta_sim_time)
   {
     assert(id < get_num_objects());
     auto agent_ptr = _objects[id]->agent_ptr;
     return get_agent_pose<IgnMathPose3d>(agent_ptr, delta_sim_time);
   }
   
   template<typename IgnMathPose3d>
   IgnMathPose3d CrowdSimInterface::get_agent_pose(
     const AgentPtr agent_ptr, double delta_sim_time)
   {
     //calculate future position in delta_sim_time. currently in 2d
     assert(agent_ptr);
     double px = static_cast<double>(agent_ptr->_pos.x()) +
       static_cast<double>(agent_ptr->_vel.x()) * delta_sim_time;
     double py = static_cast<double>(agent_ptr->_pos.y()) +
       static_cast<double>(agent_ptr->_vel.y()) * delta_sim_time;
   
     double x_rot = static_cast<double>(agent_ptr->_orient.x());
     double y_rot = static_cast<double>(agent_ptr->_orient.y());
   
     IgnMathPose3d agent_pose(px, py, 0, 0, 0, std::atan2(y_rot, x_rot));
     return agent_pose;
   }
   
   template<typename IgnMathPose3d>
   void CrowdSimInterface::update_external_agent(
     size_t id, const IgnMathPose3d& model_pose)
   {
     assert(id < get_num_objects());
     auto agent_ptr = _objects[id]->agent_ptr;
     update_external_agent<IgnMathPose3d>(agent_ptr, model_pose);
   }
   
   template<typename IgnMathPose3d>
   void CrowdSimInterface::update_external_agent(
     const AgentPtr agent_ptr, const IgnMathPose3d& model_pose)
   {
     assert(agent_ptr);
     agent_ptr->_pos.setX(model_pose.Pos().X());
     agent_ptr->_pos.setY(model_pose.Pos().Y());
   }
   
   } // namespace crowd_simulator
   
   #endif // CROWD_SIMULATION_COMMON__CROWD_SIMULATOR_COMMON_HPP