11. [Option] Action Server & Client 작성해보기 – python

🖋️

11. [Option] Action Server & Client 작성해보기 – python

Import

Constructor 분석

Odom의 등장

Action Callback 분석

Image Converter

Maze Action Client

Github Repo

이번 시간에는, 지난 강의에서 살펴보았던 미로찾기 예제의 코드를 분석해보고자 하며, 분석 과정에서, 제가 마주쳤던 문제들과, 디버깅 포인트들도 함께 공유하고자 합니다.

$ roslaunch gcamp_gazebo maze_world.launch

# 새 터미널
$ rosrun action_tutorial maze_action_server.py
==== MazeActionClass Constructed ====
==== Waiting for Client Goal...  ====

# 새 터미널
$ rosrun action_tutorial maze_action_client.py
[INFO] [1610254457.453111, 322.000000]: Action Server Found.../maze_action_server
Enter numbers [or stop] : 

• 우선, 패키지 파일 구조를 살펴볼까요??

action_tutorial/
├── CMakeLists.txt
├── action
│   └── Maze.action
├── launch
│   ├── fibonacci_server.launch
│   └── maze_runner.launch
├── package.xml
└── src
    ├── fibonacci_action_client.py
    ├── fibonacci_action_server.py
    ├── gazebo_handler_test.py
    ├── maze_action_client.py
    ├── maze_action_server.py
    └── mazepkg
        ├── __init__.py
        ├── basic_cmd_vel.py
        ├── gazebo_handler.py
        └── image_converter.py

gcamp_gazebo/
├── CMakeLists.txt
├── launch
│   ├── gazebo_world.launch
│   ├── maze_world.launch
│   └── robot_description.launch
└── ...
    

• maze_world.launch 를 통해 사진과 같은 새로운 world를 gazebo_ros와 함께 사용할 수 있습니다.

• maze_runner.launch 파일은 단순히 action server를 실행시키기 위한 간단한 launch file입니다.

<launch>
    <node pkg="action_tutorial" type="maze_action_server.py" name="maze_action_server" output="screen" />
</launch>

server 부터 살펴보겠습니다. class 설계를 하기 앞서, 해당 클래스에게 필요한 instance, method를 기능에 맞추어 생각해 보겠습니다.

1. client로부터 경로의 배열이 들어오면

2. 해당 방향으로 우선 위치를 틀고 (이전 service 시간에 했던 기능과 동일하네요)

3. 벽이 나올때까지 전진합니다. (이건, topic시간에 parking 예제로 살펴보았습니다.)

4. 경로의 배열 끝에 다다를 때까지 위 과정을 반복합니다. (action으로 설계하면, feedback으로 진행 상황을 공유할 수 있고, 그동안 client는 다른 일을 할 수 있겠지요.)

5. 최종 위치에 도착하면, 초록 박스가 보이는지 확인합니다. (이미지 데이터를 다뤄야 합니다.)

6. 초록 박스에 도달했다면 success, 실패했다면 fail!!

자, 위 상황들을 유념하면서 server의 MazeActionClass를 분석해 보겠습니다.

Import

---

import math
import time
import rospy
import actionlib

from mazepkg.basic_cmd_vel import GoForward, Stop, Turn
from mazepkg.gazebo_handler import GazeboResetSimulation
from mazepkg.image_converter import ImageConverter

from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
from tf.transformations import euler_from_quaternion

from action_tutorial.msg import MazeAction, MazeFeedback, MazeResult

Constructor 분석

---

class MazeActionClass(object):

    _feedback = MazeFeedback()
    _result = MazeResult()

    def __init__(self, name):
        self._action_name = name
        self._yaw = 0.0
        self._scan = []
        self._current_direction = 3  # 0 1 2 3

        self._cmd_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
        self._odom_sub = rospy.Subscriber("/odom", Odometry, self.odom_callback)
        self._scan_sub = rospy.Subscriber("/scan", LaserScan, self.scan_calback)
        # self._image_sub = rospy.Subscriber('')
        self._action_server = actionlib.SimpleActionServer(
            self._action_name, MazeAction, execute_cb=self.ac_callback, auto_start=False
        )
        self._action_server.start()

        self._rate = rospy.Rate(5)

        print("==== MazeActionClass Constructed ====")
        print("==== Waiting for Client Goal...  ====")

• 외부에서도 접근이 가능하게 static variable로 _feedback, _result를 담고 있으며, ActionServer를 제외하고 크게 세가지 class를 생성하고 있습니다.
  • _cmd_pub : 로봇에게 이동 명령을 줍니다.
  • _odom_sub: 뒤에 이야기하겠지만 로봇의 회전을 보다 정확하게 하기 위해서 사용합니다.
  • _scan_sub : parking 예제와 같이 lidar data를 받기 위해 사용됩니다.

self._odom_sub = rospy.Subscriber("/odom", Odometry, self.odom_callback)

위 부분이 왜 들어갔는지에 대해 설명해보겠습니다.

while time.time() - start_time < request.time_duration:
        velocity_publisher.publish(cmd_vel)

    print("Done ...")
    cmd_vel.angular.z = 0.0
    velocity_publisher.publish(cmd_vel)

이전, turning service server의 일부인데요, python time 패키지를 사용하여 특정 시간만큼 로봇을 이동시키고 있습니다.

위 방식도 나쁘지는 않지만, 문제점이 있습니다.

• 바로 오차가 누적된다는 점입니다. (정확성)

예를 들어, 눈을 감고, 똑바로 걸어보면, 아무리 내가 반듯하게 걸었다 할지라고 눈을 떠보면 실상 걸어온 길이 반듯하지 않지요?

한걸음 한걸음은 또바로 걸었을지 모르겠지만, 조금씩의 오차가 모여 결과의 큰 차이를 만드는 경우입니다.

우리 로봇도 미로 안에서 계속 회전을 하면서 오차가 누적될 것입니다.

그럼, 이를 어떻게 해결할까요?

Odom의 등장

---

image from : answer.ros.org

self._odom_sub = rospy.Subscriber("/odom", Odometry, self.odom_callback)

(...)

def odom_callback(self, data):
        orientation_q = data.pose.pose.orientation
        orientation_list = [
            orientation_q.x,
            orientation_q.y,
            orientation_q.z,
            orientation_q.w,
        ]
        _, _, self._yaw = euler_from_quaternion(orientation_list)

지금 우리는 gazebo 시뮬레이션을 사용하고 있기 때문에, 로봇의 절대적인 위치, 방향을 알아낼 수 있습니다. 👍👍 그리고, 이렇게 도출된 데이터가 바로 /odom topic안에 담겨있습니다. 대신 계산의 편의를 위해 quaternion으로 들어있으므로 다시금 euler angle로 바꿔주었습니다.

💡

ROS에서 이미 두 각도 사이의 변환을 제공하고 있습니다. ⇒ tf.transformations

from tf.transformations import euler_from_quaternion, quaternion_from_euler

시뮬레이션이 아닌, 실제 로봇은 이를 어떻게 알아낼까요?

사실 이는 로보틱스에서 해결하고하는 주된 문제 중 하나입니다. Localization in Robotics이라는 이름으로 검색해 보시면 활발하게 진행중인 연구들을 찾아보실 수 있습니다.

def robot_turn(self, euler_angle):
        target_rad = euler_angle * math.pi / 180

        turn_offset = 100
        self._rate.sleep()

        while abs(turn_offset) > 0.005:
            turn_offset = 0.7 * (target_rad - self._yaw)

            Turn.angular.z = turn_offset
            self._cmd_pub.publish(Turn)

        self._cmd_pub.publish(Stop)

결론적으로, 미로찾기 예제에서는 로봇의 회전을 odom을 이용해 보다 정확하게 구현하였습니다.

• 여기서의 turn_offset = 0.7 * (target_rad - self._yaw) 은 P Gain Control 이며,

  이 또한 PID Control이라는 이름으로 검색해 보시면 흥미로운 내용들을 습득하실 수 있을 것입니다. 😉

• Matlab - Understanding PID Control, Part 1: What is PID Control?

Understanding PID Control, Part 1: What is PID Control?

Explore the fundamentals behind PID control. - Download Code Examples to Learn How to Automatically Tune PID Controller Gainshttps: http://bit.ly/2HKBh12Chan...

https://www.youtube.com/watch?v=wkfEZmsQqiA

Action Callback 분석

---

• sequence loop

Maze.action

int32[] turning_sequence
---
bool success
---
string feedback_msg

def ac_callback(self, goal):
        success = True
        print("==== Maze Action Server Executing ====")

				# python enumerate
        for i, val in enumerate(goal.turning_sequence):
            # check that preempt has not been requested by the client
            if self._action_server.is_preempt_requested():
                rospy.logwarn("%s: Preempted" % self._action_name)
                self._action_server.set_preempted()
                success = False
                break
						
						# 터미널에 표시될 메세지
            self._feedback.feedback_msg = "Turning to " + direction_str_dict[val]
            self._action_server.publish_feedback(self._feedback)

						# 회전과 전진을 반복합니다.
            print("Turning Sequence : " + str(val))
            self.robot_turn(direction_dict[val])

            self._feedback.feedback_msg = "Moving Forward ..."
            self._action_server.publish_feedback(self._feedback)
            self.robot_go_forward()

            self._rate.sleep()

대부분의 코드는 이전 시간에 살펴보았으며, 양은 많지만 단순히 goal.turning_sequence를 선회하면서 회전, 전진을 반복시키는 부분임을 알 수 있습니다.

• robot_go_forward

def robot_go_forward(self):
        self._rate.sleep()

        while self._scan[360] > 0.9:
            self._cmd_pub.publish(GoForward)
        self._cmd_pub.publish(Stop)

• Success Condition

				if success:
            ic = ImageConverter()
            center_pixel = ic.center_pixel

            if sum(center_pixel) < 300 and center_pixel[1] > 100:
                self._result.success = True
                rospy.loginfo("Maze Escape Succeeded")
            else:
                self._result.success = False
                rospy.logerr("Maze Escape Failed")

            self._action_server.set_succeeded(self._result)

주어진 이동경로대로 로봇이 임무를 완료한 순간!! 초록 박스의 유무에 따라 최종 임무 성패를 결정짓는다고 했었지요? 위 코드가 바로 그 부분입니다.

• center_pixel은 도착 시, 로봇에 장착된 카메라 이미지 중심 픽셀값입니다. 이미지 픽셀을 다루기 위해 OpenCV를 사용하였으며, cv::Mat 포맷에 따라 녹색에 해당하는 값을 기준으로 조건을 걸었습니다.

OpenCV에 대한 깊은 설명은 생략하고, ROS의 image message를 cv::Mat로 전환하는 부분을 짚고 넘어가겠습니다.

Image Converter

---

#! /usr/bin/env python

import rospy
import rospy
import cv2

from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image


class ImageConverter:
    def __init__(self):
        # self.image_pub = rospy.Publisher("image_topic_2", Image, queue_size=1)

        self._center_pixel = []
        self._bridge = CvBridge()
        self._image_sub = rospy.Subscriber("camera/rgb/image_raw", Image, self.callback)

    def callback(self, data):
        try:
            cv_image = self._bridge.imgmsg_to_cv2(data, "bgr8")
        except CvBridgeError as e:
            print(e)

        # green
        # [  0 110   0]
        # gray
        # [105 105 105]

        self._center_pixel = cv_image[400, 400]

    @property
    def center_pixel(self):
        self._rate = rospy.Rate(5)
        self._rate.sleep()
        return self._center_pixel

bgr8 형식으로 바꾸었기 때문에, center_pixel안에는 blue, green, red 순으로 pixel 값이 채워져 있습니다.

• 나머지 코드들은 기존과 큰 차이가 없으며, 다음과 같이 방향을 나타내기 위해 사용한 dictionary등이 있습니다.

direction_dict = {0: -90, 1: 180, 2: 90, 3: 0}
direction_str_dict = {0: "Up", 1: "Right", 2: "Down", 3: "Left"}

좋습니다! 그럼 client로 이동해 보겠습니다!! 🥳🥳

Maze Action Client

---

#! /usr/bin/env python

"""
action client node for maze gazebo example

referenced from robotigniteacademy
url : https://get-help.robotigniteacademy.com/t/understanding-ros-actions-client-class-exercise/1498

list input part is referenced from geeksforgeeks
url : https://www.geeksforgeeks.org/python-get-a-list-as-input-from-user/
"""

import time
import rospy
import actionlib

from enum import IntEnum
from action_tutorial.msg import MazeAction, MazeGoal
from tf.transformations import euler_from_quaternion, quaternion_from_euler

# For more detail, search actionlib_msgs/GoalStatus
class ActionState(IntEnum):
    PENDING = 0
    ACTIVE = 1
    PREEMPTED = 2
    SUCCEEDED = 3
    ABORTED = 4
    REJECTED = 5
    PREEMPTING = 6
    RECALLING = 7
    RECALLED = 8
    LOST = 9


def fb_callback(feedback):
    print(feedback)


action_server_name = "/maze_action_server"
rospy.init_node("maze_action_client")
action_client = actionlib.SimpleActionClient(action_server_name, MazeAction)

rospy.loginfo("Action Server Found..." + action_server_name)

goal = MazeGoal()
user_list = []


# try block to handle the exception
try:
    print("Enter numbers [or stop] : ")

    while True:
        user_list.append(int(input()))
# if the input is not-integer, just print the list
except:
    print("Your sequence list : ", user_list)

goal.turning_sequence = user_list

action_client.send_goal(goal, feedback_cb=fb_callback)
state_result = action_client.get_state()

rate = rospy.Rate(1)
rospy.loginfo("State Result from Server : " + str(state_result))

while state_result < ActionState.PREEMPTED:
    # Doing Stuff while waiting for the Server to give a result....
    rate.sleep()
    state_result = action_client.get_state()

if state_result == ActionState.SUCCEEDED:
    rospy.loginfo("Action Done State Result : " + str(state_result))
else:
    rospy.logwarn("Something went wrong, result state : " + str(state_result))

ㄴ 피보나치 예제와 크게 다르지 않지요?

• 차이가 있다면 , Action Status를 enum class로 미리 설정해 두었습니다. (사실 내부적으로는 이미 아래와 같이 정의되어 있답니다.)

from enum import IntEnum

(...)

class ActionState(IntEnum):
    PENDING = 0
    ACTIVE = 1
    PREEMPTED = 2
    SUCCEEDED = 3
    ABORTED = 4
    REJECTED = 5
    PREEMPTING = 6
    RECALLING = 7
    RECALLED = 8
    LOST = 9

• 이와 더불어 로봇 회전 service 예시에서도 보았던 사용자로부터 input을 받는 부분, 그에 따른 예외처리가 추가되었습니다. 🙂

# try block to handle the exception
try:
    print("Enter numbers [or stop] : ")

    while True:
        user_list.append(int(input()))
# if the input is not-integer, just print the list
except:
    print("Your sequence list : ", user_list)

지금까지 미로찾기 Action 예제의 설명이었습니다. 👍 부족한 코드와 설명이기에 부끄럽지만, 재미있는 작업이었습니다 😎😎

Contribution은 언제나 환영합니다!!

Github Repo

---

Road-Balance/gcamp_ros_basic

Contribute to Road-Balance/gcamp_ros_basic development by creating an account on GitHub.

https://github.com/Road-Balance/gcamp_ros_basic