URDF를 이용해 만든 로봇에 가상 자동차 연결하기 (2)

이번 포스팅은 URDF를 이용해 만든 로봇에 가상 자동차 연결하기 (1) 과정을 통해서 만들어진 가상의 자동차가 주는 속도 정보를 기반으로 자동차의 현재 위치를 계산하고 그 결과를 Rviz에서 확인할 수 있는 기능을 만드는 과정입니다.

 

 이 포스트는 다음 과정을 완료한 후에 참고하시길 바랍니다.

• URDF를 이용한 간단한 로봇 만들기 (1)
• URDF를 이용한 간단한 로봇 만들기 (2)
• URDF를 이용한 간단한 로봇 만들기 (3)
• URDF를 이용해 만든 로봇에 가상 자동차 연결하기 (1)

1. 회전변환행렬

• 아래 그림과 같이 임의의 점 $P=(x,y)$를 $\theta$ 만큼 회전한 점 $P'= (x', y')$를 어떻게 계산할 수 있을까요?

• 아래와 같은 회전변환행렬을 사용하면 $P'= (x', y')$를 구할 수 있습니다.

$$\begin{pmatrix}x' \\y'\end{pmatrix}=\begin{pmatrix}\cos \theta & -\sin \theta \\\sin \theta & \cos \theta\end{pmatrix}\begin{pmatrix}x \\y\end{pmatrix}$$

• 회전변환행렬에 대한 증명은 위키백과 회전변환행렬을 참고하시면 확인할 수 있습니다.
• 아래 그림과 같이 자동차의 위치가 $P_1^{(g)}=(x_1^{(g)},y_1^{(g)},\theta_{z_1}^{(g)})=(4, 2, \pi/2)$의 위치에서 자동차를 기준으로 x 방향으로 3 미터, y 방향으로 1 미터 반시계 방향으로 $\pi/6$ 회전한 경우를 가정해 보겠습니다. 이와 같은 경우 회전행렬변환을 이용해서 자동차의 위치를 계산할 수 있습니다.

• 자동차의 위치를 계산하는 수식은 다음과 같습니다.

$$\begin{pmatrix}x_2^{(g)} \\y_2^{(g)} \\\theta_{z_2}^{(g)}\end{pmatrix}=\begin{pmatrix}x_1^{(g)} \\y_1^{(g)} \\\theta_{z_1}^{(g)}\end{pmatrix}+\begin{pmatrix}\cos \theta_{z_1}^{(g)} & -\sin \theta_{z_1}^{(g)} & 0\\\sin \theta_{z_1}^{(g)} & \cos \theta_{z_1}^{(g)} & 0 \\0 & 0 & 1\end{pmatrix}\begin{pmatrix}\Delta x^{(c)} \\\Delta y^{(c)} \\
\Delta \theta_z^{(c)}\end{pmatrix}$$

• 위 식에 값을 넣어 계산하면 아래와 같습니다.

$$\begin{pmatrix}x_2^{(g)} \\y_2^{(g)} \\\theta_{z_2}^{(g)}\end{pmatrix}=\begin{pmatrix}4 \\2 \\\pi / 2\end{pmatrix}+\begin{pmatrix}0 & -1 & 0\\1 & 0 & 0 \\0 & 0 & 1\end{pmatrix}\begin{pmatrix}3 \\1 \\\pi/6\end{pmatrix}
=\begin{pmatrix}3 \\5 \\2\pi/3\end{pmatrix}$$

• 이와 같은 방식으로 로봇의 위치를 계산하는 방식을 Odometry라고 합니다.

2. car_odom package 생성 및 설정하기

• Odometry를 직접 구현하는 절차입니다.
• 터미널에서 아래 명령을 실행해서 car_odom package를 생성합니다. 아래 명령에서 build_type을 C++ 형식으로 지정했는데 이유는 python으로 처리할 경우 실행에 지연이 발생해서 계산이 부정확해지는 이슈가 있기 때문에 상대적으로 빠른 C++로 기능을 만들기 위해서입니다.

$ cd ~/Workspace/ros_ws/src/
$ ros2 pkg create --build-type ament_cmake car_odom

• 'src/car_odom/package.xml' 파일을 아래와 같이 편집합니다.

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>car_odom</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="cchyun@todo.todo">cchyun</maintainer>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <depend>geometry_msgs</depend>
  <depend>rclcpp</depend>
  <depend>tf2</depend>
  <depend>tf2_ros</depend>
  <depend>nav_msgs</depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

• 'src/car_odom/CMakeLists.txt' 파일을 아래와 같이 편집합니다.

cmake_minimum_required(VERSION 3.8)
project(car_odom)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.

find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(rclcpp  REQUIRED)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

add_executable(car_odom src/car_odom.cpp)
ament_target_dependencies(car_odom rclcpp tf2 tf2_ros nav_msgs geometry_msgs)
install(TARGETS
  car_odom
	DESTINATION lib/${PROJECT_NAME}
)

ament_package()

3. car_odom 코드

• Odometry는 아래 그림과 같이 Driver에서 속도/방향에 대한 측정값을 보내면 이 값을 기준으로 회전변환행렬을 이용해서 자동차의 실제 위치를 계산하는 역할을 수행합니다.

• 'src/car_odom/src/car_odom.cpp' 파일을 만들고 아래와 같이 편집합니다.

#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>


using std::placeholders::_1;


class CarOdom:public rclcpp::Node
{
    private:
        rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr sub_vel_raw;
        std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster;

        double scale_x = 1.0;
        double scale_y = 1.0;
        double scale_z = 1.0;

        double delta_time = 0.0;
        double vel_x = 0.0;
        double vel_y = 0.0;
        double vel_z = 0.0;
        double delta_x = 0.0;
        double delta_y = 0.0;
        double delta_z = 0.0;
        double pos_x = 0.0;
        double pos_y = 0.0;
        double pos_z = 0.0;

        rclcpp::Time last_time;
        geometry_msgs::msg::TransformStamped tf;

    public:
        CarOdom() : Node("car_odom") {
            this->declare_parameter<double>("scale_x", 1.0);
            this->declare_parameter<double>("scale_y", 1.0);
            this->declare_parameter<double>("scale_z", 1.0);

            this->get_parameter<double>("scale_x", scale_x);
            this->get_parameter<double>("scale_y", scale_y);
            this->get_parameter<double>("scale_z", scale_z);

            tf.header.frame_id = "odom";
            tf.child_frame_id = "base_link";

            tf_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
            sub_vel_raw = this->create_subscription<geometry_msgs::msg::TwistStamped>("vel_raw", 10, std::bind(&CarOdom::vel_raw_callback, this, _1));
        }
    
    private:
        void vel_raw_callback(const std::shared_ptr<geometry_msgs::msg::TwistStamped> msg) {
            // delta time
            rclcpp::Time curr_time = msg->header.stamp;
            if (last_time.seconds() == 0) {
                last_time = curr_time;
                return;
            }
            delta_time = (curr_time - last_time).seconds();
            last_time = curr_time;
            // recv value
            vel_x = msg->twist.linear.x * scale_x;
    		vel_y = msg->twist.linear.y * scale_y;
    		vel_z = msg->twist.angular.z * scale_z;
            // delta value
            delta_x = (vel_x * cos(pos_z) - vel_y * sin(pos_z)) * delta_time;
            delta_y = (vel_x * sin(pos_z) + vel_y * cos(pos_z)) * delta_time;
            delta_z = vel_z * delta_time;
            // position value
            pos_x += delta_x;
    		pos_y += delta_y;
			pos_z += delta_z;
            // Euler to Quaternion
            tf2::Quaternion quaternion;
            quaternion.setRPY(0.00, 0.00, pos_z);

            // publish tf
            tf.header.stamp = curr_time;
            tf.transform.translation.x = pos_x;
            tf.transform.translation.y = pos_y;
            tf.transform.translation.z = 0.0;
            tf.transform.rotation.x = quaternion.x();
            tf.transform.rotation.y = quaternion.y();
            tf.transform.rotation.z = quaternion.z();
            tf.transform.rotation.w = quaternion.w();
            tf_broadcaster->sendTransform(tf);
        }
};

int main(int argc, char * argv[])
{
	rclcpp::init(argc, argv);
	rclcpp::spin(std::make_shared<CarOdom>());
	rclcpp::shutdown();
    return 0;
}

• 위 코드에서 63 ~ 65라인 코드가 회전변화행렬을 이용해서 차량의 위치를 계산하는 코드입니다.

• 이렇게 변환된 좌표를 tf Topic을 이용해서 전달합니다.

4. car_odom 테스트

• 새로 추가한 car_odom을 테스트하는 과정입니다.
• 'src/car_tutorial/launch/fake.launch.py' 파일을 아래와 같이 편집합니다. car_odom Node가 실행되도록 추가했습니다.

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
import xacro

def generate_launch_description():
    package_name = 'car_tutorial'

    # robot_state_publisher
    pkg_path = os.path.join(get_package_share_directory(package_name))
    xacro_file = os.path.join(pkg_path, 'urdf', 'car.xacro')
    robot_description = xacro.process_file(xacro_file)
    params = {'robot_description': robot_description.toxml(), 'use_sim_time': False}

    rsp = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        output='screen',
        parameters=[params],
    )

    # fake driver
    fake_driver = Node(
        package='car_tutorial',
        executable='fake_driver',
        output='screen',
        parameters=[],
    )

    # odometry publisher
    odometry_publisher = Node(
        package='car_odom',
        executable='car_odom',
        output='screen',
        parameters=[],
    )

    # rviz2
    rviz = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        arguments=['-d', 'src/car_tutorial/config/car.rviz'],
    )

    return LaunchDescription(
        [
            rsp,
            fake_driver,
            odometry_publisher,
            rviz,
        ]
    )

• 첫 번째 터미널에서 아래 명령을 실행합니다.

$ cd ~/Workspace/ros_ws
$ colcon build --symlink-install
$ source install/setup.bash
$ ros2 launch car_tutorial fake.launch.py

• Rviz에서 Fixed Frame을 'odom'으로 변경합니다.

• 두 번째 터미널에서 아래 명령을 실행합니다.

$ ros2 run teleop_twist_keyboard teleop_twist_keyboard

• 두 번째 터미널에서 키를 입력해서 자동차를 조정합니다. 아래 그림과 같이 자동차가 이동하는 것을 확인할 수 있습니다. car_odom Node에서 계산한 차량의 위치를 Rviz가 표현하고 있는 겁니다.

• 세 번째 터미널에서 아래 명령을 실행합니다.

$ rqt_graph

• 아래 그림과 같이 car_odom이 fake_driver로부터 /cmd_raw Topic을 받아서 변환 후 /tf Topic을 Rviz에게 전달하는 것을 확인할 수 있습니다.