23. [ROS RC카] 응용 실습, Object Following

📗

23. [ROS RC카] 응용 실습, Object Following

두번째 응용 사례인 Object Following 예제를 시작해 보도록 하겠습니다. 🎆

먼저 실행 모습부터 살펴보겠습니다.

jetson을 구매하셨다면 모두들 가지고 있는 이 녹색 박스를 따라서 조향하는 예제입니다.

• 먼저 실행시켜보기

$ roscore
# camera image publisher
$ rosrun csi_camera csi_pub.py
# CV Magic, Bind Ball with Pixel Value
$ rosrun donkey_cv find_ball.py 

# Blob Point to Twist
$ rosrun donkey_control chase_the_ball.py
# PWM Control node
$ rosrun donkey_control blob_chase.py

# image_view 실행
$ rosrun image_view image_view image:=/blob/image_blob

문제없이 잘 실행 되셨나요?? 전체적인 구조를 살펴보도록 하겠습니다.

rqt_graph

• csi_pub : 카메라 이미지를 게속해서 송출해주는 노드이지요?

• blob_detector : 컴퓨터 비전이 적용된 노드로, 녹색 물체를 찾아내고, 지속적으로 추적하는 노드

• chase_ball : 이전 노드에서 녹색 물체의 위치가 픽셀 단위로 오게 됩니다. 이 픽셀의 위치에 따라서 제어 신호를 Twist 형식으로 변환합니다.

• image_view : 이전에도 살펴보았던, 이미지 토픽을 볼 수 있게 해주는 노드입니다.

• blob_chase_node : 실질적인 PWM 제어를 담당하는 노드입니다.

💡

카메라만으로는 물체와의 거리를 알아내기에 한계가 있습니다. 원근법을 이용한 방법이 있긴 하지만, 우리가 쓰고 있는 광각 렌즈에서는 정확도가 떨어집니다. 그렇기에 따로 Twist Topic을 받도록 해준 모습이 보입니다.

지난 프로젝트 분석과 마찬가지로, 이번에도 뒷부분의 node에서부터 차근차근 분석해 보겠습니다.

참고로 이번 프로젝트는 다음의 오픈소스를 참고삼아 제작하였습니다.

ROS and OpenCv for beginners | Blob Tracking and Ball Chasing with Raspberry Pi

ROS and OpenCV can work together for accomplishing incredible tasks. In this tutorial for beginners we are going to use the Rover's camera for detecting and ...

https://www.youtube.com/watch?v=We6CQHhhOFo&list=PLuteWQUGtU9BU0sQIVqRQa24p-pSBCYNv&index=11

blob_chase.py

---

#!/usr/bin/env python

"""
referenced from those projects

DkLowLevelCtrl, ServoConvert part from tizianofiorenzani/ros_tutorials
url: https://github.com/tizianofiorenzani/ros_tutorials

PCA9685 part from donkeycar
url: https://github.com/autorope/donkeycar/blob/99c853b1737f12019ae598d3c7f00699d2166472/donkeycar/parts/actuator.py#L12

Listens to /dkcar/control/cmd_vel for corrective actions to the /cmd_vel coming from keyboard or joystick

"""
import rospy
import time
from geometry_msgs.msg import Twist

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 ServoConvert:
    def __init__(self, id=1, center_value=370, range=90, direction=1):
        self.value = 0.0
        self.value_out = center_value
        self._center = center_value
        self._range = range
        self._half_range = 0.5 * range # 45
        self._dir = direction # 1 or -1 
        self.id = id

        # --- Convert its range in [-1, 1]
        self._sf = 1.0 / self._half_range # 1 / 45

    def get_value_out(self, value_in):
        # --- twist type value is in  [-1, 1]
        self.value = value_in
        self.value_out = int(self._dir * value_in * self._half_range + self._center)
        return self.value_out


def saturate(value, min, max):
    if value <= min:
        return min
    elif value >= max:
        return max
    else:
        return value


class DkLowLevelCtrl:
    def __init__(self):
        rospy.loginfo("Setting Up the Node...")

        # --- Initialize the node
        rospy.init_node("blob_chase_node")

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

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

        self.actuators = {}
        self.actuators["throttle"] = ServoConvert(
            id=1, center_value=350, range=80
        )
        self.actuators["steering"] = ServoConvert(
            id=2, center_value=375, range=80, direction=1
        )  # -- positive left
        rospy.loginfo("> Actuators corrrectly initialized")

        # --- Create a debug publisher for resulting cmd_vel
        self.ros_pub_debug_command = rospy.Publisher(
            "/dkcar/debug/cmd_vel", Twist, queue_size=1
        )
        rospy.loginfo("> Publisher corrrectly initialized")

        # --- Create the Subscriber to Twist commands
        self.ros_sub_twist = rospy.Subscriber(
            "/cmd_vel", Twist, self.update_message_from_command
        )
        rospy.loginfo("> Subscriber corrrectly initialized")

        # --- Create the Subscriber to obstacle_avoidance commands
        self.ros_sub_twist = rospy.Subscriber(
            "/dkcar/control/cmd_vel", Twist, self.update_message_from_chase
        )
        rospy.loginfo("> Subscriber corrrectly initialized")

        self.throttle_cmd = 0.0
        self.throttle_chase = 1.0
        self.steer_cmd = 0.0
        self.steer_chase = 0.0

        self._debud_command_msg = Twist()

        # --- Get the last time e got a commands
        self._last_time_cmd_rcv = time.time()
        self._last_time_chase_rcv = time.time()
        self._timeout_ctrl = 100
        self._timeout_blob = 1

        rospy.loginfo("Initialization complete")

    def update_message_from_command(self, message):
        self._last_time_cmd_rcv = time.time()
        self.throttle_cmd = message.linear.x
        self.steer_cmd = message.angular.z

    def update_message_from_chase(self, message):
        self._last_time_chase_rcv = time.time()
        self.throttle_chase = message.linear.x
        self.steer_chase = message.angular.z
        # print(self.throttle_chase, self.steer_chase)

    def compose_command_velocity(self):
        self.throttle = saturate(self.throttle_cmd * self.throttle_chase, -1, 1)

        # -- Add steering
        self.steer = saturate(self.steer_cmd + self.steer_chase, -1, 1)

        self._debud_command_msg.linear.x = self.throttle
        self._debud_command_msg.angular.z = self.steer

        self.ros_pub_debug_command.publish(self._debud_command_msg)

        self.set_actuators_from_cmdvel(self.throttle, self.steer)

    def set_actuators_from_cmdvel(self, throttle, steering):
        """
        Get a message from cmd_vel, assuming a maximum input of 1
        """
        # -- Convert vel into servo values
        self.actuators["throttle"].get_value_out(throttle)
        self.actuators["steering"].get_value_out(steering)
        # rospy.loginfo("Got a command v = %2.1f  s = %2.1f"%(throttle, steering))

        self.set_pwm_pulse(self.actuators["throttle"].value_out, self.actuators["steering"].value_out)

        print( "throttle value : " + str(self.actuators["throttle"].value_out))
        print( "steering value : " + str(self.actuators["steering"].value_out))


    def set_pwm_pulse(self, speed_pulse, steering_pulse):
        self._throttle.run(speed_pulse)
        self._steering_servo.run(steering_pulse)


    def set_actuators_idle(self):
        # -- Convert vel into servo values
        self.throttle_cmd = 0.0
        self.steer_cmd = 0.0

    def reset_avoid(self):
        self.throttle_chase = 1.0
        self.steer_avoid = 0.0

    @property
    def is_controller_connected(self):
        # print time.time() - self._last_time_cmd_rcv
        return time.time() - self._last_time_cmd_rcv < self._timeout_ctrl

    @property
    def is_chase_connected(self):
        return time.time() - self._last_time_chase_rcv < self._timeout_blob

    def run(self):

        # --- Set the control rate
        rate = rospy.Rate(10)

        while not rospy.is_shutdown():
            self.compose_command_velocity()

            if not self.is_controller_connected:
                self.set_actuators_idle()

            if not self.is_chase_connected:
                self.reset_avoid()

            rate.sleep()


if __name__ == "__main__":
    dk_llc = DkLowLevelCtrl()
    dk_llc.run()

😵 지금껏 봐왔던 예제에 비해 상당히 긴 코드인데요, 하지만 중복되는 부분도 많아 크게 걱정하지 않으셔도 좋습니다. 차근차근 살펴보겠습니다!!

이 프로젝트는 donkeycar와 youtube 프로젝트에서 필요 부분들을 취합하여 만들었다고 보시면 됩니다.

• import

import rospy
import time
from geometry_msgs.msg import Twist

• PCA9685 Class

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
		
		...

이전 게임 컨트롤러 예제와 완전 동일한 클래스입니다. 자세한 설명은 이전 강의를 참조해주세요 🙂

• ServoConvert Class

class ServoConvert:
    def __init__(self, id=1, center_value=370, range=90, direction=1):
        self.value = 0.0
        self.value_out = center_value
        self._center = center_value
        self._range = range
        self._half_range = 0.5 * range # 45
        self._dir = direction # 1 or -1 
        self.id = id

        # --- Convert its range in [-1, 1]
        self._sf = 1.0 / self._half_range # 1 / 45

    def get_value_out(self, value_in):
        # --- twist type value is in  [-1, 1]
        self.value = value_in
        self.value_out = int(self._dir * value_in * self._half_range + self._center)
        # print self.id, self.value_out
        return self.value_out

Twist 형식의 속도 값을 받아 PWM Pulse로 매핑시켜주는 클래스입니다.

• 클래스 멤버 변수들을 살펴보겠습니다.

self.value = 0.0
self.value_out = center_value
self._center = center_value
self._range = range
self._half_range = 0.5 * range # 45
self._dir = direction # 1 or -1 
self.id = id

# --- Convert its range in [-1, 1]
self._sf = 1.0 / self._half_range # 1 / 45

• self.value_out : output value

• self._center : 해당 모터의 영점

• self._range : 해당 모터의 최소 ~ 최대 가동범위

• self._half_range : 가동범위의 절반입니다.

• self._dir : 영점을 기준으로의 방향, 1 혹은 -1 이 됩니다.

다음의 수식이 메인이라고 보시면 됩니다.

-1 ~ 1 사이의 value_in 값이 들어오게 되며, 아래와 같은 수식을 통해 최종 PWM Pulse를 계산합니다.

self.value_out = int(self._dir * value_in * self._half_range + self._center)

예를 들어 봅시다.

• dir = 1

• half_range = 45 (range = 90)

• center = 370

위와 같은 상황에서 value_in이 -1 ~ 1 까지 변화한다고 생각하면

1. value_in = -1 ⇒ {1 * (-1) * 45} + 370 = 325

2. value_in = 1 ⇒ {1 * (1) * 45} + 370 = 415

value_in은 325 ~ 415의 값을 갖게 됩니다.

이 계산은 새로운 Twist 값이 subscribe 될 때마다 실행됩니다.

def set_actuators_from_cmdvel(self, throttle, steering):
        """
        Get a message from cmd_vel, assuming a maximum input of 1
        """
        # -- Convert vel into servo values
        self.actuators["throttle"].get_value_out(throttle)
        self.actuators["steering"].get_value_out(steering)

• DkLowLevelCtrl Class

def run(self):

        # --- Set the control rate
        rate = rospy.Rate(10)

        while not rospy.is_shutdown():
            self.compose_command_velocity()

            # print self._last_time_cmd_rcv, self.is_controller_connected
            if not self.is_controller_connected:
                self.set_actuators_idle()

            if not self.is_chase_connected:
                self.reset_avoid()

            rate.sleep()

if __name__ == "__main__":
    dk_llc = DkLowLevelCtrl()
    dk_llc.run()

가장 길고, 복잡한 class 인데요, 보시다시피 main을 살펴보면, 실제로는 run 함수가 동작되는 것이 보이기에 여기서부터 분석해 보도록 하겠습니다.

예외처리에 해당하는 부분을 살피겠습니다.

self._timeout_ctrl = 100

def set_actuators_idle(self):
    # -- Convert vel into servo values
    self.throttle_cmd = 0.0
    self.steer_cmd = 0.0

def reset_avoid(self):
    self.throttle_chase = 1.0
    self.steer_avoid = 0.0

@property
def is_controller_connected(self):
    # print time.time() - self._last_time_cmd_rcv
    return time.time() - self._last_time_cmd_rcv < self._timeout_ctrl

@property
def is_chase_connected(self):
    return time.time() - self._last_time_chase_rcv < self._timeout_blob

1. 특정 시간동안 외부에서 cmd_vel 제어 신호가 없다면, 제어와 관련된 멤버 변수를 초기 상태로 되돌립니다.

2. 특정 시간동안 추적해야 할 이미지가 보이지 않았다면, 진행 방향을 정면으로 초기화시킵니다.

• compose_command_velocity

def compose_command_velocity(self):
        self.throttle = saturate(self.throttle_cmd * self.throttle_chase, -1, 1)

        # -- Add steering
        self.steer = saturate(self.steer_cmd + self.steer_chase, -1, 1)

        self._debud_command_msg.linear.x = self.throttle
        self._debud_command_msg.angular.z = self.steer

        self.ros_pub_debug_command.publish(self._debud_command_msg)

        self.set_actuators_from_cmdvel(self.throttle, self.steer)

😎 하나하나씩 살펴봅시다.

• saturate

self.throttle = saturate(self.throttle_cmd * self.throttle_chase, -1, 1)
self.steer = saturate(self.steer_cmd + self.steer_chase, -1, 1)

def saturate(value, min, max):
    if value <= min:
        return min
    elif value >= max:
        return max
    else:
        return value

최대, 최소값을 넘지 않도록 한계를 정해주는 함수입니다.

예를 들면, -1.2 ⇒ -1 / 1.3 ⇒ 1 이렇게 변환되겠지요

				# --- Create a debug publisher for resulting cmd_vel
        self.ros_pub_debug_command = rospy.Publisher(
            "/dkcar/debug/cmd_vel", Twist, queue_size=1
        )

				...
				self._debud_command_msg.linear.x = self.throttle
				self._debud_command_msg.angular.z = self.steer
				
				self.ros_pub_debug_command.publish(self._debud_command_msg)

디버깅 토픽으로 publish 하는 모습입니다.

• set_actuators_from_cmdvel

				self.set_actuators_from_cmdvel(self.throttle, self.steer)

    def set_actuators_from_cmdvel(self, throttle, steering):
        """
        Get a message from cmd_vel, assuming a maximum input of 1
        """
        # -- Convert vel into servo values
        self.actuators["throttle"].get_value_out(throttle)
        self.actuators["steering"].get_value_out(steering)

        self.set_pwm_pulse(self.actuators["throttle"].value_out, self.actuators["steering"].value_out)

        print( "throttle value : " + str(self.actuators["throttle"].value_out))
        print( "steering value : " + str(self.actuators["steering"].value_out))

아까 살펴보았던 수식인 get_value_out 가 사용되고 있습니다!!

PWM pulse를 계산하여 최종적으로 set_pwm_pulse를 통해 PCA9685 클래스에 pulse modulation을 하고 있습니다.

• set_pwm_pulse

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

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

def set_pwm_pulse(self, speed_pulse, steering_pulse):
    self._throttle.run(speed_pulse)
    self._steering_servo.run(steering_pulse)

거의 다 살펴보았습니다.

마지막으로 subscriber 생성 부분을 살펴보겠습니다.

				# --- Create the Subscriber to Twist commands
        self.ros_sub_twist = rospy.Subscriber(
            "/cmd_vel", Twist, self.update_message_from_command
        )
        rospy.loginfo("> Subscriber corrrectly initialized")

        # --- Create the Subscriber to obstacle_avoidance commands
        self.ros_sub_twist = rospy.Subscriber(
            "/dkcar/control/cmd_vel", Twist, self.update_message_from_chase
        )
        rospy.loginfo("> Subscriber corrrectly initialized")


		def update_message_from_command(self, message):
        self._last_time_cmd_rcv = time.time()
        self.throttle_cmd = message.linear.x
        self.steer_cmd = message.angular.z

    def update_message_from_chase(self, message):
        self._last_time_chase_rcv = time.time()
        self.throttle_chase = message.linear.x
        self.steer_chase = message.angular.z

앞서 말씀드렸지만, 현 프로젝트는 전진, 후진 제어는 사용자로 부터 따로 받고 있지요?

이 부분에 대해 애로사항이 있으실 것입니다. 그래서, 초음파 센서를 통해 이를 보완하는 또다른 강의를 첨부하니, 다음 강의까지 완료하신 이후 도전해 보시기를 추천드립니다.

ROS and SONARS for OBSTACLE AVOIDANCE | Tutorial #6 | ROS and Raspberry Pi

Let's write our first OBSTACLE AVOIDANCE algorithm, using ROS Kinetic, a Raspberry Pi and our Donkey Car. Thanks to 3 cheap sonar sensors we define new nodes...

https://www.youtube.com/watch?v=JYnMRKVwBuQ&list=PLuteWQUGtU9BU0sQIVqRQa24p-pSBCYNv&index=8

다음 강의에 이어서 나머지 노드들도 분석해 보겠습니다!!