[8] 메카넘휠 로봇의 파라미터 변경을 통한 통합 런치파일 구성

ROS2 파라미터

각 노드는 자체적으로 파라미터 서버를 둘 수 있다.

사실 정말 유용하게 사용되는 기능인데, 노드가 실행되는 도중에도 실시간으로 파라미터를 변경하여 적용이 가능하다.

기존에 코드를 열고 변수를 바꿔줬던 것에 비해서 상당히 효율적이다!

 

이전 글에서 말했다시피 모터의 각속도 데이터를 시리얼통신으로 아두이노에게 보내주었다.

앞바퀴 둘, 뒷바퀴 둘 해서 각각 나누어 받게 될텐데, 이 때 노드를 앞바퀴용과 뒤바퀴용을 따로 만들어 구동하면 노드 스크립트를 두 개나 만들게 돼서 비효율적이다.

따라서 노드 스크립트는 하나만 사용하고 대신 파라미터를 변경할 수 있도록 해서 런치파일 작성시 각자의 파라미터만 지정해주어 편리하게 사용하고자 했다.

 

motor_control.py

전체 소스코드는 다음과 같이 구성하였다.

(진행중 수정이 필요하거나 오류가 발생하면 지속적으로 업데이트할 예정이다.)

# Copyright 2016 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.

# Editor : Hyoujun Oh

#Arduino Mega - 1 : /dev/ttyACM0 2 : /dev/ttyACM1
import rclpy
from rclpy.node import Node
import numpy as np
import serial
from geometry_msgs.msg import Twist
import time
#from rcl_interfaces.msg import Parameter
from rclpy.parameter import Parameter
#from rclpy.parameter import ParameterType
#from rcl_interfaces.msg import ParameterValue
#from rcl_interfaces.srv import SetParameters

class CmdVelSubscriber(Node):

    def __init__(self):
        super().__init__('motor_control')
        #self.my_param = Parameter('my_param', Parameter.Type.STRING, 'hello')
        self.declare_parameter('mobile_robot_length', 0.15)
        self.length = self.get_parameter('mobile_robot_length').value # 중심점으로부터 모터의 세로 위치
        self.declare_parameter('qos_depth', 10)
        qos_depth = self.get_parameter('qos_depth').value #qos파라미터 셋팅
        self.declare_parameter('mobile_robot_width', 0.17)
        self.width = self.get_parameter('mobile_robot_width').value # 중심점으로부터 모터의 가로 위치
        self.declare_parameter('mobile_robot_radius', 0.05)
        self.radius = self.get_parameter('mobile_robot_radius').value # 메카넘휠 반지름
        self.declare_parameter('baudrate', 115200)
        self.baudrate = self.get_parameter('baudrate').get_parameter_value().integer_value #시리얼 보드레이트
        self.declare_parameter('arduino_port', "/dev/ttyACM0")
        self.port= self.get_parameter('arduino_port').get_parameter_value().string_value #시리얼 포트
        self.declare_parameter('arduino_num', 0)
        self.arduino_num= Parameter('arduino_num', Parameter.Type.INTEGER,1).value #시리얼 포트
        self.subscription = self.create_subscription(
            Twist,
            'cmd_vel',
            self.get_cmd_vel,
            qos_depth)
        self.subscription  # prevent unused variable warning
        self.ser = serial.Serial(self.port, self.baudrate) #OpenCR Port COM3, MEGE Port COM4
        self.get_logger().info("포트 : {0}, 보드레이트 : {1}".format(self.port, self.baudrate))

    def get_cmd_vel(self, msg):
        self.arduino_num= self.get_parameter('arduino_num').get_parameter_value().integer_value
        self.Vx = msg.linear.x #m/s
        self.Vy = msg.linear.y #m/s
        self.Rz = msg.angular.z #rad/s
        self.cmd_vel2rpm()
        if self.ser.readable():
            if self.arduino_num == 0:                
                trans_data = str(round(float(self.rpm_value[0][0]),2)) + "," + str(round(float(self.rpm_value[1][0]),2))
                trans_data = trans_data.encode()
                self.ser.write(trans_data)
                self.get_logger().info("Front motor data transfer is successful!")
                self.get_logger().info(trans_data)
            else:
                trans_data = str(round(float(self.rpm_value[0][0]),2)) + "," + str(round(float(self.rpm_value[1][0]),2))
                trans_data = trans_data.encode()
                self.ser.write(trans_data)
                self.get_logger().info("Rear motor data transfer is successful!")
                self.get_logger().info(trans_data)
        self.ser.flush() # 시리얼 버퍼 초기화
        # 0.001초 딜레이 적용 완료 아두이노는 10ms 간격
        time.sleep(0.001)
       
    def cmd_vel2rpm(self):
        if self.arduino_num == 0:
            w1 = (-self.Vx/self.radius) + (self.Vy/self.radius) + self.Rz*(self.length + self.width)
            w2 = (self.Vx/self.radius) + (self.Vy/self.radius) - self.Rz*(self.length + self.width)
            self.rpm_value = np.array([[w1*9.5492968],[w2*9.5492968]]) # rad/s -> rpm
        else:          
            w3 = -(self.Vx/self.radius) + (self.Vy/self.radius) - self.Rz*(self.length + self.width)
            w4 = (self.Vx/self.radius) + (self.Vy/self.radius) + self.Rz*(self.length + self.width)
            self.rpm_value = np.array([[w3*9.5492968],[w4*9.5492968]]) # rad/s -> rpm
            
def main(args=None):
    rclpy.init(args=args)

    cmd_vel_sub = CmdVelSubscriber()

    rclpy.spin(cmd_vel_sub)

    cmd_vel_sub.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

중요한 부분만 설명하고자 한다.

 

self.declare_parameter('mobile_robot_length', 0.15)
self.length = self.get_parameter('mobile_robot_length').value # 중심점으로부터 모터의 세로 위치
self.declare_parameter('qos_depth', 10)
qos_depth = self.get_parameter('qos_depth').value #qos파라미터 셋팅
self.declare_parameter('mobile_robot_width', 0.17)
self.width = self.get_parameter('mobile_robot_width').value # 중심점으로부터 모터의 가로 위치
self.declare_parameter('mobile_robot_radius', 0.05)
self.radius = self.get_parameter('mobile_robot_radius').value # 메카넘휠 반지름
self.declare_parameter('baudrate', 115200)
self.baudrate = self.get_parameter('baudrate').get_parameter_value().integer_value #시리얼 보드레이트
self.declare_parameter('arduino_port', "/dev/ttyACM0")
self.port= self.get_parameter('arduino_port').get_parameter_value().string_value #시리얼 포트
self.declare_parameter('arduino_num', 0)
self.arduino_num= Parameter('arduino_num', Parameter.Type.INTEGER,1).value #시리얼 포트
self.subscription = self.create_subscription(
    Twist,
    'cmd_vel',
    self.get_cmd_vel,
    qos_depth)
self.subscription  # prevent unused variable warning
self.ser = serial.Serial(self.port, self.baudrate) #OpenCR Port COM3, MEGE Port COM4
self.get_logger().info("포트 : {0}, 보드레이트 : {1}".format(self.port, self.baudrate))

위 코드는 클래스 선언후 __init__에서 기입한 내용이다.

노드를 처음 실행할 때 파라미터값을 불러와서 시리얼 포트를 열어주고 통신을 준비한다.

파라미터는 qos뎁스, 로봇의 가로와 세로, 바퀴 반지름, 시리얼포트, 보드레이트 등이 있다.

코드는 단순하다.

 

1. 파라미터를 선언한다.

 self.declare_parameter(파라미터 이름, 파라미터 초기 값)

2. 변수에 파라미터값을 불러온다.

변수 이름 = self.get_parameter(파라미터 이름).get_parameter_value().변수의 타입

변수의 타입 ex) string_value 문자열 값일 때

 

그 다음 서브스크라이브 콜백 함수 내용이다.

def get_cmd_vel(self, msg):
        self.arduino_num= self.get_parameter('arduino_num').get_parameter_value().integer_value
        self.Vx = msg.linear.x #m/s
        self.Vy = msg.linear.y #m/s
        self.Rz = msg.angular.z #rad/s
        self.cmd_vel2rad()
        if self.ser.readable():
            if self.arduino_num == 0:                
                trans_data = str(round(float(self.rpm_value[0][0]),2)) + "," + str(round(float(self.rpm_value[1][0]),2))
                trans_data = trans_data.encode()
                self.ser.write(trans_data)
                self.get_logger().info("Front motor data transfer is successful!")
                self.get_logger().info(trans_data)
            else:
                trans_data = str(round(float(self.rpm_value[0][0]),2)) + "," + str(round(float(self.rpm_value[1][0]),2))
                trans_data = trans_data.encode()
                self.ser.write(trans_data)
                self.get_logger().info("Rear motor data transfer is successful!")
                self.get_logger().info(trans_data)
        self.ser.flush() # 시리얼 버퍼 초기화
        # 0.001초 딜레이 적용 완료 아두이노는 10ms 간격
        time.sleep(0.001)

값을 받아온 다음 역기구학 계산을 해주는 cmd_vel2rad() 멤버함수로 각각의 바퀴 각속도를 받아온다.

그리고 위에서 열어둔 시리얼 포트로 데이터를 아두이노로 전송한다.

이 때 처음에 파라미터 yaml파일에서 받아온 아두이노의 고유번호를 토대로 앞 바퀴와 뒷 바퀴 값을 구분한다.

(앞 바퀴 담당 아두이노는 0번, 뒷 바퀴 담당 아두이노는 1번으로 할당해주었다.)

구분 값에 따라서 아두이노로 데이터가 잘 전송된다.

 

motor_control.launch.py

전체 소스코드는 다음과 같다.

 

#!/usr/bin/env python3
import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    param_dir = LaunchConfiguration(
        'param_dir',
        default=os.path.join(
            get_package_share_directory('motor_control'),
            'param',
            'motor_config.yaml'))
    
    return LaunchDescription([
        DeclareLaunchArgument(
            'param_dir',
            default_value=param_dir,
            description='front path of parameter file'),

        Node(
            package='motor_control',
            executable='motor_control',
            name='front_motor_control',
            parameters=[param_dir],
            output='screen'),

        Node(
            package='motor_control',
            executable='motor_control',
            name='rear_motor_control',
            parameters=[param_dir],
            output='screen'),
    ])

파라미터 파일의 위치를 변수로 저장해준 후 return 값에서 각 노드에 할당해주면 된다.

여기서 주목할점은 executable에서는 두 개의 실행 노드가 동일하지만 name에서 front와 rear를 구분해준 것이다.

하나의 노드 스크립트로 여러개의 노드가 생성 가능하다.

 

motor_config.yaml

소스코드는 다읍과 같다.

 

front_motor_control:
  ros__parameters:
    qos_depth: 10
    arduino_num: 0
    arduino_port: "/dev/ttyACM0"
    mobile_robot_length: 0.15
    mobile_robot_width: 0.17
    mobile_robot_wheel_radius: 0.05
    baudrate: 115200

rear_motor_control:
  ros__parameters:
    qos_depth: 10
    arduino_num: 1
    arduino_port: "/dev/ttyACM1"
    mobile_robot_length: 0.15
    mobile_robot_width: 0.17
    mobile_robot_wheel_radius: 0.05
    baudrate: 230400

yaml 파일의 포맷은 단순하다.

 

노드의 이름:

  ros__parameters:

    [파라미터들]

 

서로다른 노드의 파라미터를 지정해주고 싶다면 아래와 같이 하면 된다.

 

노드_1 의 이름:

  ros__parameters:

    [파라미터들]

 

노드_2 의 이름:

  ros__parameters:

    [파라미터들]

 

결과적으로 모터가 정상적으로 작동하였다.

해결해야 할 문제

가끔씩 컴파일을 해줘야만 아두이노가 정상 작동하는일이 잦았다.

아두이노의 시리얼 모니터를 켜면 갑자기 데이터 전송이 먹통이 되어버린다.

이유를 찾아서 해결해야 할 듯 하다.

또한 각 모터가 정확한 RPM으로 구동하게끔 엔코더 데이터를 토대로 속도제어가 필요하다.