with-RL
← 블로그 목록
로봇 / ROS2024년 2월 13일

URDF를 이용해 만든 로봇에 Unity 자동차 연결하기 (3)

이번 포스팅은 URDF를 이용해 만든 로봇에 Unity 자동차 연결하기 (2) 과정을 통해서 만들어진 로봇을 Unity 로봇을 ROS와 연동하고 제어해 보는 과정입니다.

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

1. Unity - ROS 연동하기 (Unity)

2. Unity - ROS 연동하기 (ROS)

$ cd ~/Workspace/ros_ws/src
$ unzip ROS-TCP-Endpoint-main-ros2.zip
$ mv ROS-TCP-Endpoint-main-ros2/ ros_tcp_endpoint
$ rm ROS-TCP-Endpoint-main-ros2.zip
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],
    )

    # ros tcp endpoint
    ros_tcp_endpoint = Node(
        package='ros_tcp_endpoint',
        executable='default_server_endpoint',
        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,
            ros_tcp_endpoint,
            odometry_publisher,
            rviz,
        ]
    )
$ cd ~/Workspace/ros_ws
$ colcon build --symlink-install
$ source install/setup.bash
$ ros2 launch car_tutorial unity.launch.py

3. Unity에서 JointStates Topic 발송하기

        public static double NowTimeInSeconds
        {
            get
            {
                return Mode switch
                {
                    //ClockMode.UnityScaled => Time.timeAsDouble + UnityUnscaledTimeSinceFrameStart * Time.timeScale,
                    ClockMode.UnityScaled => SecondsSinceUnixEpoch,
                    // ClockMode.UnityUnscaled => Time.realtimeSinceStartupAsDouble,
                    // ClockMode.UnixEpoch => SecondsSinceUnixEpoch,
                    _ => throw new NotImplementedException()
                };
            }
        }
using System;
using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
using RosMessageTypes.Sensor;
using RosMessageTypes.BuiltinInterfaces;
using Unity.Robotics.Core;

public class JointStatesPublisher : MonoBehaviour
{
    [SerializeField] private string topicName = "joint_states";
    [SerializeField] private float publishFrequency = 0.1f;

    ROSConnection ros;
    private float timeElapsed = 0.0f;

    private JointStateMsg joint_states;

    private void Start()
    {
        ros = ROSConnection.GetOrCreateInstance();
        ros.RegisterPublisher<JointStateMsg>(topicName);

        joint_states = new JointStateMsg();
        joint_states.header.frame_id = "joint_states";
        joint_states.name = new string[] { "left_wheel_joint", "right_wheel_joint" };
        joint_states.position = new double[] { 0.0, 0.0 };
    }

    private void FixedUpdate()
    {
        timeElapsed += Time.deltaTime;

        if (timeElapsed >= publishFrequency)
        {
            timeElapsed = 0;
            // now timestamp
            var now = Clock.Now;
            var stamp = new TimeMsg
            {
                sec = (int)now,
                nanosec = (uint)((now - Math.Floor(now)) * Clock.k_NanoSecondsInSeconds)
            };
            // publish ros topic
            joint_states.header.stamp = stamp;
            ros.Publish(topicName, joint_states);
            // simulate wheel rotate
            joint_states.position[0] += 0.05;
            joint_states.position[1] += 0.05;
        }
    }
}

4. Unity에서 VelRaw Topic 발송하기

using System;
using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
using RosMessageTypes.Geometry;
using RosMessageTypes.BuiltinInterfaces;
using Unity.Robotics.Core;

public class VelRawPublisher : MonoBehaviour
{
    [SerializeField] private string topicName = "vel_raw";
    [SerializeField] private float publishFrequency = 0.1f;

    ROSConnection ros;
    private float timeElapsed = 0.0f;

    private TwistStampedMsg vel_raw;

    private void Start()
    {
        ros = ROSConnection.GetOrCreateInstance();
        ros.RegisterPublisher<TwistStampedMsg>(topicName);

        vel_raw = new TwistStampedMsg();
    }

    private void FixedUpdate()
    {
        timeElapsed += Time.deltaTime;

        if (timeElapsed >= publishFrequency)
        {
            // now timestamp
            var now = Clock.Now;
            var stamp = new TimeMsg
            {
                sec = (int)now,
                nanosec = (uint)((now - Math.Floor(now)) * Clock.k_NanoSecondsInSeconds)
            };
            // cal vel_raw
            vel_raw.twist.linear.x = 0;
            vel_raw.twist.linear.y = 0;
            vel_raw.twist.angular.z = 0;
            // init timeElapsed
            timeElapsed = 0;
            // publish ros topic
            vel_raw.header.stamp = stamp;
            ros.Publish(topicName, vel_raw);
        }
    }
}

5. ROS에서 Key보드로 Unity 자동차 조종하기

using System;
using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
using RosMessageTypes.Geometry;
using RosMessageTypes.BuiltinInterfaces;
using Unity.Robotics.Core;

public class CmdVelSubscriber : MonoBehaviour
{
    [SerializeField] private string topicName = "cmd_vel";
    [SerializeField] private float publishFrequency = 0.1f;

    [SerializeField] private float motorForce;
    [SerializeField] private float horizontalRate;
    [SerializeField] private WheelCollider leftWheelCollider;
    [SerializeField] private WheelCollider rightWheelCollider;

    ROSConnection ros;
    private float timeElapsed = 0.0f;

    private void Start()
    {
        ros = ROSConnection.GetOrCreateInstance();
        ros.Subscribe<TwistMsg>(topicName, CmdVelCallback);
    }

    private void CmdVelCallback(TwistMsg msg)
    {
        leftWheelCollider.motorTorque = (float)(msg.linear.x - msg.angular.z * horizontalRate) * motorForce;
        rightWheelCollider.motorTorque = (float)(msg.linear.x + msg.angular.z * horizontalRate) * motorForce;
    }
}
$ ros2 run teleop_twist_keyboard teleop_twist_keyboard

6. Unity에서 자동차 움직임을 Rivz에 표현하기

using System;
using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
using RosMessageTypes.Geometry;
using RosMessageTypes.BuiltinInterfaces;
using Unity.Robotics.Core;

public class VelRawPublisher : MonoBehaviour
{
    [SerializeField] private string topicName = "vel_raw";
    [SerializeField] private float publishFrequency = 0.1f;

    ROSConnection ros;
    private float timeElapsed = 0.0f;

    private TwistStampedMsg vel_raw;
    private Vector3 prev_position;
    private Quaternion prev_rotation;

    private void Start()
    {
        ros = ROSConnection.GetOrCreateInstance();
        ros.RegisterPublisher<TwistStampedMsg>(topicName);

        vel_raw = new TwistStampedMsg();
    }

    private void FixedUpdate()
    {
        timeElapsed += Time.deltaTime;

        if (timeElapsed >= publishFrequency)
        {
            if (prev_position == null || prev_rotation == null)
            {
                prev_position = transform.position;
                prev_rotation = transform.rotation;
                timeElapsed = 0;
                return;
            }
            // now timestamp
            var now = Clock.Now;
            var stamp = new TimeMsg
            {
                sec = (int)now,
                nanosec = (uint)((now - Math.Floor(now)) * Clock.k_NanoSecondsInSeconds)
            };
            // cal val
            Vector3 distance = transform.position - prev_position;
            float angle = Quaternion.Angle(transform.rotation, prev_rotation);
            // cal vel_raw
            vel_raw.twist.linear.x = distance.z / timeElapsed;
            vel_raw.twist.linear.y = 0;
            vel_raw.twist.angular.z = angle * Mathf.Deg2Rad / timeElapsed;
            // init values
            prev_position = transform.position;
            prev_rotation = transform.rotation;
            timeElapsed = 0;
            // publish ros topic
            vel_raw.header.stamp = stamp;
            ros.Publish(topicName, vel_raw);
        }
    }
}
$ ros2 run teleop_twist_keyboard teleop_twist_keyboard
URDF를 이용해 만든 로봇에 Unity 자동차 연결하기 (3) | with-RL