代码改变世界

ros 雷达 slam 导航

2022-04-05 15:44  jym蒟蒻  阅读(592)  评论(0编辑  收藏  举报

ros 雷达 slam 导航 文件分析

    • robot_slam_laser.launch
      • robot_lidar.launch
      • lidar.launch
      • raplidar.launch
      • karto.launch
      • gmapping.launch
      • cartographer.launch
    • robot_navigation.launch
      • map.yaml
      • map.pgm
      • amcl_params.yaml
      • move_base.launch
      • costmap_common_params.yaml
      • local_costmap_params.yaml
      • global_costmap_params.yaml
      • move_base_params.yaml:
      • dwa_local_planner_params.yaml
      • teb_local_planner_params.yaml
    • 多点导航
      • multi_points_navigation.launch
      • multi_navigation.launch
      • multi_goal_point.py:
    • 多点全自动巡航
      • way_point.launch
      • multi_goal_point.py

 

lidar文件夹下面有多个雷达功能包。

还包括一个udev规则转换脚本。雷达使用cp2102usb转串口芯片。

多个设备通过usb连主机,很难分辨各个端口名所对应的设备。

给USB 转串口的芯片设定规则,根据芯片的 Vender ID 和 Product ID 进行筛选。读取到芯片后做一个符号链接,链接到对应端口。取名为rplidar等。

initenv.sh
#!/bin/bash
echo  'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0777", SYMLINK+="hldslidar"' >/etc/udev/rules.d/hldslidar.rules

echo  'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0777", GROUP:="dialout",  SYMLINK+="ydlidar"' >/etc/udev/rules.d/ydlidar.rules

echo  'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0777", GROUP:="dialout",  SYMLINK+="iiilidar"' >/etc/udev/rules.d/iiilidar.rules

echo  'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0777", GROUP:="dialout",  SYMLINK+="rplidar"' >/etc/udev/rules.d/rplidar.rules

echo  'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0777", GROUP:="dialout",  SYMLINK+="sclidar"' >/etc/udev/rules.d/sclidar.rules

systemctl daemon-reload
service udev reload
sleep 1
service udev restart

robot_navigation这个文件夹很重要。

robot:
roslaunch robot_navigation lidar.launch  启动雷达
PC:
roslaunch robot_navigation lidar_rviz.launch  运行rviz工具

robot:
roslaunch robot_navigation robot_slam_laser.launch  激光雷达slam建图
pc:
roslaunch robot_navigation slam_rviz.launch

robot:保存地图
roscd robot_navigation/maps
rosrun map_server map_saver -f map

robot:
roslaunch robot_navigation robot_navigation.launch   导航和避障
pc:
roslaunch robot_navigation navigation_rviz.launch  

robot:
roslaunch robot_navigation robot_slam_laser.launch slam_methods:=hector 建图算法切换
pc:
roslaunch robot_navigation navigation_rviz.launch

robot:
roslaunch robot_navigation robot_navigation.launch planner:=dwa 路径规划算法切换

robot:
roslaunch robot_navigation robot_slam_laser.launch planner:=teb 导航建图
pc:
roslaunch robot_navigation slam_rviz.launch

文件里面包含:

rviz文件夹:rviz相关配置文件
maps文件夹:地图文件和地图配置信息相关的文件
param文件夹:yaml文件
launch文件夹
script文件夹:脚本文件

launch文件夹里面:

includes文件夹:建图算法的launch文件
lidar文件夹:雷达的launch文件
rviz文件夹:rviz的launch文件

robot_slam_laser.launch

robot_slam_laser.launch:

这里面主要就是launch文件的循环调用,比如,通过robot_slam_laser.launch文件调用了robot_lidar.launch文件。

这样做的好处是具有模块化。可以避免重复启动多个节点。

<launch>
  <!-- Arguments -->
  
  <arg name="slam_methods" default="gmapping" doc="slam type [gmapping, hector, karto, cartographer]"/><!-- 可传入参数slam_methods,选择SLAM建图算法 -->
  <arg name="open_rviz" default="false"/>
  <arg name="simulation" default= "false"/> <!-- 判断启动实体机器人还是 stage 仿真器 -->
  <arg name="planner"  default="" doc="opt: dwa, teb"/> <!-- 无地图环境下边建图边导航,可选参数为dwa、teb局部路径规划器 -->

  <param name="/use_sim_time" value="$(arg simulation)" />  

  <!-- simulation robot with lidar and map -->
  <!-- simulation为true 时启动仿真软件运行仿真机器人-->
  <group if="$(arg simulation)">
    <include file="$(find robot_simulation)/launch/simulation_one_robot.launch"/>
  </group>

  <!-- robot with lidar -->
  <!--simulation为false 启动launch文件夹下的robot_lidar.launch-->
  <group unless="$(arg simulation)">
    <include file="$(find robot_navigation)/launch/robot_lidar.launch"/>
  </group>
  <!-- SLAM: Gmapping, Cartographer, Hector, Karto -->
  <!-- 根据 slam_methods 参数选用不同的建图算法 -->
  <include file="$(find robot_navigation)/launch/includes/$(arg slam_methods).launch">
    <arg name="simulation"            value="$(arg simulation)"/>
  </include>
  <!-- move_base 无地图环境下导航建图功能,传入planner参数为dwa或teb,就会启动导航堆栈-->
  <group unless="$(eval planner == '')"><!--启动move_base.launch-->
    <include file="$(find robot_navigation)/launch/move_base.launch" unless="$(eval planner == '')">
        <arg name="planner"            value="$(arg planner)"/>
    </include>
  </group>
    
  <!--因为gmapping会发布map话题,这里不用启动地图服务器从本地读取地图也可以正常导航-->

  <!-- rviz open_rviz 参数为 true时,打开rviz-->
  <group if="$(arg open_rviz)"> 
    <node pkg="rviz" type="rviz" name="rviz" required="true"
          args="-d $(find robot_navigation)/rviz/slam.rviz"/><!-- 使用rviz文件夹中预置的配置文件slam.rviz -->
  </group>
  
</launch>

robot_lidar.launch

robot_lidar.launch:启动底盘和雷达这两个硬件

<launch>
    <!-- config param -->
    <arg name="pub_imu"       default="False" />
    <arg name="sub_ackermann"       default="False" />
    <arg name="lidar_frame" default="base_laser_link"/>  

    <!-- include 标签包含了两个 launch -->
    
    <include file="$(find base_control)/launch/base_control.launch">
        <arg name="pub_imu"            value="$(arg pub_imu)"/>  
        <arg name="sub_ackermann"            value="$(arg sub_ackermann)"/>  
    </include>
	
    <include file="$(find robot_navigation)/launch/lidar.launch">
        <arg name="lidar_frame"            value="$(arg lidar_frame)"/>  
    </include>
    
</launch>

lidar.launch

lidar.launch:这里面启动了雷达的launch文件

<launch>
    <!--robot bast type use different tf value-->
    <!-- 读取机器人底盘类型 -->
    <arg name="base_type"       default="$(env BASE_TYPE)" />
    <!-- robot frame -->
    
    <arg name="base_frame"       default="/base_footprint" />    
    <!-- 读取机器人雷达类型 -->
    <arg name="lidar_type"       default="$(env LIDAR_TYPE)" />   
    <arg name="lidar_frame" default="base_laser_link"/>  

    <!-- 根据lidar_type类型启动节点,如果是rplidar,就启动rplidar.launch -->
    <include file="$(find robot_navigation)/launch/lidar/$(arg lidar_type).launch">
        <arg name="lidar_frame"            value="$(arg lidar_frame)"/>
    </include>

    <!-- TF 坐标转换 -->
    <group if="$(eval base_type == 'NanoRobot')">
        <node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"
            args="-0.01225 0.0 0.18 3.14159265 0.0 0.0 $(arg base_frame) $(arg lidar_frame) 20">
        </node>
    </group>
    <group if="$(eval base_type == 'NanoRobot_Pro')">
        <node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"
            args="-0.0515 0.0 0.18 -1.5708 0.0 0.0 $(arg base_frame) $(arg lidar_frame) 20">
        </node>
    </group>
    <group if="$(eval base_type == '4WD')">
        <node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"
            args="0.01 0.0 0.25 3.14159265 0.0 0.0 $(arg base_frame) $(arg lidar_frame) 20">
        </node>
    </group>
    <group if="$(eval base_type == '4WD_OMNI')">
        <node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"
            args="0.01 0.0 0.25 3.14159265 0.0 0.0 $(arg base_frame) $(arg lidar_frame) 20">
        </node>
    </group>
    <group if="$(eval base_type == 'NanoCar')">
        <node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"
            args="0.1037 0.0 0.115 3.14159265 0.0 0.0 $(arg base_frame) $(arg lidar_frame) 20">
        </node>
    </group>
    <group if="$(eval base_type == 'NanoCar_Pro')">
        <node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"
            args="0.1037 0.0 0.165 3.14159265 0.0 0.0 $(arg base_frame) $(arg lidar_frame) 20">
        </node>
    </group>
    <group if="$(eval base_type == 'NanoCar_SE')">
        <node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"
            args="0.0955 0.0 0.115 1.5708 0.0 0.0 $(arg base_frame) $(arg lidar_frame) 20">
        </node>
    </group>

</launch>

raplidar.launch

raplidar.launch 雷达数据

端口名称使用了udev规则的名称。

<param name="serial_port"         type="string" value="/dev/rplidar"/>
<launch>
    <arg name="lidar_frame" default="lidar"/>   

    <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
        <param name="serial_port"         type="string" value="/dev/rplidar"/>
        <param name="serial_baudrate"     type="int"    value="115200"/><!--A1/A2 -->
        <!--param name="serial_baudrate"     type="int"    value="256000"--><!--A3 -->
        <param name="frame_id"            type="string" value="$(arg lidar_frame)"/>
        <param name="inverted"            type="bool"   value="false"/>
        <param name="angle_compensate"    type="bool"   value="true"/>
    </node>
</launch>

karto.launch

karto.launch文件,配置参数是通过yaml文件载入的。

<launch>

  <arg name="simulation" default= "false"/> 

  <param name="/use_sim_time" value="$(arg simulation)" />  

  <!-- slam_karto -->

  <node pkg="slam_karto" type="slam_karto" name="slam_karto" output="screen">

    <rosparam command="load" file="$(find robot_navigation)/config/karto_params.yaml" />

  </node>

</launch>

karto_params.yaml

# General Parameters

base_frame: base_footprint

map_update_interval: 5

use_scan_matching: true

use_scan_barycenter: true

minimum_travel_distance: 0.12 

minimum_travel_heading: 0.174             #in radians

scan_buffer_size: 70

scan_buffer_maximum_scan_distance: 3.5

link_match_minimum_response_fine: 0.12

link_scan_maximum_distance: 3.5

loop_search_maximum_distance: 3.5

do_loop_closing: true

loop_match_minimum_chain_size: 10

loop_match_maximum_variance_coarse: 0.4   # gets squared later

loop_match_minimum_response_coarse: 0.8

loop_match_minimum_response_fine: 0.8



# Correlation Parameters - Correlation Parameters

correlation_search_space_dimension: 0.3

correlation_search_space_resolution: 0.01

correlation_search_space_smear_deviation: 0.03



# Correlation Parameters - Loop Closure Parameters

loop_search_space_dimension: 8.0

loop_search_space_resolution: 0.05

loop_search_space_smear_deviation: 0.03



# Scan Matcher Parameters

distance_variance_penalty: 0.3      # gets squared later

angle_variance_penalty: 0.349       # in degrees (gets converted to radians then squared)

fine_search_angle_offset: 0.00349   # in degrees (gets converted to radians)

coarse_search_angle_offset: 0.349   # in degrees (gets converted to radians)

coarse_angle_resolution: 0.0349     # in degrees (gets converted to radians)

minimum_angle_penalty: 0.9

minimum_distance_penalty: 0.5

use_response_expansion: false

与上面对比的话,就是通过launch文件直接传参数。他们两个效果其实是一样的。

gmapping.launch

<launch>
  <arg name="set_base_frame" default="base_footprint"/>
  <arg name="set_odom_frame" default="odom"/>
  <arg name="set_map_frame"  default="map"/>
  <arg name="scan_topic" default="/scan" />
  <arg name="simulation" default= "false"/> 
  <param name="/use_sim_time" value="$(arg simulation)" />  
  <!-- Gmapping -->
  <node pkg="gmapping" type="slam_gmapping" name="gmapping" output="screen">
    <param name="base_frame" value="$(arg set_base_frame)"/>
    <param name="odom_frame" value="$(arg set_odom_frame)"/>
    <param name="map_frame"  value="$(arg set_map_frame)"/>
    <param name="map_update_interval" value="2.0"/>
    <param name="maxUrange" value="5.0"/>
    <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.2"/>
    <param name="temporalUpdate" value="0.5"/>
    <param name="resampleThreshold" value="0.5"/>
    <param name="particles" value="100"/>
    <param name="xmin" value="-10.0"/>
    <param name="ymin" value="-10.0"/>
    <param name="xmax" value="10.0"/>
    <param name="ymax" value="10.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"/>
    <remap from="scan" to="$(arg scan_topic)"/>
  </node>
</launch>

cartographer.launch

它里面使用了lua脚本语言载入配置文件。

-configuration_basename revo_lds_$(arg version).lua" 

revo_lds_melodic.lua

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
-- tracking_frame = "base_laser_link",
-- published_frame = "base_laser_link",
-- odom_frame = "odom",
-- provide_odom_frame = true,
-- use_odometry = false,

  tracking_frame = "base_footprint",
  published_frame = "odom",
  odom_frame = "odom",
  provide_odom_frame = false,
  publish_frame_projected_to_2d = false,
  use_odometry = true,
  use_nav_sat = false,
  use_landmarks = false,
  num_laser_scans = 1,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 0,
  lookup_transform_timeout_sec = 0.2,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 5e-3,
  trajectory_publish_period_sec = 30e-3,
  rangefinder_sampling_ratio = 1.,
  odometry_sampling_ratio = 1.,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
  landmarks_sampling_ratio = 1.,
}

MAP_BUILDER.use_trajectory_builder_2d = true

TRAJECTORY_BUILDER_2D.submaps.num_range_data = 35
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 8.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10.
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1

POSE_GRAPH.optimization_problem.huber_scale = 1e2
POSE_GRAPH.optimize_every_n_nodes = 35
POSE_GRAPH.constraint_builder.min_score = 0.65

return options

cartographer.launch

<launch>  
  <arg name="simulation" default= "true"/> 
  <arg name="version" default="$(env ROS_DISTRO)"/><!-- 读取环境变量ROS_DISTRO的版本 -->
  <param name="/use_sim_time" value="$(arg simulation)" />  
  <!-- cartographer_node -->
  <node name="cartographer_node" pkg="cartographer_ros"  
        type="cartographer_node" args="  
            -configuration_directory $(find robot_navigation)/config  
            -configuration_basename revo_lds_$(arg version).lua"  
        output="screen">  <!--根据version的参数选择配置文件-->
  </node>  
  <group if="$(eval version == 'melodic')"><!--对于melodic版本,手动启动下面这个节点-->
      <node pkg="cartographer_ros" type="cartographer_occupancy_grid_node" name="cartographer_occupancy_grid_node"/>
  </group>
</launch>

robot_navigation.launch

两个group,每个都,实现了机器人、雷达、地图、amcl四个节点的启动。

<launch>
  <!-- Arguments -->
    <!--传入地图文件所在的路径-->
  <arg name="map_file" default="$(find robot_navigation)/maps/map.yaml"/><!--参数map_file传入maps文件下的map.yaml文件 -->
  <!--如果地图名称和保存路径是自定义的,只需修改传入的map_file的值-->
  <arg name="simulation" default= "false"/> 
  <arg name="planner"  default="teb" doc="opt: dwa, teb"/> 
  <arg name="open_rviz" default="false"/>
  <arg name="use_dijkstra" default= "true"/> 

  <group if="$(arg simulation)">
    <!-- simulation robot with lidar and map-->
    <!-- simulation 参数为 true,在那个launch文件中启动地图服务 -->
    <include file="$(find robot_simulation)/launch/simulation_one_robot_with_map.launch"/>
  </group>

  <group unless="$(arg simulation)">
    <!-- robot with lidar 启动底盘和雷达-->
    <include file="$(find robot_navigation)/launch/robot_lidar.launch">
          <!--<arg name="robot_name"            value="$(arg robot_name)"/>-->
    </include>
    <!-- Map server 启动地图服务器读入map_file参数获取地图文件-->
    <node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)">
      <param name="frame_id" value="map"/>
    </node>
    <!-- AMCL,启动一个定位节点 amcl -->
    <node pkg="amcl" type="amcl" name="amcl" output="screen">
      <!-- 通过yaml文件读取配置文件-->
      <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/amcl_params.yaml" command="load" />
      <!-- 通过下面的设置,机器人启动时候默认就是位于地图零点 -->
        <!-- 如果机器人从一个固定位置启动且不是原点,可以修改参数 -->
      <param name="initial_pose_x"            value="0.0"/>
      <param name="initial_pose_y"            value="0.0"/>
      <param name="initial_pose_a"            value="0.0"/><!--航向-->
    </node>
  </group>

  <!-- move_base 导航堆栈-->
  <include file="$(find robot_navigation)/launch/move_base.launch" >
    <!--传入的参数-->
    <arg name="planner"            value="$(arg planner)"/>
    <arg name="simulation"            value="$(arg simulation)"/>
    <arg name="use_dijkstra"     value="$(arg use_dijkstra)"/>

  </include>

  <!-- rviz -->
  <group if="$(arg open_rviz)"> 
    <node pkg="rviz" type="rviz" name="rviz" required="true"
          args="-d $(find robot_navigation)/rviz/navigation.rviz"/>
  </group>

</launch>

map.yaml

包括了地图的一些基本配置信息。

image: map.pgm
resolution: 0.050000
origin: [-10.000000, -10.000000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

里面的参数含义:可以在维基里面找对应的功能包,有英文介绍里面的参数。

image:地图名称。
resolution:地图分辨率,米/像素
origin:地图中左下角像素的二维姿态,为x,y,yaw。偏航为逆时针旋转,偏航=0表示无旋转
occupied_thresh:大于此阈值的像素,完全占用。
free_thresh:小于此阈值的像素,完全空闲。
negate:
像素的COLOR值x在[0,255]范围内。
首先,将整数x转换为一个浮点数p,转换公式具体取决于yaml中negate标志的定义。
如果negate=0,则p =(255 - x)/ 255.0。这意味着黑色(0)现在具有最高值(1.0)而白色(255)具有最低值(0.0)。
如果negate=1,则p = x / 255.0。

map.pgm

pgm是一个图像的格式。

在这里插入图片描述

amcl_params.yaml

可以通过amcl的ros wiki介绍页面查看参数信息。

use_map_topic: true



odom_frame_id: "odom"

base_frame_id: "base_footprint"

global_frame_id: "map"



## Publish scans from best pose at a max of 10 Hz

odom_model_type: "diff"

odom_alpha5: 0.1

gui_publish_rate: 10.0

laser_max_beams: 60

laser_max_range: 12.0

min_particles: 500

max_particles: 2000

kld_err: 0.05

kld_z: 0.99

odom_alpha1: 0.2

odom_alpha2: 0.2

## translation std dev, m 

odom_alpha3: 0.2

odom_alpha4: 0.2

laser_z_hit: 0.5

aser_z_short: 0.05

laser_z_max: 0.05

laser_z_rand: 0.5

laser_sigma_hit: 0.2

laser_lambda_short: 0.1

laser_model_type: "likelihood_field" # "likelihood_field" or "beam"

laser_likelihood_max_dist: 2.0

update_min_d: 0.25

update_min_a: 0.2



resample_interval: 1



## Increase tolerance because the computer can get quite busy 

transform_tolerance: 1.0

recovery_alpha_slow: 0.001

recovery_alpha_fast: 0.1


move_base.launch

move_base.launch:ros下的导航堆栈

默认传入的 planner 参数是 dwa,但是robot_navigation.launch调用move_base.launch时,传入的参数是teb。所以这里planner参数置为teb。

<launch>
  <!-- Arguments -->
  <arg name="cmd_vel_topic" default="cmd_vel" />
  <arg name="odom_topic" default="odom" />
  <arg name="planner"  default="dwa" doc="opt: dwa, teb"/> <!--默认传入的 planner 参数是 dwa-->
  <arg name="simulation" default= "false"/> 
  <arg name="use_dijkstra" default= "true"/>  
  

  <arg name="base_type" default= "$(env BASE_TYPE)"/> <!--不同车型不同配置-->
  <arg name="move_forward_only" default="false"/><!--使用深度相机导航和建图,只有前向视野没有后向视野-->
    <!--禁止车的后向运行保证运行安全-->

  <!-- 使用了两个 group 区分DWA和TEB两个不同的局部路径规划算法 -->
  <!-- move_base use DWA planner-->
  <group if="$(eval planner == 'dwa')">
    <!-- 启动move_base节点 -->
    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
      <!-- use global_planner replace default navfn as global planner ,global_planner support A* and dijkstra algorithm-->
      <!--设置传入节点的参数-->
      <param name="base_global_planner" value="global_planner/GlobalPlanner"/>
      <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
      <!-- <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/global_planner_params.yaml" command="load" />       -->
      <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/costmap_common_params.yaml" command="load" ns="global_costmap" />
      <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/costmap_common_params.yaml" command="load" ns="local_costmap" />
      <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/local_costmap_params.yaml" command="load" />
      <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/global_costmap_params.yaml" command="load" />
      <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/move_base_params.yaml" command="load" />
      <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/dwa_local_planner_params.yaml" command="load" />
      <remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
      <remap from="odom" to="$(arg odom_topic)"/>
        <!-- move_forward_only时,不让有负速度(后向运动) -->
      <param name="DWAPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" />
      <!--default is True,use dijkstra algorithm;set to False,usd A* algorithm-->
      <param name="GlobalPlanner/use_dijkstra " value="$(arg use_dijkstra)" />    
    </node>
  </group>
  <!-- move_base use TEB planner-->
  <group if="$(eval planner == 'teb')">
    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
      <!-- use global_planner replace default navfn as global planner ,global_planner support A* and dijkstra algorithm-->
      <param name="base_global_planner" value="global_planner/GlobalPlanner"/>
      <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
      <!-- <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/global_planner_params.yaml" command="load" /> -->
      <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/costmap_common_params.yaml" command="load" ns="global_costmap" />
      <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/costmap_common_params.yaml" command="load" ns="local_costmap" />
      <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/local_costmap_params.yaml" command="load" />
      <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/global_costmap_params.yaml" command="load" />
      <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/move_base_params.yaml" command="load" />
      <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/teb_local_planner_params.yaml" command="load" />
      <remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
      <remap from="odom" to="$(arg odom_topic)"/>
      <param name="TebLocalPlannerROS/max_vel_x_backwards" value="0.0" if="$(arg move_forward_only)" />
      <!--default is True,use dijkstra algorithm;set to False,usd A* algorithm-->
      <param name="GlobalPlanner/use_dijkstra " value="$(arg use_dijkstra)" />
      <!--stage simulator takes the angle instead of the rotvel as input (twist message)-->

      
    </node>
  </group>
    <!-- move_base -->
</launch>

costmap_common_params.yaml

其中一个车型的costmap_common_params.yaml文件如下。

#---(in meters)---
#单位是米

footprint: [ [-0.035,-0.1], [0.18,-0.1], [0.18,0.1], [-0.035,0.1] ]
#机器人的矩形轮廓尺寸
#以机器人中心为坐标轴原点,这四个坐标点(x,y)分别对应机器人的四个角
#路径规划器获取到机器人轮廓大小从而规划相应的路径

#参数含义通过move_base的ros wiki查看
transform_tolerance: 0.2

obstacle_layer:

 enabled: true

 obstacle_range: 2.5

 raytrace_range: 3.0

 inflation_radius: 0.05

 track_unknown_space: false

 combination_method: 1

 observation_sources: laser_scan_sensor

 laser_scan_sensor: {data_type: LaserScan, topic: scan, marking: true, clearing: true}

inflation_layer:

  enabled:              true

  cost_scaling_factor:  10.0  # exponential rate at which the obstacle cost drops off (default: 10)

  inflation_radius:     0.1  # max. distance from an obstacle at which costs are incurred for planning paths.



static_layer:

  enabled:              true

  map_topic:            "/map"

local_costmap_params.yaml

机器人避障功能通过local costmap实现。全局地图不存在障碍,通过局部地图发现障碍然后重新规划路线实现。

#指定发布频率和更新频率
local_costmap:

  global_frame: map

  robot_base_frame: base_footprint

  update_frequency: 5.0

  publish_frequency: 5.0

  rolling_window: true#滑动窗口
  #以机器人所在位置为中心的 3*3 滑动窗口
  #这个框设置得越小路径规划效果越差,越大机器人负担的运算就越大
  width: 3
  height: 3

  resolution: 0.05

  transform_tolerance: 0.5

  plugins:#设置了一个地图的层
  #静态层
   - {name: static_layer,        type: "costmap_2d::StaticLayer"}
  #障碍物层,地图中不存在而实际扫描到的障碍物,会被识别在障碍物层。
   - {name: obstacle_layer,      type: "costmap_2d::ObstacleLayer"}

global_costmap_params.yaml

#设置一些发布频率等信息
global_costmap:

  global_frame: map

  robot_base_frame: base_footprint

  update_frequency: 1.0

  publish_frequency: 0.5

  # static_map: true 

  transform_tolerance: 0.5

  plugins:

    - {name: static_layer,            type: "costmap_2d::StaticLayer"}

    - {name: obstacle_layer,          type: "costmap_2d::VoxelLayer"}

    - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}

move_base_params.yaml:

#进行一些频率设置

#对底盘的控制频率
controller_frequency: 2.0
controller_patience: 15.0

#路径规划频率
planner_frequency: 0.2
planner_patience: 5.0

conservative_reset_dist: 3.0

#机器人在一个点持续震荡时最大允许震荡时间
oscillation_timeout: 10.0
oscillation_distance: 0.2

clearing_rotation_allowed: false 
    
#clearing_rotation_allowed为True时,机器人被困住,会原地 360 度转一圈重新扫描找到一条出路(激光雷达不是360才需要用)
#阿克曼结构,不具备原地转向的能力,需要禁用

dwa_local_planner_params.yaml

DWAPlannerROS:

# Robot Configuration Parameters
#设置机器人导航的最大最小速度
#x方向最大速度的绝对值,单位m/s
  max_vel_x: 0.25
#x方向最小值绝对值,如果为负值表示可以后退
  min_vel_x: -0.25
#y轴速度,麦克纳姆轮才用
  max_vel_y: 0.0

  min_vel_y: 0.0


# The velocity when robot is moving in a straight line
#最大角速度,单位rad/s
  max_vel_trans:  0.26

  min_vel_trans:  0.13


  max_vel_theta: 1.0

  min_vel_theta: 0.5

#x方向的加速度绝对值,m/s2
  acc_lim_x: 2.5
#y方向的加速度绝对值,该值只有全向移动的机器人才需配置.
  acc_lim_y: 0.0
#旋转加速度的绝对值.rad/s2
  acc_lim_theta: 3.2 


# Goal Tolerance Parametes
#允许机器人所到目标的坐标(以米为单位)偏差,过小可能导致机器人在目标位置附近不断调整到精确的目标位置
  xy_goal_tolerance: 0.1
#角度的容忍
  yaw_goal_tolerance: 0.2

  latch_xy_goal_tolerance: false


# Forward Simulation Parameters

  sim_time: 2.0

  vx_samples: 20

  vy_samples: 0

  vtheta_samples: 40

  controller_frequency: 10.0


# Trajectory Scoring Parameters

  path_distance_bias: 32.0

  goal_distance_bias: 20.0

  occdist_scale: 0.02

  forward_point_distance: 0.325

  stop_time_buffer: 0.2

  scaling_speed: 0.25

  max_scaling_factor: 0.2


# Oscillation Prevention Parameters

  oscillation_reset_dist: 0.05


# Debugging

  publish_traj_pc : true

  publish_cost_grid_pc: true
    

  prune_plan: false

  use_dwa: true

  restore_defaults: false

teb_local_planner_params.yaml

TebLocalPlannerROS:

 odom_topic: odom

 # Trajectory

 teb_autosize: True

 dt_ref: 0.3

 dt_hysteresis: 0.1

 max_samples: 500

 global_plan_overwrite_orientation: True

 allow_init_with_backwards_motion: True

 max_global_plan_lookahead_dist: 3.0

 global_plan_viapoint_sep: -1

 global_plan_prune_distance: 1

 exact_arc_length: False

 feasibility_check_no_poses: 2

 publish_feedback: False

 # Robot       
#最大x向前速度
 max_vel_x: 0.2
#最大x后退速度
 max_vel_x_backwards: 0.2

 max_vel_y: 0.0
#最大转向角速度
 max_vel_theta: 0.53 # the angular velocity is also bounded by min_turning_radius in case of a carlike robot (r = v / omega)
#最大x加速度
 acc_lim_x: 0.5
#最大角速度
 acc_lim_theta: 0.5

 # ********************** Carlike robot parameters ********************
#设置最小转向半径的参数
 min_turning_radius: 0.375        # Min turning radius of the carlike robot (compute value using a model or adjust with rqt_reconfigure manually)
#轮距参数
 wheelbase: 0.145                 # Wheelbase of our robot

 #将收到的角度消息转换为操作上的角度变化
 cmd_angle_instead_rotvel: False  # stage simulator takes the angle instead of the rotvel as input (twist message)
#使用仿真器仿真阿克曼车型,将cmd_angle_instead_rotvel改为true,
#stage 仿真器中”Car”车型接受的 cmd_vel 话题里面的z轴角速度并非给定机器人角速度,而是指转向结构的转向角度。
#cmd_angle_instead_rotvel: False,角速度就是定机器人真实的角速度
 # ********************************************************************

 footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"

   type: "line"

   line_start: [0.05, 0.0] # for type "line"

   line_end: [0.10, 0.0] # for type "line"

 # GoalTolerance

 xy_goal_tolerance: 0.1

 yaw_goal_tolerance: 0.2

 free_goal_vel: False

 complete_global_plan: True

 # Obstacles

 min_obstacle_dist: 0.12 # This value must also include our robot's expansion, since footprint_model is set to "line".

 inflation_dist: 0.6

 include_costmap_obstacles: True

 costmap_obstacles_behind_robot_dist: 1.5

 obstacle_poses_affected: 15
    

 dynamic_obstacle_inflation_dist: 0.6

 include_dynamic_obstacles: True 


 costmap_converter_plugin: ""

 costmap_converter_spin_thread: True

 costmap_converter_rate: 5


 # Optimization

 no_inner_iterations: 5

 no_outer_iterations: 4

 optimization_activate: True

 optimization_verbose: False

 penalty_epsilon: 0.1

 obstacle_cost_exponent: 4

 weight_max_vel_x: 2

 weight_max_vel_theta: 1

 weight_acc_lim_x: 1

 weight_acc_lim_theta: 1

 weight_kinematics_nh: 1000

 weight_kinematics_forward_drive: 1

 weight_kinematics_turning_radius: 1

 weight_optimaltime: 1 # must be > 0

 weight_shortest_path: 0

 weight_obstacle: 100

 weight_inflation: 0.2

 weight_dynamic_obstacle: 10 # not in use yet

 weight_dynamic_obstacle_inflation: 0.2

 weight_viapoint: 1

 weight_adapt_factor: 2


 # Homotopy Class Planner


 enable_homotopy_class_planning: False

 enable_multithreading: True

 max_number_classes: 4

 selection_cost_hysteresis: 1.0

 selection_prefer_initial_plan: 0.95

 selection_obst_cost_scale: 1.0

 selection_alternative_time_cost: False


 roadmap_graph_no_samples: 15

 roadmap_graph_area_width: 5

 roadmap_graph_area_length_scale: 1.0

 h_signature_prescaler: 0.5

 h_signature_threshold: 0.1

 obstacle_heading_threshold: 0.45

 switching_blocking_period: 0.0

 viapoints_all_candidates: True

 delete_detours_backwards: True

 max_ratio_detours_duration_best_duration: 3.0

 visualize_hc_graph: False

 visualize_with_time_as_z_axis_scale: False


# Recovery


 shrink_horizon_backup: True

 shrink_horizon_min_duration: 10

 oscillation_recovery: False

 oscillation_v_eps: 0.1

 oscillation_omega_eps: 0.1

 oscillation_recovery_min_duration: 10

 oscillation_filter_duration: 10


多点导航

多点导航:
普通的导航只能发布一个目标点。
在抵达目标点前,如果发布了新的目标点,则会舍弃原本的目标点直接前往新的目标点。
多目标点导航则是按照目标点发布顺序依次前往。

stage 仿真:roslaunch robot_navigation robot_navigation.launch simulation:=true
实体机器人上运行则无需传入 simulation:=true

启动多点导航工具:roslaunch robot_navigation multi_points_navigation.launch
启动rviz:roslaunch robot_navigation multi_navigation.launch

rviz多出了一个2D Nav Goal按钮,可以连续给机器人指定多个目标点,机器人会依次去往各个目标点

multi_points_navigation.launch

multi_points_navigation.launch,多点导航

<launch>

    <!--<node name="robot_pose_publisher" pkg="robot_pose_publisher" type="robot_pose_publisher" />-->
    <!--启动了一个 multi_goal_point.py 的节点-->
    <node name="multi_mark" pkg="robot_navigation" type="multi_goal_point.py" output="screen" />

</launch>

multi_navigation.launch

<launch>
  <!-- rviz -->
    <node pkg="rviz" type="rviz" name="rviz" required="true"
          args="-d $(find robot_navigation)/rviz/multi_navigation.rviz"/>
</launch>

multi_goal_point.py:

核心思路:

通过rviz按钮,发布goal话题,而不是发布/move_base_simple/goal话题,因为/move_base_simple/goal话题会直接发送给movebase 直接产生一个新的导航目标。

通过markerArray把goal缓存起来,通过move_base对每一个点导航的结果,触发status callback,执行下一个目标点。

循环往复。

#!/usr/bin/python


from visualization_msgs.msg import Marker
from visualization_msgs.msg import MarkerArray
import rospy
import math
from geometry_msgs.msg import PointStamped,PoseStamped
import actionlib
from move_base_msgs.msg import *
import tf

def status_callback(msg):#收到move base导航结果时,判断是读取下一个点继续发布;或是当前已是最后一个点做标记标识

    global goal_pub, index,markerArray
    global add_more_point,try_again

    if(msg.status.status == 3):#状态是3,导航成功到目标点
        try_again = 1
        if add_more_point == 0:
            print 'Goal reached'

        if index < count:

            pose = PoseStamped()
            pose.header.frame_id = "map"
            pose.header.stamp = rospy.Time.now()
            pose.pose.position.x = markerArray.markers[index].pose.position.x#给pose一个新的值
            #从markerArray读取一个新的目标点
            pose.pose.position.y = markerArray.markers[index].pose.position.y
            pose.pose.orientation.w = 1
            goal_pub.publish(pose)#发布给move base

            index += 1
        elif index == count:#执行到 最后一个点
            add_more_point = 1
    else:
    # status值及其含义
    # uint8 PENDING         = 0   # The goal has yet to be processed by the action server
    # uint8 ACTIVE          = 1   # The goal is currently being processed by the action server
    # uint8 PREEMPTED       = 2   # The goal received a cancel request after it started executing
    #                             #   and has since completed its execution (Terminal State)
    # uint8 SUCCEEDED       = 3   # The goal was achieved successfully by the action server (Terminal State)
    # uint8 ABORTED         = 4   # The goal was aborted during execution by the action server due
    #                             #    to some failure (Terminal State)
    # uint8 REJECTED        = 5   # The goal was rejected by the action server without being processed,
    #                             #    because the goal was unattainable or invalid (Terminal State)
    # uint8 PREEMPTING      = 6   # The goal received a cancel request after it started executing
    #                             #    and has not yet completed execution
    # uint8 RECALLING       = 7   # The goal received a cancel request before it started executing,
    #                             #    but the action server has not yet confirmed that the goal is canceled
    # uint8 RECALLED        = 8   # The goal received a cancel request before it started executing
    #                             #    and was successfully cancelled (Terminal State)
    # uint8 LOST            = 9   # An action client can determine that a goal is LOST. This should not be
    #                             #    sent over the wire by an action server
        #如果是失败
        print 'Goal cannot reached has some error :',msg.status.status," try again!!!!"
        if try_again == 1:#把目标点再发给move base,让它重试
            pose = PoseStamped()
            pose.header.frame_id = "map"
            pose.header.stamp = rospy.Time.now()
            pose.pose.position.x = markerArray.markers[index-1].pose.position.x
            pose.pose.position.y = markerArray.markers[index-1].pose.position.y
            pose.pose.orientation.w = 1
            goal_pub.publish(pose)
            try_again = 0
        else:#已经重试过但还是到不了,就跳过它执行下个点
            if index < len(markerArray.markers):
                pose = PoseStamped()
                pose.header.frame_id = "map"
                pose.header.stamp = rospy.Time.now()
                pose.pose.position.x = markerArray.markers[index].pose.position.x
                pose.pose.position.y = markerArray.markers[index].pose.position.y
                pose.pose.orientation.w = 1
                goal_pub.publish(pose)
                index += 1


def click_callback(msg):#rviz给定一个多目标点,会触发
    global markerArray,count
    global goal_pub,index
    global add_more_point

    marker = Marker()
    marker.header.frame_id = "map"
    marker.header.stamp = rospy.Time.now()
    # marker.type = marker.TEXT_VIEW_FACING
    marker.type = marker.CYLINDER
    marker.action = marker.ADD
    marker.scale.x = 0.2
    marker.scale.y = 0.2
    marker.scale.z = 0.5
    marker.color.a = 1.0
    marker.color.r = 0.0
    marker.color.g = 1.0
    marker.color.b = 0.0
    marker.pose.orientation.x = 0.0
    marker.pose.orientation.y = 0.0
    marker.pose.orientation.z = 0.0
    marker.pose.orientation.w = 1.0
    marker.pose.position.x = msg.pose.position.x#读出msg中的位置信息,传到marker消息中
    marker.pose.position.y = msg.pose.position.y
    marker.pose.position.z = msg.pose.position.z

    markerArray.markers.append(marker)

    # Renumber the marker IDs
    id = 0
    for m in markerArray.markers:
       m.id = id
       id += 1

    # Publish the MarkerArray
    mark_pub.publish(markerArray)#发布

    #first goal,第一次发布多目标点的时候,直接把话题发布出去
    if count==0:#没有目标点的时候
        pose = PoseStamped()#把话题读到pose消息
        pose.header.frame_id = "map"
        pose.header.stamp = rospy.Time.now()
        pose.pose.position.x = msg.pose.position.x
        pose.pose.position.y = msg.pose.position.y
        pose.pose.position.z = msg.pose.position.z
        pose.pose.orientation.x = msg.pose.orientation.x
        pose.pose.orientation.y = msg.pose.orientation.y
        pose.pose.orientation.z = msg.pose.orientation.z
        pose.pose.orientation.w = msg.pose.orientation.w
        goal_pub.publish(pose)#通过goal_pub发布
        #goal_pub发布的就是move base需要的goal话题。包含了目标点的位置和角度信息
        index += 1

        #add_more_point=1时,在ros发布一个status=3,触发status_callback,这样能再次发布新的目标点
        #为解决当所有导航点完成后想要再发布新的导航点的问题
    if add_more_point and count > 0:
        add_more_point = 0
        move =MoveBaseActionResult()
        move.status.status = 3
        move.header.stamp = rospy.Time.now()
        goal_status_pub.publish(move)
    quaternion = (msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w)
    theta = tf.transformations.euler_from_quaternion( quaternion)[2]
    count += 1#每点击一次,count加1
    # print 'add a path goal point %f %f %f'%(msg.pose.position.x,msg.pose.position.y,theta*180.0/3.14)


markerArray = MarkerArray()#定义一个变量

count = 0       #total goal num 统计发布的目标点数量
index = 0       #current goal point index 统计当前执行到哪个目标点
add_more_point = 0 # after all goal arrive, if add some more goal 标志位
try_again = 1  # try the fail goal once again 尝试次数

rospy.init_node('multi_goal_point_demo') #创建节点

mark_pub = rospy.Publisher('/path_point_array', MarkerArray,queue_size=100)#发布器发布MarkARRAY类型数据
click_goal_sub = rospy.Subscriber('/goal',PoseStamped,click_callback)#订阅器订阅goal话题,在rviz中发布多目标点所发布的话题,每次接收到goal进入click_callback
goal_pub = rospy.Publisher('/move_base_simple/goal',PoseStamped,queue_size=1)#发布器发布/move_base_simple/goal话题。
#触发move_base进行导航的话题
goal_status_sub = rospy.Subscriber('/move_base/result',MoveBaseActionResult,status_callback)#订阅movebase导航的结果。
#after all goal arrive, if add some more goal
#we deleberate pub a topic to trig the goal sent
goal_status_pub = rospy.Publisher('/move_base/result',MoveBaseActionResult,queue_size=1)
rospy.spin()#在节点中发布/move_base/result
#


多点全自动巡航

仿真:roslaunch robot_navigation robot_navigation.launch simulation:=true
实体机器人上运行则无需传入 simulation:=true 参数

rviz: roslaunch robot_navigation navigation_rviz.launch

pc开终端:rostopic echo /move_base_simple/goal
(订阅/move_base_simple/goal 话题)

通过“2D Nav Goal”按钮设置巡航的点,记录输出的内容。
pose:
	position:
	    x:
	    y:
	    z
	orizntation:
	    x
	    v
	    z:
	
打开robot_navigation 功能包下的 way_point.launch 文件修改goallistx,y,z

巡航次数:
roslaunch robot_navigation way_point.launch loopTimes loopTimes:=2
loopTimes参数为巡航次数,2,机器人在巡航点来回巡航两遍后停止;0,一直巡航。

巡航中每到达一个巡航点,输出下一巡航点的目标ID,巡航次数到达后会输出Loop: 2 Times Finshed 信息。

way_point.launch

<launch>
    <!-- For Simulation -->
    <arg name="sim_mode" default="false" />
    <param name="/use_sim_time" value="$(arg sim_mode)"/>
    <arg name="loopTimes"       default="0" />
    <!-- move base -->
    <!--launch 文件启动 way_point.py节点-->
    <node pkg="robot_navigation" type="way_point.py" respawn="false" name="way_point" output="screen">
        <!-- params for move_base 编写好预设的地点-->
        <param name="goalListX" value="[2.0, 4.0, 3.0]" />
        <param name="goalListY" value="[3.0, 2.0, 2.0]" />
        <param name="goalListZ" value="[0.0, 0.0, 0.0]" />
        <param name="loopTimes" value="$(arg loopTimes)"/>
        <param name="map_frame" value="map" />
    </node>
    

</launch>

multi_goal_point.py

核心思路:

先预置一系列路径点,然后依次发布,只有在上一个点到达后,再发下一个点。

#!/usr/bin/python

import rospy
import string
import math
import time
import sys

from std_msgs.msg import String
from move_base_msgs.msg import MoveBaseActionResult
from actionlib_msgs.msg import GoalStatusArray
from geometry_msgs.msg import PoseStamped
import tf
class MultiGoals:
    def __init__(self, goalListX, goalListY, goalListZ,loopTimes, map_frame):
        #订阅器,订阅move_base/result,move base导航结果话题发布,每次收到导航结果话题就会进入statusCB回调
        self.sub = rospy.Subscriber('move_base/result', MoveBaseActionResult, self.statusCB, queue_size=10)
        #发布器,发布move_base_simple/goal,move_base真正接收的目标点话题
        self.pub = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=10)   
        # params & variables
        self.goalListX = goalListX
        self.goalListY = goalListY
        self.goalListZ = goalListZ
        self.loopTimes = loopTimes
        self.loop = 1
        self.wayPointFinished = False 
        self.goalId = 0#执行当前目标点的序列号
        self.goalMsg = PoseStamped()
        self.goalMsg.header.frame_id = map_frame
        self.goalMsg.pose.orientation.z = 0.0
        self.goalMsg.pose.orientation.w = 1.0
        # Publish the first goal
        time.sleep(1)
        #把goalList中xyz分别付给goalMsg消息
        self.goalMsg.header.stamp = rospy.Time.now()
        self.goalMsg.pose.position.x = self.goalListX[self.goalId]
        self.goalMsg.pose.position.y = self.goalListY[self.goalId]
        self.goalMsg.pose.orientation.x = 0.0
        self.goalMsg.pose.orientation.y = 0.0
        if abs(self.goalListZ[self.goalId]) > 1.0:
            self.goalMsg.pose.orientation.z = 0.0
            self.goalMsg.pose.orientation.w = 1.0
        else:
            w = math.sqrt(1 - (self.goalListZ[self.goalId]) ** 2)
            self.goalMsg.pose.orientation.z = self.goalListZ[self.goalId]
            self.goalMsg.pose.orientation.w = w
        self.pub.publish(self.goalMsg) #赋值完goalMsg,通过发布器,发布给move ase
        rospy.loginfo("Current Goal ID is: %d", self.goalId)  #输出日志
        self.goalId = self.goalId + 1 #目标点发步出去就会等待statusCB回调

        #机器人到达一个目标点就进入statusCB函数
    def statusCB(self, data):
        if self.loopTimes and (self.loop > self.loopTimes):
            rospy.loginfo("Loop: %d Times Finshed", self.loopTimes)
            self.wayPointFinished = True
        if data.status.status == 3 and (not self.wayPointFinished): # reached,而且路径巡航没结束
            self.goalMsg.header.stamp = rospy.Time.now()                
            self.goalMsg.pose.position.x = self.goalListX[self.goalId]
            self.goalMsg.pose.position.y = self.goalListY[self.goalId]
            if abs(self.goalListZ[self.goalId]) > 1.0:
                self.goalMsg.pose.orientation.z = 0.0
                self.goalMsg.pose.orientation.w = 1.0
            else:
                w = math.sqrt(1 - (self.goalListZ[self.goalId]) ** 2)
                self.goalMsg.pose.orientation.z = self.goalListZ[self.goalId]
                self.goalMsg.pose.orientation.w = w
            self.pub.publish(self.goalMsg) #读取下一个点然后发布出去
            rospy.loginfo("Current Goal ID is: %d", self.goalId)              
            if self.goalId < (len(self.goalListX)-1):
                self.goalId = self.goalId + 1
            else:
                self.goalId = 0
                self.loop += 1


if __name__ == "__main__":
    try:    
        # ROS Init    
        rospy.init_node('way_point', anonymous=True)

        # Get params 获取launch文件中传入的goalListXYZ
        goalListX = rospy.get_param('~goalListX', '[2.0, 2.0]')
        goalListY = rospy.get_param('~goalListY', '[2.0, 4.0]')
        goalListZ = rospy.get_param('~goalListZ', '[0.0, 0.0]')
        map_frame = rospy.get_param('~map_frame', 'map' )
        loopTimes = int(rospy.get_param('~loopTimes', '0')) 

        goalListX = goalListX.replace("[","").replace("]","")
        goalListY = goalListY.replace("[","").replace("]","")
        goalListZ = goalListZ.replace("[","").replace("]","")
        goalListX = [float(x) for x in goalListX.split(",")]
        goalListY = [float(y) for y in goalListY.split(",")]
        goalListZ = [float(z) for z in goalListZ.split(",")]#处理成数组的形式
        if len(goalListX) == len(goalListY) == len(goalListZ) & len(goalListY) >=2:  #判断三个数组长度等
            # Constract MultiGoals Obj 
            rospy.loginfo("Multi Goals Executing...")
            mg = MultiGoals(goalListX, goalListY, goalListZ, loopTimes, map_frame)#循环次数 地图基座标
            rospy.spin()
        else:
            rospy.errinfo("Lengths of goal lists are not the same")
    except KeyboardInterrupt:
        print("shutting down")