Program Listing for File gz_system_interface.hpp

↰ Return to documentation for file (/tmp/ws/src/gz_ros2_control/gz_ros2_control/include/gz_ros2_control/gz_system_interface.hpp)

// Copyright 2021 Open Source Robotics Foundation, Inc.
//
// 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 GZ_ROS2_CONTROL__GZ_SYSTEM_INTERFACE_HPP_
#define GZ_ROS2_CONTROL__GZ_SYSTEM_INTERFACE_HPP_

#include <map>
#include <memory>
#include <string>
#include <vector>

#ifdef GZ_HEADERS
#include <gz/sim/System.hh>
namespace sim = gz::sim;
#else
#include <ignition/gazebo/System.hh>
namespace sim = ignition::gazebo;
#endif

#include <hardware_interface/system_interface.hpp>
#include <hardware_interface/types/hardware_interface_type_values.hpp>

#include <rclcpp/rclcpp.hpp>

namespace gz_ros2_control
{


template<class ENUM, class UNDERLYING = typename std::underlying_type<ENUM>::type>
class SafeEnum
{
public:
  SafeEnum()
  : mFlags(0) {}
  explicit SafeEnum(ENUM singleFlag)
  : mFlags(singleFlag) {}
  SafeEnum(const SafeEnum & original)
  : mFlags(original.mFlags) {}

  SafeEnum & operator|=(ENUM addValue) {mFlags |= addValue; return *this;}
  SafeEnum operator|(ENUM addValue) {SafeEnum result(*this); result |= addValue; return result;}
  SafeEnum & operator&=(ENUM maskValue) {mFlags &= maskValue; return *this;}
  SafeEnum operator&(ENUM maskValue) {SafeEnum result(*this); result &= maskValue; return result;}
  SafeEnum operator~() {SafeEnum result(*this); result.mFlags = ~result.mFlags; return result;}
  explicit operator bool() {return mFlags != 0;}

protected:
  UNDERLYING mFlags;
};

// SystemInterface provides API-level access to read and command joint properties.
class GazeboSimSystemInterface
  : public hardware_interface::SystemInterface
{
public:
  virtual bool initSim(
    rclcpp::Node::SharedPtr & model_nh,
    std::map<std::string, sim::Entity> & joints,
    const hardware_interface::HardwareInfo & hardware_info,
    sim::EntityComponentManager & _ecm,
    int & update_rate) = 0;

  // Methods used to control a joint.
  enum ControlMethod_
  {
    NONE      = 0,
    POSITION  = (1 << 0),
    VELOCITY  = (1 << 1),
    EFFORT    = (1 << 2),
  };

  typedef SafeEnum<enum ControlMethod_> ControlMethod;

protected:
  rclcpp::Node::SharedPtr nh_;
};

}  // namespace gz_ros2_control

#endif  // GZ_ROS2_CONTROL__GZ_SYSTEM_INTERFACE_HPP_