무브잇으로 만든 매니퓰레이터에 뎁스카메라 설치하기

1. Xacro 파일을 수정한다.

전문은 다음과 같다.

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

<xacro:property name="width" value="0.05"/>
<xacro:property name="height" value="0.2"/>
<xacro:property name="mass" value="0.1"/>

    <link name="world"/>

    <joint name="base" type="fixed">
        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
        <parent link="world"/>
        <child link="base_link"/>
        <axis xyz="0.0 0.0 0.0"/>
        <limit lower="0.0" upper="0.0" effort="0.0" velocity="0.0"/>
    </joint>

    <link name="base_link">
        <inertial>
            <origin xyz="0.0 0.0 ${height/2}" rpy="0.0 0.0 0.0"/>
            <mass value="${mass}"/>
            <inertia ixx="${mass/12*(width*width+width*width)}" ixy="0.0" ixz="0.0" 
                iyy="${mass/12*(width*width+width*width)}" iyz="0.0" 
                izz="${mass/12*(height*height+height*height)}"/>
        </inertial>
        <visual>
            <origin xyz="0.0 0.0 ${height/2}" rpy="0.0 0.0 0.0"/>
            <geometry>
                <box size="${width} ${width} ${height}"/>
            </geometry>
            <material name="base_link">
                <color rgba="1.0 0.0 0.0 1.0"/>
            </material>
        </visual>
        <collision>
            <origin xyz="0.0 0.0 ${height/2}" rpy="0.0 0.0 0.0"/>
            <geometry>
                <box size="${width} ${width} ${height}"/>
            </geometry>
        </collision>
    </link>

    <joint name="Joint1" type="revolute">
        <origin xyz="0.0 0.0 0.25" rpy="0.0 0.0 0.0"/>
        <parent link="base_link"/>
        <child link="Link1"/>
        <axis xyz="0.0 0.0 1.0"/>
        <limit lower="-3.14" upper="3.14" effort="30" velocity="1"/>
    </joint>

    <link name="Link1">
        <inertial>
            <origin xyz="0.0 0.0 ${height/2}" rpy="0.0 0.0 0.0"/>
            <mass value="${mass}"/>
            <inertia ixx="${mass/12*(width*width+width*width)}" ixy="0.0" ixz="0.0" 
                iyy="${mass/12*(width*width+width*width)}" iyz="0.0" 
                izz="${mass/12*(height*height+height*height)}"/>
        </inertial>
        <visual>
            <origin xyz="0.0 0.0 ${height/2}" rpy="0.0 0.0 0.0"/>
            <geometry>
                <box size="${width} ${width} ${height}"/>
            </geometry>
            <material name="link1">
                <color rgba="0.0 1.0 0.0 1.0"/>
            </material>
        </visual>
        <collision>
            <origin xyz="0.0 0.0 ${height/2}" rpy="0.0 0.0 0.0"/>
            <geometry>
                <box size="${width} ${width} ${height}"/>
            </geometry>
        </collision>
    </link>
    
    <joint name="Joint2" type="revolute">
        <origin xyz="0.0 0.0 0.25" rpy="0.0 0.0 0.0"/>
        <parent link="Link1"/>
        <child link="Link2"/>
        <axis xyz="1.0 0.0 0.0"/>
        <limit lower="-3.14" upper="3.14" effort="30" velocity="1"/>
    </joint>

    <link name="Link2">
        <inertial>
            <origin xyz="0.0 0.0 ${height/2}" rpy="0.0 0.0 0.0"/>
            <mass value="${mass}"/>
            <inertia ixx="${mass/12*(width*width+width*width)}" ixy="0.0" ixz="0.0" 
                iyy="${mass/12*(width*width+width*width)}" iyz="0.0" 
                izz="${mass/12*(height*height+height*height)}"/>
        </inertial>
        <visual>
            <origin xyz="0.0 0.0 ${height/2}" rpy="0.0 0.0 0.0"/>
            <geometry>
                <box size="${width} ${width} ${height}"/>
            </geometry>
            <material name="link2">
                <color rgba="0.0 0.0 1.0 1.0"/>
            </material>
        </visual>
        <collision>
            <origin xyz="0.0 0.0 ${height/2}" rpy="0.0 0.0 0.0"/>
            <geometry>
                <box size="${width} ${width} ${height}"/>
            </geometry>
        </collision>
    </link>

    <joint name="Joint3" type="revolute">
        <origin xyz="0.0 0.0 0.25" rpy="0.0 0.0 0.0"/>
        <parent link="Link2"/>
        <child link="Link3"/>
        <axis xyz="1.0 0.0 0.0"/>
        <limit lower="-3.14" upper="3.14" effort="30" velocity="1"/>
    </joint>

    <link name="Link3">
        <inertial>
            <origin xyz="0.0 0.0 ${height/2}" rpy="0.0 0.0 0.0"/>
            <mass value="${mass}"/>
            <inertia ixx="${mass/12*(width*width+width*width)}" ixy="0.0" ixz="0.0" 
                iyy="${mass/12*(width*width+width*width)}" iyz="0.0" 
                izz="${mass/12*(height*height+height*height)}"/>
        </inertial>
        <visual>
            <origin xyz="0.0 0.0 ${height/2}" rpy="0.0 0.0 0.0"/>
            <geometry>
                <box size="${width} ${width} ${height}"/>
            </geometry>
            <material name="link3">
                <color rgba="0.0 1.0 1.0 1.0"/>
            </material>
        </visual>
        <collision>
            <origin xyz="0.0 0.0 ${height/2}" rpy="0.0 0.0 0.0"/>
            <geometry>
                <box size="${width} ${width} ${height}"/>
            </geometry>
        </collision>
    </link>

    <joint name="Joint4" type="fixed">
        <origin xyz="0.01 0.0 0.21" rpy="0.0 -1.57 0.0"/>
        <parent link="Link3"/>
        <child link="Link4"/>
        <axis xyz="0.0 0.0 1.0"/>
        <limit lower="-3.14" upper="3.14" effort="30" velocity="1"/>
    </joint>

    <link name="Link4">
        <inertial>
            <origin xyz="0.0 0.0 0.01" rpy="0.0 0.0 0.0"/>
            <mass value="${mass}"/>
            <inertia ixx="${mass/12*0.16*(width*width+width*width)}" ixy="0.0" ixz="0.0" 
                iyy="${mass/12*0.16*(width*width+width*width)}" iyz="0.0" 
                izz="${mass/12*0.16*(height*height+height*height)}"/>
        </inertial>
        <visual>
            <origin xyz="0.0 0.0 0.01" rpy="0.0 0.0 0.0"/>
            <geometry>
                <box size="0.02 0.02 0.02"/>
            </geometry>
            <material name="link4">
                <color rgba="1.0 1.0 0.0 1.0"/>
            </material>
        </visual>
        <collision>
            <origin xyz="0.0 0.0 0.01" rpy="0.0 0.0 0.0"/>
            <geometry>
                <box size="0.02 0.02 0.02"/>
            </geometry>
        </collision>
    </link>

    <joint name="Joint5" type="fixed">
        <origin xyz="0.0 0.0 0.2" rpy="0.0 0.0 -1.57"/>
        <parent link="Link3"/>
        <child link="frame_link"/>
        <axis xyz="0.0 0.0 1.0"/>
        <limit lower="0.0" upper="0.0" effort="30" velocity="1"/>
    </joint>

    <link name="frame_link"/>

    <transmission name="transmission1">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="Joint1">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="actuator1">
            <mechanicalReduction>1.0</mechanicalReduction>
            <!-- <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> -->
        </actuator>
    </transmission>

    <transmission name="transmission2">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="Joint2">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="actuator2">
            <mechanicalReduction>1.0</mechanicalReduction>
            <!-- <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> -->
        </actuator>
    </transmission>

    <transmission name="transmission3">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="Joint3">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="actuator3">
            <mechanicalReduction>1.0</mechanicalReduction>
            <!-- <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> -->
        </actuator>
    </transmission>

<!-- gazebo plugin -->

  <gazebo reference="Link4">
    <sensor name="camera_link_camera" type="depth">
      <update_rate>20</update_rate>
      <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="camera_link_controller" filename="libgazebo_ros_openni_kinect.so">
        <baseline>0.2</baseline>
        <alwaysOn>true</alwaysOn>
        <visualize>true</visualize>
        <updateRate>1.0</updateRate>
        <cameraName>robot_camera</cameraName>
        <imageTopicName>/robot_camera/color/image_raw</imageTopicName>
        <cameraInfoTopicName>/robot_camera/color/camera_info</cameraInfoTopicName>
        <depthImageTopicName>/robot_camera/depth/image_raw</depthImageTopicName>
        <depthImageInfoTopicName>/robot_camera/depth/camera_info</depthImageInfoTopicName>
        <pointCloudTopicName>/robot_camera/depth/points</pointCloudTopicName>
        <frameName>frame_link</frameName>
        <pointCloudCutoff>0.5</pointCloudCutoff>
        <pointCloudCutoffMax>3.0</pointCloudCutoffMax>
        <distortionK1>0.00000001</distortionK1>
        <distortionK2>0.00000001</distortionK2>
        <distortionK3>0.00000001</distortionK3>
        <distortionT1>0.00000001</distortionT1>
        <distortionT2>0.00000001</distortionT2>
        <CxPrime>0</CxPrime>
        <Cx>0</Cx>
        <Cy>0</Cy>
        <focalLength>0</focalLength>
        <hackBaseline>0</hackBaseline>
      </plugin>
    </sensor>
  </gazebo>

  <!-- Gazebo Reference macro -->
  <xacro:macro name="RefLink" params="ref">
    <gazebo reference="${ref}">
      <kp>1000000.0</kp>
      <kd>100.0</kd>
      <mu1>30.0</mu1>
      <mu2>30.0</mu2>
      <maxVel>1.0</maxVel>
      <minDepth>0.001</minDepth>
    </gazebo>
  </xacro:macro>

  <gazebo reference="world"/>
  <RefLink ref="Link1"/>
  <RefLink ref="Link2"/>
  <RefLink ref="Link3"/>

  <!-- ros_control plugin -->
  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <controlPeriod>0.001</controlPeriod>
      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
      <legacyModeNS>true</legacyModeNS>
    </plugin>
  </gazebo>

</robot>

이전 게시물의 xacro 파일에서 변경된 점은

1-1. 카메라 gazebo plugin이 추가되었다.

       여기서 gazebo reference 뒤에 나오는 이름은 카메라를 달고자 하는 링크 이름을 입력하면 되고

       frameName에 들어가는 내용은 뎁스 카메라로 인식한 내용을 출력해낼 때 사용하는 링크의 이름을 입력하면 된다.

 

1-2. Joint4의 rpy를 바꿔준다.

       뎁스 카메라를 Join4에 달아주면 x축을 바라보도록 기본설정이 되어 있는데

       z축을 바라보게 하고 싶기 때문에 pitch 값을 -90도 회전시키기 위해 0.0 -1.57 0.0으로 변경했다.

 

1-3. Joint5와 frame_link를 생성한다.

       뎁스 카메라를 Link4에 달아서 뎁스값을 받은 이후에 다시 이를 출력해낼 때 똑같이 Link4를 사용하면

       실제 위치와 다른 곳에 point cloud가 출력된다.

       이를 맞춰주기 위해 형태가 없고 존재만 하는 rame_link를 만들고

       Joint5를 만들어 Joint4와 같은 이유로 yaw 값을 -90도 회전시키기 위해 0.0 0.0 -1.57으로 변경했다.

 

2. config/sensors_3d.yaml 파일을 다음과 같이 수정한다.

sensors:
  - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
    point_cloud_topic: /robot_camera/depth/points
    max_range: 5.0
    point_subsample: 1
    padding_offset: 0.1
    padding_scale: 1.0
    max_update_rate: 1.0
    filtered_cloud_topic: filtered_cloud

point_cloud_topic의 경로가

xacro 파일의 gazebo plugin에 pointCloudTopicName의 경로와 일치하는지 확인한다.

 

3. lauch/sensor_manager.launch.xml 파일에 다음 내용을 추가해준다.

<param name="octomap_frame" type="string" value="base_link" />

pointcloud가 생성될 때 기준점이 움직이면 안 되기 때문에 (특히 mobile 형태의 로봇의 경우) xacro파일 중에서 고정되어있는 링크를 하나 선택한다.