为rplidar A1激光雷达做参数优化

src/robot_bringup/launch/odom_ekf.launch

@@ -1,6 +0,0 @@
-<launch>
-  <node pkg="robot_bringup" type="odom_ekf.py" name="odom_ekf" output="screen">
-    <remap from="input" to="/robot_pose_ekf/odom_combined"/>
-    <remap from="output" to="/odom_ekf"/>
-  </node>
-</launch>

src/robot_bringup/launch/robot_bringup.launch

@@ -12,13 +12,13 @@
 
     <!-- (2) startup mobilebase arduino launch -->
     <include file="$(find ros_arduino_python)/launch/arduino.launch" />
-    
+
     <!-- (3) startup rasp imu-6DOF board-->
     <include file="$(find rasp_imu_hat_6dof)/launch/imu_data_pub.launch" />
 
     <!-- (4) startup robot_pose_ekf node -->
     <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
-        <param name="output_frame" value="odom"/>
+        <param name="output_frame" value="odom_combined"/>
         <param name="freq" value="10.0"/>
         <param name="sensor_timeout" value="1.0"/>
         <param name="odom_used" value="true"/>
@@ -27,5 +27,11 @@
         <param name="debug" value="false"/>
         <param name="self_diagnose" value="false"/>
   </node>
+
+  <!-- (5) /odom_combined view in rviz>
+  <node pkg="robot_bringup" type="odom_ekf.py" name="odom_ekf" output="screen">
+      <remap from="input" to="/robot_pose_ekf/odom_combined" />
+      <remap from="output" to="/odom_ekf" />
+  </node>
 </launch>
 

src/robot_navigation/config/base_local_planner_params.yaml

@@ -20,13 +20,13 @@ clearing_rotation_allowed: false
 
 TrajectoryPlannerROS:
    #Robot Configuration Parameters
-   max_vel_x: 0.17
-   min_vel_x: 0.12
+   max_vel_x: 0.15
+   min_vel_x: 0.10
    y_vels: [-0.17,-0.1,0.1,0.17]
    max_vel_theta: 0.5
    min_vel_theta: -0.5
    min_in_place_vel_theta: 0.3
-   escape_vel: -0.12
+   escape_vel: -0.10
 
    acc_lim_x: 0.05
    acc_lim_y: 0.05   # accelerator speed 
@@ -66,4 +66,4 @@ TrajectoryPlannerROS:
 
    #Global Plan Parameters
    prune_plan: true
-   
+

src/robot_navigation/config/costmap_common_params.yaml

@@ -4,7 +4,7 @@
 # 代价地图通用配置文件，各参数意义如下：
 #   obstacle_range: 最大障碍物检测范围,超过该范围不认为是障碍物
 #   raytrace_range: 检测自由空间的最大范围
-#   robot_radius: 机器人本体的半径
+#   robot_radius: 机器人本体的半径大小,单位:米
 #   inflation_radius: 与障碍物的安全半径距离,如果机器人经常撞到
 #     障碍物就需要增大该值,若无法通过狭窄地方就减小该值
 #   observation_sources: 观察源是激光雷达
@@ -15,6 +15,7 @@ obstacle_range: 2.0
 raytrace_range: 3.0
 robot_radius: 0.126
 inflation_radius: 0.2
+
 observation_sources: scan
 scan: {data_type: LaserScan, topic: scan, marking: true, clearing: true}
 

src/robot_navigation/config/global_costmap_params.yaml

@@ -16,7 +16,7 @@
 global_costmap:
    global_frame: /map
    robot_base_frame: /base_footprint
-   update_frequency: 2.0
+   update_frequency: 1.0
    publish_frequency: 0
    static_map: true
    rolling_window: false

src/robot_navigation/config/local_costmap_params.yaml

@@ -17,7 +17,7 @@
 #
 
 local_costmap:
-   global_frame: /odom
+   global_frame: /odom_combined
    robot_base_frame: /base_footprint
    update_frequency: 3.0
    publish_frequency: 2.0

src/robot_navigation/launch/amcl_rplidar_a1.launch

@@ -0,0 +1,53 @@
+<launch>
+  <!-- Run the map server with a map -->
+  <node name="map_server" pkg="map_server" type="map_server" args="$(find robot_navigation)/maps/my_room.yaml" />
+
+  <!-- startup rplidar node -->
+  <include file="$(find robot_navigation)/launch/rplidar.launch" />
+
+  <!-- startup move_base node -->
+  <include file="$(find robot_navigation)/launch/move_base.launch" />
+
+  <!-- Run amcl -->
+  <node pkg="amcl" type="amcl" name="robot_amcl">
+    <param name="odom_model_type" value="omni"/>
+
+    <param name="base_frame_id" value="base_footprint"/>
+    <param name="global_frame_id" value="map"/>
+    <param name="odom_frame_id" value="odom_combined"/>
+
+    <param name="use_map_topic" value="true"/>
+    <param name="odom_alpha5" value="0.1"/>
+    <param name="transform_tolerance" value="0.5" />
+    <param name="gui_publish_rate" value="1.0"/>
+    <param name="laser_max_beams" value="300"/>
+    <param name="min_particles" value="500"/>
+    <param name="max_particles" value="5000"/>
+    <param name="kld_err" value="0.1"/>
+    <param name="kld_z" value="0.99"/>
+    <param name="odom_alpha1" value="0.2"/>
+    <param name="odom_alpha2" value="0.2"/>
+    <param name="odom_alpha3" value="0.2"/>
+    <param name="odom_alpha4" value="0.2"/>
+    <param name="odom_alpha5" value="0.2"/>
+
+    <param name="laser_z_hit" value="0.9"/>
+    <param name="laser_z_short" value="0.05"/>
+    <param name="laser_z_max" value="0.05"/>
+    <param name="laser_z_rand" value="0.05"/>
+    <param name="laser_sigma_hit" value="0.2"/>
+    <param name="laser_lambda_short" value="0.1"/>
+    <param name="laser_model_type" value="likelihood_field"/>
+    <param name="laser_model_type" value="beam"/>
+    <param name="laser_min_range" value="0.14"/>
+    <param name="laser_max_range" value="7.0"/>
+    <param name="laser_likelihood_max_dist" value="2.0"/>
+    <param name="update_min_d" value="0.2"/>
+    <param name="update_min_a" value="0.5"/>
+    <param name="resample_interval" value="2"/>
+    <param name="transform_tolerance" value="3.0"/>
+    <param name="recovery_alpha_slow" value="0.0"/>
+    <param name="recovery_alpha_fast" value="0.0"/>
+  </node>
+</launch>
+

src/robot_navigation/launch/gmapping_rplidar_a1.launch

@@ -0,0 +1,43 @@
+<launch>
+  <param name="use_sim_time" value="false"/>  
+
+  <!-- startup rplidar node -->
+  <include file="$(find robot_navigation)/launch/rplidar.launch" />
+
+  <!-- startup move_base node -->
+  <include file="$(find robot_navigation)/launch/move_base.launch" />
+
+  <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">  
+    <param name="odom_frame" value="odom_combined" />
+
+    <param name="map_update_interval" value="5.0"/>
+    <param name="maxUrange" value="7.5"/>
+    <param name="sigma" value="0.05"/>
+    <param name="kernelSize" value="1"/>
+    <param name="lstep" value="0.05"/>
+    <param name="astep" value="0.05"/>
+    <param name="iterations" value="5"/>
+    <param name="lsigma" value="0.075"/>
+    <param name="ogain" value="3.0"/>
+    <param name="lskip" value="0"/>
+    <param name="minimumScore" value="50"/>
+    <param name="srr" value="0.1"/>
+    <param name="srt" value="0.2"/>
+    <param name="str" value="0.1"/>
+    <param name="stt" value="0.2"/>
+    <param name="linearUpdate" value="1.0"/>
+    <param name="angularUpdate" value="0.5"/>
+    <param name="temporalUpdate" value="3.0"/>
+    <param name="resampleThreshold" value="0.5"/>
+    <param name="particles" value="30"/>
+    <param name="xmin" value="-5.0"/>
+    <param name="ymin" value="-5.0"/>
+    <param name="xmax" value="5.0"/>
+    <param name="ymax" value="5.0"/>
+    <param name="delta" value="0.05"/>
+    <param name="llsamplerange" value="0.01"/>
+    <param name="llsamplestep" value="0.01"/>
+    <param name="lasamplerange" value="0.005"/>
+    <param name="lasamplestep" value="0.005"/>
+  </node>
+</launch>

src/robot_navigation/launch/move_base.launch

@@ -12,11 +12,15 @@
 -->
 <launch>
   <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
+    <param name="controller_frequency" value="9" />
+    <param name="controller_patiente" value="10" />
+
     <rosparam file="$(find robot_navigation)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
     <rosparam file="$(find robot_navigation)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
     <rosparam file="$(find robot_navigation)/config/local_costmap_params.yaml" command="load" />
     <rosparam file="$(find robot_navigation)/config/global_costmap_params.yaml" command="load" />
     <rosparam file="$(find robot_navigation)/config/base_local_planner_params.yaml" command="load" />
+
     <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
   </node>
 </launch>

src/robot_navigation/rviz/view_navigation.rviz

@@ -8,7 +8,7 @@ Panels:
         - /TF1/Tree1
         - /Global Map1/Planner1
         - /Local Map1/Planner1
-        - /Imu1/Axes properties1
+        - /Odometry1/Shape1
       Splitter Ratio: 0.605556011
     Tree Height: 359
   - Class: rviz/Selection
@@ -50,7 +50,7 @@ Visualization Manager:
         Y: 0
         Z: 0
       Plane: XY
-      Plane Cell Count: 10
+      Plane Cell Count: 30
       Reference Frame: <Fixed Frame>
       Value: true
     - Alpha: 1
@@ -68,6 +68,11 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        base_imu_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
         base_link:
           Alpha: 1
           Show Axes: false
@@ -91,13 +96,15 @@ Visualization Manager:
         All Enabled: false
         base_footprint:
           Value: true
+        base_imu_link:
+          Value: true
         base_link:
           Value: true
         lidar:
           Value: true
         map:
           Value: true
-        odom:
+        odom_combined:
           Value: true
       Marker Scale: 0.300000012
       Name: TF
@@ -106,11 +113,12 @@ Visualization Manager:
       Show Names: true
       Tree:
         map:
-          odom:
+          odom_combined:
             base_footprint:
               base_link:
-                lidar:
-                  {}
+                base_imu_link:
+                  lidar:
+                    {}
       Update Interval: 0
       Value: true
     - Alpha: 1
@@ -128,9 +136,9 @@ Visualization Manager:
       Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 253
+      Max Intensity: 47
       Min Color: 0; 0; 0
-      Min Intensity: 252
+      Min Intensity: 47
       Name: LaserScan
       Position Transformer: XYZ
       Queue Size: 1000
@@ -378,41 +386,18 @@ Visualization Manager:
       Name: Odometry
       Position Tolerance: 0.200000003
       Shape:
-        Alpha: 1
+        Alpha: 0.699999988
         Axes Length: 1
         Axes Radius: 0.100000001
-        Color: 255; 25; 0
-        Head Length: 0.300000012
-        Head Radius: 0.100000001
-        Shaft Length: 1
+        Color: 31; 87; 239
+        Head Length: 0.100000001
+        Head Radius: 0.0500000007
+        Shaft Length: 0.150000006
         Shaft Radius: 0.0500000007
         Value: Arrow
       Topic: /odom_ekf
       Unreliable: false
       Value: true
-    - Acceleration properties:
-        Acc. vector alpha: 1
-        Acc. vector color: 255; 0; 0
-        Acc. vector scale: 1
-        Derotate acceleration: true
-        Enable acceleration: false
-      Axes properties:
-        Axes scale: 1
-        Enable axes: true
-      Box properties:
-        Box alpha: 1
-        Box color: 79; 162; 86
-        Enable box: true
-        x_scale: 0.0500000007
-        y_scale: 0.0500000007
-        z_scale: 0.0500000007
-      Class: rviz_imu_plugin/Imu
-      Enabled: true
-      Name: Imu
-      Topic: /imu
-      Unreliable: false
-      Value: true
-      fixed_frame_orientation: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -437,25 +422,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 21.2202759
+      Distance: 7.83131266
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.0599999987
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -7.59059811
-        Y: -0.358938813
-        Z: 0.18467477
+        X: 2.21107078
+        Y: 1.08232093
+        Z: 0.0960710421
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.0500000007
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.00999999978
-      Pitch: 1.25979471
+      Pitch: 0.859797716
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 1.48496842
+      Yaw: 4.52996302
     Saved: ~
 Window Geometry:
   Displays:
@@ -463,7 +448,7 @@ Window Geometry:
   Height: 876
   Hide Left Dock: true
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000393fc0200000005fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000002800000393000000d700ffffff000000010000010f00000393fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000393000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000006400000032600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000326fc0200000005fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000002800000326000000d700ffffff000000010000010f00000393fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000393000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000006400000032600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -474,4 +459,4 @@ Window Geometry:
     collapsed: true
   Width: 1600
   X: 0
-  Y: 24
+  Y: 204