URDF로 만든 로봇을 GAZEBO상에서 컨트롤 하기 SLAM (1/2)

이 글은 pinkLab의 PinkWink 강사님의 강의자료를 참고하여 작성되었습니다.

https://pinkwink.kr/

PinkWink

 

 

 

https://kimbrain.tistory.com/entry/URDF-XACRO-%EB%A7%88%EC%8A%A4%ED%84%B0-%ED%95%98%EA%B8%B0-ADDBOT-Rviz%EC%97%90%EC%84%9C-%EA%B5%AC%ED%98%84%ED%95%98%EA%B8%B0-12

URDF, XACRO 마스터 하기 ! (ADDBOT Rviz에서 구현하기) (1/2)

 

이 글에서 rviz에 ADDBOT이라는 주행로봇을 띄웠습니다.

(근데, 파일을 날려버려서 다시 해야합니다..)

 

시작합시다.

 

우선 지금 상황은

 

이상태여야합니다.

 

 

시작합니다.

 

우선 패키지를 분할하고자 합니다.

그래서 가제보용 패키지와 로봇을 묘사하는 패키지로 나누겠습니다.

일단 패키지 생성은 이따가 진행하도록하고

 

패키지가 분리되었다면 문제가 생깁니다.

어떻게 가제보에 띄울거냐?

 

간단합니다.

로봇을 묘사하는 코드를 그냥 토픽으로 발행하면

가제보는 이걸 구독하면 됩니다.

 

그러기 위해서 launch폴더데 파이썬코드를 작성합니다.

이름은

upload_robot.launch.py 입니다.

import os
from os import environ

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, Shutdown
from launch.substitutions import PathJoinSubstitution, Command, LaunchConfiguration, PythonExpression
from launch_ros.substitutions import FindPackageShare
from launch_ros.actions import Node


def generate_launch_description():
    ld = LaunchDescription()

    is_sim = DeclareLaunchArgument("is_sim", default_value="false")
    prefix = DeclareLaunchArgument("prefix", default_value="")

    rsp_node = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        output='screen',
        parameters=[{
            'ignore_timestamp': False,
            'frame_prefix': LaunchConfiguration('prefix'),
            'robot_description':
                Command([
                    'xacro ',
                    PathJoinSubstitution([
                        FindPackageShare('Addbot_description'),
                        'urdf',
                        'robot.urdf.xacro',
                    ]),
                    ' is_sim:=', LaunchConfiguration('is_sim'),
                    ' prefix:=', LaunchConfiguration('prefix'),
                ]),
        }]
    )

    ld.add_action(is_sim)
    ld.add_action(prefix)
    ld.add_action(rsp_node)

    return ld

robot_state_publiser라는 친구가 이제 토픽으로 발행을 하게 하는 코드입니다.

코드는 여기저기 찾다보면 나온다고 합니다.

 

 

그리고 urdf폴더에 두 개의 파일이 추가되어야합니다.

Addbot_gazebo.urdf.xacro

해당파일에는 insert_gazebo라는 함수가 들어갑니다.

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

	<!-- 인서트 가제보 라는 친구 -->
    <xacro:macro name="insert_gazebo" params="prefix">
        <gazebo>
        <!--로스2 컨트롤러 디퍼런션 드라이버 (시엠디벨 토픽 같은) --> 
            <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
                <parameters>$(find Addbot_gazebo)/config/Addbot_controllers_gazebo.yaml</parameters>
            </plugin>
        </gazebo>

	<!-- 바퀴의 동적 정적 마찰? 탄성계수 -->
        <gazebo reference="${prefix}l_wheel">
            <mu1>200</mu1>
            <mu2>200</mu2>
            <kp>10000000.0</kp>
            <kd>1.0</kd>
            <!-- <minDepth>0.001</minDepth> -->
        </gazebo>

        <gazebo reference="${prefix}r_wheel">
            <mu1>200</mu1>
            <mu2>200</mu2>
            <kp>10000000.0</kp>
            <kd>1.0</kd>
            <!-- <minDepth>0.001</minDepth> -->
        </gazebo>

	<!-- 베이스링크의 마찰, 탄성계수-->
        <gazebo reference="${prefix}base_link">
            <mu1>0.0</mu1>
            <mu2>0.0</mu2>
            <!-- <minDepth>0.001</minDepth> -->
        </gazebo>
	
	<!-- 나중에 gpu글자 때야함.-->
	<!-- 가제보에서 레이저스캐너를 쓰고싶을때 설정-->
        <gazebo reference="${prefix}laser_link">
            <sensor name="laser" type="ray">
                <pose>0 0 0 0 0 0</pose>
                <always_on>1</always_on>
                <update_rate>15</update_rate>
                <!-- 라이다 보고 싶으면 이거 true로 <visualize>false</visualize>  -->
                <visualize>ture</visualize>
                <ray>
                    <scan>
                        <horizontal>
                            <samples>375</samples>
                            <resolution>1</resolution>
                            <min_angle>${-180*pi/180}</min_angle>
                            <max_angle>${180*pi/180}</max_angle>
                        </horizontal>
                    </scan>
                    <range>
                        <min>0.12</min>
                        <max>8.0</max>
                        <resolution>0.05</resolution>
                    </range>
                    <noise>
                        <type>gaussian</type>
                        <mean>0.0</mean>
                        <stddev>0.01</stddev>
                    </noise>
                </ray>
                <plugin name="laser" filename="libgazebo_ros_ray_sensor.so">
                    <ros>
                        <remapping>~/out:=scan</remapping>
                    </ros>
                    <frame_name>${prefix}laser_link</frame_name>
                    <output_type>sensor_msgs/LaserScan</output_type>
                </plugin>
            </sensor>
        </gazebo>

	<!-- 카메라 설정용  -->
        <gazebo reference="${prefix}camera_link">
            <sensor name="${prefix}camera" type="camera">
                <update_rate>30.0</update_rate>
                <camera name="${prefix}camera">
                    <pose>0 0 0 0 -1.5707 0</pose>
                    <horizontal_fov>1.08</horizontal_fov>
                    <image>
                        <width>1280</width>
                        <height>720</height>
                        <format>R8G8B8</format>
                    </image>
                    <clip>
                        <near>0.02</near>
                        <far>300</far>
                    </clip>
                    <noise>
                    <type>gaussian</type>
                        <mean>0.0</mean>
                        <stddev>0.01</stddev>
                    </noise>
                </camera>
                <plugin name="${prefix}camera_controller" filename="libgazebo_ros_camera.so">
                    <ros>
                        <!-- <namespace>${prefix}camera</namespace> -->
                        <!-- <remapping>camera1/image_raw:=camera/image_test</remapping>
                        <remapping>camera_info:=camera_info_test</remapping> -->
                    </ros>
                    <frame_name>camera_link</frame_name>
                    <hack_baseline>0.07</hack_baseline>
                </plugin>
            </sensor>
        </gazebo>
    </xacro:macro>
</robot>

이것도 여기저기서 때오는 것이라 들었습니다.

직접짤 수는 없고, 여기저기서 들고왔다가 시뮬을 하면서 수정을 하면 됩니다.

 

 

 

Addbot_ros2_control.urdf.xacro

컨트롤러란 그냥 쉽게 모듈이라고 생각하면됩니다.

ROS2 컨트롤러를 안쓴다면 펌웨어용 cmd_vel, pid 등 모든 코드를 다 짜야합니다.

근데, ROS2 컨트롤러를 쓸 수 있다면 모듈처럼 가져다 쓸 수 있다고하는데, 절대 간단하지 않다고 합니다.

가제보에서는 무조건 컨트롤러를 써야하며 docker같은 친구를 사용할 수 있습니다.

이 코드 또한 어디서 때오는 친구입니다.

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:macro name="insert_ros2_control" params="prefix is_sim:=^|false robot_port_name robot_baudrate">
        <ros2_control name="${prefix}Addbot_system" type="system">
        <!-- 시뮬레이션인가? 트루면 가제보용으로 실행해라. -->
            <xacro:if value="$(arg is_sim)">
                <hardware>
                    <plugin>gazebo_ros2_control/GazeboSystem</plugin>
                </hardware>
      <!-- 아니면 아두이노와 연결을 시도합니다.-->
            </xacro:if>
            <xacro:unless value="$(arg is_sim)">
                <hardware>
                    <plugin>Addbot_hardware/AddbotSystemHardware</plugin>
                    <param name="port_name">$(arg robot_port_name)</param>
                    <param name="baudrate">$(arg robot_baudrate)</param>
                </hardware>
            </xacro:unless>

	<!-- 나머지는 컨트롤러와 관련된 애들 -->
            <joint name="${prefix}l_wheel_joint">
                <command_interface name="velocity">
                    <param name="min">-1.0</param>
                    <param name="max">1.0</param>
                </command_interface>

                <state_interface name="position"/>
                <state_interface name="velocity"/>
            </joint>

            <joint name="${prefix}r_wheel_joint">
                <command_interface name="velocity">
                    <param name="min">-1.0</param>
                    <param name="max">1.0</param>
                </command_interface>

                <state_interface name="position"/>
                <state_interface name="velocity"/>
            </joint>
        </ros2_control>
    </xacro:macro>
</robot>

 

 

이제 robot.urdf.xacro 파일을 수정합니다.

앞부분을 다음과 같이 수정합니다.

<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro" name="addbot">

    <xacro:include filename="$(find Addbot_description)/urdf/common/inertia_macro.urdf.xacro"/>
    <xacro:include filename="$(find Addbot_description)/urdf/Addbot.urdf.xacro"/>
    <xacro:include filename="$(find Addbot_description)/urdf/Addbot_gazebo.urdf.xacro"/>
    <xacro:include filename="$(find Addbot_description)/urdf/Addbot_ros2_control.urdf.xacro"/>

    <xacro:arg name="prefix" default=""/>
    <xacro:arg name="is_sim" default="false"/>
    <xacro:arg name="robot_port_name" default="/dev/ttyArduino"/>
    <xacro:arg name="robot_baudrate" default="500000"/>
    
    <xacro:property name="prefix" value="$(arg prefix)"/>

    <!-- insert robot base -->
    <xacro:insert_robot prefix="$(arg prefix)"/>
    <xacro:insert_ros2_control prefix="$(arg prefix)" is_sim="$(arg is_sim)" robot_port_name="$(arg robot_port_name)" robot_baudrate="$(arg robot_baudrate)"/>
    <xacro:if value="$(arg is_sim)">
        <xacro:insert_gazebo prefix="$(arg prefix)"/>
    </xacro:if>

 

 

이상태에서 빌드 한 번 합니다.

좋아요.

 

 

 

이제 패키지를 하나 새로 만듭시다.

ros2 pkg create --build-type ament_cmake Addbot_gazebo

 

패키지를 들어가서 CMakeList.txt에 다음을 추가합니다.

install(
  DIRECTORY
    launch
    worlds
    models
    config
  DESTINATION share/${PROJECT_NAME}
)

 

package.xml에 다음을 추가합니다.

  <depend>gazebo_ros</depend>
  <depend>gazebo_ros_pkgs</depend>
  <depend>gazebo_ros2_control</depend>
  <depend>gazebo_plugins</depend>
  <depend>Addbot_description</depend>

 

 

 

 

패키지로 들어가서 4개의 폴더을 만듭니다.

 

먼저 config폴더로 들어가서

Addbot_controllers_gazebo.yaml

이라는 파일을 하나 만듭니다.

이 yaml파일은 control 수치들을 정의하는 파라미터의 모임이라고 합니다.

때오는 코드라고 합니다.

controller_manager:
  ros__parameters:
    update_rate: 25
    use_sim_time: true

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    base_controller:
      type: diff_drive_controller/DiffDriveController

base_controller:
  ros__parameters:
    left_wheel_names: ["l_wheel_joint"]
    right_wheel_names: ["r_wheel_joint"]
    write_op_modes: ["motor_controller"]

    wheel_separation: 0.185
    wheels_per_side: 1
    wheel_radius: 0.034

    wheel_separation_multiplier: 1.0
    left_wheel_radius_multiplier: 1.0
    right_wheel_radius_multiplier: 1.0

    odom_frame_id: odom
    base_frame_id: base_footprint
    pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
    twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]

    position_feedback: true
    open_loop: false
    enable_odom_tf: true

    cmd_vel_timeout: 0.5
    publish_limited_velocity: false
    velocity_rolling_window_size: 10
    use_stamped_vel: false

    linear.x.has_velocity_limits: true
    linear.x.has_acceleration_limits: true
    linear.x.has_jerk_limits: false
    linear.x.max_velocity: 0.25
    linear.x.min_velocity: -0.25
    linear.x.max_acceleration: 0.8
    linear.x.min_acceleration: -0.8
    # linear.x.max_jerk: 0.0
    # linear.x.min_jerk: 0.0

    angular.z.has_velocity_limits: true
    angular.z.has_acceleration_limits: true
    angular.z.has_jerk_limits: false
    angular.z.max_velocity: 1.0
    angular.z.min_velocity: -1.0
    angular.z.max_acceleration: 1.5
    angular.z.min_acceleration: -1.5
    # angular.z.max_jerk: 0.0
    # angular.z.min_jerk: 0.0

 

 

이제 월드를 하나 만들겁니다.

(어디서 가져온 파일이라고 합니다.)

worlds 폴더로 들어가서 

empty.world 

<?xml version="1.0" ?>
<sdf version="1.5">
  <world name="default">
    <!-- A global light source -->
    <include>
      <uri>model://sun</uri>
    </include>
    <!-- A ground plane -->
    <include>
      <uri>model://ground_plane</uri>
    </include>
  </world>
</sdf>

라는 파일 하나

simple_building.world

<?xml version="1.0" ?>
<sdf version="1.5">
  <world name="default">
    <!-- A global light source -->
    <include>
      <uri>model://sun</uri>
    </include>
    <!-- A ground plane -->
    <include>
      <uri>model://ground_plane</uri>
    </include>
    <!-- A Building -->
    <include>
      <uri>model://simple_building</uri>
      <pose>2.0 0.7 0 0 0 0</pose>
    </include>
  </world>
</sdf>

라는 파일 하나

models라는 폴더에 simple_building이라는 파일을 하나 만듭니다.

model.config라는 파일

(이건 없어도 되는 파일인가.)

<?xml version="1.0" ?>
<model>
    <name>simple_building</name>
    <version>1.0</version>
    <sdf version="1.7">model.sdf</sdf>
    <author>
        <name></name>
        <email></email>
    </author>
    <description></description>
</model>

그리고 model.sdf라는 파일을 하나 만듭니다.

<?xml version='1.0'?>
<sdf version='1.7'>
  <model name='simple_building'>
    <pose>-0.49455 -5.1275 0 0 -0 0</pose>
    <link name='Wall_0'>
      <collision name='Wall_0_Collision'>
        <geometry>
          <box>
            <size>5 0.15 0.5</size>
          </box>
        </geometry>
        <pose>0 0 0.25 0 -0 0</pose>
      </collision>
      <visual name='Wall_0_Visual'>
        <pose>0 0 0.25 0 -0 0</pose>
        <geometry>
          <box>
            <size>5 0.15 0.5</size>
          </box>
        </geometry>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/Bricks</name>
          </script>
          <ambient>1 1 1 1</ambient>
        </material>
        <meta>
          <layer>0</layer>
        </meta>
      </visual>
      <pose>-2.42545 -2.6275 0 0 -0 -1.5708</pose>
    </link>
    <link name='Wall_1'>
      <collision name='Wall_1_Collision'>
        <geometry>
          <box>
            <size>5 0.15 0.5</size>
          </box>
        </geometry>
        <pose>0 0 0.25 0 -0 0</pose>
      </collision>
      <visual name='Wall_1_Visual'>
        <pose>0 0 0.25 0 -0 0</pose>
        <geometry>
          <box>
            <size>5 0.15 0.5</size>
          </box>
        </geometry>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/Bricks</name>
          </script>
          <ambient>1 1 1 1</ambient>
        </material>
        <meta>
          <layer>0</layer>
        </meta>
      </visual>
      <pose>-0.00045 -5.0525 0 0 -0 0</pose>
    </link>
    <link name='Wall_10'>
      <collision name='Wall_10_Collision'>
        <geometry>
          <box>
            <size>1 0.15 0.5</size>
          </box>
        </geometry>
        <pose>0 0 0.25 0 -0 0</pose>
      </collision>
      <visual name='Wall_10_Visual'>
        <pose>0 0 0.25 0 -0 0</pose>
        <geometry>
          <box>
            <size>1 0.15 0.5</size>
          </box>
        </geometry>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/Wood</name>
          </script>
          <ambient>1 1 1 1</ambient>
        </material>
        <meta>
          <layer>0</layer>
        </meta>
      </visual>
      <pose>0.65507 -3.95198 0 0 -0 0.785398</pose>
    </link>
    <link name='Wall_11'>
      <collision name='Wall_11_Collision'>
        <geometry>
          <box>
            <size>1.25 0.15 0.5</size>
          </box>
        </geometry>
        <pose>0 0 0.25 0 -0 0</pose>
      </collision>
      <visual name='Wall_11_Visual'>
        <pose>0 0 0.25 0 -0 0</pose>
        <geometry>
          <box>
            <size>1.25 0.15 0.5</size>
          </box>
        </geometry>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/Wood</name>
          </script>
          <ambient>1 1 1 1</ambient>
        </material>
        <meta>
          <layer>0</layer>
        </meta>
      </visual>
      <pose>0.955591 -3.10146 0 0 -0 1.5708</pose>
    </link>
    <link name='Wall_12'>
      <collision name='Wall_12_Collision'>
        <geometry>
          <box>
            <size>2.5 0.15 0.5</size>
          </box>
        </geometry>
        <pose>0 0 0.25 0 -0 0</pose>
      </collision>
      <visual name='Wall_12_Visual'>
        <pose>0 0 0.25 0 -0 0</pose>
        <geometry>
          <box>
            <size>2.5 0.15 0.5</size>
          </box>
        </geometry>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/Wood</name>
          </script>
          <ambient>1 1 1 1</ambient>
        </material>
        <meta>
          <layer>0</layer>
        </meta>
      </visual>
      <pose>-0.219409 -2.55146 0 0 -0 3.14159</pose>
    </link>
    <link name='Wall_14'>
      <collision name='Wall_14_Collision'>
        <geometry>
          <box>
            <size>2.5 0.15 0.5</size>
          </box>
        </geometry>
        <pose>0 0 0.25 0 -0 0</pose>
      </collision>
      <visual name='Wall_14_Visual'>
        <pose>0 0 0.25 0 -0 0</pose>
        <geometry>
          <box>
            <size>2.5 0.15 0.5</size>
          </box>
        </geometry>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/Wood</name>
          </script>
          <ambient>1 1 1 1</ambient>
        </material>
        <meta>
          <layer>0</layer>
        </meta>
      </visual>
      <pose>-0.27045 -1.3925 0 0 -0 0</pose>
    </link>
    <link name='Wall_16'>
      <collision name='Wall_16_Collision'>
        <geometry>
          <box>
            <size>2.5 0.15 0.5</size>
          </box>
        </geometry>
        <pose>0 0 0.25 0 -0 0</pose>
      </collision>
      <visual name='Wall_16_Visual'>
        <pose>0 0 0.25 0 -0 0</pose>
        <geometry>
          <box>
            <size>2.5 0.15 0.5</size>
          </box>
        </geometry>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/Wood</name>
          </script>
          <ambient>1 1 1 1</ambient>
        </material>
        <meta>
          <layer>0</layer>
        </meta>
      </visual>
      <pose>1.62455 -3.2675 0 0 -0 -1.5708</pose>
    </link>
    <link name='Wall_17'>
      <collision name='Wall_17_Collision'>
        <geometry>
          <box>
            <size>0.75 0.15 0.5</size>
          </box>
        </geometry>
        <pose>0 0 0.25 0 -0 0</pose>
      </collision>
      <visual name='Wall_17_Visual'>
        <pose>0 0 0.25 0 -0 0</pose>
        <geometry>
          <box>
            <size>0.75 0.15 0.5</size>
          </box>
        </geometry>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/Wood</name>
          </script>
          <ambient>1 1 1 1</ambient>
        </material>
        <meta>
          <layer>0</layer>
        </meta>
      </visual>
      <pose>1.32455 -4.4425 0 0 -0 3.14159</pose>
    </link>
    <link name='Wall_19'>
      <collision name='Wall_19_Collision'>
        <geometry>
          <box>
            <size>1.29022 0.15 0.5</size>
          </box>
        </geometry>
        <pose>0 0 0.25 0 -0 0</pose>
      </collision>
      <visual name='Wall_19_Visual'>
        <pose>0 0 0.25 0 -0 0</pose>
        <geometry>
          <box>
            <size>1.29022 0.15 0.5</size>
          </box>
        </geometry>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/Wood</name>
          </script>
          <ambient>1 1 1 1</ambient>
        </material>
        <meta>
          <layer>0</layer>
        </meta>
      </visual>
      <pose>2.02455 -0.6875 0 0 -0 -0.841241</pose>
    </link>
    <link name='Wall_2'>
      <collision name='Wall_2_Collision'>
        <geometry>
          <box>
            <size>5 0.15 0.5</size>
          </box>
        </geometry>
        <pose>0 0 0.25 0 -0 0</pose>
      </collision>
      <visual name='Wall_2_Visual'>
        <pose>0 0 0.25 0 -0 0</pose>
        <geometry>
          <box>
            <size>5 0.15 0.5</size>
          </box>
        </geometry>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/Bricks</name>
          </script>
          <ambient>1 1 1 1</ambient>
        </material>
        <meta>
          <layer>0</layer>
        </meta>
      </visual>
      <pose>2.42455 -2.6275 0 0 -0 1.5708</pose>
    </link>
    <link name='Wall_3'>
      <collision name='Wall_3_Collision'>
        <geometry>
          <box>
            <size>5 0.15 0.5</size>
          </box>
        </geometry>
        <pose>0 0 0.25 0 -0 0</pose>
      </collision>
      <visual name='Wall_3_Visual'>
        <pose>0 0 0.25 0 -0 0</pose>
        <geometry>
          <box>
            <size>5 0.15 0.5</size>
          </box>
        </geometry>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/Bricks</name>
          </script>
          <ambient>1 1 1 1</ambient>
        </material>
        <meta>
          <layer>0</layer>
        </meta>
      </visual>
      <pose>-0.00045 -0.2025 0 0 -0 3.14159</pose>
    </link>
    <link name='Wall_5'>
      <collision name='Wall_5_Collision'>
        <geometry>
          <box>
            <size>0.75 0.15 0.5</size>
          </box>
        </geometry>
        <pose>0 0 0.25 0 -0 0</pose>
      </collision>
      <visual name='Wall_5_Visual'>
        <pose>0 0 0.25 0 -0 0</pose>
        <geometry>
          <box>
            <size>0.75 0.15 0.5</size>
          </box>
        </geometry>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/Wood</name>
          </script>
          <ambient>1 1 1 1</ambient>
        </material>
        <meta>
          <layer>0</layer>
        </meta>
      </visual>
      <pose>-1.71545 -4.7425 0 0 -0 1.5708</pose>
    </link>
    <link name='Wall_7'>
      <collision name='Wall_7_Collision'>
        <geometry>
          <box>
            <size>1.75 0.15 0.5</size>
          </box>
        </geometry>
        <pose>0 0 0.25 0 -0 0</pose>
      </collision>
      <visual name='Wall_7_Visual'>
        <pose>0 0 0.25 0 -0 0</pose>
        <geometry>
          <box>
            <size>1.75 0.15 0.5</size>
          </box>
        </geometry>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/Wood</name>
          </script>
          <ambient>1 1 1 1</ambient>
        </material>
        <meta>
          <layer>0</layer>
        </meta>
      </visual>
      <pose>-1.63545 -3.6225 0 0 -0 0</pose>
    </link>
    <link name='Wall_9'>
      <collision name='Wall_9_Collision'>
        <geometry>
          <box>
            <size>1 0.15 0.5</size>
          </box>
        </geometry>
        <pose>0 0 0.25 0 -0 0</pose>
      </collision>
      <visual name='Wall_9_Visual'>
        <pose>0 0 0.25 0 -0 0</pose>
        <geometry>
          <box>
            <size>1 0.15 0.5</size>
          </box>
        </geometry>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/Wood</name>
          </script>
          <ambient>1 1 1 1</ambient>
        </material>
        <meta>
          <layer>0</layer>
        </meta>
      </visual>
      <pose>-0.07045 -4.2525 0 0 -0 0</pose>
    </link>
    <static>1</static>
  </model>
</sdf>

(모두다 어디선가 가져온 파일입니다. 출처를 찾고있는데, 찾을 수가 없네요.)

 

 

그다음에는 launch 폴더에 

bringup_gazebo.launch.py

라는 파이썬 코드를 작성합니다. (실행에 관련되어있습니다.)

import os
from os import environ
from os import pathsep

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, ExecuteProcess, RegisterEventHandler, SetEnvironmentVariable
from launch.event_handlers import OnProcessExit
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.substitutions import FindPackageShare
from launch_ros.actions import Node
from launch.substitutions import PathJoinSubstitution, LaunchConfiguration, EnvironmentVariable
from ament_index_python.packages import get_package_share_directory

from pathlib import Path

def generate_launch_description():
    robot_name = DeclareLaunchArgument("robot_name", default_value="Addbot")
    robot_prefix = DeclareLaunchArgument("robot_prefix", default_value="")
    world_name = DeclareLaunchArgument("world_name", default_value="empty.world")

    environ['QT_AUTO_SCREEN_SCALE_FACTOR'] = '0'
    gz_resource_path = SetEnvironmentVariable(name='GAZEBO_MODEL_PATH', value=[
                                    EnvironmentVariable('GAZEBO_MODEL_PATH', default_value=''),
                                    '/usr/share/gazebo-11/models/:',
                                    str(Path(get_package_share_directory('Addbot_description')).parent.resolve()),
                                    ':',
                                    str(Path(get_package_share_directory('Addbot_gazebo')).parent.resolve()) + "/Addbot_gazebo/models",
                        ])

    gz_server = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            FindPackageShare('gazebo_ros'),
            '/launch/gzserver.launch.py'
        ]),
        launch_arguments={
            "verbose": "true",
            "physics": "ode",
            "lockstep": "true", # 사양에 따라서 꼭 필요합니다. 
            "world": PathJoinSubstitution([
                        FindPackageShare('Addbot_gazebo'),
                        'worlds',
                        LaunchConfiguration('world_name'),
            ])
        }.items(),
    )

    gz_client = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            FindPackageShare('gazebo_ros'),
            '/launch/gzclient.launch.py'
        ]),
    )

    upload_robot = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            FindPackageShare('Addbot_description'),
            '/launch/upload_robot.launch.py']
        ),
        launch_arguments = {
            'is_sim' : 'true',
            'prefix': LaunchConfiguration('robot_prefix'),
        }.items()
    )

    spawn_robot = Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        name='spawn_robot',
        output='screen',
        arguments=[
            '-entity', LaunchConfiguration('robot_name'),
            '-topic', 'robot_description',
            '-timeout', '20.0',
            '-x', '0.0',
            '-y', '0.0',
            '-package_to_model'
        ],
        prefix="bash -c 'sleep 2.0; $0 $@' ",
        parameters=[{
            "use_sim_time": True
        }],
    )

    load_joint_state_controller = ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
                'joint_state_broadcaster'],
        output='screen'
    )

    load_base_controller = ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
                'base_controller'],
        output='screen'
    )

    return LaunchDescription([
        RegisterEventHandler(
            event_handler=OnProcessExit(
                target_action=spawn_robot,
                on_exit=[load_joint_state_controller],
            )
        ),
        RegisterEventHandler(
            event_handler=OnProcessExit(
                target_action=load_joint_state_controller,
                on_exit=[load_base_controller],
            )
        ),
        gz_resource_path,
        robot_name,
        robot_prefix,
        world_name,
        gz_server,
        gz_client,
        upload_robot,
        spawn_robot,
    ])

좋아요. 이제 빌드하고 실행합시다.

 

ros2 launch Addbot_gazebo bringup_gazebo.launch.py world_name:=simple_building.world

 

와.. 로봇 토픽이 발행이 안되는 것으로 보입니다.

 

디버깅해야지..

 

토픽이 발행이 안되고 있습니다.

문제는 일단.

 

갑자기 죽었다니까 아무래도 ros state publisher를 건드는 코드를 좀 손봐야할 듯합니다.

 

 

 

무슨 이유에선가 Add_robot_urdf 코드가 조금 이상해진거 같은데

 

변수를 하나 빠뜨렸다.

이제 될듯?

 

그건 나중 생각하고일단

 

이렇게 나오면 성공이라고 봅니다.

이를 움직이는 토픽을 한 번 쏴봅시다.

새로운 터미널을 열고 환경을 불러오고 다음을 입력합니다.

 

ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r cmd_vel:=base_controller/cmd_vel_unstamped

움직일 수 있습니다.

i j k l 키로

다음은 SLAM을 한 번 해봅시다.