Writing a New Controller

Introduction

ros_control은 pr2를 제어하기 위해서 만들었던 framework를 다른 로봇에도 적용할 수 있게 일반화 해 놓은 meta package이다. 그리고, ros_control에서 쓸 수 있게 ros_controllers에 기본적인 controller plugin이 마련되어 있다. ros-controls/ros_controllers 링크를 보면 현재 사용 가능한 controller들을 볼 수 있다. 하나만 예를 들면 effort_controllers 패키지는 로봇에게 토크 입력을 주는 제어기를 구현한 것으로서 다음과 같은 제어기들이 있다.

• joint_effort_controller: 단축 토크 제어기
• joint_group_effort_controller: 다축 토크 제어기
• joint_group_position_controller: 다축 위치 제어기
• joint_position_controller: 단축 위치 제어기
• joint_velocity controller: 단축 속도 제어기

하지만, 꼭 여기서 제공하는 제어기만을 사용해야 하는 것은 아니며 기본 제어기를 참조해서 사용자가 입맛에 맞게 자신만의 제어기를 추가할 수도 있다. 이번 페이지에서는 그 방법에 대해서 설명한다.

plugin

새로운 제어기는 plugin 형태로 작성하므로, ros pluginlib에 대한 이해가 선행되어야 한다. plugin은 임의의 ROS package에서 loading, unloading할 수 있는 C++ library를 일컬으며, runtime에 동적으로 불러 올 수 있도록 동적 library 형태로 작성된다. 그냥 동적 library라면 부르는 쪽에서 명시적으로 해당 라이브러리를 link시켜 줘야 하지만, pluginlib를 이용하면 사용되는 library 측에서 자신을 export 시켜서 임의의 ros package에서 편하게 불러 쓸 수 있다. 그러려면, pluginlib에서 제시하는 작성법을 따라야 하고 상세한 방법은 ros pluginlib를 참조한다.

Package 파일 작성

가장 먼저 할 일은 ros package를 생성하는 것이다. my_controllers 패키지를 만들고 package.xml 파일에서 controller_interface plugin으로 export 해 준다.

  <export>
    <controller_interface plugin="${prefix}/my_controllers_plugins.xml"/>
  </export>

그리고 나서 my_controllers_plugins.xml 파일을 package.xml 파일과 같은 디렉토리에 작성해 준다. 하나의 controller package에서 복수 개의 제어기도 작성할 수 있는데 2개의 제어기를 작성한다고 했을 때 다음과 같이 하면 된다.

<library path="lib/libmy_controllers">

  <class name="my_controllers/NewController1" type="my_controllers::NewController1" base_class_type="controller_interface::ControllerBase">
    <description>
	My New Controller 1
    </description>
  </class>

  <class name="my_controllers/NewController2" type="my_controllers::NewController2" base_class_type="controller_interface::ControllerBase">
    <description>
	My New Controller 2
    </description>
  </class>
</library>

먼저 library path에는 빌드될 경로를 지정하게 되는데 위와 같이 작성하면 빌드 후에 libmy_controllers.so 파일이 ~/catkin_ws/devel/lib 폴더에 생성된다. 다음으로 각각의 class 이름을 namespace와 같이 기술하고, Base class로는 controller_interface::ControllerBase 를 지정해 주면 된다.

CMake 파일 작성

Build를 위해서 CMakeLists.txt를 작성한다. 먼저, 의존성이 있는 패키지들을 참조해준다.

find_package(catkin REQUIRED COMPONENTS
  controller_interface
  control_msgs
)

include_directories(include ${catkin_INCLUDE_DIRS})

catkin_package(
  CATKIN_DEPENDS
    controller_interface
    control_msgs
  LIBRARIES ${PROJECT_NAME}
)

기본적으로 controller_interface, control_msgs를 참조해주는데 필요에 따라서 ros_control에서 지원해주는 control_toolbox, realtime-tools 등도 참조할 수 있다.

다음으로, 참조한 라이브러리를 링크해서 제어기 라이브러리를 만들어 준다.

add_library(${PROJECT_NAME}
  src/new_controller_1.cpp
  src/new_controller_2.cpp
)

target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})

소스 파일 작성

끝으로 각각 제어기에 따라 new_controller_1.cpp와 new_controller_2.cpp를 작성해준다. new_controller_1.cpp만 예로 들어보겠다.

#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>
#include <pluginlib/class_list_macros.h>

namespace my_controllers{

class NewController1 : public controller_interface::Controller<hardware_interface::EffortJointInterface>
{
public:
  bool init(hardware_interface::EffortJointInterface* hw, ros::NodeHandle &n)
  {
    // get joint name from the parameter server
    std::string my_joint;
    if (!n.getParam("joint", my_joint)){
      ROS_ERROR("Could not find joint name");
      return false;
    }

    // get the joint object to use in the realtime loop
    joint_ = hw->getHandle(my_joint);  
    return true;
  }

  void update(const ros::Time& time, const ros::Duration& period)
  {
    double error = setpoint_ - joint_.getPosition();
    joint_.setCommand(error*gain_);
  }

  void starting(const ros::Time& time) { }
  void stopping(const ros::Time& time) { }

private:
  hardware_interface::JointHandle joint_;
  double gain_ = 2.0;
  double setpoint_ = 3.0;
};
PLUGINLIB_DECLARE_CLASS(package_name, NewController1, controller_ns::NewController1, controller_interface::ControllerBase);
}//namespace

먼저 필요한 라이브러리를 include 해준다.

#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>
#include <pluginlib/class_list_macros.h>

<controller_interface/controller.h>는 말 그대로 controller_interface를 상속해서 제어기를 작성하기 위함이고, <hardware_interface/joint_command_interface.h>는 hardware_interface 중에 토크 출력을 줄 수 있는 effort_interface를 이용하기 위함이다. hardware_interface는 하드웨어에 따라 position_interface, velocity_interface 등이 있으며 사용하는 모터 드라이브 종류에 따라서 선택할 수 있다.

controller class는 namespace 안에 특정 하드웨어 인터페이스를 사용하는 제어기 베이스 클래스를 상속받아서 작성한다.

namespace my_controllers{

class NewController1 : public controller_interface::Controller<hardware_interface::EffortJointInterface>
{
     ...

my_controllers namespace 안에 hardware_interface로 EffortJointInterface를 사용하는 controller를 상속받아서 class를 작성하였다.

class 내부는 init, starting, update, stop 함수를 오버라이딩 해서 기본 기능을 구현하고, 추가적으로 command를 subscribe할 수 있도록 callback 함수 등을 추가해 줄 수도 있다. init은 제어기가 controller_manager에 의해서 로딩 될 때 불리고, starting, stop은 제어기가 시작되고 끝날 때 한번씩 불리며, update함수는 제어 주기마다 매번 불리게 된다.

init 함수는 joint handle을 얻어서 클래스에서 쓸 수 있게 해주면, ros node handle도 입력으로 들어오기 때문에 node와 관련된 토픽, 서비스, 파라미터 핸들링 등을 필요에 따라 작성하여 쓸 수 있다.

  bool init(hardware_interface::EffortJointInterface* hw, ros::NodeHandle &n)
  {
    // get joint name from the parameter server
    std::string my_joint;
    if (!n.getParam("joint", my_joint)){
      ROS_ERROR("Could not find joint name");
      return false;
    }

    // get the joint object to use in the realtime loop
    joint_ = hw->getHandle(my_joint);  // throws on failure
    return true;
  }

update 함수에는 제어기 주 로직이 들어가게 되며, joint handle을 이용해서 명령을 줄 수 있다.

  void update(const ros::Time& time, const ros::Duration& period)
  {
    double error = setpoint_ - joint_.getPosition();
    joint_.setCommand(error*gain_);
  }

hardware_interface가 무엇이냐에 따라서 setCommand는 다른 의미를 지니는데, 여기서는 EffortJointInterface를 선택했으므로 setCommand는 토크값을 모터 드라이브에 넘겨주는 함수가 된다. 만약에 PositionJointInterface를 선택했으면 위치명령을 주는 함수가 된다.

실제 예제는 일단 time delay controller, gravity compensation controller를 작성한 예제를 참조한다. (추후에 1자유도 짜리 pendulum이나 rrbot으로 쉬운 예제를 작성해보려고 한다..)

Reference

1. Metapackages
2. Write a new controller