无论传感器是雷达还是相机，都只能看到障碍物的一部分。如果障碍物比较大，而目标点在障碍物内(我们知道，机器人不知道)，机器人会绕过它看到的障碍部分，前往目标点，然后它又看到障碍，于是接着绕行，这样会绕很久。目前只能设置参数planner_patience，绕的时间太长时，进行Abort

向move_base发布目标的频率太高，会出错

写一个loop程序，高频率循环发两个目标点，连C++程序都不用，写bash脚本即可

#!/bin/bash

# 每隔1秒就发布下一个目标点
while true; do

rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped "header:
  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: 'map'
pose:
  position:
    x: 20
    y: 7
    z: 0.0
  orientation:
    x: 0.0
    y: 0.0
    z: 1
    w: 0" & sleep 1 && kill -2 $!

# -2 是用信号量表示的     $!是子进程号   相当于执行ctrl+C

rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped "header:
  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: 'map'
pose:
  position:
    x: 24
    y: 7
    z: 0.0
  orientation:
    x: 0.0
    y: 0.0
    z: 0
    w: 1" & sleep 1 && kill -2 $!

    sleep 1
    echo "     ---------- start next loop ----------       "
done

sleep 1 && kill -2 $!是关键，sleep 1秒就杀死进程。 如果没有这句，机器人执行第一次导航时，必须等待Ctrl+C才能进入下一次导航

经过测试，向move_base发布目标点的频率如果太高，会报错，最快为0.6秒一次。 但是关掉rviz后，可达到0.5秒一次，可见优化程序会有效。

init.py的2140行
rospy client.py 251行
这里就涉及ROS源码了，想彻底优化就得着手ROS源码了，但是感觉没有必要。