Program Listing for File utils.hpp

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

#ifndef SRC__RMF_BUILDING_SIM_COMMON__UTILS_HPP
#define SRC__RMF_BUILDING_SIM_COMMON__UTILS_HPP

#include <cmath>
#include <iostream>

namespace rmf_building_sim_common {

struct MotionParams
{
  double v_max = 0.2;
  double a_max = 0.1;
  double a_nom = 0.08;
  double dx_min = 0.01;
  double f_max = 10000000.0;
};

//==============================================================================
// TODO(MXG): Refactor the use of this function to replace it with
// compute_desired_rate_of_change()
double compute_ds(
  double s_target,
  double v_actual,
  const double v_max,
  const double accel_nom,
  const double accel_max,
  const double dt);

//==============================================================================
double compute_desired_rate_of_change(
  double _s_target,
  double _v_actual,
  const MotionParams& _motion_params,
  const double _dt);

//==============================================================================
void sanitize_node_name(std::string& node_name);

//==============================================================================
template<typename SdfPtrT, typename SdfElementPtrT>
bool get_element_required(
  SdfPtrT& _sdf,
  const std::string& _element_name,
  SdfElementPtrT& _element)
{
  if (!_sdf->HasElement(_element_name))
  {
    std::cerr << "Element [" << _element_name << "] not found" << std::endl;
    return false;
  }
  // using GetElementImpl() because for sdf::v9 GetElement() is not const
  _element = _sdf->GetElementImpl(_element_name);
  return true;
}

//==============================================================================
template<typename T, typename SdfPtrT>
bool get_sdf_attribute_required(SdfPtrT& sdf, const std::string& attribute_name,
  T& value)
{
  if (sdf->HasAttribute(attribute_name))
  {
    if (sdf->GetAttribute(attribute_name)->Get(value))
    {
      std::cout << "Using specified attribute value [" << value
                << "] for property [" << attribute_name << "]"
                << std::endl;
      return true;
    }
    else
    {
      std::cerr << "Failed to parse sdf attribute for [" << attribute_name
                << "]" << std::endl;
    }
  }
  else
  {
    std::cerr << "Attribute [" << attribute_name << "] not found" << std::endl;
  }

  return false;
}

//==============================================================================
template<typename T, typename SdfPtrT>
bool get_sdf_param_required(SdfPtrT& sdf, const std::string& parameter_name,
  T& value)
{
  if (sdf->HasElement(parameter_name))
  {
    if (sdf->GetElement(parameter_name)->GetValue()->Get(value))
    {
      std::cout << "Using specified value [" << value << "] for property ["
                << parameter_name << "]" << std::endl;
      return true;
    }
    else
    {
      std::cerr << "Failed to parse sdf value for [" << parameter_name << "]"
                <<std::endl;
    }
  }
  else
  {
    std::cerr << "Property [" << parameter_name << "] not found" << std::endl;
  }

  return false;
}

//==============================================================================
template<typename T, typename SdfPtrT>
void get_sdf_param_if_available(SdfPtrT& sdf, const std::string& parameter_name,
  T& value)
{
  if (sdf->HasElement(parameter_name))
  {
    if (sdf->GetElement(parameter_name)->GetValue()->Get(value))
    {
      std::cout << "Using specified value [" << value << "] for property ["
                << parameter_name << "]" << std::endl;
    }
    else
    {
      std::cerr << "Failed to parse sdf value for [" << parameter_name
                << "]" << std::endl;
    }
  }
  else
  {
    std::cout << "Using default value [" << value << "] for property ["
              << parameter_name << "]" << std::endl;
  }
}

} // namespace rmf_building_sim_common

#endif // SRC__RMF_BUILDING_SIM_COMMON__UTILS_HPP