ROS2 EtherCAT Beckhoff Digital I/O Slave 구동 예제

 

ROS2는 나름 다룬 경력도 오래됐고, ROS2와 EtherCAT을 연동해 로봇을 구동하는 프로젝트를 진행한지도 어언 1년이 넘어가고 있지만 ROS 관련 글을 왠만하면 포스팅하지 않으려 했다 (다뤄야되는 내용이 꽤나 전문적이다보니 글이 길어지는 경향이...)

 

그런데...

 

블로그 댓글을 보고 완전히 무시하는건 또 도리가 아닌것 같아 간단하게나마 글을 남겨야한다는 의무감이 생긴다 ㅠ

ROS 관련해선 최대한 이론이나 작동 원리에 대한 설명은 배제한채 담백하게 코드 위주로만 글을 올려봐야겠다

---

※ 이 글에서는 Ubuntu에 PREEMPT_RT 리눅스 커널이 설치된 실시간 환경과, ROS2 Humble 및 IgH EtherCAT Master가 설치된 환경을 필요로 한다

환경 구축 방법에 대한 내용은 아래 두 글을 참고한다

EtherCAT Master 환경 구축 - Ubuntu 22.04 LTS + PREEMPT_RT 커널 패치

EtherCAT Master 환경 구축 - Ubuntu 22.04 LTS + PREEMPT_RT 커널 패치

ROS2 + IgH EtherCAT Master 구동 환경 구축

ROS2 + IgH EtherCAT Master 구동 환경 구축

---

이번 글의 주제는 ROS2 + EtherCAT을 통한 GPIO 슬레이브(slave) 제어

이더캣 슬레이브 자체가 비싸기 때문에 모듈 이것저것을 테스트해볼 수 없는 점이 아쉽긴 하지만, 일단 GPIO 모듈이 하나 있으니 간단하게 연동하는 방법에 대해서만 다뤄보자

1. Hardware (EtherCAT Slave) Introduction

Bechkoff사에서 만든 FB1111-0412라는 piggyback controller board를 하나 가지고 있다

https://www.beckhoff.com/ko-kr/products/i-o/ethercat-development-products/elxxxx-etxxxx-fbxxxx-hardware/fb1111.html

EtherCAT piggyback controller board

 

원래는 Bechkoff의 EL9820이라는 EtherCAT evaluation kit에 이더캣 통신용으로 장착하는 모듈인데, 단순한 GPIO 테스트용으로는 단독으로 사용해도 무방하다

 

EtherCAT Slave Information (ESI) 파일은 아래 링크 참고

Beckhoff FB1XXX.xml

0.05MB

 

 

내가 보유중인 모델은 Rev.no가 0x01F5008E이니 해당 device 태그만 확인해두면 된다

<Device Physics="YY">
    <Type ProductCode="#x04570862" RevisionNo="#x01F5008E">FB1111 Dig. Out</Type>
    <HideType RevisionNo="#x01F4008E"></HideType>
    <Name LcId="1033"><![CDATA[FB1111 32 Ch. Dig. Output 2xMII]]></Name>
    <Name LcId="1031"><![CDATA[FB1111 32 K. Dig. Ausgang 2xMII]]></Name>
    <URL LcId="1033"><![CDATA[http://www.beckhoff.com/FB1111]]></URL>
    <URL LcId="1031"><![CDATA[http://www.beckhoff.de/FB1111]]></URL>
    <GroupType>FB1XXX</GroupType>
    <Fmmu OpOnly="1">Outputs</Fmmu>
    <Sm StartAddress="#x0f00" ControlByte="#x44" Enable="1" OpOnly="1">Outputs</Sm>
    <Sm StartAddress="#x0f01" ControlByte="#x44" Enable="1" OpOnly="1">Outputs</Sm>
    <Sm StartAddress="#x0f02" ControlByte="#x44" Enable="1" OpOnly="1">Outputs</Sm>
    <Sm StartAddress="#x0f03" ControlByte="#x44" Enable="1" OpOnly="1">Outputs</Sm>
    <RxPdo Fixed="1" Sm="0">
        <Index>#x1a00</Index>
        <Name>Byte 0</Name>
        <Entry>
            <Index>#x3101</Index>
            <SubIndex>1</SubIndex>
            <BitLen>8</BitLen>
            <Name>Output</Name>
            <DataType>BITARR8</DataType>
        </Entry>
    </RxPdo>
    <RxPdo Fixed="1" Sm="1">
        <Index>#x1a01</Index>
        <Name>Byte 1</Name>
        <Entry>
            <Index>#x3101</Index>
            <SubIndex>2</SubIndex>
            <BitLen>8</BitLen>
            <Name>Output</Name>
            <DataType>BITARR8</DataType>
        </Entry>
    </RxPdo>
    <RxPdo Fixed="1" Sm="2">
        <Index>#x1a02</Index>
        <Name>Byte 2</Name>
        <Entry>
            <Index>#x3101</Index>
            <SubIndex>3</SubIndex>
            <BitLen>8</BitLen>
            <Name>Output</Name>
            <DataType>BITARR8</DataType>
        </Entry>
    </RxPdo>
    <RxPdo Fixed="1" Sm="3">
        <Index>#x1a03</Index>
        <Name>Byte 3</Name>
        <Entry>
            <Index>#x3101</Index>
            <SubIndex>4</SubIndex>
            <BitLen>8</BitLen>
            <Name>Output</Name>
            <DataType>BITARR8</DataType>
        </Entry>
    </RxPdo>
    <Dc>
        <OpMode>
            <Name>DcOff</Name>
            <Desc>DC unused</Desc>
            <AssignActivate>#x0000</AssignActivate>
        </OpMode>
        <OpMode>
            <Name>DcSync</Name>
            <Desc>DC for synchronization</Desc>
            <AssignActivate>#x0100</AssignActivate>
            <CycleTimeSync0 Factor="1">0</CycleTimeSync0>
            <ShiftTimeSync0>0</ShiftTimeSync0>
        </OpMode>
    </Dc>
    <Eeprom>
        <ByteSize>2048</ByteSize>
        <ConfigData>040F00441027FFFF</ConfigData>
    </Eeprom>
</Device>

 

일반적인 서보모터 슬레이브랑은 달리 Sync Manager 0, 1, 2, 3 각각에 RxPDO가 한개씩 배정되며, 각 PDO는 엔트리가 하나씩밖에 없다

Sync Manager	PDO Index	PDO Entry
		Index	Sub Index	Bit Length
0	RxPDO, 0x1A00	0x3101	0x01	8
1	RxPDO, 0x1A01	0x3101	0x02	8
2	RxPDO, 0x1A02	0x3101	0x03	8
3	RxPDO, 0x1A03	0x3101	0x04	8

2. EtherCAT Master 시동 및 슬레이브 정보 확인

슬레이브에 LED 하나를 장착하고 EtherCAT Master와 이더넷 케이블로 연결해준다

(아무리 뒤져봐도 집에 DIP 타입 LED가 하나밖에 없네... 나중에 추가로 구매해서 좀 더 연결해봐야겠다)

 

이더캣 마스터를 동작시킨다

$ sudo ethercatctl start

 

현재 상태가 'running'으로 표시되면 정상 동작 중

$ sudo ethercatctl status

 

슬레이브 정보를 얻어와보자

$ sudo ethercat slaves

$sudo ethercat slaves -v

 

슬레이브의 PDO 정보를 읽어와보자

$ sudo ethercat pdos

앞서 ESI에서 확인한 것과 동일한 것을 알 수 있다

 

cstruct 명령어를 통해 C/C++ 구현 시 구조체를 어떻게 짜야하는지 템플릿을 얻을 수 있다

이걸 이용하면 ROS2에서 slave yaml 설정파일을 구현할 때도 도움이 된다

$ sudo ethercat cstruct

 

3. ROS2 EtherCAT 코드 구현

FB1111-0142 간단한 GPIO 구현 코드를 깃허브 저장소에 올려뒀다

https://github.com/YOGYUI/ros2_ethercat_example_gpio

GitHub - YOGYUI/ros2_ethercat_example_gpio

3.1. 이더캣 슬레이브 설정파일 (yaml)

[beckhoff_fb1111.yaml] 파일

슬레이브 ESI의 PDO 및 SM을 어떤 식으로 EtherCAT ROS2 Stack 플러그인 연동을 위한 설정파일로 포팅하는지 알 수 있다

vendor_id: 0x00000002
product_id: 0x04570862
# assign_activate: 0x0100
assign_activate: 0x00
rpdo:
  - index: 0x1a00
    channels:
      - {index: 0x3101, sub_index: 0x01, type: uint8, command_interface: dig.out.byte0}
  #- index: 0x1a01
  #  channels:
  #    - {index: 0x3101, sub_index: 0x02, type: uint8, command_interface: dig.out.byte1}
  #- index: 0x1a02
  #  channels:
  #    - {index: 0x3101, sub_index: 0x03, type: uint8, command_interface: dig.out.byte2}
  #- index: 0x1a03
  #  channels:
  #    - {index: 0x3101, sub_index: 0x04, type: uint8, command_interface: dig.out.byte3}
sm:
  - {index: 0, type: output, pdo: rpdo, watchdog: enable}
  #- {index: 1, type: output, pdo: rpdo, watchdog: enable}
  #- {index: 2, type: output, pdo: rpdo, watchdog: enable}
  #- {index: 3, type: output, pdo: rpdo, watchdog: enable}

※ 안타깝게도 ROS2 EtherCAT 플러그인은 Sync Manager 각각에 서로 다른 RxPDO를 등록하는 기능이 지원되지 않기 때문에 한번에 8비트 I/O 제어만 가능하다... 소스코드를 한번 수정해볼까나?

3.2. ROS2 Control 설정파일 (yaml)

[ros2_controllers.yaml] 파일

GPIO는 딱히 제어할 수 있는 방법이 ForwardCommandController를 통한 PDO 출력값 변경말고는 없는 걸로 파악하고 있다

EtherCAT 제어주기는 무난하게 1ms (1000Hz)로 맞춰주고, ForwardCommandController 설정 방법에 맞춰서 컨트롤러를 추가해주자

controller_manager:
  ros__parameters:
    update_rate: 1000

    gpio_controller:
      type: forward_command_controller/ForwardCommandController

gpio_controller:
  ros__parameters:
    joints:
      - joint_gpio
    interface_name: dig.out.byte0

3.3. URDF (xacro)

GPIO 슬레이브도 '로봇 입장'에서는 하나의 Joint로 설정해줘야 ROS2 컨트롤러에서 접근할 수 있다

[robot.urdf.xacro] 파일로 로봇 URDF xacro 파일과 ROS2 Control 관련 xacro 설정파일을 불러오게 구현한 뒤

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="ros2_ethercat_example_gpio">
  <xacro:include filename="robot_core.xacro"/>
  <xacro:include filename="ros2_control.xacro"/>
</robot>

 

[robot_core.xacro] 파일에서는 단순하게 'joint_gpio' 조인트 하나만 추가해주자

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
  <joint name="joint_gpio"/>
</robot>

 

그리고 [ros2_control.xacro] 파일에서는 ros2 control 하드웨어 플러그인을 설정해주고 'joint_gpio'가 어떤 command interface를 갖고 있는지, 그리고 ec_module 태그를 통해 슬레이브의 alias와 position, 그리고 yaml 설정파일 경로를 지정해주면 된다

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
  <ros2_control name="ec_test" type="system">
    <hardware>
      <plugin>ethercat_driver/EthercatDriver</plugin>
        <param name="master_id">0</param>
        <param name="control_frequency">1000</param>
      </hardware>

    <gpio name="joint_gpio">
      <command_interface name="dig.out.byte0"/>
      <!--<command_interface name="dig.out.byte1"/>-->
      <!--<command_interface name="dig.out.byte2"/>-->
      <!--<command_interface name="dig.out.byte3"/>-->
      <ec_module name="FB1111">
        <plugin>ethercat_generic_plugins/GenericEcSlave</plugin>
        <param name="alias">0</param>
        <param name="position">0</param>
        <param name="slave_config">$(find ros2_ethercat_example_gpio)/config/beckhoff_fb1111.yaml</param>
      </ec_module>
    </gpio>
  </ros2_control>
</robot>

3.4. launch 파일

ROS2 컨트롤러를 활성화하려면 controller_manager 패키지의 'ros2_control_node'를 통해 로봇 URDF 파일과 ros2 control yaml 파일을 로드해 하드웨어를 구동시켜야 하며, 동일 패키지의 'spawner'를 통해 컨트롤러(ForwardCommandController)를 구동해야 한다

 

두 과정을 커맨드라인으로 일일이 입력하면 굉장히 번거로우므로 아래와 같이 한번에 실행시킬 수 있도록 런치(launch) 파이썬 스크립트를 작성해뒀다

import os
import xacro
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument

def generate_launch_description():
    package_name = 'ros2_ethercat_example_gpio'
    xacro_file_name = 'robot.urdf.xacro'
    xacro_file_path = os.path.join(
        get_package_share_directory(package_name),
        'description/' + xacro_file_name
    )
    doc = xacro.process_file(xacro_file_path)
    robot_desc = doc.toprettyxml(indent='  ')
    ros2_control_cfg_path = os.path.join(
        get_package_share_directory(package_name),
        'config/ros2_controllers.yaml'
    )
    
    ld = LaunchDescription()
    ld.add_action(DeclareLaunchArgument("publish_frequency", default_value="1000.0"))

    # ros2 control node
    ros2_control_node = Node(
        package="controller_manager", 
        executable="ros2_control_node", 
        parameters=[
            {'robot_description': robot_desc},
            ros2_control_cfg_path
        ]
    )
    ld.add_action(ros2_control_node)

    # spawn controllers node
    controller_names = [
        "gpio_controller"
    ]
    for name in controller_names:
        node = Node(
            package="controller_manager",
            executable="spawner",
            arguments=[name],
            output="screen"
        )
        ld.add_action(node)

    return ld

 

위에서 설명한 소스코드는 아래와 같이 ROS2 작업경로 (이글에서는 ~/ros2_ws를 가정)로 클론한 뒤 colcon으로 빌드하면 동작 준비가 완료된다

$ cd ~/ros2_ws/src
$ git clone https://github.com/YOGYUI/ros2_ethercat_example_gpio.git
$ colcon build --packages-select ros2_ethercat_example_gpio --symlink-install

4. DEMO

4.1. 런치 스크립트 실행

ros2 launch 명령어로 파이썬 스크립트를 실행하면

$ ros2 launch ros2_ethercat_example_gpio init.launch.py

 

위와 같이 EtherCAT 마스터 설정 및 슬레이브 등록, 'gpio_controller' 구동이 한번에 완료된다

 

이 상태에서 ros2 control 커맨드 라인을 통해 'list_controllers', 'list_hardware_components', 'list_hardware_interfaces' 명령어를 통해 구동중인 컨트롤러, 인터페이스를 확인할 수 있다

$ ros2 control list_controllers

$ ros2 control list_hardware_components

$ ros2 control list_hardware_interfaces

4.2. check ros2 topic

앞서 언급했듯 ForwardCommandController는 ros2 컨트롤러 백엔드에서 EtherCAT 제어주기(이 글에서는 1ms로 설정)마다 컨트롤러 내부에 설정된 값을 PDO로 전송하게 되는데, 이렇게 주기적으로 송신되는 값을 DDS 토픽을 통해 변경할 수 있다

 

토픽 리스트 및 인터페이스 확인 방법은 아래 프로세스를 따르면 된다

$ ros2 topic list

$ ros2 topic info /gpio_controller/commands

$ ros2 interface show std_msgs/msg/Float64MultiArray

4.3. Example Script (Blink)

단순히 켜고 끄는 명령은 쉬우므로 간단한 파이썬 스크립트를 통해 1초에 한번씩 켰다 껐다 반복하는 기능을 구현해보자

import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray

class BlinkPublisher(Node):
    def __init__(self):
        super().__init__("blink_publisher")
        self.value = 1
        self.publisher = self.create_publisher(Float64MultiArray, "/gpio_controller/commands", 1)
        self.timer = self.create_timer(1.0, self.period_publish)

    def period_publish(self):
        msg = Float64MultiArray()
        msg.data = [float(self.value)]
        self.publisher.publish(msg)
        if self.value == 1:
            self.value = 0
        else:
            self.value = 1

def main(args=None):
    rclpy.init(args=args)
    node = BlinkPublisher()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == "__main__":
    main()

 

GPIO 0 ~ 7 총 8비트의 GPIO를 제어할 수 있는데, 본 예제에서는 GPIO0에 LED를 연결해뒀으므로 값을 0 혹은 1로만 바꾸면 된다

 

이제 파이썬 스크립트를 실행해보면

$ python3 blink.py

 

1초에 한번씩 깜빡거린다 ㅎㅎ

---

TODO: LED 좀 더 구한 다음에 멋드러진 제어 예제를 만들어보자