ROS 가제보(Gazebo) 연습하기 - Pan/Tilt

렌틴 조셉의 좋은 소스코드를 만나 Pan/Tilt를 표현하는 URDF와 이를 이용해서 RViz로 시각화하는 예제[바로가기]를 따라해보았었는데요. 그 때 사용한 Pan/Tilt 예제에서 한 발짝 더 가서 Gazebo라는 아이를 이용해서 움직여보기도 할려고 합니다.^^ 뭐 언제나 그렇듯 그저그저 따라하기 수준이지요^^

일단~~~ 지난번[바로가기] 예제에서 몇몇 내용을 추가하거나 바꿔서 시도해볼려구요~~

일단... 폴더 config를 추가하고 거기엔 pan_tilt_control.yaml, 기존의 launch 폴더에는 gazebo_pan_tilt.launch, pan_tilt_gazebo_control.launch를 추가하고, urdf에 있는 pan_tilt.xacro는 수정할 예정입니다. 먼저... pan_tilt.xacro는 

<?xml version="1.0"?>

<robot xmlns:xacro="http://www.ros.org/wiki/xacro" 
    xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
    xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
    xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
    name="ex_urdf_pan_tilt">
    

    <xacro:property name="base_link_length" value="0.01" />
    <xacro:property name="base_link_radius" value="0.2" />

    <xacro:property name="pan_link_length" value="0.4" />
    <xacro:property name="pan_link_radius" value="0.04" />

    <xacro:property name="tilt_link_length" value="0.4" />
    <xacro:property name="tilt_link_radius" value="0.04" />


    <xacro:macro name="inertial_matrix" params="mass">
               <inertial>
                       <mass value="${mass}" />
                       <inertia ixx="1." ixy="0.0" ixz="0.0"
                                iyy="1." iyz="0.0"
                                izz="1." />
               </inertial>
    </xacro:macro>

   <xacro:macro name="transmission_block" params="joint_name">
    <transmission name="tran1">
      <type>transmission_interface/SimpleTransmission</type>
      <joint name="${joint_name}">
        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      </joint>
      <actuator name="motor1">
        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
      </actuator>
    </transmission>
   </xacro:macro>


  <link name="base_link">

    <visual>
      <geometry>
         <cylinder length="${base_link_length}" radius="${base_link_radius}"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="yellow">
        <color rgba="1 1 0 1"/>
      </material>
    </visual>

    <collision>
      <geometry>
         <cylinder length="${base_link_length+0.02}" radius="0.2"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
    <xacro:inertial_matrix mass="1"/>
  </link>

  <gazebo reference="base_link">
    <material>Gazebo/Black</material>
  </gazebo>

  <joint name="pan_joint" type="revolute">
    <parent link="base_link"/>
    <child link="pan_link"/>
    <origin xyz="0 0 0.1"/>
    <axis xyz="0 0 1" />
    <limit effort="300" velocity="0.1" lower="-3.14" upper="3.14"/>
    <dynamics damping="50" friction="1"/>
  </joint>

  <link name="pan_link">
    <visual>
      <geometry>
         <cylinder length="${pan_link_length}" radius="${pan_link_radius}"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0.09"/>
      <material name="red">
        <color rgba="0 0 1 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
         <cylinder length="${pan_link_length}" radius="${pan_link_radius+0.02}"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0.09"/>
    </collision>
    <xacro:inertial_matrix mass="1"/>
  </link>

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

  <joint name="tilt_joint" type="revolute">
    <parent link="pan_link"/>
    <child link="tilt_link"/>
    <origin xyz="0 0 0.2"/>
    <axis xyz="0 1 0" />
    <limit effort="300" velocity="0.1" lower="-4.71239" upper="-1.570796"/>
    <dynamics damping="50" friction="1"/>
  </joint>

  <link name="tilt_link">
    <visual>
      <geometry>
         <cylinder length="${tilt_link_length}" radius="${tilt_link_radius}"/>
      </geometry>
      <!-- <origin rpy="0 1.570796 0" xyz="0 0 0"/> -->
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="green">
        <color rgba="1 0 0 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
         <cylinder length="${tilt_link_length}" radius="${tilt_link_radius+0.2}"/>
      </geometry>
      <!-- <origin rpy="0 1.570796 0" xyz="0 0 0"/> -->
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
    <xacro:inertial_matrix mass="1"/>
  </link>

  <gazebo reference="tilt_link">
    <material>Gazebo/Red</material>
  </gazebo>

   <xacro:transmission_block joint_name="pan_joint"/>
   <xacro:transmission_block joint_name="tilt_joint"/>

  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/ex_urdf_pan_tilt</robotNamespace>
    </plugin>
  </gazebo>


</robot>

와 같이 구성합니다.. 구동되는 joint를 transmission으로 지정하고 움직이는 hardware는 심플하게 잡습니다. 그리고, link의 색상들을 지정하고, gazebo_ros_control을 사용하도록 합니다.^^

<launch>

  <!-- these are the arguments you can pass this launch file, for example paused:=true -->
  <arg name="paused" default="false"/>
  <arg name="use_sim_time" default="true"/>
  <arg name="gui" default="true"/>
  <arg name="headless" default="false"/>
  <arg name="debug" default="false"/>

  <!-- We resume the logic in empty_world.launch -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="debug" value="$(arg debug)" />
    <arg name="gui" value="$(arg gui)" />
    <arg name="paused" value="$(arg paused)"/>
    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
    <arg name="headless" value="$(arg headless)"/>
  </include>

  <!-- Load the URDF into the ROS Parameter Server -->
  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find ex_urdf)/urdf/pan_tilt.xacro'" /> 

  <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
    args="-urdf -model ex_urdf_pan_tilt -param robot_description"/> 


</launch>

그리고 gazebo_pan_tilt.launch를 구성합니다. 

<launch>
  <!-- Launch Gazebo  -->
  <include file="$(find ex_urdf)/launch/gazebo_pan_tilt.launch" />   

  <!-- Load joint controller configurations from YAML file to parameter server -->
  <rosparam file="$(find ex_urdf)/config/pan_tilt_control.yaml" command="load"/>

  <!-- load the controllers -->
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
    output="screen" ns="/ex_urdf_pan_tilt" args="joint_state_controller
                      joint1_position_controller
                      joint2_position_controller"/>

  <!-- convert joint states to TF transforms for rviz, etc -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
    respawn="false" output="screen">
    <remap from="/joint_states" to="/ex_urdf_pan_tilt/joint_states" />
  </node>

</launch>

그걸 이용해서 pan_tilt_gazebo_control.launch에서 각 조인트를 움직일 수 있는 부분을 추가합니다. 여기서 rosparam에서 사용할려는 pan_tilt_control.yaml은

ex_urdf_pan_tilt:
  # Publish all joint states -----------------------------------
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50  
  
  # Position Controllers ---------------------------------------
  joint1_position_controller:
    type: position_controllers/JointPositionController
    joint: pan_joint
    pid: {p: 100.0, i: 0.01, d: 10.0}
  joint2_position_controller:
    type: position_controllers/JointPositionController
    joint: tilt_joint
    pid: {p: 100.0, i: 0.01, d: 10.0}

이렇게 구성됩니다.^^

이제... roslaunch 명령으로 gazebo_pan_tilt.launch만 실행하면 위 그림처럼 gazebo가 실행됩니다.^^ 이제 다시~~ pan_tilt_gazebo.launch를 roslaunch로 실행한 후

위 그림처럼 rostopic list를 보면 joint1_position_controller라는 아이가 보입니다....

위 명령처럼 실행하면 살짝~ gazebo의 pan_tilt가 움직이는 것이 보이실 겁니다.^^