21. [ROS RC카] ROS 연동 RC카 주행 2

🎮

21. [ROS RC카] ROS 연동 RC카 주행 2

지난 시간에 이어서 코드 설명을 계속해 보겠습니다.

• joy node ⇒ joy teleop axes :

  publish : sensor_msgs/Joy

• joy teleop axes ⇒ donkey_control :

  subscribe : sensor_msgs/Joy

  publish : ackermann_msgs/AckermannDriveStamped

• donkey_control :

  subscribe : ackermann_msgs/AckermannDriveStamped

  ⇒ PWM (PCA9685)

끝단 노드에서부터 역순으로 올라오면서 살펴보도록 하겠습니다.

donkey_control

---

• joy_control.py

코드는 다음 위치에 있습니다. ⇒ donkey_ros/donkey_control/src

#!/usr/bin/env python

"""
Node for control PCA9685 using AckermannDriveStamped msg 
referenced from donekycar
url : https://github.com/autorope/donkeycar/blob/dev/donkeycar/parts/actuator.py
"""

import time
import rospy
from threading import Thread
from ackermann_msgs.msg import AckermannDriveStamped


class PCA9685:
    """
    PWM motor controler using PCA9685 boards.
    This is used for most RC Cars
    """

    def __init__(
        self, channel, address=0x40, frequency=60, busnum=None, init_delay=0.1
    ):

        self.default_freq = 60
        self.pwm_scale = frequency / self.default_freq

        import Adafruit_PCA9685

        # Initialise the PCA9685 using the default address (0x40).
        if busnum is not None:
            from Adafruit_GPIO import I2C

            # replace the get_bus function with our own
            def get_bus():
                return busnum

            I2C.get_default_bus = get_bus
        self.pwm = Adafruit_PCA9685.PCA9685(address=address)
        self.pwm.set_pwm_freq(frequency)
        self.channel = channel
        time.sleep(init_delay)  # "Tamiya TBLE-02" makes a little leap otherwise

        self.pulse = 340
        self.prev_pulse = 340
        self.running = True

    def set_pwm(self, pulse):
        try:
            self.pwm.set_pwm(self.channel, 0, int(pulse * self.pwm_scale))
        except:
            self.pwm.set_pwm(self.channel, 0, int(pulse * self.pwm_scale))

    def run(self, pulse):
        pulse_diff = pulse - self.prev_pulse

        if abs(pulse_diff) > 40:
            if pulse_diff > 0:
                pulse += 0.7 * pulse_diff
            else:
                pulse -= 0.7 * pulse_diff

        self.set_pwm(pulse)
        self.prev_pulse = pulse

    def set_pulse(self, pulse):
        self.pulse = pulse

    def update(self):
        while self.running:
            self.set_pulse(self.pulse)


class Vehicle(object):
    def __init__(self, name="donkey_ros"):

        self._throttle = PCA9685(channel=0, busnum=1)
        rospy.loginfo("Throttle Controller Awaked!!")

        self._steering_servo = PCA9685(channel=1, busnum=1)
        rospy.loginfo("Steering Controller Awaked!!")

        self._name = name
        self._teleop_sub = rospy.Subscriber(
            "/donkey_teleop",
            AckermannDriveStamped,
            self.joy_callback,
            queue_size=1,
            buff_size=2 ** 24,
        )
        rospy.loginfo("Teleop Subscriber Awaked!! Waiting for joystick...")

    def joy_callback(self, msg):
        speed_pulse = msg.drive.speed
        steering_pulse = msg.drive.steering_angle

        print(
            "speed_pulse : "
            + str(speed_pulse)
            + " / "
            + "steering_pulse : "
            + str(steering_pulse)
        )

        self._throttle.run(speed_pulse)
        self._steering_servo.run(steering_pulse)


if __name__ == "__main__":

    rospy.init_node("donkey_control")
    myCar = Vehicle("donkey_ros")

    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        rate.sleep()

항상 그랬던 것과 같이 앞에서부터 차근차근 살펴보도록 하겠습니다. 😎😎

• import

앞서 살펴본 ackermann_msgs의 AckermannDriveStamped가 보입니다.

import time
import rospy
from threading import Thread
from ackermann_msgs.msg import AckermannDriveStamped

어떠한 모습이었는지 다시 한 번 짚고 넘어가겠습니다.

ackermann_msgs/AckermannDriveStamped Documentation

http://docs.ros.org/en/jade/api/ackermann_msgs/html/msg/AckermannDriveStamped.html

$ rosmsg info ackermann_msgs/AckermannDriveStamped 
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
ackermann_msgs/AckermannDrive drive
  float32 steering_angle
  float32 steering_angle_velocity
  float32 speed
  float32 acceleration
  float32 jerk

• main

if __name__ == "__main__":

    rospy.init_node("donkey_control")
    myCar = Vehicle("donkey_ros")

    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        rate.sleep()

Vehicle이라는 클래스를 생성하고, 10Hz의 주기를 갖고 반복시키고 있습니다.

• Vehicle Class

실질적인 subscriber입니다. PCA9685 클래스 2개를 선정하고 있으며, 유의하셔야 할 점으로, 버벅임을 줄이기 위해 buff_size를 추가 설정한 것이 보입니다. 로그를 찍고 있는 모습도 보이네요.

class Vehicle(object):
    def __init__(self, name="donkey_ros"):

        self._throttle = PCA9685(channel=0, busnum=1)
        rospy.loginfo("Throttle Controller Awaked!!")

        self._steering_servo = PCA9685(channel=1, busnum=1)
        rospy.loginfo("Steering Controller Awaked!!")

        self._name = name
        self._teleop_sub = rospy.Subscriber(
            "/donkey_teleop",
            AckermannDriveStamped,
            self.joy_callback,
            queue_size=1,
            buff_size=2 ** 24,
        )
        rospy.loginfo("Teleop Subscriber Awaked!! Waiting for joystick...")

    def joy_callback(self, msg):
        speed_pulse = msg.drive.speed
        steering_pulse = msg.drive.steering_angle

        print(
            "speed_pulse : "
            + str(speed_pulse)
            + " / "
            + "steering_pulse : "
            + str(steering_pulse)
        )

        self._throttle.run(speed_pulse)
        self._steering_servo.run(steering_pulse)

• PCA9685 Class

이 class는 donkeycar 프로젝트에서 가져온 것입니다. donekycar는 오로지 파이썬으로 짜여있기 때문에 대부분의 작업들이 threading을 통해 병렬 처리됩니다. ROS를 사용하게 되면, 이러한 작업은 크게 신경쓰지 않아도 된다는 장점이 있습니다.

class PCA9685:
    """
    PWM motor controler using PCA9685 boards.
    This is used for most RC Cars
    """

    def __init__(
        self, channel, address=0x40, frequency=60, busnum=None, init_delay=0.1
    ):

        self.default_freq = 60
        self.pwm_scale = frequency / self.default_freq

        import Adafruit_PCA9685

        # Initialise the PCA9685 using the default address (0x40).
        if busnum is not None:
            from Adafruit_GPIO import I2C

            # replace the get_bus function with our own
            def get_bus():
                return busnum

            I2C.get_default_bus = get_bus
        self.pwm = Adafruit_PCA9685.PCA9685(address=address)
        self.pwm.set_pwm_freq(frequency)
        self.channel = channel
        time.sleep(init_delay)  # "Tamiya TBLE-02" makes a little leap otherwise

        self.pulse = 340
        self.prev_pulse = 340
        self.running = True

    def set_pwm(self, pulse):
        try:
            self.pwm.set_pwm(self.channel, 0, int(pulse * self.pwm_scale))
        except:
            self.pwm.set_pwm(self.channel, 0, int(pulse * self.pwm_scale))

    def run(self, pulse):
        pulse_diff = pulse - self.prev_pulse

        if abs(pulse_diff) > 40:
            if pulse_diff > 0:
                pulse += 0.7 * pulse_diff
            else:
                pulse -= 0.7 * pulse_diff

        self.set_pwm(pulse)
        self.prev_pulse = pulse

    def set_pulse(self, pulse):
        self.pulse = pulse

    def update(self):
        while self.running:
            self.set_pulse(self.pulse)

Adafruit_PCA9685 모듈을 가져오는 모습이 보이며, PWM Pulse를 조절하여 모터를 구동시킬 수 있습니다.

• 이전 calibration 강의에서 보았듯, 갑자기 큰 조향을 주면, 멈춰버릴 수 있기 때문에 다음과 같은 처리가 들어갔습니다. (maze 예제에서 보았던 방식이지요?)

        if abs(pulse_diff) > 40:
            if pulse_diff > 0:
                pulse += 0.7 * pulse_diff
            else:
                pulse -= 0.7 * pulse_diff

다시금 PCA9685 클래스를 사용하는 부분을 가져왔습니다.

throttle, steering이 각각 0번 1번 채널을 사용하고 있다는 점 상기시켜드립니다.

class Vehicle(object):
    def __init__(self, name="donkey_ros"):

        self._throttle = PCA9685(channel=0, busnum=1)
        rospy.loginfo("Throttle Controller Awaked!!")

        self._steering_servo = PCA9685(channel=1, busnum=1)
        rospy.loginfo("Steering Controller Awaked!!")

joy teleop axes

---

이 노드는, mit-racecar 라는 프로젝트에서 필요한 부분을 가져와 수정한 것입니다. 훌륭한 프로젝트이니 시간이 되실 때 함께 살펴보시면 좋습니다. 👍👍

Originally from https://github.com/ros-teleop/teleop_tools Pulled on April 28, 2017. Edited by Winter Guerra on April 28, 2017 to allow for default actions.

mit-racecar/racecar

Contribute to mit-racecar/racecar development by creating an account on GitHub.

https://github.com/mit-racecar/racecar

코드를 볼 필요없이, 사용하기 편리하게 설정 부분을 따로 yaml 파일로 빼둔 프로젝트인데요. 다음과 같은 위치에 있습니다.

• donkey_joy/config/joy_teleop.yaml

joy_node:
  deadzone: 0.01
  autorepeat_rate: 20
  coalesce_interval: 0.01

teleop:
  # Default mode - Stop for safety
  default:
    type: topic
    is_default: true
    message_type: ackermann_msgs/AckermannDriveStamped
    topic_name: donkey_teleop
    message_value:
      -
        target: drive.speed
        value: 370.0
      -
        target: drive.steering_angle
        value: 375.0

  # Enable Human control by holding Left Bumper
  human_control:
    type: topic
    message_type: ackermann_msgs/AckermannDriveStamped
    topic_name: donkey_teleop
    deadman_buttons: [4]
    axis_mappings:
      -
        axis: 1
        target: drive.speed
        scale: 40.0                   # joystick will command plus or minus 2 meters / second
        offset: 380.0
      -
        axis: 2
        target: drive.steering_angle
        scale: 78.0                  # joystick will command plus or minus ~20 degrees steering angle
        offset: 375.0

  # Enable autonomous control by pressing right bumper
  # This switch causes the joy_teleop to stop sending messages to input/teleop
  # And send messages to /dev/null (an unused ROS topic)
  autonomous_control:
    type: topic
    message_type: std_msgs/Int8
    topic_name: /dev/null
    deadman_buttons: [5]
    message_value:
      -
        target: data
        value: 0

• message_type : 사용하는 topic msg 타입입니다.

• topic_name : publish할 topic 이름입니다.

• deadman_buttons : 트리거 버튼입니다. (컨트롤러 뒤에 LB버튼을 눌러줘야 한다고 말했었지요?)

• axis_mappings/offset : 조종 신호의 영점으로, 앞선 donkey car calibration 당시 알아낸 값을 사용하였습니다.

• axis_mappings/scale : offset 을 기점으로 어느 정도의 범위를 가지고 변화할 것인지 나타내는 값입니다.

joy node

---

이 노드는, ros에서 기본 제공하는 패키지입니다. 그래서 sudo apt-get 으로 설치가 가능했었지요.

사용하는 message type을 살펴보겠습니다.

$ rosmsg info  sensor_msgs/Joy
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
float32[] axes
int32[] buttons

sensor_msgs/Joy Documentation

Reports the state of a joysticks axes and buttons. Header header # timestamp in the header is the time the data is received from the joystick float32[] axes # the axes measurements from a joystick int32[] buttons # the buttons measurements from a joystick

http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/Joy.html

• axes : 양쪽 조이패드 상하좌우 버튼에 매핑되며, 모두 최합하여 배열로 publish됩니다.

• buttons : X Y A B 그리고 작은 타원형 버튼, 후면 L/R 버튼까지 모두를 mapping해서 배열로 전달하게 됩니다.

터미널을 통해 확인해 볼까요?

# connection check!!
$ sudo jstest /dev/input/js0

$ roscore
$ rosrun joy joy_node
$ rostopic echo /joy

Wiki

This stack contains a ROS node to interface with joysticks, and drivers for joysticks that are not well supported by a common Linux distribution. This stack contains a ROS node to interface with joysticks, and drivers for joysticks that are not well supported by a common Linux distribution.

http://wiki.ros.org/joystick_drivers

Reference

---

ROS on Multiple Computers | Connecting Raspberry Pi with PC over LAN

In this blog I will provide instruction for setting up Network Configuration on both of your raspberry pi and PC. In my last blog i showed the way to install ROS on raspberry pi zero , so now we ca...

https://razbotics.wordpress.com/2018/01/23/ros-distributed-systems/

RACECAR/J - ROS Teleoperation - JetsonHacks

After completion of the RACECAR/J robot assembly, ROS software installation and VESC programming, it is time to test teleoperation using a game controller. Looky here: In the software installation article, we installed a custom ROS software stack for the MIT RACECAR.

https://www.jetsonhacks.com/2018/08/19/racecar-j-ros-teleoperation/

mit-racecar/racecar

Contribute to mit-racecar/racecar development by creating an account on GitHub.

https://github.com/mit-racecar/racecar

첫번째 프로젝트를 마쳤습니다. 이 코드들을 사용해서 응용할 수 있는 것들이 마구마구 생각나지 않나요? 😉 저는 다음 강의로 넘어가서 카메라를 사용해보도록 하겠습니다. 📸