从零开始，搭建一个autonomous mobile robot的gazebo仿真（2）：gazebo中自动导航

首先需要一个lidar来感知周围的环境，如何添加lidar参见博主前面的文章

ROS gazebo 机器人仿真，环境与robot建模，添加相机 lidar，控制robot运动-CSDN博客

 在robot的xacro文件中，把lidar集成进去

    <link name="lidar_support">
        <visual>
            <geometry>
                <cylinder length="0.5" radius="0.02"/>
            </geometry>
            <material name="grey">
                <color rgba="0.5 0.5 0.5 1.0"/>
            </material>
        </visual>
 
        <collision>
            <geometry>
                <cylinder length="0.5" radius="0.02"/>
            </geometry>
        </collision>
    </link>

    <joint name="lidar_suppert2base_floor_joint" type="fixed">
        <parent link="base_floor" />
        <child link="lidar_support" />
        <origin xyz="0 0 0.25" rpy="0 0 0" />
    </joint>

    <xacro:include filename="$(find velodyne_description)/urdf/VLP-16.urdf.xacro"/>
    <xacro:VLP-16 parent="lidar_support" name="velodyne" topic="/velodyne_points" hz="10" samples="440" gpu="false">
		<origin xyz="0 0 0.25" rpy="0 0 0" />
	</xacro:VLP-16>

然后在clearpath这家公司的jackal robot的github的repository中，把他们的world文件down下来，这里面有一些障碍物

launch文件改写为这样

<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="world_name" value="$(find ros_whill)/worlds/whill_world.world" />
    <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 '$(find ros_whill)/xacro/modelc.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 whill_modelc -param robot_description -x 5"/> 

  <!-- 运行joint_state_publisher节点，发布机器人的关节状态  -->
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
  
  <!-- 运行robot_state_publisher节点，发布tf  -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
    <remap from="/joint_states" to="/whill_modelc/joint_states" />
  </node>

 
</launch>

然后再新建一个功能包，这个功能包中，用ros的move_base来实现mobile的自动navigation。

navigation功能包的launch文件这样写：

<launch>
    <node pkg="move_base" type="move_base" respawn="false" name="moev_base" output="screen">
        <rosparam file="$(find whill_navigation)/params/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find whill_navigation)/params/costmap_common_params.yaml" command="load" ns="local_costmap" />

        <rosparam file="$(find whill_navigation)/params/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find whill_navigation)/params/local_costmap_params.yaml" command="load" />

        <rosparam file="$(find whill_navigation)/params/base_local_planner_params.yaml" command="load" />
        <rosparam file="$(find whill_navigation)/params/move_base_params.yaml" command="load" />

        <param name="base_global_planner" type="string" value="navfn/NavfnROS" />
        <param name="base_local_planner" value="base_local_planner/TrajectoryPlannerROS"/>

        <remap from="odom" to="odometry/filtered" />

    </node>
</launch>

一些yaml配置文件：

costmap_common_params.yaml :

map_type: costmap
origin_z: 0.0
z_resolution: 1
z_voxels: 2

obstacle_range: 2.5
raytrace_range: 3.0

publish_voxel_map: false
transform_tolerance: 0.5
meter_scoring: true

footprint: [[-0.4, -0.3], [-0.4, 0.3], [0.4, 0.3], [0.4, -0.3]]
footprint_padding: 0.1

plugins:
- {name: obstacles_layer, type: "costmap_2d::ObstacleLayer"}
- {name: inflater_layer, type: "costmap_2d::InflationLayer"}

obstacles_layer:
  observation_sources: scan
  scan: {sensor_frame: velodyne, data_type: PointCloud2, topic: /velodyne_points, marking: true, clearing: true, min_obstacle_height: 0.1, max_obstacle_height: 2.0, obstacle_range: 2.5, raytrace_range: 3.0}


inflater_layer:
 inflation_radius: 0.30

footprint是机器人的轮廓，scan是接收lidar点云，建立local costmap

global_costmap_params.yaml :

global_costmap:
  global_frame: odom
  robot_base_frame: base_link
  update_frequency: 20
  publish_frequency: 5
  width: 40.0
  height: 40.0
  resolution: 0.05
  origin_x: -20.0
  origin_y: -20.0
  static_map: true
  rolling_window: false

local_costmap_params.yaml :

local_costmap:
   global_frame: odom
   robot_base_frame: base_link
   update_frequency: 20.0
   publish_frequency: 5.0
   width: 10.0
   height: 10.0
   resolution: 0.05
   static_map: false
   rolling_window: true

base_local_planner_params.yaml :

TrajectoryPlannerROS:

  # Robot Configuration Parameters
  acc_lim_x: 10.0
  acc_lim_theta:  20.0

  max_vel_x: 0.5
  min_vel_x: 0.1

  max_vel_theta: 1.57
  min_vel_theta: -1.57
  min_in_place_vel_theta: 0.314

  holonomic_robot: false
  escape_vel: -0.5

  # Goal Tolerance Parameters
  yaw_goal_tolerance: 0.157
  xy_goal_tolerance: 0.25
  latch_xy_goal_tolerance: false

  # Forward Simulation Parameters
  sim_time: 2.0
  sim_granularity: 0.02
  angular_sim_granularity: 0.02
  vx_samples: 6
  vtheta_samples: 20
  controller_frequency: 20.0

  # Trajectory scoring parameters
  meter_scoring: true # Whether the gdist_scale and pdist_scale parameters should assume that goal_distance and path_distance are expressed in units of meters or cells. Cells are assumed by default (false).
  occdist_scale:  0.1 #The weighting for how much the controller should attempt to avoid obstacles. default 0.01
  pdist_scale: 0.75  #     The weighting for how much the controller should stay close to the path it was given . default 0.6
  gdist_scale: 1.0 #     The weighting for how much the controller should attempt to reach its local goal, also controls speed  default 0.8

  heading_lookahead: 0.325  #How far to look ahead in meters when scoring different in-place-rotation trajectories
  heading_scoring: false  #Whether to score based on the robot's heading to the path or its distance from the path. default false
  heading_scoring_timestep: 0.8   #How far to look ahead in time in seconds along the simulated trajectory when using heading scoring (double, default: 0.8)
  dwa: true #Whether to use the Dynamic Window Approach (DWA)_ or whether to use Trajectory Rollout
  simple_attractor: false
  publish_cost_grid_pc: true

  #Oscillation Prevention Parameters
  oscillation_reset_dist: 0.05 #How far the robot must travel in meters before oscillation flags are reset (double, default: 0.05)
  escape_reset_dist: 0.1
  escape_reset_theta: 0.1

move_base_params.yaml :

shutdown_costmaps: false

controller_frequency: 20.0
controller_patience: 15.0

planner_frequency: 20.0
planner_patience: 5.0

oscillation_timeout: 0.0
oscillation_distance: 0.5

recovery_behavior_enabled: true
clearing_rotation_allowed: true

分别运行gazebo仿真launch文件，navigation的launch文件，再运行rviz，在rviz中指定2D navi goal，就可以让mobile robot autonomous navigation了。