Program Listing for File crowd_simulator_common.hpp

↰ Return to documentation for file (/tmp/ws/src/rmf_simulation/rmf_building_sim_common/include/rmf_building_sim_common/crowd_simulator_common.hpp)

/*
 * 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