ROS2 Gazebo

1. WareHouse 제작

(1) 링크의 내용 중 아마존 aws에서 제작한 warehouse model을 사용하기로 했다. 다른 모델들 대부분은 ROS2를 지원하지 않았고, 이는 수정이 오래걸리는 것으로 예상되기에 채택하지 않았다.

설치방법

# go to your ros2_ws/src
git clone -b foxy-devel https://github.com/aws-robotics/aws-robomaker-small-warehouse-world.git
cd ..
rosws update
rosdep install --from-paths src --ignore-src -r -y
colcon build
# 만약 ~/.bashrc안에 아래의 명령어 넣어놨으면 그걸 사용
source install/setup.bash
ros2 launch aws_robomaker_small_warehouse_world no_roof_small_warehouse_launch.py

성공시 위와 같은 사진이 나온다.

---

2. Turtlebot3 불러오기

위 gazebo world에 turtlebot3를 불러오기 전에, turtlebot3_simulation을 설치하자.

git clone -b foxy-devel https://github.com/ROBOTIS-GIT/turtlebot3_simulations.git
rosdep install --from-paths src --ignore-src -r -y
colcon build
source ~/.bashrc
ros2 launch turtlebot3_gazebo empty_world.launch.py

성공하면 위와 같은 가제보를 볼 수 있다. warehouse파일과 turtlebot 파일이 확실히 되는 것을 알았으니, 다음과 같이 파일을 수정해보자.

1. 'src/aws-robomakers-small-warehouse-world/worlds/no_roof_small_warehouse/no_roof_small_warehouse.world' 마지막에 다음과 같은 코드를 삽입한다.

<include>
  <pose>-2.0 -0.5 0.01 0.0 0.0 0.0</pose>
  <uri>model://turtlebot3_waffle</uri>
</include>

2. 'src/aws-robomakers-small-warehouse-world/models/' 안에 turtlebot3_waffle 혹은 burger 모델을 넣는다. 
위같이 하고 아까와 동일하게 파일을 launch하면 이번에는 터틀봇이 있는 상태로 존재한다.

만일 불안하다면 복사한 파일에서 실행하도록 하자.

---

3. RVIZ에서 보기

위에서 실행한 상태로 RVIZ를 키고 세팅하도록 하자. 그러면 다음과 같은 에러가 주루루루룩 나올 거다. 게다가 tf를 보면 map과 odom만 나올 것이다. 즉 우리는 로봇의 tf를 publish하지 않은 것이다(물론 이상태로도 turtlebot을 움직이는 것은 가능). 정말 고맙게도 우리는 robot_state_publisher를 사용하여 이를 해결할 수 있을 것이다. 특히 로보티스 사에서 launch 파일에 robot_state_publisher를 만들어 주었으니 이를 활용하도록 하자.

즉 우리는 2가지를 launch해야한다. 한가지는 aws패키지, 다른 것은 robot_state_publisher이다. 다음과 같은 launch 파일을 만들도록 하자.
turtlebot3_warehouse.launch.py

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import ExecuteProcess, DeclareLaunchArgument
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PythonExpression
from launch.conditions import IfCondition


TURTLEBOT3_MODEL = os.environ['TURTLEBOT3_MODEL']
WORLD_MODEL = os.environ['WAREHOUSE_MODEL']

def generate_launch_description():
    use_sim_time = LaunchConfiguration('use_sim_time', default='True')

    aws_small_warehouse_dir = os.path.join(get_package_share_directory('aws_robomaker_small_warehouse_world'), 'launch')
    
    if WORLD_MODEL == 'small_warehouse':
        aws_small_warehouse = IncludeLaunchDescription(
            PythonLaunchDescriptionSource([aws_small_warehouse_dir, '/no_roof_small_warehouse_launch.py'])
        )

    elif WORLD_MODEL == 'big_warehouse':
        aws_small_warehouse = IncludeLaunchDescription(
            PythonLaunchDescriptionSource([aws_small_warehouse_dir, '/big_warehouse_launch.py'])
        )


    launch_file_dir = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch')
    robot_state_publisher = IncludeLaunchDescription(
            PythonLaunchDescriptionSource([launch_file_dir, '/robot_state_publisher.launch.py']),
            launch_arguments={'use_sim_time': use_sim_time}.items(),
    )

    ld = LaunchDescription()
    ld.add_action(aws_small_warehouse)
    ld.add_action(robot_state_publisher)

    return ld

이후 빌드하고 실행하면 다음과 같이 rviz로도 내용을 확인할 수 있다.

---

4. Intel Realsense Gazebo Plugin 사용

위에서 한 것들을 좀 더 자세히 생각해보자. 우리는 urdf와 sdf를 정말 이해하고 있는가? 난 이제야 쪼~오금 더 이해가 간다. urdf는 기본적으로 tf 중심으로 짜여져 있으며, 거기서 topic이 나오지 않는다. robot_state_publisher로 tf관계도만 나온다는 것이다. 그렇다면 gazebo 상의 센서값들은 우리가 어디서 보는 것일까? 바로 sdf파일에 작성되어 있는 것들로 본다. 그러므로 만일 우리가 realsense gazebo plugin을 사용하고 싶다? urdf파일도 수정해야 하고 sdf파일도 수정해야 한다는 것과 같다. sdf와 urdf의 차이점은 (2)에서 설명이 되어있다. 요약하면 다음과 같다.

• urdf = Unified Robotic Description Format
• sdf = Simulation Description Format
• why sdf?

  URDF can only specify the kinematic and dynamic properties of a single robot in isolation. URDF can not specify the pose of the robot itself within a world. It is also not a universal description format since it cannot specify joint loops (parallel linkages), and it lacks friction and other properties. Additionally, it cannot specify things that are not robots, such as lights, heightmaps, etc.

결과적으로 가상환경에 알맞는 형식이 아니라고 한다. 그래서 gazebo는 sdf형식의 파일을 만들어서 사용한다고 한다.

• 결론 = rviz에서는 tf형식만! gazebo에서 plugin을 이용한 topic이 나온다.

위의 결론을 보고 나니 우리는 urdf와 sdf를 비교할 필요가 있다. 그래야 위에서 다운로드 받은 intel realsense d435i plugin을 사용할 수 있지 않겠나?

urdf vs sdf

어떤 tag가 사용가능/대체/추가 되는지 간단하게 알아보자.

Required

<link> 태그 안에는 <inertia>가 꼭 있어야 한다.

Optional

* <link> 안에 <gazebo> element를 넣는 것.
  - color 를 gazebo format으로 바꿀 수 있음
  - Add sensor plugins
* Add a <gazebo> element for every <joint>
  - Set proper damping dynamics
  - Add actuator control plugins

적당히 알아보는데, 더 알아보고 싶다면 (2)에서 더 알아보자.

아니 그래서 sdf가 필요한 이유는 알겠는데 그래서 어떻게 realsense를 가제보 상에서 사용할건데?
가제보에 올라가는 것은 결국엔 model.sdf파일이니, 이부분을 수정해보도록하자.

top-level launch = turtlebot3_warehouse.launch.py
second-level launch = no_roof_small_warehouse_launch.py, robot_state_publisher.launch.py
no_roof_small_warehouse_launch.py = gzserver, gzclient, waffle model.sdf
robot_state_publisher.launch.py = urdf parse.

즉 waffle의 model.sdf파일, turtlebot3_waffle_d435i.urdf를 사용함. 둘을 수정해야 한다는 뜻.

더 찾아보니 urdf를 gazebo로 spawn하는 방법이 있다. 단 inertia와 플러그인 다른 방식을 통해 작성한다. 예시는 아래와 같다.

  <xacro:macro name="box_inertia" params="m w h d">
    <inertial>
      <origin xyz="0 0 0" rpy="${pi/2} 0 ${pi/2}"/>      
      <mass value="${m}"/>
      <inertia ixx="${(m/12) * (h*h + d*d)}" ixy="0.0" ixz="0.0" iyy="${(m/12) * (w*w + d*d)}" iyz="0.0" izz="${(m/12) * (w*w + h*h)}"/>
    </inertial>
  </xacro:macro>

  <link name="base_link">
    <visual>
      <geometry>
        <box size="${base_length} ${base_width} ${base_height}"/>
      </geometry>
      <material name="Cyan">
        <color rgba="0 1.0 1.0 1.0"/>
      </material>
    </visual>

    <collision>
      <geometry>
        <box size="${base_length} ${base_width} ${base_height}"/>
      </geometry>
    </collision>

    <xacro:box_inertia m="15" w="${base_width}" d="${base_length}" h="${base_height}"/>
  </link>

이때 xacro, xml macro 같은 것을 통해 간단하게 표시해주자.

4. d435i Plugin 포기.

포기 이유.

1. urdf로 작성된 파일을 sdf로 작성해줘야하는 수고로움이 너무 큼.
2. 만약 ros2의 rtab-map을 맛보고 싶은거라면, depth camera plugin을 사용하면 됨. (4)에서 kinect카메라를 사용함. --> libDepthCameraPlugin.so로 바뀜. 다양한 plugin을 확인하고 싶으면 아래의 폴더에서 확인하자.

• /usr/lib/x86_64-linux-gnu/gazebo-11/plugins

3. isaac sim으로 빠른 시일안에 넘어갈 것.

gazebo는 navigation 파라미터 수정용으로만 사용해보자.

기본 plugin 활용

최종적인 화면이다. 사용법은 정말 간단했다. camera 타입을 바꿔주는 것. 아래와 같다.

camera.sdf

<sensor name="camera" type="camera">
  <always_on>true</always_on>
  <visualize>true</visualize>
  <update_rate>30</update_rate>
  <camera name="intel_realsense_r200">
    <horizontal_fov>1.02974</horizontal_fov>
    <image>
      <width>1920</width>
      <height>1080</height>
      <format>R8G8B8</format>
    </image>
    <clip>
      <near>0.02</near>
      <far>300</far>
    </clip>
    <noise>
      <type>gaussian</type>
      <!-- Noise is sampled independently per pixel on each frame.
                  That pixel's noise value is added to each of its color
                  channels, which at that point lie in the range [0,1]. -->
      <mean>0.0</mean>
      <stddev>0.007</stddev>
    </noise>
  </camera>
  <plugin name="camera_driver" filename="libgazebo_ros_camera.so">
    <ros>
      <!-- <namespace>test_cam</namespace> -->
      <!-- <remapping>image_raw:=image_demo</remapping> -->
      <!-- <remapping>camera_info:=camera_info_demo</remapping> -->
    </ros>
    <!-- camera_name>omit so it defaults to sensor name</camera_name-->
    <!-- frame_name>omit so it defaults to link name</frameName-->
    <!-- <hack_baseline>0.07</hack_baseline> -->
  </plugin>
</sensor>

camera.urdf

  <gazebo reference="camera_link">
    <sensor name="depth_camera" type="depth">
      <visualize>true</visualize>
      <update_rate>30.0</update_rate>
      <camera name="camera">
        <horizontal_fov>1.047198</horizontal_fov>
        <image>
          <width>640</width>
          <height>480</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.05</near>
          <far>3</far>
        </clip>
      </camera>
      <plugin name="depth_camera_controller" filename="libgazebo_ros_camera.so">
        <baseline>0.2</baseline>
        <alwaysOn>true</alwaysOn>
        <updateRate>0.0</updateRate>
        <frame_name>camera_depth_frame</frame_name>
        <pointCloudCutoff>0.5</pointCloudCutoff>
        <pointCloudCutoffMax>3.0</pointCloudCutoffMax>
        <distortionK1>0</distortionK1>
        <distortionK2>0</distortionK2>
        <distortionK3>0</distortionK3>
        <distortionT1>0</distortionT1>
        <distortionT2>0</distortionT2>
        <CxPrime>0</CxPrime>
        <Cx>0</Cx>
        <Cy>0</Cy>
        <focalLength>0</focalLength>
        <hackBaseline>0</hackBaseline>
      </plugin>
    </sensor>
  </gazebo>

바뀐 부분은 다음과 같다.

• type의 camera가 depth로 바뀜.
• depth frame
• pointCloudCutoff+Max

위와 같은 내용들이 추가되었다. distortion이나 focalLength는 기본적인 것이니 굳이 넣으라 말라 하지 않겠다.

---

5. Google Cartographer

현재 중요한 문제가 있다. 계속해서 map과 odom 사이의 transform이 생성되지 않아 로봇이나 scan msg가 맵상에서 보이지 않고 있다. 웃기는건 cartographer노드에서 나오는 토픽은 다 된다는 점. 하지만 여전히 TF에러가 있다. 어디서 잘못된건지 파악이 필요하다.

---

6. RTAB-Map

먼저 rtabmap을 lidar로만 만든 형식이다. 어제 만든 urdf가 계속해서 문제를 일으키고 있다. 정확히는 urdf가 아니라 sdf파일이 굉장히 까다로운 부분이 있는 것 같다. 특히 위에서 map과 odom사이의 transform이 안나와서 뭔가 안되는 것을 보면 이부분이 굉장히 스트레스다. 정작 tf_tree를 확인하면 문제는 없고... 아직은 갈길이 먼것 같다. 찾아보니 tf tree의 문제는 없었다. 단순히 gazebo가 에러를 일으켰던 것. 어쨋든 위와 같이 pcl을 이용한 mapping을 하였더니 아래와 같은 map을 구했다. 본인 카메라의 fov나 focal length같은 것을 고려해서 다시 해보자.

---

7. Navigation

튜닝의 끝은 순정이라는 말을 믿고 기본 튜토리얼을 실행해보자.
아래는 cartographer로 만든 맵이다. 해당 맵을 기준으로 navigation을 한 모습이다. 도착까지 잘 한다.

Feed back

• 로봇의 어떤 움직임이 loop closure에 방해가 될까 -> 지속적인 좌우 회전 혹은 제자리 회전이 로봇 맵 전체적으로 영향을 주었다.
• urdf 작성 방식 -> xacro 를 적극적으로 사용해보자. 혹은 stl파일을 사용하는 것도 좋다.

[ref]

• (1) https://automaticaddison.com/useful-world-files-for-gazebo-and-ros-2-simulations/
• (2) http://gazebosim.org/tutorials/?tut=ros_urdf
• (3) https://velog.io/@cjh1995-ros/ROS-URDF-to-SDF
• (4) http://gazebosim.org/tutorials?tut=ros_gzplugins