ROS2 4일차(1) Maze World

Maze World

ros2 launch gcamp_gazebo maze_world.launch.py

#Terminal 1
ros2 run py_action_pkg maze_action_server

#Terminal 2
ros2 run py_action_pkg maze_action_client

로봇이 미로를 탈출하기 위해선

• 지속적으로 로봇의 이동 방향을 알려주기 => Action
• 특정 방향으로 로봇을 회전시키기 => Odom
• 충돌 전까지 로봇을 직진시키기 => LaserScan & Twist
• 초록 박스를 인식하기 => Image

image sub

# Gazebo를 실행시킨 상태에서
$ ros2 run image_view image_view --ros-args --remap /image:=/diffbot/camera_sensor/image_raw

그러나 왜인지는 모르겠지만 이 컴퓨터에는 image_view가 없었음.. 그래서 아래의 코드로 설치를 할 수도 있고

sudo apt update
sudo apt install ros-foxy-image-view

image_view와 비슷한 기능을 하는 image_tools라는 패키지를 검색했더니 아래 두개의 실행파일이 떴다

ros2 pkg executables image_tools
image_tools cam2image
image_tools showimage

그래서 아래의 showimage를 이용해 화면을 띄워보면

ros2 run image_tools showimage --ros-args --remap /image:=/diffbot/camera_sensor/image_raw

image_tools showimage로 받아온 image

 

원래 하려고 했던 image_view로도 똑같은 결과를 얻을 수 있음

ros2 run image_view image_view --ros-args --remap /image:=/diffbot/camera_sensor/image_raw

 

ROS2는 이미지를 다루기 위한 msg 형식을 갖고있다

컴퓨터 비전에서 많이 쓰이는 OpenCV형식을 ROS2의 형식으로 바꿔줘야 하는데 이를 해주는 것이 cv_bridge이다

cv_bridge를 통해 image subscriber 코딩

로봇으로부터 publish되고 있는 image topic

$ ros2 topic list
/clock
/diffbot/camera_sensor/camera_info
/diffbot/camera_sensor/image_raw
/diffbot/cmd_vel
/diffbot/odom
/diffbot/scan
/joint_states
/parameter_events
/robot_description
/rosout
/tf
/tf_static

image_sub.py

# !/usr/bin/env/ python3
#
# Basic ROS 2 program to publish real-time streaming 
# video from your built-in webcam
# Author:
# - Addison Sears-Collins
# - https://automaticaddison.com

# opencv 사용을 위해 필요합니다.
import cv2
import rclpy

from rclpy.node import Node
# 둘 사이의 변환을 담당하는 cv_bridge입니다.
from cv_bridge import (
    CvBridge,
    CvBridgeError,
)  # Package to convert between ROS and OpenCV Images

import rclpy
from rclpy.node import Node
# ROS2의 image 형식입니다.
from sensor_msgs.msg import Image  # Image is the message type

class ImageSubscriber(Node):
    """
    Create an ImageSubscriber class, which is a subclass of the Node class.
    """

    def __init__(self):
        """
        Class constructor to set up the node
        """
        super().__init__("image_subscriber")
        self.sub_period = 10  # Hz

        # topic subscriber를 생성합니다. 기억나지 않는다면 이전 강의를 복습해보시길 추천합니다.
        self.subscription = self.create_subscription(
            Image,
            "/diffbot/camera_sensor/image_raw",
            self.listener_callback,
            self.sub_period,
        )
        self.subscription

        # ROS2 <=> OpenCV를 해주는 cv_bridge 
        self.cv_bridge = CvBridge()

    def listener_callback(self, data):

        # 지금은 ROS2 image를 OpenCV image로 바꿔볼 것입니다.
        try:
            # OpenCV에서 사용하는 여러 색상체계가 있는데요, 지금은 bgr8을 사용합니다.
            current_frame = self.cv_bridge.imgmsg_to_cv2(data, "bgr8")
        except CvBridgeError as e:
            self.get_logger().info(e)

        # 하단 주석을 풀고 다시 실행시켜보세요!!
        # Display image
        cv2.imshow("camera", current_frame)
        cv2.waitKey(1)

        # 현 시점 image의 정 가운데 pixel을 가져옵니다.
        # 후에 녹색 박스를 탐지하는데 쓰입니다.
        self.center_pixel = current_frame[400, 400]


def main(args=None):

    rclpy.init(args=args)

    image_subscriber = ImageSubscriber()

    rclpy.spin(image_subscriber)

    image_subscriber.destroy_node()

    rclpy.shutdown()


if __name__ == "__main__":
    main()

ros2 run py_action_pkg img_subscriber_node

여기서 생기는 의문이 왜 파일이름과 실행이름이 다른거지? 파일이름으로 실행시켜야했던것 아닌가 라는 의문이 생겨서 알아본

결과

setup.py

entry_points={
        'console_scripts': [
            'fibonacci_action_server        = py_action_pkg.fibonacci_action_server:main',
            'fibonacci_action_server_cancel = py_action_pkg.fibonacci_action_server_cancel:main',
            'fibonacci_action_client        = py_action_pkg.fibonacci_action_client:main',
            'fibonacci_action_client_cancel = py_action_pkg.fibonacci_action_client_cancel:main',
            'maze_action_server             = py_action_pkg.maze_action_server:main',
            'maze_action_client             = py_action_pkg.maze_action_client:main',
            'img_subscriber_node            = py_action_pkg.image_sub:main',
            'odom_sub_node                  = py_action_pkg.odom_sub:main',
            'robot_controller               = py_action_pkg.robot_controller:main',
        ],
    },

setup.py 내부에 있는 entry_points가 중요한 것이었다!

 

odom sub

Gazebo 시뮬레이션을 사용하면 로봇의 절대적인 위치, 방향을 알아낼 수 있다.

이렇게 도출된 데이터가 바로 /odom topic안에 담겨있음

대신 odom의 각도 체계는 quaternion으로 되어있으므로 다시 euler angle로 바꿔줘야 한다.

 

로봇으로부터 publish되고 있는 odom topic

ros2 topic list
/clicked_point
/clock
/diffbot/camera_sensor/camera_info
/diffbot/camera_sensor/image_raw
/diffbot/cmd_vel
/diffbot/odom
/diffbot/scan
/goal_pose
/initialpose
/joint_states
/map
/map_metadata
/parameter_events
/robot_description
/rosout
/scan
/slam_toolbox/feedback
/slam_toolbox/graph_visualization
/slam_toolbox/scan_visualization
/slam_toolbox/update
/tf
/tf_static

 

odom_sub.py

#!/usr/bin/env/ python3
#
# Copyright 2021 Seoul Business Agency Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

import cv2

# Odometry는 nav_msgs안에 담겨 있습니다.
from nav_msgs.msg import Odometry  # Odometry is the message type
import numpy as np
import rclpy
from rclpy.node import Node

# quaternion에서 euler angle로 변환하는 함수입니다.
# 주된 내용은 수학 계산이므로 굳이 이해하지 않으셔도 좋습니다.
# https://gist.github.com/salmagro/2e698ad4fbf9dae40244769c5ab74434
def euler_from_quaternion(quaternion):
    """
    Converts quaternion (w in last place) to euler roll, pitch, yaw
    quaternion = [x, y, z, w]
    Bellow should be replaced when porting for ROS 2 Python tf_conversions is done.
    """
    x = quaternion.x
    y = quaternion.y
    z = quaternion.z
    w = quaternion.w

    sinr_cosp = 2 * (w * x + y * z)
    cosr_cosp = 1 - 2 * (x * x + y * y)
    roll = np.arctan2(sinr_cosp, cosr_cosp)

    sinp = 2 * (w * y - z * x)
    pitch = np.arcsin(sinp)

    siny_cosp = 2 * (w * z + x * y)
    cosy_cosp = 1 - 2 * (y * y + z * z)
    yaw = np.arctan2(siny_cosp, cosy_cosp)

    return roll, pitch, yaw


class OdometrySubscriber(Node):
    """
    Create an OdometrySubscriber class, which is a subclass of the Node class.
    """

    def __init__(self):
        """
        Class constructor to set up the node
        """
        # Initiate the Node class's constructor and give it a name
        super().__init__("odom_subscriber")
        self.sub_period = 10  # Hz

        # 로봇으로부터의 odom subscribe
        self.subscription = self.create_subscription(
            Odometry,
            "diffbot/odom",
            self.listener_callback,
            self.sub_period,
        )
        self.subscription  # prevent unused variable warning

    def listener_callback(self, data):
        orientation = data.pose.pose.orientation
        # 우리가 필요한 부분은 yaw 값 뿐입니다.
        _, _, self._yaw = euler_from_quaternion(orientation)

        self.get_logger().info(f"Current Yaw Angle : {self._yaw}")


def main(args=None):

    rclpy.init(args=args)

    Odometry_subscriber = OdometrySubscriber()

    rclpy.spin(Odometry_subscriber)

    Odometry_subscriber.destroy_node()

    rclpy.shutdown()


if __name__ == "__main__":
    main()