with-RL
← 블로그 목록
로봇 / ROS2024년 1월 28일

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

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

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

1. 회전변환행렬

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

$$\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}$$

2. car_odom package 생성 및 설정하기

$ cd ~/Workspace/ros_ws/src/
$ ros2 pkg create --build-type ament_cmake car_odom
<?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>
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 코드

#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;
}

4. car_odom 테스트

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
$ ros2 run teleop_twist_keyboard teleop_twist_keyboard
$ rqt_graph