[RIS] Navigation 관련 문제 (2/24 해결)

2/24에 해당 문제 해결 (아랫 부분에 설명)

 

- Navigation을 위한 Software 구성

• https://youngseong.tistory.com/130

[RIS] 1/11 (전체 Sotfware 구성)

• 위 과정을 통해 Navigation Stack 구성을 완료하고 SLAM을 통해 작성한 Map상에서 Navgation을 구현하려 했으나 아래와 같이 TF의 위치가 계속 변함

 

 

 

- 2/24 위 문제점 해결

• 기존 launch file의 내용

<?xml version="1.0"?>
<launch>
	<!-- Transformation Configuration -->
	<!-- Use static transform publisher because each frames doesn`t change through time-->
	<!-- 			    static_transform_publisher, broadcaster name, x y z yaw pitch roll frame_id child_frame_id period_in_ms -->
	<node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0.15 0 0.13 0 0 0 base_link laser 30" />
	<!-- base_link to imu_link will be provided by the iahrs_driver -->
	<node pkg="tf" type="static_transform_publisher" name="base_link_broadcaster" args="0 0 0 0 0 0 base_footprint base_link 30" />
	<!-- odom to base_footprint transform will be provided by the robot_pose_ekf node -->
	<!-- map to odom will be provided by the AMCL -->
	<node pkg="tf" type="static_transform_publisher" name="map_to_odom" args="0 0 0 0 0 0 map odom 30" />

	<!-- Launch IMU(Iahrs) -->
	<!-- Publish : /imu/data -->
	<param name="m_bSingle_TF_option" type="bool" value="true" /> <!--false-->
	<node pkg="iahrs_driver" type="iahrs_driver" name="iahrs_driver" output="screen" />

	<!-- Launch Lidar(RPLidarA2) -->
	<!-- Publish : /scan -->
	<node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
	<param name="serial_port"         type="string" value="/dev/ttyRPLidar"/>
	<param name="serial_baudrate"     type="int"    value="115200"/>
	<param name="frame_id"            type="string" value="laser"/>
	<param name="inverted"            type="bool"   value="false"/>
	<param name="angle_compensate"    type="bool"   value="true"/>
	</node>
    
	<!-- Launch Motor Driver(MD200T) -->
	<node pkg="md" type="md_robot_node" name="md_robot_node" output="screen">
    <param name = "use_MDUI"                    value = "0"/>       <!-- 0: not use MDUI, 1: use MDUI -->
    <param name = "wheel_radius"                value = "0.065"/>  <!-- unit: meter -->
    <param name = "wheel_length"                value = "0.295"/>   <!-- unit: meter -->
    <param name = "motor_pole"                  value = "20"/> 
    <param name = "reduction"                   value = "4.33"/> 	<!-- Reduction Ratio : 1/4.33 -->
    <param name = "reverse_direction"           value = "0"/>       <!-- 0: forward, 1: reverse -->
    <param name = "maxrpm"                      value = "400"/>    <!-- unit: RPM -->
    <param name = "motor_posi"                  value = "1"/>       <!-- motor pisition 0: hall sensor, 1: encoder -->
    <param name = "encoder_PPR"                 value = "4096"/>    <!-- if use encoder position, encoder PPR -->
    <param name = "position_proportion_gain"    value = "20"/>      <!-- reference PID 203(PID_GAIN) -->
    <param name = "speed_proportion_gain"       value = "50"/>      <!-- reference PID 203(PID_GAIN) -->
    <param name = "integral_gain"               value = "1800"/>    <!-- reference PID 203(PID_GAIN) -->
    <param name = "slow_start"                  value = "300"/>     <!-- unit: RPM -->
    <param name = "slow_down"                   value = "300"/>     <!-- unit: RPM -->
	</node>
    
	<!-- Initial Pose and Goal Publisher -->
	<!-- Publish: /initialpose, /move_base_simple/goal -->

	<!-- Subscribe: /initialpose, /move_base_simple/goal -->
	<!-- Publish: /initial_2d, /goal_2d --> 
	<!-- Launch rviz_click_to_2d node -->
	<node pkg="localization_data" type="rviz_click_to_2d" name="rviz_click_to_2d">
	</node>
    
	<!-- Wheel Odometry Publisher -->
	<!-- Subscribe: /right_ticks, /left_ticks, /initial_2d -->
	<!-- Publish: /odom_data_euler, /odom_data_quat -->
	<node pkg="localization_data" type="ekf_odom_pub" name="ekf_odom_pub">
	</node>

	<!-- Extended Kalman Filter from robot_pose_ekf Node-->
	<!-- Subscribe: /odom, /imu_data, /vo -->
	<!-- Publish: /robot_pose_ekf/odom_combined -->
	<remap from="odom" to="odom_data_quat" />
	<remap from="imu_data" to="imu/data" />
	<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
	<param name="output_frame" value="odom"/>
	<param name="base_footprint_frame" value="base_footprint"/>
	<param name="freq" value="30.0"/>
	<param name="sensor_timeout" value="1.0"/>
	<param name="odom_used" value="true"/>
	<param name="imu_used" value="true"/>
	<param name="vo_used" value="false"/>
	<param name="gps_used" value="false"/>
	<param name="debug" value="false"/>
	<param name="self_diagnose" value="false"/>
	</node>
    
    	<!-- Map File -->
  	<arg name="map_file" default="/home/ris/catkin_ws/maps/test.yaml"/>
	
  	<!-- Map Server -->
  	<!-- Publish: /map, /map_metadata -->
  	<node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)" />
	
  	<!-- Add AMCL example for differential drive robots for Localization -->
  	<!-- Subscribe: /scan, /tf, /initialpose, /map -->
  	<!-- Publish: /amcl_pose, /particlecloud, /tf -->
  	<include file="$(find amcl)/examples/amcl_diff.launch"/>
    
	<!-- Move Base Node -->
	<!-- Subscribe: /move_base_simple/goal -->
	<!-- Publish: /cmd_vel -->
	<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    	<rosparam file="$(find navstack)/param/costmap_common_params.yaml" command="load" ns="global_costmap"/>
    	<rosparam file="$(find navstack)/param/costmap_common_params.yaml" command="load" ns="local_costmap"/>
    	<rosparam file="$(find navstack)/param/local_costmap_params.yaml" command="load" ns="local_costmap"/>
    	<rosparam file="$(find navstack)/param/global_costmap_params.yaml" command="load" ns="global_costmap"/>
    	<rosparam file="$(find navstack)/param/base_local_planner_params.yaml" command="load"/>
  </node>
</launch>

 

• AMCL을 통해 localization을 수행하기 위해선 AMCL에 Lidar의 scan data와 odometry값을 줘야 함
• 이때 robot이 움직임에 따라 오차가 쌓이므로 TF중 map -> odom간의 관계를 조정하여 오차를 수정했어야 함
• 그러나 위의 launch file에서 이 두 frame을 고정시켜버려 robot이 움직일수록 두 frame이 충돌이 일어나며 위에 있는 동영상과 같이 떨리는 현상이 발생했던 것 

 

<!-- Transformation Configuration -->
	<!-- Use static transform publisher because each frames doesn`t change through time-->
	<!-- 			    static_transform_publisher, broadcaster name, x y z yaw pitch roll frame_id child_frame_id period_in_ms -->
	<node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0.15 0 0.13 0 0 0 base_link laser 30" />
	<!-- base_link to imu_link will be provided by the iahrs_driver -->
	<node pkg="tf" type="static_transform_publisher" name="base_link_broadcaster" args="0 0 0 0 0 0 base_footprint base_link 30" />
	<!-- odom to base_footprint transform will be provided by the robot_pose_ekf node -->
	<!-- map to odom will be provided by the AMCL -->
	<!--<node pkg="tf" type="static_transform_publisher" name="map_to_odom" args="0 0 0 0 0 0 map odom 30" />-->

• 따라서 위와 같이 map -> odom간의 TF를 정의하는 구문을 없애 오차 수정이 되도록 함

 

• 문제 해결