ros_control API를 이용한 하드웨어 제어프로그램 작성
ros_control은 ros에서 임베디드 컨트롤러를 제어하기 위해서 만들어진 API입니다.
하나의 패키지라기보다는 여러개의 계층을 가진 패키지들의 묶음이라고 생각하는게 편하겠네요.
이해하고나면 쉬운데... 전 처음에 이 그림을 보고 무슨말인지 잘 모르겠더군요.
큼직한 블록을 하나하나 분해해봅시다.
Real Robot - 액추에이터(모터) 장치입니다. 모터+센서+임베디드컨트롤러로 구성되어있고, 계층과는 시리얼통신같은 통신으로 작동합니다. 다시 말해 ros밖의 영역입니다.
hardware_interface::RobotHW - 임베디드 컨트롤러와 ros내부와의 인터페이스를 담당합니다. 보통 모터는 로봇의 관절에 붙어있으니까 ros 내부에선 모터를 joint라고 취급합니다.
Controller & Controller Manager - controller는 joint로부터 정보를 읽고, ros의 어플리케이션에서 생성한 메시지를 joint로 보낼수 있게 해줍니다. 이때, controller를 생성하고, 인터페이스 계층과 연결시켜주는 것이 controller manager입니다.
Hardware Resource Interface Layer - joint와 Controller을 중간에 이어주는 인터페이스 계층입니다.
여기서 ros프로그래머가 작성해주어야 하는 부분은 hardware_interface::RobotHW, Hardware Resource Interface Layer 계층과 Controller계층입니다.
우선 hardware_interface::RobotHW계층을 만들어보겠습니다.
MyRobot클래스 선언파일 <sagalbot.h>
#ifndef ___SAGALBOT_H___
#define ___SAGALBOT_H___
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>
#include <vector>
#include <algorithm>
#include <sstream>
#include <fstream>
namespace sagalbot{
class MyRobot : public hardware_interface::RobotHW{
private:
hardware_interface::JointStateHandle* jnt_state_handles;
hardware_interface::JointStateInterface jnt_state_interface;
hardware_interface::JointHandle* jnt_pos_handles;
hardware_interface::PositionJointInterface jnt_pos_interface;
double* cmd;
double* pos;
double* vel;
double* eff;
std::vector<std::string> joint_list;
void MyRobot_init();
public:
MyRobot(const size_t& _num_joints=0);
MyRobot(const std::string& _joint_list_file);
MyRobot(const std::vector<std::string>& _joint_list_vector);
~MyRobot();
void read();
void write();
};
}
#endif /*___SAGALBOT_H___*/MyRobot 클래스 정의파일
#include "./sagalbot.h"
using namespace sagalbot;
MyRobot::MyRobot(const size_t& _num_joints){
/*Generate joint names automatically*/
ROS_INFO("Hello, sagal-bot with %ld joints", _num_joints);
/*Make joint state handles*/
std::stringstream ss;
for(size_t i=0; i<_num_joints; ++i){
ss.clear();
ss << "Joint_"<<i;
joint_list.push_back(ss.str());
}
MyRobot_init();
}
MyRobot::MyRobot(const std::string& _joint_list_file){
/*Get joint list from file*/
ROS_INFO("Hello, sagal-bot with joint list file: %s", _joint_list_file.c_str());
std::ifstream joint_list_stream(_joint_list_file);
std::string st_joint_name;
while(joint_list_stream){
getline(joint_list_stream, st_joint_name);
joint_list.push_back(st_joint_name);
}
MyRobot_init();
}
MyRobot::MyRobot(const std::vector<std::string>& _joint_list_vector){
/*Get joint list from vector*/
ROS_INFO("Hello, sagal-bot with joint list vector. Loading joint names...");
std::for_each(_joint_list_vector.begin(), _joint_list_vector.end(), [&](const std::string& _st_jnt_name)->
void{
joint_list.push_back(_st_jnt_name);
});
MyRobot_init();
}
MyRobot::~MyRobot(){
delete[] cmd;
delete[] pos;
delete[] vel;
delete[] eff;
delete[] jnt_state_handles;
delete[] jnt_pos_handles;
}
void MyRobot::MyRobot_init(){
const size_t num_joints = joint_list.size();
cmd = new double[num_joints];
pos = new double[num_joints];
vel = new double[num_joints];
eff = new double[num_joints];
jnt_state_handles = new hardware_interface::JointStateHandle[num_joints];
jnt_pos_handles = new hardware_interface::JointHandle[num_joints];
ROS_INFO("Joint name loaded: ");
std::for_each(joint_list.begin(), joint_list.end(), [](const std::string& _st_jnt_name)->void{
ROS_INFO("Joint name: %s", _st_jnt_name.c_str());
});
for(size_t i=0; i<num_joints; ++i){
cmd[i] = 0;
pos[i] = 0;
vel[i] = 0;
eff[i] = 0;
/*Register state handles to interface*/
jnt_state_handles[i] = hardware_interface::JointStateHandle(joint_list[i],&pos[i],&vel[i],&eff[i]);
jnt_state_interface.registerHandle(jnt_state_handles[i]);
}
registerInterface(&jnt_state_interface);
/*Make joint position handles and register it to interface*/
for(size_t i=0;i<num_joints;++i){
jnt_pos_handles[i] = hardware_interface::JointHandle(jnt_state_handles[i],&cmd[i]);
jnt_pos_interface.registerHandle(jnt_pos_handles[i]);
}
registerInterface(&jnt_pos_interface);
}
void MyRobot::read(){
for(size_t i=0;i<joint_list.size();++i){
pos[i] += 1.0;
if(pos[i] > 10) pos[i] = 0;
vel[i] += 1.0;
if(vel[i] > 20) vel[i] = 10;
eff[i] += 1.0;
if(eff[i] > 30) eff[i] = 20;
}
}
void MyRobot::write(){
std::stringstream ss;
for(size_t i=0; i<joint_list.size(); ++i){
ss<<joint_list[i]<<" cmd: "<<cmd[i]<<std::endl;
}
std::cout<<ss.str();
}요약하면,
조인트 이름을 입력(숫자, 파일명, 조인트이름벡터)으로 받고, 그 조인트 숫자만큼 위치(엔코더 등), 속도(모터 회전수), 출력(duty비)같은 패러미터를 생성해줍니다.
또, 그 조인트에 대해 hardware_interface::JointStateHandle오브젝트들과 hardware_interface::JointHandle오브젝트들을 만들고, 이 오브젝트들을 각각 hardware_interface::JointStateInterface 오브젝트와 hardware_interface::PositionJointInterface오브젝트에 등록해줍니다.
그 다음엔 저 인터페이스 오브젝트들을 registerInterface()를 사용해서 컨트롤러가 접근할 수 있게 해줍니다. (이걸 안해주면 [ERROR] [1653112133.735512726]: This controller requires a hardware interface of type 'hardware_interface::JointStateInterface'. Make sure this is registered in the hardware_interface::RobotHW class. 같은 에러메시지를 만나게 됩니다.)
read메소드와 write메소드는 맨 위의 계층 다이어그램에서 나오듯이, 주기적으로 임베디드 컨트롤러와 통신하기 위한 메소드입니다.
이제 MyRobot 오브젝트를 생성하고 사용하는 노드를 만들어봅시다.
<run.cpp>
#include "./sagalbot.h"
#include "controller_manager/controller_manager.h"
#include "ros/ros.h"
#include <string>
int main(int argc, char **argv){
ros::init(argc, argv, "sagal_bot_main");
/*Create a node handle*/
ros::NodeHandle node_obj;
std::vector<std::string> sagal_joint_names;
sagal_joint_names.push_back("snake_joint");
sagal_joint_names.push_back("scorpion_joint");
auto sagal_robot = std::make_unique<sagalbot::MyRobot>(sagal_joint_names);
controller_manager::ControllerManager cm(sagal_robot.get());
ROS_INFO("sagal robot has generated");
std::vector<std::string> interface_names;
interface_names = sagal_robot->getNames();
std::for_each(interface_names.begin(), interface_names.end(),[](const std::string& _name)->void{
ROS_INFO("Interface %s",_name.c_str());
});
ros::AsyncSpinner spinner(1);
spinner.start();
ros::Time t = ros::Time::now();
ros::Rate rate(10.0);
while (ros::ok())
{
ros::Duration d = ros::Time::now() - t;
ros::Time t = ros::Time::now();
sagal_robot->read();
cm.update(t, d);
sagal_robot->write();
rate.sleep();
}
return 0;
}scorpion_joint와 snake_joint라는 조인트 이름을 스트링벡터에 넣고, 이걸로 생성자를 호출해주었습니다.
일반적으로 단일노드는 단일코어 직렬연산을 쓰니까, 제어주기를 구현하는 스피너를 async로 만들어서, 주기를 재는거만큼은 별도 스레드로 수행하게 해줍니다.
그럼 한번 실행해보죠.
rosrun sagal_fakebot sagal_fakebot_run
write()에서 써주었던 코드들이 작동하고
우리가 만든 노드는 load_controller같은 서비스등이 퍼블리시 되기를 기다리고 있습니다.
여기서 controller_manager가 등장할 차례입니다. 그전에, snake_joint, sagal_joint와 연결될 컨트롤러들을 정의해주죠.
<controller_config.yaml>
joint_state_controller:
type: "joint_state_controller/JointStateController"
publish_rate: 10
position_controller_snake:
type: "position_controllers/JointPositionController"
joint: snake_joint
position_controller_scorpion:
type: "position_controllers/JointPositionController"
joint: scorpion_joint이제 정의된 패러미터들을 사용해서 controller_manager을 호출해주는 런치파일을 만듭니다.
런치파일 만드는 김에 아까 만든 노드도 실행해주게 해줍시다.
<start_sagalbot.launch>
<launch>
<group ns="sagal_robot">
<rosparam file="$(find sagal_fakebot)/config/controller_config.yaml" command="load"/>
<node name="controller_manager"
pkg="controller_manager"
type="spawner" respawn="false"
output="screen"
args="joint_state_controller
position_controller_snake
position_controller_scorpion"/>
<node name="sagal_fakebot" pkg="sagal_fakebot" type="sagal_fakebot_run" output="screen" respawn="false" />
</group>
</launch>이제 이 런치파일을 실행해주면...
roslaunch sagal_fakebot start_sagalbot.launch
컨트롤러가 스폰되었고, 로드되었다는 말이 나오고
controller_manager가 현재 관리중인 컨트롤러의 정보를 보여주거나, 교체하려는 서비스를 만든 것을 볼 수 있습니다.
또 눈여겨볼 점은 position_controller_snake와 position_controller_scorpion에 관해서 /command 토픽을 구독하려는 노드가 나왔는데요. 또, joint_states도 궁금해집니다.
그렇다면 이 command 토픽을 퍼블리시 해볼까요?
rostopic pub /sagal_robot_position_controller_scorpion/command std_msgs/Float64 5.0
아까 넣은 입력에 따라서 scorpion조인트에 해당하는 cmd값이 바뀐걸 확인할 수 있습니다.
joint_states토픽을 구독하는 노드도 만들어보죠.
사실은 rqt_topic으로 봐도 되긴 합니다.
#include "ros/ros.h"
#include "sensor_msgs/JointState.h"
#include <iostream>
#include <sstream>
void msg_callback(const sensor_msgs::JointState& _msg){
std::stringstream ss;
ss<<"Position: ";
std::for_each(_msg.position.begin(),_msg.position.end(),[&ss](const double _state)->
void{
ss<<_state<<" ";
});
ss<<std::endl;
ss<<"Effort: ";
std::for_each(_msg.effort.begin(),_msg.effort.end(),[&ss](const double _state)->
void{
ss<<_state<<" ";
});
ss<<std::endl;
ss<<"Velocity: ";
std::for_each(_msg.velocity.begin(),_msg.velocity.end(),[&ss](const double _state)->
void{
ss<<_state<<" ";
});
ROS_INFO("%s",ss.str().c_str());
}
int main(int argc, char **argv){
ros::init(argc, argv, "sagalbot_state_subscriber");
ros::NodeHandle node_obj;
/*Register callback function on the master*/
ros::Subscriber number_subscriber = node_obj.subscribe("/sagal_robot/joint_states", 10, msg_callback);
/*Yield the control to the master and wait*/
/*Callback function may be called for each publish*/
ros::spin();
return 0;
}간단하게 sensor_msgs::JointState타입의 내용을 구독을 해서 화면에 뿌려줍니다.
역시 클래스 정의파일에 적어주었던 position, effort, velocity가 동작하는걸 확인 할 수 있습니다.
위와같이 JointState 메시지를 구독하는 걸로 조인트의 정보를 읽어올 수 있고
조인트의 /command토픽으로 Float64를 퍼블리시 하는 것을 통해 조인트의 제어가 가능한 걸 알 수 있습니다.
rqt_graph로 확인해봐도 알 수 있습니다.
왼쪽은 제가 직접 rostopic pub 로 cmd를 쏘는거고 오른쪽은 구독노드가 joint_state를 읽고있습니다.
위 소스코드는 깃허브에도 있습니다.
https://github.com/SagalinNagoya/SimpleHWInterface
GitHub - SagalinNagoya/SimpleHWInterface
Contribute to SagalinNagoya/SimpleHWInterface development by creating an account on GitHub.
github.com
를 참고해주세요. 직접 돌려보실거면 패키지(디렉토리)명을 적절히 수정해주셔야 할겁니다. sagal_fakebot