Program Listing for File door_common.hpp

↰ Return to documentation for file (/tmp/ws/src/rmf_simulation/rmf_building_sim_common/include/rmf_building_sim_common/door_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__DOOR_COMMON_HPP
#define RMF_BUILDING_SIM_COMMON__DOOR_COMMON_HPP

#include <rclcpp/rclcpp.hpp>
#include <rclcpp/logger.hpp>

#include <rmf_door_msgs/msg/door_mode.hpp>
#include <rmf_door_msgs/msg/door_state.hpp>
#include <rmf_door_msgs/msg/door_request.hpp>

#include "utils.hpp"

#include <vector>
#include <unordered_map>
#include <unordered_set>

namespace rmf_building_sim_common {

using DoorMode = rmf_door_msgs::msg::DoorMode;
using DoorState = rmf_door_msgs::msg::DoorState;
using DoorRequest = rmf_door_msgs::msg::DoorRequest;

//==============================================================================
class DoorCommon
{

public:

  struct DoorUpdateRequest
  {
    std::string joint_name;
    double position;
    double velocity;
  };

  struct DoorUpdateResult
  {
    std::string joint_name;
    double velocity;
    double fmax;
  };

  template<typename SdfPtrT>
  static std::shared_ptr<DoorCommon> make(
    const std::string& door_name,
    rclcpp::Node::SharedPtr node,
    SdfPtrT& sdf);

  rclcpp::Logger logger() const;

  std::vector<std::string> joint_names() const;

  MotionParams& params();

  std::vector<DoorUpdateResult> update(const double time,
    const std::vector<DoorUpdateRequest>& request);


  struct DoorElement
  {
    double closed_position;
    double open_position;
    double current_position;
    double current_velocity;

    DoorElement() {}

    DoorElement(
      const double lower_limit,
      const double upper_limit,
      const bool flip_direction = false)
    : current_position(0.0),
      current_velocity(0.0)
    {
      if (flip_direction)
      {
        closed_position = lower_limit;
        open_position = upper_limit;
      }
      else
      {
        closed_position = upper_limit;
        open_position = lower_limit;
      }
    }
  };

  // Map joint name to its DoorElement
  using Doors = std::unordered_map<std::string, DoorElement>;

  template<typename SdfPtrT>
  static std::shared_ptr<DoorCommon> make(
    const std::string& door_name,
    rclcpp::Node::SharedPtr node,
    SdfPtrT& sdf,
    Doors& doors);

private:
  DoorMode requested_mode() const;

  void publish_state(const uint32_t door_value, const rclcpp::Time& time);

  double calculate_target_velocity(
    const double target,
    const double current_position,
    const double current_velocity,
    const double dt);

  DoorCommon(const std::string& door_name,
    rclcpp::Node::SharedPtr node,
    const MotionParams& params,
    const Doors& doors);

  bool all_doors_open();

  bool all_doors_closed();

  rclcpp::Node::SharedPtr _ros_node;
  rclcpp::Publisher<DoorState>::SharedPtr _door_state_pub;
  rclcpp::Subscription<DoorRequest>::SharedPtr _door_request_sub;

  DoorState _state;
  DoorRequest _request;

  MotionParams _params;

  double _last_update_time = 0.0;
  // random start time offset to prevent state message crossfire
  double _last_pub_time = ((double) std::rand()) / ((double) (RAND_MAX));

  bool _initialized = false;

  // Map of joint_name and corresponding DoorElement
  Doors _doors;
};

template<typename SdfPtrT>
std::shared_ptr<DoorCommon> DoorCommon::make(
  const std::string& door_name,
  rclcpp::Node::SharedPtr node,
  SdfPtrT& sdf)
{
  // We work with a clone to avoid const correctness issues with
  // get_sdf_param functions in utils.hpp
  auto sdf_clone = sdf->Clone();

  MotionParams params;
  get_sdf_param_if_available<double>(sdf_clone, "v_max_door", params.v_max);
  get_sdf_param_if_available<double>(sdf_clone, "a_max_door", params.a_max);
  get_sdf_param_if_available<double>(sdf_clone, "a_nom_door", params.a_nom);
  get_sdf_param_if_available<double>(sdf_clone, "dx_min_door", params.dx_min);
  get_sdf_param_if_available<double>(sdf_clone, "f_max_door", params.f_max);

  auto door_element = sdf_clone;
  std::string left_door_joint_name;
  std::string right_door_joint_name;
  std::string door_type;

  // Get the joint names and door type
  if (!get_element_required(sdf_clone, "door", door_element) ||
    !get_sdf_attribute_required<std::string>(
      door_element, "left_joint_name", left_door_joint_name) ||
    !get_sdf_attribute_required<std::string>(
      door_element, "right_joint_name", right_door_joint_name) ||
    !get_sdf_attribute_required<std::string>(
      door_element, "type", door_type))
  {
    RCLCPP_ERROR(node->get_logger(),
      " -- Missing required parameters for [%s] plugin",
      door_name.c_str());
    return nullptr;
  }

  if ((left_door_joint_name == "empty_joint" &&
    right_door_joint_name == "empty_joint") ||
    (left_door_joint_name.empty() && right_door_joint_name.empty()))
  {
    RCLCPP_ERROR(node->get_logger(),
      " -- Both door joint names are missing for [%s] plugin, at least one"
      " is required", door_name.c_str());
    return nullptr;
  }

  std::unordered_set<std::string> joint_names;
  if (!left_door_joint_name.empty() && left_door_joint_name != "empty_joint")
    joint_names.insert(left_door_joint_name);
  if (!right_door_joint_name.empty() && right_door_joint_name != "empty_joint")
    joint_names.insert(right_door_joint_name);

  Doors doors;

  auto extract_door = [&](SdfPtrT& joint_sdf)
    {
      auto joint_sdf_clone = joint_sdf->Clone();
      std::string joint_name;
      get_sdf_attribute_required<std::string>(
        joint_sdf_clone, "name", joint_name);
      const auto it = joint_names.find(joint_name);
      if (it != joint_names.end())
      {
        auto element = joint_sdf_clone;
        get_element_required(joint_sdf_clone, "axis", element);
        get_element_required(element, "limit", element);
        double lower_limit = -1.57;
        double upper_limit = 0.0;
        get_sdf_param_if_available<double>(element, "lower", lower_limit);
        get_sdf_param_if_available<double>(element, "upper", upper_limit);
        DoorCommon::DoorElement door_element;
        if (joint_name == right_door_joint_name)
          door_element =
            DoorCommon::DoorElement{lower_limit, upper_limit, true}
        ;
        else if (joint_name == left_door_joint_name)
          door_element = DoorCommon::DoorElement{lower_limit, upper_limit}
        ;
        doors.insert({joint_name, door_element});
      }
    };

  // Get the joint limits from parent sdf
  auto parent = sdf->GetParent();
  if (!parent)
  {
    RCLCPP_ERROR(node->get_logger(),
      "Unable to access parent sdf to retrieve joint limits");
    return nullptr;
  }

  auto joint_element = parent->GetElement("joint");
  if (!joint_element)
  {
    RCLCPP_ERROR(node->get_logger(),
      "Parent sdf missing required joint element");
    return nullptr;
  }

  extract_door(joint_element);
  // Find next joint element if present
  while (joint_element)
  {
    extract_door(joint_element);
    joint_element = joint_element->GetNextElement("joint");
  }

  std::shared_ptr<DoorCommon> door_common(new DoorCommon(
      door_name,
      node,
      params,
      doors));

  return door_common;

}

template<typename SdfPtrT>
std::shared_ptr<DoorCommon> DoorCommon::make(
  const std::string& door_name,
  rclcpp::Node::SharedPtr node,
  SdfPtrT& sdf,
  Doors& doors)
{
  // We work with a clone to avoid const correctness issues with
  // get_sdf_param functions in utils.hpp
  auto sdf_clone = sdf->Clone();

  MotionParams params;
  get_sdf_param_if_available<double>(sdf_clone, "v_max_door", params.v_max);
  get_sdf_param_if_available<double>(sdf_clone, "a_max_door", params.a_max);
  get_sdf_param_if_available<double>(sdf_clone, "a_nom_door", params.a_nom);
  get_sdf_param_if_available<double>(sdf_clone, "dx_min_door", params.dx_min);
  get_sdf_param_if_available<double>(sdf_clone, "f_max_door", params.f_max);

  std::shared_ptr<DoorCommon> door_common(new DoorCommon(
      door_name,
      node,
      params,
      doors));

  return door_common;
}

} // namespace rmf_building_sim_common

#endif // RMF_BUILDING_SIM_COMMON__DOOR_COMMON_HPP