무작정 공부하기 - 혼자 공부하는 로봇 SW 따라하기 (11)

썸네일

이 글에서는 Ch11. 디버그와 관찰을 위한 여러 도구들 에 대해서 써보고자 한다.

 

1. 목적

실제로 시스템을 공부하거나 개발할 때 조금만 복잡해도 의도한 동작이 구현되지 않는 경우가 있다.

그러한 때에 유용한 도구가 로그이다. 시간 정보와 잘 기록된 로그 정보를 보면 효율적으로 로봇을 점검할 수 있다.

 

이 글에서는 이럴 때 사용할 수 있는 도구인 로그에 대해서 어떻게 사용을 하는지 알아보고, rosbag과 GUI 환경에서 지금까지 다룬 ROS 명령을 사용할 수 있게 해주는 rqt 등도 다뤄볼 것이다.

 

2. 로그

(1) 간단히 rqt_console을 이용해 로그 확인하기

혹시라도 오류가 나지 않기 위해서 rqt 패키지를 설치한다.

다음 명령어를 통해서 간단하게 설치 가능하다.

sudo apt install ros-humble-rqt*

이후 turtlesim과 rqt를 열어보자.

 

rqt를 열면 다음과 같은 창이 나오게 된다.

여기서 Plugins의 Logging의 Console을 선택한다.

이 화면에서 다음 명령으로 직선으로 주행하도록 시킨다.

ros2 topic pub -r 1 /turtle1/cmd_vel geometry_msgs/msg/Twist "{linear: {x: 2, y: 0, z: 0.}, angular: {x: 0, y: 0, z: 0.}}"

그리고 벽에 부딪히면 다음과 같이 [WARN] 메시지가 나오면서 메시지가 보이고, 그 메시지가 rqt에도 나오는 것을 볼 수 있다.

 

(2) 로그 메시지 직접 만들기

dist_turtle_action_server.py 파일에 DistTurtleServer 클래스의 던더 init 함수에 클래스가 시작된다는 것을 로그 레벨 info로 남기고자 한다.

다음 코드를 추가해보자.

self.get_logger().info('Dist turtle action server is started.')

이후 실행해 보면 메시지가 출력되는 모습을 볼 수 있다.

 

그리고 다시 quantile_time과 almost_time 변수를 로그로 초기치를 출력하는 코드를 작성해보고자 한다.

다음과 같이 코드를 변경해 보자.

 

전체 코드는 다음과 같다.

import rclpy as rp
from rclpy.action import ActionServer
from rclpy.executors import MultiThreadedExecutor
from rclpy.node import Node

from turtlesim.msg import Pose
from geometry_msgs.msg import Twist
from my_first_package_msgs.action import DistTurtle
from my_first_package.my_subscriber import TurtlesimSubscriber

from rcl_interfaces.msg import SetParametersResult

import math
import time

class TurtleSub_Action(TurtlesimSubscriber):
    def __init__(self, ac_server):
        super().__init__()
        self.ac_server = ac_server

    def callback(self, msg):
        self.ac_server.current_pose = msg

class DistTurtleServer(Node):
    def __init__(self):
        super().__init__('dist_turtle_action_server')
        self.total_dist = 0
        self.is_first_time = True
        self.current_pose = Pose()
        self.previous_pose = Pose()
        self.publisher = self.create_publisher(Twist, '/turtle1/cmd_vel', 10)
        self.action_server = ActionServer(self, DistTurtle, 'dist_turtle', self.excute_callback)

        self.get_logger().info('Dist turtle action server is started.')

        self.declare_parameter('quatile_time', 0.75)
        self.declare_parameter('almost_goal_time', 0.95)

        (quantile_time, almosts_time) = self.get_parameters(
                                            ['quatile_time', 'almost_goal_time'])
        self.quantile_time = quantile_time.value
        self.almosts_time = almosts_time.value

        output_msg = "quantile_time is " + str(self.quantile_time) + ". "
        output_msg = output_msg + "and almost_goal_time is " + str(self.almosts_time) + ". "
        self.get_logger().info(output_msg)

        self.add_on_set_parameters_callback(self.parameter_callback)

    def parameter_callback(self, params):
        for param in params:
            print(param.name, " is changed to ", param.value)

            if param.name == 'quatile_time':
                self.quantile_time = param.value
            if param.name == 'almost_goal_time':
                self.almosts_time = param.value

        output_msg = "quantile_time is " + str(self.quantile_time) + ". "
        output_msg = output_msg + "and almost_goal_time is " + str(self.almosts_time) + ". "
        self.get_logger().info(output_msg)

        return SetParametersResult(successful=True)

    def calc_diff_pose(self):
        if self.is_first_time:
            self.previous_pose.x = self.current_pose.x
            self.previous_pose.y = self.current_pose.y
            self.is_first_time = False

        diff_dist = math.sqrt((self.current_pose.x - self.previous_pose.x)**2 +\
                             (self.current_pose.y - self.previous_pose.y)**2)

        self.previous_pose = self.current_pose

        return diff_dist

    def excute_callback(self, goal_handle):
        feedback_msg = DistTurtle.Feedback()

        msg = Twist()
        msg.linear.x = goal_handle.request.linear_x
        msg.angular.z = goal_handle.request.angular_z

        while True:
            self.total_dist += self.calc_diff_pose()
            feedback_msg.remained_dist = goal_handle.request.dist - self.total_dist
            goal_handle.publish_feedback(feedback_msg)
            self.publisher.publish(msg)

            tmp = feedback_msg.remained_dist - goal_handle.request.dist * self.quantile_time
            tmp = abs(tmp)

            if tmp < 0.02:
                output_msg = 'The turtle passes the ' + str(self.quantile_time) + ' point. '
                output_msg = output_msg + ' : ' + str(tmp)
                self.get_logger().info(output_msg)

            time.sleep(0.01)

            if feedback_msg.remained_dist < 0.2:
                break

        goal_handle.succeed()
        result = DistTurtle.Result()

        result.pos_x = self.current_pose.x
        result.pos_y = self.current_pose.y
        result.pos_theta = self.current_pose.theta
        result.result_dist = self.total_dist

        self.total_dist = 0
        self.is_first_time = True

        return result


def main(args=None):
    rp.init(args=args)

    executor = MultiThreadedExecutor()

    ac = DistTurtleServer()
    sub = TurtleSub_Action(ac_server = ac)

    executor.add_node(sub)
    executor.add_node(ac)

    try:
        executor.spin()

    finally:
        executor.shutdown()
        sub.destroy_node()
        ac.destroy_node()
        rp.shutdown()


if __name__ == '__main__':
    main()

 

이후 dist_turtle_action_server, turtlesim, ros2 action(2,2,2), rqt를 실행해 보자.

 

다음과 같이 rqt만 실행 한 뒤 turtlesim, dist_turtle_action_server순으로 실행하고, ros2 action을 실행한다.

그러면 다음과 같이 로그 정보가 제대로 출력되는 것을 볼 수 있다.

이렇게 진행하면 다음과 같은 결과를 볼 수 있다.

 

3. rqt

(1) rqt_graph

rqt_graph를 활용하기 위해서 turtlesim, rqt_grph, ros2 topic 만 실행해 보자.

그리고 rqt_graph를 이용하면 다음과 같은 모습을 볼 수 있다.

 

(2) rqt_plot

이후 rqt_graph를 중단하고 rqt를 실행한 뒤 Plugins의 Visualization의 Plot을 선택해 보자.

이후 topic 부분에 원하는 토픽을 입력하고 +버튼을 누르면 추가되고, -버튼을 누르면 선택해서 제거 가능하다.

 

 

(3) topic monitor

그리고 Plugins의 Topics의 Topic Monitor을 보면 토픽리스트와 원하는 토픽을 선택하여 관찰할 수 있다.

 

(4) topic publisher

topic publisher을 선택해서 보면 발행하고자 하는 토픽을 선택하고 그 값을 변경할 수 있다.

원하는 값으로 변경하고 체크 박스를 클릭하면 그 토픽이 발행된다.

 

그러면 다음과 같이 토픽이 중첩되는 모습을 볼 수 있다.

 

4. rosbag

이렇게 실행한 뒤 새 터미널에 다음과 같이 입력하여 토픽을 기록한다.

ros2 bag record -o turtle_test -a

이후 적절한 타이밍에 ctrl+c로 종료한다.

 

다시 turtlesim을 실행하고 다음 명령을 통해서 저장한 토픽을 다시 발행할 수 있다.

ros2 bag play turtle_test

또한 rqt의 Logging의 Bag에서 저장한 폴더를 클릭하면 토픽이 보이고, publish를 클릭하면 토픽이 발행된다.

이를 통해서 토픽의 발행 타이밍을 관찰 할 수 있다.

 

5. ROSLAUNCH

ROS에서는 쉽게 여러 노드를 실행하거나 손쉽게 노드와 파라미터의 설정을 지정할 수 있는 roslaunch가 있다.

(1) roslaunch 기본

my_first_package 폴더에 launch 폴더를 만들고 다음과 같은 파일을 작성한다.

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription(
        [
            Node(
                namespace= "turtlesim", package='turtlesim',
                executable= "turtlesim_node", output='screen'),
            Node(
                namespace= "pub_cmd_vel", package='my_first_package',
                executable='my_publisher', output='screen'),
        ]
    )

이후 이를 setup.py에 등록한다.

그리고 네임스페이스를 따로 지정했으므로 my_publisher.py 파일을 열어서 토픽 이름을 /turtlesim/turtle1/cmd_vel 로 변경한다.

이후 빌드하고 ros2 launch 를 통해서 py 파일을 실행한다.

 

이를 보면 한 번에 실행된 것을 볼 수 있으며, rqt_graph로 확인하면 다음과 같은 모습을 볼 수 있다.

여기서 지정한 네임스페이스가 정상적으로 나온다는 것을 확인할 수 있다.

 

(2) parameter 지정하기

ros launch에서 파라미터를 지정하기 위해서 LaunchDescription으로 선언만 한 다음, add_action을 통해서 추가한다.

다음과 같이 작성하고 빌드하면 정상적으로 변경되는 것을 확인할 수 있다.

 

파일 이름은 dist_turtle_action.launch.py로 하였다.

from launch import LaunchDescription
from launch_ros.actions import Node, ExcuteProcess

def generate_launch_description():
    my_launch = LaunchDescription()

    turtlesim_node = Node(
        package='turtlesim', 
        executable='turtlesim_node', 
        output='screen',
        parameters=[
            {"background_r": 255},
            {"background_g": 192},
            {"background_b": 203},
        ]
    )

    dist_turtle_action_node = Node(
        package='my_first_package', 
        executable='dist_turtle_action_server', 
        output='screen',
    )

    my_launch.add_action(turtlesim_node)
    my_launch.add_action(dist_turtle_action_node)

    return my_launch

 

6. 마무리

이것으로 무작정 따라 해보는 시간을 가져보았다.

이것을 시작하기 전에 저자이신 민형기 박사님께 작성해도 되냐고 여쭤보는 등 여러 가지 일들도 있었고, 다 이해할 수 있을지 걱정도 되었지만, 결국 ROS2의 첫 발정도는 내민 거 같아서 기분이 좋다.

 

이제는 다음 권을 기다리면서 다른 책도 공부하고 로봇팔 또한 진행해 볼 예정이다.

 

지금까지의 github은 아래 링크로 첨부하였다.

 

https://github.com/MOSW626/ros2_study.git

GitHub - MOSW626/ros2_study

https://github.com/PinkWink/ros2_basic.git

GitHub - PinkWink/ros2_basic

 

이러한 책을 볼 수 있게 책을 집필해 주신, 민형기 박사님 감사합니다!