토픽 결과물: 로봇의 실제 위치를 알 수 있어요!
이번 토픽은 로봇의 실제 위치 (x, y, theta)를 ROS topic 형태로 출력하는 부분입니다. 실제 위치를 Ground Truth Pose라고 하며, SLAM의 오차 계산이라던지, 로봇이 실제 움직인 궤적등에 사용됩니다. Ground truth pose를 획득하는 방법은 몇 가지가 있는데, libgazebo_ros_p3d.so라는 라이브러리를 이용하는 방법을 소개하기로 합니다. 가장 간단한 방법은 Gazebo가 출력하는 model 정보로부터 획득하는 방법이 있지만 여기서는 위 라이브러리를 이용하는 방법을 설명하겠습니다.
설명 동영상
kobuki.urdf.xacro 수정
mobile_robot_sim 패키지의 urdf 디렉터리 밑에 있는 kobuki.urdf.xacro의 앞부분은 다음과 같습니다.
<robot name="kobuki" xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:include filename="$(find kobuki_description)/urdf/common_properties.urdf.xacro"/>
<xacro:include filename="$(find kobuki_description)/urdf/kobuki_gazebo.urdf.xacro"/>
-->
이것을 아래와 같이 변경합니다. 단순히 인클루드 하는 위치를 변경한 것입니다. 나중에 kobuki_gazebo.urdf.xacro을 수정하여 Ground Truth Pose를 출력하게 할 것입니다.
<robot name="kobuki" xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:include filename="$(find mobile_robot_sim)/urdf/common_properties.urdf.xacro"/>
<xacro:include filename="$(find mobile_robot_sim)/urdf/kobuki_gazebo.urdf.xacro"/>
Ground Truth Pose Topic 설정
기존의 kobuki_description의 urdf 디렉터리 밑의 kobuki_gazebo.urdf.xacro 파일을 mobile_robot_sim 패키지의 urdf 디렉터리로 복사합니다.
$ roscd kobuki_description/urdf
$ cp kobuki_gazebo.urdf.xacro ~/catkin_ws/src/mobile_robot_sim/urdf
위 파일에 아래와 같이 아래부분을 추가합니다.
<?xml version="1.0"?>
<robot name="kobuki_sim" xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro name="kobuki_sim">
<gazebo reference="wheel_left_link">
<mu1>1.0</mu1>
<mu2>1.0</mu2>
<kp>1000000.0</kp>
<kd>100.0</kd>
<minDepth>0.001</minDepth>
<maxVel>1.0</maxVel>
</gazebo>
<gazebo reference="wheel_right_link">
<mu1>1.0</mu1>
<mu2>1.0</mu2>
<kp>1000000.0</kp>
<kd>100.0</kd>
<minDepth>0.001</minDepth>
<maxVel>1.0</maxVel>
</gazebo>
<gazebo reference="caster_front_link">
<mu1>0.0</mu1>
<mu2>0.0</mu2>
<kp>1000000.0</kp>
<kd>100.0</kd>
<minDepth>0.001</minDepth>
<maxVel>1.0</maxVel>
</gazebo>
<gazebo reference="caster_back_link">
<mu1>0.0</mu1>
<mu2>0.0</mu2>
<kp>1000000.0</kp>
<kd>100.0</kd>
<minDepth>0.001</minDepth>
<maxVel>1.0</maxVel>
</gazebo>
<gazebo reference="base_link">
<mu1>0.3</mu1>
<mu2>0.3</mu2>
<sensor type="contact" name="bumpers">
<always_on>1</always_on>
<update_rate>50.0</update_rate>
<visualize>true</visualize>
<contact>
<collision>base_footprint_collision_base_link</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="cliff_sensor_left_link">
<sensor type="ray" name="cliff_sensor_left">
<always_on>true</always_on>
<update_rate>50</update_rate>
<visualize>true</visualize>
<ray>
<scan>
<horizontal>
<samples>50</samples>
<resolution>1.0</resolution>
<min_angle>-0.0436</min_angle> <!-- -2.5 degree -->
<max_angle>0.0436</max_angle> <!-- 2.5 degree -->
</horizontal>
<!-- Can't use vertical rays until this bug is resolved: -->
<!-- https://bitbucket.org/osrf/gazebo/issue/509/vertical-sensor-doesnt-works -->
<!-- <vertical> -->
<!-- <samples>50</samples> -->
<!-- <resolution>1.0</resolution> -->
<!-- <min_angle>-0.0436</min_angle> -2.5 degree -->
<!-- <max_angle>0.0436</max_angle> 2.5 degree -->
<!-- </vertical> -->
</scan>
<range>
<min>0.01</min>
<max>0.15</max>
<resolution>1.0</resolution>
</range>
</ray>
</sensor>
</gazebo>
<gazebo reference="cliff_sensor_right_link">
<sensor type="ray" name="cliff_sensor_right">
<always_on>true</always_on>
<update_rate>50</update_rate>
<visualize>true</visualize>
<ray>
<scan>
<horizontal>
<samples>50</samples>
<resolution>1.0</resolution>
<min_angle>-0.0436</min_angle> <!-- -2.5 degree -->
<max_angle>0.0436</max_angle> <!-- 2.5 degree -->
</horizontal>
<!-- Can't use vertical rays until this bug is resolved: -->
<!-- https://bitbucket.org/osrf/gazebo/issue/509/vertical-sensor-doesnt-works -->
<!-- <vertical> -->
<!-- <samples>50</samples> -->
<!-- <resolution>1.0</resolution> -->
<!-- <min_angle>-0.0436</min_angle> -2.5 degree -->
<!-- <max_angle>0.0436</max_angle> 2.5 degree -->
<!-- </vertical> -->
</scan>
<range>
<min>0.01</min>
<max>0.15</max>
<resolution>1.0</resolution>
</range>
</ray>
</sensor>
</gazebo>
<gazebo reference="cliff_sensor_front_link">
<sensor type="ray" name="cliff_sensor_front">
<always_on>true</always_on>
<update_rate>50</update_rate>
<visualize>true</visualize>
<ray>
<scan>
<horizontal>
<samples>50</samples>
<resolution>1.0</resolution>
<min_angle>-0.0436</min_angle> <!-- -2.5 degree -->
<max_angle>0.0436</max_angle> <!-- 2.5 degree -->
</horizontal>
<!-- Can't use vertical rays until this bug is resolved: -->
<!-- https://bitbucket.org/osrf/gazebo/issue/509/vertical-sensor-doesnt-works -->
<!-- <vertical> -->
<!-- <samples>50</samples> -->
<!-- <resolution>1.0</resolution> -->
<!-- <min_angle>-0.0436</min_angle> -2.5 degree -->
<!-- <max_angle>0.0436</max_angle> 2.5 degree -->
<!-- </vertical> -->
</scan>
<range>
<min>0.01</min>
<max>0.15</max>
<resolution>1.0</resolution>
</range>
</ray>
</sensor>
</gazebo>
<gazebo reference="gyro_link">
<sensor type="imu" name="imu">
<always_on>true</always_on>
<update_rate>50</update_rate>
<visualize>false</visualize>
<imu>
<noise>
<type>gaussian</type>
<rate>
<mean>0.0</mean>
<stddev>${0.0014*0.0014}</stddev> <!-- 0.25 x 0.25 (deg/s) -->
<bias_mean>0.0</bias_mean>
<bias_stddev>0.0</bias_stddev>
</rate>
<accel> <!-- not used in the plugin and real robot, hence using tutorial values -->
<mean>0.0</mean>
<stddev>0.0</stddev>
<bias_mean>0.0</bias_mean>
<bias_stddev>0.0</bias_stddev>
<!--
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev> -->
</accel>
</noise>
</imu>
</sensor>
</gazebo>
<gazebo>
<plugin name="kobuki_controller" filename="libgazebo_ros_kobuki.so">
<publish_tf>1</publish_tf>
<left_wheel_joint_name>wheel_left_joint</left_wheel_joint_name>
<right_wheel_joint_name>wheel_right_joint</right_wheel_joint_name>
<wheel_separation>.230</wheel_separation>
<wheel_diameter>0.070</wheel_diameter>
<torque>1.0</torque>
<velocity_command_timeout>0.6</velocity_command_timeout>
<cliff_sensor_left_name>cliff_sensor_left</cliff_sensor_left_name>
<cliff_sensor_center_name>cliff_sensor_front</cliff_sensor_center_name>
<cliff_sensor_right_name>cliff_sensor_right</cliff_sensor_right_name>
<cliff_detection_threshold>0.04</cliff_detection_threshold>
<bumper_name>bumpers</bumper_name>
<imu_name>imu</imu_name>
</plugin>
</gazebo>
<!-- Ground Truth Pose -->
<gazebo>
<plugin name="gazebo_ros_p3d" filename="libgazebo_ros_p3d.so">
<alwaysOn>true</alwaysOn>
<updateRate>30.0</updateRate>
<bodyName>base_footprint</bodyName>
<topicName>ground_truth_pose</topicName>
<gaussianNoise>0.0</gaussianNoise>
<frameName>map</frameName>
</plugin>
</gazebo>
</xacro:macro>
</robot>
위 코드에서 Ground Truth Pose를 출력하기 위해 <plugin name="gazebo_ros_p3d" filename="libgazebo_ros_p3d.so"> plugin을 추가한 것을 볼 수 있습니다. 로봇의 여러 frame 중에서 어떤 frame을 ground truth pose로 할지 <bodyName> 태그에서 설정합니다. 여기서는 로봇의 base_footprint 즉 로봇의 중심의 바닥면에 위치한 frame의 위치값이 출력됩니다. 이 때 기준이 되는 frame은 <frameName> 태그에서 설정합니다. 여기서는 map frame 기준으로 위치값이 출력된다. 이 때 출력되는 topic 이름은 <topicName>에서 설정하며 이 경우는 ground_truth_pose 입니다. 주의할 것은 odom frame은 로봇의 odometry의 기준 좌표로 고정되어 있습니다. 하지만 drift나 slip이 발생하여 조금씩 틀어질 수는 있습니다. 이 odom frame을 로봇에 붙어서 이동하는 frame과 혼동하지 않아야 합니다.
(주의) gazebo_ros_p3d 소스코드를 보면 아래와 같은 코드가 있는데, <frameName>으로는 아래 세가지만 허용됩니다. 따라서, odom으로 하면 ground truth가 publish 안됩니다. 따라서 여기서는 map이라고 설정하였습니다. 이상한 점은 tf에서 map frame이 없으는데도 제대로 publish된다는 점입니다. 왜 그럴까요??
if (this->frame_name_ != "world" &&
this->frame_name_ != "/map" &&
this->frame_name_ != "map")
{
this->reference_link_ = this->model_->GetLink(this->frame_name_);
if (!this->reference_link_)
{
ROS_ERROR("gazebo_ros_p3d plugin: frameName: %s does not exist, will"
" not publish pose\n", this->frame_name_.c_str());
return;
}
}
아래와 같이 tf tree를 출력해 보면 map frame은 없습니다.
$ rosrun rqt_tf_tree rqt_tf_tree
Gazebo가 출력하는 tf tree 
Ground Truth Pose Topic 확인
먼저 Gazebo 시뮬레이션 launch 파일을 실행시킨 후, rostopic list 명령으로 /ground_truth_posetopic이 존재하는지 확인합니다. 그리고 나서 rostopic echo /ground_truth_pose 명령으로 실제 값을 출력해 봅니다.
$ roslaunch mobile_robot_sim turtlebot_hokuyo.launch
$ rostopic list
$ rostopic echo /ground_truth_pose
실행결과 다음과 같은 값들이 출력되는 것을 확인합니다.
위치와 속도에 대한 Covariance값이 모두 0인 것이 확인됩니다. 즉 에러가 추가되지 않은 것을 알 수 있습니다.
