# 프로그래밍 ROS < 속도 증감 >

소개

하나의 참고서에 따라 ROS(Robot Operating System)를 어려움 없이 다루는 것이 목적이다. 그 제 16 탄으로서 속도의 증감을 다룬다.

환경

가상 환경





소프트
VMware Workstation 15

실장 RAM
2GB

OS
우분투 64비트

ISO 파일
ubuntu-mate-20.04.1-desktop-amd64.iso



컴퓨터





장치
MSI

프로세서
Intel(R) Core(TM) i5-7300HQ CPU @ 2.50GHz 2.50GHz

실장 RAM
8.00GB(7.89GB 사용 가능)

OS
Windows (Windows 10 Home, 버전: 1909)



ROS





Distribution
noetic

프로그래밍 언어
파이썬 3.8.5

물리적 문제

마지막으로 로봇의 최대 속도를 지정할 수있게되었지만 유한 가속도라는 물리적 문제를 다룰 필요가 나온다.

질량을 가진 많은 물체와 마찬가지로 로봇도 순간적으로 시작하거나 정지 할 수 없습니다. 로봇의 이동을 물리학으로 파악하면 시간과 함께 점점 가속해 가는 물체이다. 갑자기 멈추려고하면 모터에 급격한 부하가 걸립니다. 예를 들어, 미끄러짐, 미끄러짐, 진동, 기계적 구동 시스템에서 무언가가 부서지는 문제로 발전 할 수 있습니다. 이를 피하기 위해 특정 시간 내에 이동 명령을 늘리거나 줄입니다.

구현

앞서 말한 것에 근거해, 요구해도 순간적으로 가속하는 것을 막는 프로그램을 배운다. 다음은 소스 코드와 그 때의 실행 방식을 보여줍니다.

소스 코드

keys_to_twist_with_ramps.py

#! /usr/bin/env python3

"""瞬間的に加速するのを防ぐために速度を一定時間で増減させる"""

import rospy
import math
from std_msgs.msg import String
from geometry_msgs.msg import Twist

#キーの割当：[angular.z, linear.x]
key_mapping = {'w':[ 0, 1], 'x':[0, -1],
               'a':[-1, 0], 'd':[1,  0],
               's':[ 0, 0]}

twist_pub      = None       #twist配信用の変数
target_twist   = None       #目標の速度インスタンス用変数
last_twist     = None       #何も押されなかった場合，直前のtwistを配信するための記録用変数
last_send_time = None       #配信した時刻を記録する変数
vel_scales     = [0.1, 0.1] #デフォルトの速度（非常に遅い）
vel_ramps      = [1, 1]     #単位はm/s　増減の度合い 

def ramped_vel(vel_prev, vel_target, time_prev, time_now, ramp_rate):
    """ 最大の速度ステップを計算する """

    step = ramp_rate * (time_now - time_prev).to_sec()

    if vel_target > vel_prev:   #目標値に達していなければsignを1.0に
        sign = 1.0
    else:                       #目標値に達しているならばsignを-1.0に
        sign = -1.0

    error = math.fabs(vel_target - vel_prev)    #絶対値計算　abs()と違って，引数がintでもfloatで返す

    if error < step:    #この時間ステップ内にそこに到達できる --> 到達した
        return vel_target
    else:
        return vel_prev + sign * step   #ターゲットに向けてステップを進める

def ramped_twist(prev, target, time_prev, time_now, ramps):
    """ 計算した速度ステップをtwistに適用 """
    twist = Twist()
    twist.angular.z = ramped_vel(prev.angular.z, target.angular.z, time_prev, time_now, ramps[0])
    twist.linear.x  = ramped_vel(prev.linear.x, target.linear.x, time_prev, time_now, ramps[1])

    return twist

def send_twist():
    """ twistを送る """
    global last_send_time, target_twist, last_twist, vel_scales, vel_ramps, twist_pub

    time_now = rospy.Time.now()

    last_twist = ramped_twist(last_twist, target_twist, last_send_time, time_now, vel_ramps)

    last_send_time = time_now
    twist_pub.publish(last_twist)       #twist配信

def keys_callback(msg):
    global target_twist, last_twist, vel_scales

    if len(msg.data) == 0 or msg.data[0] not in key_mapping.keys():
        return  #データがないもしくはキーマッピングにないデータの場合，何もせずに終了

    velocity = key_mapping[msg.data[0]] #キーマッピングからキーに合わせて抽出
    target_twist = Twist()  #Twistインスタンス生成(0に初期化)
    target_twist.angular.z = velocity[0] * vel_scales[0]    #配列の要素に速度スケール値をかけて目標角速度に代入
    target_twist.linear.x = velocity[1] * vel_scales[1]     #配列の要素に速度スケール値をかけて目標速度に代入

def fetch_param(name, default):
    """ コマンドラインでの入力の受付"""
    if rospy.has_param(name):   #コマンドラインでパラlast_send_timeメータnameが指定されているかチェック
        return rospy.get_param(name)    #指定された値を取得
    else:
        #パラメータが指定されておらず，デフォルト値を使うことを警告する
        rospy.logwarn(f"parameter {name} not defined. Defaulting to {default:.1f}")
        return default


if __name__ == '__main__':
    rospy.init_node('keys_to_twist')    #ノードの初期化
    last_send_time = rospy.Time.now()
    twist_pub = rospy.Publisher('cmd_vel', Twist, queue_size = 1)   #cmd_vel配信準備
    rospy.Subscriber('keys', String, keys_callback) #keysを購読し，コールバック関数を呼び出す．引数はさらに後ろで指定する

    target_twist = Twist()  #0に初期化
    last_twist = Twist()    #0に初期化

    vel_scales[0] = fetch_param('~angular_scale', 0.1)
    vel_scales[1] = fetch_param('~linear_scale', 0.1)
    vel_ramps[0]  = fetch_param('~angular_accel', 1.0)
    vel_ramps[1]  = fetch_param('~angular_accel', 1.0)


    rate = rospy.Rate(20)   #10Hz(100ミリ秒ごと)で出力するため
    while not rospy.is_shutdown():
        send_twist()
        rate.sleep()

실행 모드

결과

그래프로부터 확실히 스텝 형태로 변화하는 것이 아니라 점점 늘어나거나 줄어들고 있는 것을 알 수 있다. 가속과 감속에 일정한 시간이 걸리고 있기 때문에 물리적으로 실행 가능하다는 것을 확인할 수 있었다.

감상

이번에는 조금 소스 코드도 함수가 늘어나 복잡해졌지만, 전회에 우려된 스텝 커멘드에 의한 물리적 문제를 해소하는 방법을 배웠다. 특히 앞으로 로봇을 다루는 가운데 매우 중요하고 의식해 두어야 할 것이라고 생각한다.

다음은 시뮬레이션에서 실제로 turtlebot을 움직여 보겠습니다.

참고문헌

프로그래밍 ROS Python으로 로봇 애플리케이션 개발
　　　　　　Morgan Quigley, Brian Gerkey, William D.Smart
　　　　　　　　　　　　　　　　　　 河田 卓志
　　　　　　　　　마츠다 아키히로, 후쿠지 마사키, 유타니 테츠오
　　　　　　　　　　　　　　　오일러리 재팬 발행