路径规划导航

更新时间：2019-11-28

除了循迹导航，我们还提供路径规划导航，与循迹导航不同的是，用户可以通过在录制好的地图上预先设置多个中继点（目前只支持预设轨迹），程序会根据地图来计算任意两个中继点的最短路径，从而生成规划好的路径，然后再由app_tracking读取此路径文件来进行导航。规划步骤有如下几步

准备步骤：将之前录好的地图（pb后缀）放到一个文件夹内，假设此文件夹为 /tmp/navigation, 生成的pb为final.pb。注意：为保证地图规划的准确性，录制地图时应遍历所有之后规划时可能走的路径，录制地图时未走过的路径，规划时将不会采用。录制此地图时不添加中继点，中继点会在后续编辑地图时加入。录制地图时，在起点位置附近让镜头360度全方位转转以记录周围特征点，如有条件尽量录制回环轨迹。
运行以下命令，生成waypoints.csv:

./dump_map_info -map_pb /tmp/navigation/final.pb >> /tmp/navigation/waypoints.csv
运行以下命令，根据waypoints.csv生成地图map_occupancy.png和yml文件map_occupancy.yml：

./waypoints_generate_map -waypoints_file /tmp/navigation/waypoints.csv -output_folder /tmp/navigation/
运行以下命令，生成waypoints_graph.txt:

./waypoint_graph -waypoints_file /tmp/navigation/waypoints.csv -waypoints_only_mode -map_occupancy_file /tmp/navigation/map_occupancy.png -map_occupancy_specs_file /tmp/navigation/map_occupancy.yml -waypoint_graph_output /tmp/navigation/waypoints_graph.txt
运行以下命令设置轨迹中继点:

./trajectory_maker -waypoints_file /tmp/navigation/waypoints.csv -map_occupancy_file /tmp/navigation/map_occupancy.png -map_occupancy_specs_file /tmp/navigation/map_occupancy.yml -waypoint_graph_file /tmp/navigation/waypoints_graph.txt

程序运行后，会弹出以下窗口: edit1

其中左边为可操作区域，右边为程序生成的轨迹。操作方式如下:

用鼠标左键单击左边地图轨迹上的任一点，然后按下键盘字母键P，此时会在此处出现蓝色点，如下图所示
重复上一步，将设置下一个中继点，并且会在右边的窗口绘制从上一个中继点到此中继点的路径，如下图所示
可如此继续若干次，直至所有的中继点都标记完后，按ESC退出
因为以上步骤众多，建议在此时确认/tmp/navigation文件夹内包含所有所需文件
waypoints.csv
map_oppcupancy.png
map_oppcupancy.yml
waypoints_graph.txt
trajectory_planned.txt
运行app_tracking，载入整个/tmp/navigation文件夹

./app_tracking -sensor_type XP2 -cam_calib_path xxxxx.yaml -navigation_folder /tmp/navigation -path_follower robot/walk -pb_load /tmp/navigation/final.pb
注意：若用的是步科底盘，-path_follower 参数后接的是robot，且可以在上述指令后附加-controller PD参数获得更平滑的地盘行走效果（-controller PD 模式下的运动参数在XP/config/algorithm_param.yaml文件中的Navigation字段下的v_max~kp_acc_w行调节）。若是普通的UDP导航模式，-path_follower 后接walk, 并且需要加参数-guid_port 8889 -guide_ip 127.0.0.1，在UDP导航模式下请不要加入-controller PD 参数