🤔0.개요

지금까지 ROS의 시뮬레이션 기초에 대해서 강의를 들으면서 배운내용들을 확인하기 위한 과제를 수행해보자.
아래의 목차의 순서대로 진행하면서 배운내용들을 다시 돌아보자!

🗒목차

1.🚖모델 및 월드생성
2.🔌플러그인 생성
3.🤖gazebo 차량 수정
4.🎮회피 알고리즘 설계

🚖1.모델 및 월드 생성

🗿1.1 모델 생성

🗿model.sdf

<?xml version='1.0'?>
<sdf version="1.6">
    <model name="custom_mid_obj">
        <pose>0 0 0.5 0 0 0</pose>
        <static>false</static>
        <link name="link">
            <inertial>
                <mass>1.0</mass>
                <inertia>
                    <ixx>0.083</ixx>
                    <ixy>0.0</ixy>
                    <ixz>0.0</ixz>
                    <iyy>0.083</iyy>
                    <iyz>0.0</iyz>
                    <izz>0.083</izz>
                </inertia>
            </inertial>

            <!-- can -->
            <collision name="can_1">
                <pose>0 2 0 0 0 0</pose>
                <geometry>
                    <cylinder radius="0.05" length="1"/>
                </geometry>
            </collision>
            <visual name="can_visual_1">
                <pose>0 2 0 0 0 0</pose>
                <geometry>
                    <cylinder>
                        <radius>0.25</radius>
                        <length>1</length>
                    </cylinder>
                </geometry>
            </visual>

            <collision name="can_2">
                <pose>-1.732 -1 0 0 0 0 </pose>
                <geometry>
                    <cylinder>
                        <radius>0.25</radius>
                        <length>1</length>
                    </cylinder>
                </geometry>
            </collision>
            <visual name="can_visual_2">
                <pose>-1.732 -1 0 0 0 0 </pose>
                <geometry>
                    <cylinder>
                        <radius>0.25</radius>
                        <length>1</length>
                    </cylinder>
                </geometry>
            </visual>

            <collision name="can_3">
                <pose>1.732 -1 0 0 0 0 </pose>
                <geometry>
                    <cylinder>
                        <radius>0.25</radius>
                        <length>1</length>
                    </cylinder>
                </geometry>
            </collision>
            <visual name="can_visual_3">
                <pose>1.732 -1 0 0 0 0 </pose>
                <geometry>
                    <cylinder>
                        <radius>0.25</radius>
                        <length>1</length>
                    </cylinder>
                </geometry>
            </visual>
            <!-- box -->
            <collision name="box_1">
                <pose>-1 0.366 0 0 0 1.047198 </pose>
                <geometry>
                    <box>
                        <size>3.464 0.2 0.8</size>
                    </box>
                </geometry>
            </collision>
            <visual name="box_visual_1">
                <pose>-1 0.366 0 0 0 1.047198 </pose>
                <geometry>
                    <box>
                        <size>3.464 0.2 0.8</size>
                    </box>
                </geometry>
            </visual>

            <collision name="box_2">
                <pose>1 0.366 0 0 0 -1.047198 </pose>
                <geometry>
                    <box>
                        <size>3.464 0.2 0.8</size>
                    </box>
                </geometry>
            </collision>
            <visual name="box_visual_2">
                <pose>1 0.366 0 0 0 -1.047198 </pose>
                <geometry>
                    <box>
                        <size>3.464 0.2 0.8</size>
                    </box>
                </geometry>
            </visual>

            <collision name="box_3">
                <pose>0 -1 0 0 0 0 </pose>
                <geometry>
                    <box>
                        <size>3.464 0.2 0.8</size>
                    </box>
                </geometry>
            </collision>
            <visual name="box_visual_3">
                <pose>0 -1 0 0 0 0 </pose>
                <geometry>
                    <box>
                        <size>3.464 0.2 0.8</size>
                    </box>
                </geometry>
            </visual>
        </link>
    </model>
</sdf>

위와 같은 모델을 구성하는 sdf 파일이다.
기본모델인 box, cylinder를 통해 구성하였다.

모델을 생성하는 중 위와 같이 cylinder 의 형태를 인식 못하는 문제가 있었는데
아래와 같이 해결하였다.

수정 전
🗿model.sdf

...
<cylinder radius="0.25" length="1"/>
...

수정 후

...
<cylinder>
		<radius>0.25</radius>
      	<length>1</length>
</cylinder>
...

가제보에서 수정 전 코드의 tag를 인식못해 발생하는 문제 같다.

⚙️model.config

<?xml version="1.0"?>

<model>
    <name>Box Model</name>
    <version>1.0</version>
    <sdf version="1.6">model.sdf</sdf>

    <author>
        <name>Nodazi</name>
        <email>dk3146@naver.com</email>
    </author>

    <description>
        A simple box model
    </description>
</model>

모델의 설명을 config파일에 작성하였다.
가제보에서 해당 모델을 인식할 수 있게 .gazebo/models파일에 넣어두자!

🗺️1.2 월드 생성

20*20의 벽 안에 위에서 제작한 모델 4개를 10*10 안쪽에 생성하는 코드를 작성하였다.
🗺️mid_project_world.world

<?xml version="1.0" ?>
<sdf version="1.6">
    <world name="default">

        <!-- A global light source -->
        <include>
            <uri>model://sun</uri>
        </include>

        <!-- A ground plane -->
        <include>
            <uri>model://ground_plane</uri>
        </include>

        <!--static obj-->
        <population name="custom_mid_obj_population">
            <model name="custom_mid_obj">
                <include>
                <static>true</static>
                <uri>model://custom_mid_obj</uri>
                </include>
            </model>
            <pose>0 0 0 0 0 0</pose>
            <box>
                <size>10 10 0.01</size>
            </box>
            <model_count>4</model_count>
            <distribution>
                <type>uniform</type>
            </distribution>
        </population>

        <!-- walls -->
        <!-- +X grey_wall -->
        <population name="grey_wall_px">
            <model name="grey_wall">
                <include>
                    <static>true</static>
                    <uri>model://grey_wall</uri>
                </include>
            </model>
            <pose>3.333333 10.32 0.01 0 0 0</pose>
            <box>
                <size>20 20 0.01</size>
            </box>
            <model_count>3</model_count>
            <distribution>
                <type>linear-x</type>
            </distribution>
        </population>

		<!-- -X grey_wall -->
        <population name="grey_wall_mx">
            <model name="grey_wall">
                <include>
                    <static>true</static>
                    <uri>model://grey_wall</uri>
                </include>
            </model>
            <pose>3.333333 -10.32 0.01 0 0 0</pose>
            <box>
                <size>20 20 0.01</size>
            </box>
            <model_count>3</model_count>
            <distribution>
                <type>linear-x</type>
            </distribution>
        </population>

		<!-- +Y grey_wall -->
        <population name="grey_wall_py">
            <model name="grey_wall">
                <include>
                    <static>true</static>
                    <uri>model://grey_wall</uri>
                    <pose>0 0 0 0 0 1.57</pose>
                </include>
            </model>
            <pose>10.32 3.333333 0.01 0 0 0</pose>
            <box>
                <size>20 20 0.01</size>
            </box>
            <model_count>3</model_count>
            <distribution>
                <type>linear-y</type>
            </distribution>
        </population>

		<!-- -Y grey_wall_py -->
        <population name="grey_wall_my">
            <model name="grey_wall">
                <include>
                    <static>true</static>
                    <uri>model://grey_wall</uri>
                    <pose>0 0 0 0 0 1.57</pose>
                </include>
            </model>
            <pose>-10.32 3.333333 0.01 0 0 0</pose>
            <box>
                <size>20 20 0.01</size>
            </box>
            <model_count>3</model_count>
            <distribution>
                <type>linear-y</type>
            </distribution>
        </population>       
    </world>
</sdf>

깔끔하게 생성된 모습

🔌2.플러그인 생성

위에서 벽안에 둘러쌓인 모델 4개를 균일하게 배치하였다.
플러그인을 생성하여 나머지 공간에 움직이는 모델 4개를 배치해보자.

🔌2.1 플러그인 작성

🔌moving_plugin.cc

#include <functional>
#include <gazebo/common/common.hh>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <ignition/math/Vector3.hh>

namespace gazebo {
class MovingPlugin : public ModelPlugin {
public:
  void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) {
    // Store the pointer to the model
    this->model = _parent;
    this->iterations = 10 * 1000;
    if (_sdf->HasElement("iterations")) {
      this->iterations = _sdf->Get<int>("iterations");
    }
    this->linear_vel = 0.1;
    if (_sdf->HasElement("linear_vel")) {
      this->linear_vel = _sdf->Get<double>("linear_vel");
    }

    this->direction_x = 1.0;
    if (_sdf->HasElement("direction_x")) {
      this->direction_x = _sdf->Get<double>("direction_x");
    }

    this->direction_y = 0.0;
    if (_sdf->HasElement("direction_y")) {
      this->direction_y = _sdf->Get<double>("direction_y");
    }

    // this->direction = true;
    // if (_sdf->HasElement("direction")) {
    //   this->direction = _sdf->Get<bool>("direction");
    // }

    // Listen to the update event. This event is broadcast every simulation
    // iteration.
    this->updateConnection = event::Events::ConnectWorldUpdateBegin(
        std::bind(&MovingPlugin::OnUpdate, this));
  }

  // Called by the world update start event
public:
  void OnUpdate() {
    // Apply a small linear velocity to the model.
    if ((counter / iterations) % 2 == 0) {
      this->model->SetLinearVel(
          ignition::math::Vector3d(this->linear_vel * this->direction_x,
                                   this->linear_vel * this->direction_y, 0));
    } else if ((counter / iterations) % 2 == 1) {
      this->model->SetLinearVel(
          ignition::math::Vector3d(-this->linear_vel * this->direction_x,
                                   -this->linear_vel * this->direction_y, 0));
    }
    counter++;
  }
  // Pointer to the model
private:
  physics::ModelPtr model;

private:
  double linear_vel;
  int iterations;
  int counter;
  double direction_x;
  double direction_y;
  // Pointer to the update event connection
private:
  event::ConnectionPtr updateConnection;
};

// Register this plugin with the simulator
GZ_REGISTER_MODEL_PLUGIN(MovingPlugin)
} // namespace gazebo

코드를 간략히 설명하면 아래의 파라미터 값을 받아서 물체를 움직인다.
linear_vel : 물체가 움직이는 속도.
iterations : 물체가 방향 전환을 하기 까지의 시간.
counter : 물체의 방향 전환을 인식하기 위한 값.
direction_x : x방향으로 이동하는 값의 가중치.
direction_y : y방향으로 이동하는 값의 가중치.

x축 및 y축의 움직임을 유연하게 구현 하기위해 direction_x와 같은 변수를 추가하였다.
CMakeList.txt

...
add_library(moving_plugin SHARED src/moving_plugin.cc)
target_link_libraries(moving_plugin ${GAZEBO_LIBRARIES})
...

플러그인을 인식하기 위해 위의 코드를 반드시 CMakeLists.txt파일에 넣어주자.
코드를 작성하고 catkin_make를 통해 빌드를 하면 플러그인이 생성된 것을 확인 할 수 있다.(위치 sim_ws/devel/lib)

🗺️2.2 월드 파일 수정

🗺️mid_project_world.world

...
        <model name="moving_custom_mid_obj_1">
            <pose>-7.5 7.5 0.5 0 0 0</pose>
            <static>false</static>
            <include>
                <uri>model://custom_mid_obj</uri>
            </include>

            <plugin name="libmoving_plugin" filename="libmoving_plugin.so">
                <linear_vel>5.0</linear_vel>
                <iterations>3000</iterations>
                <direction_x>1.0</direction_x>
                <direction_y>0.0</direction_y>
            </plugin>
        </model>

        <model name="moving_custom_mid_obj_2">
            <pose>7.5 7.5 0.5 0 0 0</pose>
            <static>false</static>
            <include>
                <uri>model://custom_mid_obj</uri>
            </include>

            <plugin name="libmoving_plugin" filename="libmoving_plugin.so">
                <linear_vel>5.0</linear_vel>
                <iterations>3000</iterations>
                <direction_x>0.0</direction_x>
                <direction_y>-1.0</direction_y>
            </plugin>
        </model>

        <model name="moving_custom_mid_obj_3">
            <pose>7.5 -7.5 0.5 0 0 0</pose>
            <static>false</static>
            <include>
                <uri>model://custom_mid_obj</uri>
            </include>

            <plugin name="libmoving_plugin" filename="libmoving_plugin.so">
                <linear_vel>5.0</linear_vel>
                <iterations>3000</iterations>
                <direction_x>-1.0</direction_x>
                <direction_y>0.0</direction_y>
            </plugin>
        </model>

        <model name="moving_custom_mid_obj_4">
            <pose>-7.5 -7.5 0.5 0 0 0</pose>
            <static>false</static>
            <include>
                <uri>model://custom_mid_obj</uri>
            </include>

            <plugin name="libmoving_plugin" filename="libmoving_plugin.so">
                <linear_vel>5.0</linear_vel>
                <iterations>3000</iterations>
                <direction_x>0.0</direction_x>
                <direction_y>1.0</direction_y>
            </plugin>
        </model>
...

지난번에 생성한 모델에 플러그인을 설치하여, 움직임을 구현하였다.

🖥️2.3 실행화면

위와 같이 잘 움직이는 것을 확인 할 수 있었다.
나중에는 모델의 현제 위치를 받아서 방향을 바꾸는 방법도 구상하면 좋을 것 같다.

🤖3. gazebo 차량 수정

기존에 만든 로봇은 2개의 바퀴와 1개의 caster wheel로 구성돼있었다.
바퀴를 2개 더 추가하여 4개의 바퀴달린 차량을 만들고, skid_steer_drive_controller 플러그인을 추가해
사륜차량을 제어해보자.
로봇에 바퀴를 추가한 코드는 생략하겠다.

3.1 차량 추가 코드

🤖robot.gazebo

    <!-- skid steer driver -->
    <gazebo>
    <plugin name="skid_steer_drive_controller" filename="libgazebo_ros_skid_steer_drive.so">
        <updateRate>100.0</updateRate>
        <robotNamespace>/</robotNamespace>
        <leftFrontJoint>joint_chassis_left_front_wheel</leftFrontJoint>
        <rightFrontJoint>joint_chassis_right_front_wheel</rightFrontJoint>
        <leftRearJoint>joint_chassis_left_back_wheel</leftRearJoint>
        <rightRearJoint>joint_chassis_right_back_wheel</rightRearJoint>
        <wheelSeparation>1.16</wheelSeparation>
        <wheelDiameter>0.8</wheelDiameter>
        <robotBaseFrame>link_chassis</robotBaseFrame>
        <torque>20</torque>
        <topicName>cmd_vel</topicName>
        <broadcastTF>true</broadcastTF>
    </plugin>
    </gazebo>

기존의 이륜차량에는 libgazebo_ros_diff_drive 플러그인을 사용했었는데 실습중 그대로 작성하였을때는,
차량이 뭔가 제어가 잘 안되는 느낌이 가득 들었었다.
알고보니 skid_steer_drive_controller 이라는 스키드 방식의 조향 플러그인을 사용해야 한다.

참고 : https://classic.gazebosim.org/tutorials?tut=ros_gzplugins#JointPoseTrajectory

＊참고로 스키드 조향은 여러가지 바퀴의 회전을 달리하여 조향을 만들어 내는 시스템이다.
해당 플러그인을 넣어주고, 바퀴를 추가한뒤 차체 크기를 줄였다.

3.2 차량 동작 영상

전에 생성한 world에 spawn하여 키보드를 통해 조작을 해보았다.
움직이는 장애물의 크기가 크고 빠르게 움직여서 마구마구 두둘겨 맞는 우리의 로봇을 확인 할 수 있었다.
장애물 크기 및 속도를 개선해야겠다.
그리고 뭔가 조향이 마음대로 잘 안되는데 이에 대해서는 로봇의 설계를 좀 바꿔야 하는걸까..라는 생각을 하게 되었다.

차후 개선해볼 예정!

🎮4. 장애물 회피 알고리즘 설계

우선 장애물 크기를 줄이고 레이저를 통해 상황만을 인식하는 sample 코드를 입력해서 동작 시켜보았다.


상황에 대한 브리핑이 위와 같이 터미널에서 나타나는데 코드를 개선하여 움직임을 줘봐야겠다.
..to be continue