[Project#7] 장애물 생성하기

[Project#] 시리즈는 프로젝트(Trash Throwing Simulation with Deep Reinforcement Learning) 제작 과정을 설명한다. 본 포스팅은 Gazebo에 장애물을 생성하는 과정에 대한 내용이다.

모든 내용은 이전 글과 이어진다. 이전 글을 읽어봤다고 가정한 후 글을 작성하였다. (이전 글 LINK)

1. 간단한 모형 생성 후 테스트

1.1. my_obstacle_pkg 생성

cd ~/ros2_ws/src/robotic_arm_environment/
ros2 pkg create --build-type ament_python my_obstacle_pkg

1.2. description 폴더 생성

cd my_obstacle_pkg/
mkdir description

1.3. URDF 파일 생성

cd description/
touch obstacle.urdf

urdf 파일의 내용은 아래와 같다.

<?xml version="1.0"?>
<robot name="obstacle">
  <link name="world"/>

  <joint name="fixed" type="fixed">
    <parent link="world"/>
    <child link="link1"/>
  </joint>

  <link name="link1">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <box size="1 1 1" />
      </geometry>
      <material name="blue">
        <color rgba="0 0 1 1" />
      </material>
    </visual>
    
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <box size="1.01 1.01 1.01" />
      </geometry>
    </collision>
    
    <inertial>
      <mass value="1" />
      <inertia ixx= "1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
</robot>

1.4. Launch 파일 생성

• 파일 생성

      
    cd ~/ros2_ws/src/robotic_arm_environment/my_obstacle_pkg/
    mkdir launch
    cd launch/
    touch my_obstacle.launch.py
  

• 코드

    import os
    import launch
    import launch_ros.actions
      
    from ament_index_python.packages import get_package_share_directory
      
    def generate_launch_description():
      
        pkg_dir = get_package_share_directory('my_obstacle_pkg') 
        urdf_path = os.path.join(pkg_dir, 'description', 'obstacle.urdf')
        urdf = open(urdf_path).read()
          
        robot_state_publisher_node = launch_ros.actions.Node(
            name="robot_state_publisher",
            package="robot_state_publisher",
            executable="robot_state_publisher",
            parameters=[{"robot_description": urdf}]
        )
          
        return launch.LaunchDescription([robot_state_publisher_node])
  

1.5. setup.py 수정

• launch, urdf 파일 경로 설정
• 구체적으로는 import 추가 및 data_files에 launch 파일 경로와 urdf 파일 경로 생성

import os
from glob import glob
from setuptools import setup

package_name = 'my_obstacle_pkg'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*.launch.py'))),
        (os.path.join('share', package_name, 'models', 'urdf'), glob(os.path.join('models','urdf', '*.urdf')))
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='dndqodqks',
    maintainer_email='dndqodqks@gmail.com',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
        ],
    },
)

1.6. Rviz에 출력하기

• launch 파일 실행

    rf
    cb
    ros2 launch my_obstacle_pkg my_obstacle.launch.py
  

• rivz 실행

    # 새 터미널
    rf
    rviz2
  

  

• Fixed Frame 설정

  Displays에서 Fixed Frame을 확인하고자 하는 프레임으로 변경한다. 여기선 link1로 설정하였다.

  

• RobotModel 추가

  왼쪽 아래 부분에 Add 버튼이 있다. Add 버튼을 누르면 시각화를 위한 다양한 도구들이 있다. 여기서 RobotModel을 클릭하고 OK 버튼을 누른다.

  

• Description Topic 설정

  RobotModel을 클릭하면 내부에 Description Topic이라는 부분이 보인다. /robot_description 으로 변환하면 URDF 파일을 시각적으로 볼 수 있다.

  

2. Gazebo에 출력해보기

2.1. URDF 파일 수정

• gazebo에서 색상과 출력 위치를 수정한다.
• gazebo 플러그인을 추가하여 색상을 적용한다.
• xzy를 0 0 0으로 설정하면 world와 물체가 겹쳐서 세탁기 돌아가듯이 회전한다. 그래서 z축의 좌표를 0.5로 설정하였다.

<?xml version="1.0"?>
<robot name="obstacle">
  <link name="world"/>

  <joint name="fixed" type="fixed">
    <parent link="world"/>
    <child link="base_0"/>
  </joint>

  <link name="base_0">
    <visual>
      <origin xyz="0 0 0.5" rpy="0 0 0" />
      <geometry>
        <box size="1 1 1" />
      </geometry>
      <material name="blue">
        <color rgba="0 0 1 1" />
      </material>
    </visual>
    
    <collision>
      <origin xyz="0 0 0.5" rpy="0 0 0" />
      <geometry>
        <box size="1.01 1.01 1.01" />
      </geometry>
    </collision>
    
    <inertial>
      <mass value="1" />
      <inertia ixx= "1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>

  <gazebo reference="base_0">
    <material>Gazebo/Blue</material>
  </gazebo>

</robot>

2.2. 런치 파일 수정

• robot_description 토픽을 바탕으로 장애물을 출력하는 노드를 생성한다.
• Gazebo 실행 노드를 추가한다.

import os
import launch
from launch_ros.actions import Node
from launch.actions import ExecuteProcess
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():

    pkg_dir = get_package_share_directory('my_obstacle_pkg') 
    urdf_path = os.path.join(pkg_dir, 'description', 'obstacle.urdf')
    urdf = open(urdf_path).read()
    
    robot_state_publisher_node = Node(
        name="robot_state_publisher",
        package="robot_state_publisher",
        executable="robot_state_publisher",
        parameters=[{"robot_description": urdf}]
    )
    
    spawn_entity_obstacle = Node(package='gazebo_ros',
                              executable='spawn_entity.py',
                              arguments=['-entity', 'obstacle', '-topic', 'robot_description'],
                              output='screen')
    
    gazebo_node = ExecuteProcess(cmd=['gazebo', '-s', 'libgazebo_ros_factory.so'], output='screen')
    
    return launch.LaunchDescription([robot_state_publisher_node, spawn_entity_obstacle, gazebo_node])

2.3. 빌드 후 런치 파일 실행

rf
cb
ros2 launch my_obstacle_pkg my_obstacle.launch.py

3. 장애물 형태, 좌표 수정 및 메인 런치 파일에 결합하기

3.1. URDF 파일 수정

• 장애물 형태 및 좌표 수정

<?xml version="1.0"?>
<robot name="obstacle">
  <link name="world"/>

  <joint name="fixed" type="fixed">
    <parent link="world"/>
    <child link="base_0"/>
    <origin xyz="0 1.1 0.5" rpy="0 0 0" />
  </joint>

  <link name="base_0">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <box size="0.1 1 1" />
      </geometry>
      <material name="blue">
        <color rgba="0 0 1 1" />
      </material>
    </visual>
    
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <box size="0.1 1 1" />
      </geometry>
    </collision>
    
    <inertial>
      <mass value="1" />
      <inertia ixx= "1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>

  <gazebo reference="base_0">
    <material>Gazebo/Blue</material>
  </gazebo>

</robot>

3.2. my_obstacle.launch.py 파일 수정

• 스폰하는 코드만 남기고 모두 삭제하였다. 또한, urdf_path를 넣어서 스폰한다.

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

def generate_launch_description():

    pkg_dir = get_package_share_directory('my_obstacle_pkg') 
    urdf_path = os.path.join(pkg_dir, 'description', 'obstacle.urdf')
    
    spawn_entity_obstacle = Node(package='gazebo_ros',
                              executable='spawn_entity.py',
                              arguments=['-entity', 'obstacle', '-file', urdf_path],
                              output='screen')
    
    return launch.LaunchDescription([spawn_entity_obstacle])

3.3. my_environment.launch.py 파일 수정

• obstacle_object를 생성하는 노드를 추가한다.

import os
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.actions import ExecuteProcess

def generate_launch_description():

	my_doosan_robot_files = get_package_share_directory('my_doosan_pkg')
	my_sphere_files       = get_package_share_directory('my_sphere_pkg')
	my_obstacle_files	  = get_package_share_directory('my_obstacle_pkg')
	my_environmets_files  = get_package_share_directory('my_environment_pkg')

	# Start doosan robot and controller
	doosan_robot = IncludeLaunchDescription(PythonLaunchDescriptionSource(my_doosan_robot_files + '/launch/my_doosan_controller.launch.py')) 
	
	# Start sphere mark
	sphere_mark  = IncludeLaunchDescription(PythonLaunchDescriptionSource(my_sphere_files + '/launch/my_sphere.launch.py')) 

	# Start obstacle object
	obstacle_object = IncludeLaunchDescription(PythonLaunchDescriptionSource(my_obstacle_files + '/launch/my_obstacle.launch.py')) 
 
	'''
	# Start Rviz
	rviz_file = my_environmets_files + "/rviz/my_rviz_env.rviz"
	rviz_node = Node( package='rviz2',
					  executable='rviz2',
					  name='rviz2',
					  output='log',
					  arguments=['-d', rviz_file])
	'''

	# Start Gazebo   
	world_file_name = 'my_world.world'
	world = os.path.join(get_package_share_directory('my_environment_pkg'), 'worlds', world_file_name)
	gazebo_node = ExecuteProcess(cmd=['gazebo', '--verbose', world,'-s', 'libgazebo_ros_factory.so'], output='screen')

	# Node 

	ld = LaunchDescription()

	ld.add_action(doosan_robot)
	ld.add_action(sphere_mark)
	ld.add_action(obstacle_object)
	ld.add_action(gazebo_node)
	#ld.add_action (rviz_node)

	return ld

3.4. 빌드 후 실행

rf
cb
ros2 launch my_environment_pkg my_environment.launch.py

4. 참고 문헌

• https://www.youtube.com/watch?v=14G81BxpGFU
• https://blog.naver.com/PostView.nhn?isHttpsRedirect=true&blogId=heennavi1004&logNo=221591758801&redirect=Dlog&widgetTypeCall=true&directAccess=false
• https://blog.naver.com/PostView.nhn?blogId=jerry1455&logNo=221710210977
• https://velog.io/@legendre13/URDF-형식#2-visual-namename