Gazebo에선 gazebo_ros_control 플러그인이 하드웨어 인터페이스를 제공합니다. 즉, <transmission></transmission>이 설정된 조인트에 대해 정해진 하드웨어 인터페이스를 자동으로 만들어줬죠. (hardware_interface/VelocityJointInterface, hardware_interface/PositionJointInterface 등)

Reference: https://github.com/ros-controls/ros_control/wiki/hardware_interface

이제 실제 로봇과 연동하려면 어떻게 해야 할까요? 실제 로봇도 위와 마찬가지로 각 조인트별로 하드웨어 인터페이스를 제공해주면 됩니다.

이때 사용하는 것이 hardware_interface::RobotHW 입니다. hardware_interface::RobotHW 클래스를 상속받아 로봇의 코드를 작성한 후, 이를 controller_manager::ControllerManager 이용해 구동하게 됩니다.

더미 로봇을 만들면서 하나씩 살펴보도록 하겠습니다.

먼저 RobotHW를 만들어 봅니다.

더미 로봇은 두개의 조인트 "A", "B"를 갖고 있는 로봇입니다. 또 이 조인트는 포지션 제어를 하고 있습니다. 따라서 이 더미로봇의 하드웨어 인터페이스를 제공하기 위한 코드는 다음과 같습니다.

dummy_robot_hw/include/dummy_robot_hw/dummy_robot_hw_interface.h

#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>

class DummyRobotHw : public hardware_interface::RobotHW
{
public:
  DummyRobot() {}
	~DummyRobot() {}

	 
	virtual bool init(ros::NodeHandle& nh, ros::NodeHandle& pnh);
  virtual void read(const ros::Time& time, const ros::Duration& period);
  virtual void write(const ros::Time& time, const ros::Duration& period);
  virtual bool prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list, const std::list<hardware_interface::ControllerInfo>& stop_list);
  virtual void doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list, const std::list<hardware_interface::ControllerInfo>& stop_list);

private:
  hardware_interface::JointStateInterface jnt_state_interface;
  hardware_interface::PositionJointInterface jnt_pos_interface;
  double cmd[2];
  double pos[2];
  double vel[2];
  double eff[2];
};

구현은 다음과 같습니다.

dummy_robot_hw/src/dummy_robot_hw_interface.cpp

#include "dummy_robot_hw/dummy_robot_hw_interface.h"

bool DummyRobotHw::init(ros::NodeHandle& nh, ros::NodeHandle& pnh)
{
	// 로봇의 초기화
	...
	...

	//하드웨어 인터페이스 등록
   hardware_interface::JointStateHandle state_handle_a("A", &pos[0], &vel[0], &eff[0]);
   jnt_state_interface.registerHandle(state_handle_a);

   hardware_interface::JointStateHandle state_handle_b("B", &pos[1], &vel[1], &eff[1]);
   jnt_state_interface.registerHandle(state_handle_b);

   registerInterface(&jnt_state_interface);

   // connect and register the joint position interface
   hardware_interface::JointHandle pos_handle_a(jnt_state_interface.getHandle("A"), &cmd[0]);
   jnt_pos_interface.registerHandle(pos_handle_a);

   hardware_interface::JointHandle pos_handle_b(jnt_state_interface.getHandle("B"), &cmd[1]);
   jnt_pos_interface.registerHandle(pos_handle_b);

   registerInterface(&jnt_pos_interface);
}

void DummyRobotHw::read(const ros::Time& time, const ros::Duration& period)
{
	// 로봇의 각 조인트의 값을 읽어와서 멤버 변수에 등록

	// read_from_robot();

		pos[0] = 0.0;
		vel[0] = 0.0;
		eff[0] = 0.0;
	
		pos[1] = 0.0;
		vel[1] = 0.0;
		eff[1] = 0.0;
}

void DummyRobotHw::write(const ros::Time& time, const ros::Duration& period)
{
	// 로봇의 멤버 변수의 값을 이용, 로봇의 조인트에 값 전달

	// write_to_joint_A(cmd[0]);
	// write_to_joint_B(cmd[1]);
}

bool DummyRobotHw::prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list, const std::list<hardware_interface::ControllerInfo>& stop_list)
{
    for (std::list<hardware_interface::ControllerInfo>::const_iterator it = start_list.begin(); it != start_list.end(); ++it)
    {
        if (it->claimed_resources.empty())
        {
            continue;
        }
        for (std::vector<hardware_interface::InterfaceResources>::const_iterator res_it = it->claimed_resources.begin(); res_it != it->claimed_resources.end(); ++res_it)
        {
            std::vector<std::string> r_hw_ifaces = this->getNames();

            std::vector<std::string>::iterator if_name = std::find(r_hw_ifaces.begin(), r_hw_ifaces.end(), res_it->hardware_interface);
            if (if_name == r_hw_ifaces.end()) // this hardware_interface is not registered on this RobotHW
            {
                ROS_ERROR_STREAM("Bad interface: " << res_it->hardware_interface);
                std::cout << res_it->hardware_interface;
                return false;
            }

            std::vector<std::string> r_hw_iface_resources = this->getInterfaceResources(res_it->hardware_interface);
            for (std::set<std::string>::const_iterator ctrl_res = res_it->resources.begin(); ctrl_res != res_it->resources.end(); ++ctrl_res)
            {
                std::vector<std::string>::iterator res_name = std::find(r_hw_iface_resources.begin(), r_hw_iface_resources.end(), *ctrl_res);
                if (res_name == r_hw_iface_resources.end()) // this resource is not registered on this RobotHW
                {
                    ROS_ERROR_STREAM("Bad resource: " << (*ctrl_res));
                    std::cout << (*ctrl_res);
                    return false;
                }
            }
        }
    }
    return true;
}

void DummyRobotHw::doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list, const std::list<hardware_interface::ControllerInfo>& stop_list)
{
    for (std::list<hardware_interface::ControllerInfo>::const_iterator it = start_list.begin(); it != start_list.end(); ++it)
    {
        if (it->claimed_resources.empty())
        {
            continue;
        }
        for (std::vector<hardware_interface::InterfaceResources>::const_iterator res_it = it->claimed_resources.begin(); res_it != it->claimed_resources.end(); ++res_it)
        {
            std::vector<std::string> r_hw_ifaces = this->getNames();

            std::vector<std::string>::iterator if_name = std::find(r_hw_ifaces.begin(), r_hw_ifaces.end(), res_it->hardware_interface);
            if (if_name == r_hw_ifaces.end()) // this hardware_interface is not registered on this RobotHW
            {
                throw hardware_interface::HardwareInterfaceException("Hardware_interface " + res_it->hardware_interface + " is not registered");
            }

            std::vector<std::string> r_hw_iface_resources = this->getInterfaceResources(res_it->hardware_interface);
            for (std::set<std::string>::const_iterator ctrl_res = res_it->resources.begin(); ctrl_res != res_it->resources.end(); ++ctrl_res)
            {
                std::vector<std::string>::iterator res_name = std::find(r_hw_iface_resources.begin(), r_hw_iface_resources.end(), *ctrl_res);
                if (res_name == r_hw_iface_resources.end()) // this resource is not registered on this RobotHW
                {
                    throw hardware_interface::HardwareInterfaceException("Resource " + *ctrl_res + " is not registered");
                }
            }
        }
    }
}