# muti_car **Repository Path**: wb16271x/fourcar_ws ## Basic Information - **Project Name**: muti_car - **Description**: 多车固定轨迹跟踪避让 - **Primary Language**: C++ - **License**: MulanPSL-2.0 - **Default Branch**: master - **Homepage**: None - **GVP Project**: No ## Statistics - **Stars**: 2 - **Forks**: 0 - **Created**: 2023-01-05 - **Last Updated**: 2023-08-17 ## Categories & Tags **Categories**: Uncategorized **Tags**: None ## README # 多车控制 ## 1、系统综述 系统适用于单车或多车固定轨迹巡航，包含了路径交叉的解决方案。 系统采用分布式系统结构，借鉴去中心化思想，由车辆自行裁决是否让行。 - __系统结构:__ 系统采用多台轻舟机器人和一台主机，配置为ros分布式架构，主机主要实现的地图服务，车辆轨迹录制，导航点发布，避让决策（由于使用去中心化思想，增加车辆时只需要增设一个ros节点）。各个车辆使用ros move_base导航框架，在订阅各自的目标点后执行导航，随着目标点的实时切换，实现车辆不间断巡迹导航。 - __设计思路:__ 车辆在交叉路段作决策需要的信息有：1、自身位置 2、目标点位置 3、其他车辆位置 4、其他车辆目标点。 使用航迹交叉点预测的思想进行避让，将由车身位置到目标点的直线近似为车辆的航向，若两车从存在碰撞风险则满足航向交叉且交叉点到两车的距离小于安全距离或两车间距小于安安全距离，为方便叙述，定义本车到交叉点的距离为 dist1 ，其他车辆到交叉点的距离为 dist2 ，两车之间距离为 cardist ,两车之间安全距离为 safety_dist。 车辆存在碰撞风险有以下三种解决方案： （1） （航向前向交叉&& dist1>dist2）&& （dist1< afety_dist || cardist < safety_dist） : 这种情况描述了车辆存在对象相撞的可能，由于本车距离交叉点较远，避让其他车辆。 （2） （交叉点在我车前向他车后向 ）&& （max(dist1,dist2 < safety_dist ) || cardist < safety_dist） : 描述了我车存在追尾他车的可能，我车停车避让。 （3） （航向后向交叉 && dist2 > dist1）&& （dist2 < safety_dist || cardist < safety_dist） : 描述了我车车尾存在与他车车尾剐蹭的可能，由于他车距离交叉点较远，我车避让。 ## 2、系统配置方法 系统需要在统一局域网运行，为方便后期调试与运行，最好将局域网内设备IP固定。 ### （1）系统对时 若局域网内可以连接互联网，此步骤跳过。若无外网，则需要将系统内的时间进行同步，否则ros的通信、监听等会因为时间戳不同而导致失败。 系统对时配置参考 http://blog.kissdata.com/2014/10/28/ubuntu-ntp.html ### （2） ROS 分布式配置 分布式的优势在于良好的通信机制便于传递话题信息实现信息交互，本系统将主机设为 __ROS_MASTER__ 其余车辆设置为 __ROS_HOST__ 分布式配置参考 https://blog.csdn.net/qq_44989881/article/details/118635001 __注意__ 要修改 /etc/hosts 文件时 主机需要加入自身以及所有车辆的IP和用户名，车辆只需要添加本车和主机的IP及用户名。修改~/.bashrc 文件时主机的 ROS_MASTER 与 ROS_HOSTNAME 一致，车辆配置 ROS_MASTER 为主机， ROS_HOSTNAME 为本机，编辑完成后应当执行 source ~/.bashrc 命令。 ### （3） 文件编译 主机执行的工作空间为 muti_car ，轻舟执行的工作空间有两个 qingzhou_ws 和 socket_ws 如修改 .cpp 文件，请 cd 至工作空间下 执行 catkin_make 命令进行编译。如修改launch yaml .py 文件，无需编译。 __注意__ 如果 .py 文件报错无执行权限，请添加可执行权限。 所有车辆均使用 group 和 namespace 管理，修改文件时请注意逐个车辆对应。 ## 3、使用方法 ### （1） 建图 建图方式有两种：使用 gmapping建图 和使用 catrographer 建图 ，gmapping建图的优势在于实时性强，可以通过 Rviz 在线查看建图情况，缺点是由于缺少全局优化，由于传感器漂移漂移，地图可能存在拉伸或者缩短等与实际尺寸不相符的情况， catrographer 建图优势在于可以通过优化子图实现更为精准的大场景建图，缺点在于基于优化的算法对于硬件要求较高， jetson nano 无法实现实施建图。 - __gmapping建图:__ (1)将 #1车 放入沙盘中#1车起点，主机启动 roscore (2)新建2个终端并 ssh 连接至 #1车: ```ssh learningx1``` 执行以下命令： ```roslaunch qingzhou_nav qingzhou_bringup.launch ``` ```roslaunch qingzhou_nav gmapping.launch``` (3)在主机中打开 Rviz 添加 Map Tf LaserScan 等可视化话题，观察建图情况，待场景扫描完成后在主机运行如下命令进行地图保存： ``` roslaunch gettraj savemap.launch``` __注意:__（1） 若修改地图保存路径，请将 __savemap.launch__ 中第2行的 __value__ 修改为对应路径。（2）若使用其他车辆进行建图，请修改 __savemap.launch__ 第3行 __ns__ 为对应车辆ID。 - __Cartographer 建图:__ #4车配置了cartographer_ros 工具包，主要实现流程为录制bag包，使用bag文件进行离线建图。操作方式如下： （1）将 #4车 放入沙盘中#1车起点，主机启动roscore （2） 新建三个终端并 ssh 至 #4车: ``` ssh learingx4``` （3） 在一个终端中启动轻舟底盘文件： ```roslaunch qingzhou_nav qingzhou_bringup.launch``` （4） 在一个终端中启动话题转换节点(功能为将话题数据转换为 Cartographer 话题数据)： ```rosrun qingzhou_nav lasermsgchange.py``` （5） 录制 bag 包： cd 至 carto_ws 目录下并运行：```rosbag record /horizontal_laser_2d /imu ``` （6）使用手柄缓慢推动#4车移动，在沙盘所有道路中充分行驶 （7） 结束步骤（3）、（4）、（5）终端中的执行项目 ，检查 carto_ws 文件夹下存在新录制的bag文件。 （8）cd 至 carto_ws 文件夹下 ```source ./devel_isolated/setup.bash``` ``` roslaunch cartographer_ros offline_backpack_2d.launch bag_filenames:=${HOME}/carto_ws/xxxxx.bag ``` 在 Rviz 中观察优化结果，等待优化结束。 （9） 在另一个终端中保存地图： ``` rosservice call write_state "{filename: '${HOME}/carto_ws/mymap.pbstream'}" ``` 查看是否有 mymap.pbstream 文件生成 （10） 将地图保存成为pgm 和 yaml 文件： ``` roslaunch cartographer_ros assets_writer_ros_map.launch bag_filenames:=${HOME}/carto_ws/xxx.bag pose_graph_filename:=${HOME}/carto_ws/mymap.pbstream ``` （10） 将生成的 pgm 和 yaml 移动到 主机中 muti_car/src/gettraj/map/ 文件夹下。 __注意__ 修改yaml 第一行 image： 为.pgm 文家名称，不要包含路径。 __注意__ 建图结束后应当修改 主机中mapserver.launch 中的地图路径并在主机终端输入 ```roslaunch gettraj mapserver.launch``` 检查地图服务是否正常。 ### （2）记录行车轨迹 由于车辆轨迹固定，故需要对车辆行驶轨迹进行录制，每辆车的录制方式相同，以 #1车为例。 1. 将1# 车量放置在 #1车初始点 主机启动roscore 和 地图服务： ``` roscore``` ``` roslaunch gettraj mapserver.launch``` 2. 启动 #1车 底盘和导航程序： ```roslaunch qingzhou_nav qingzhou_bringup.launch``` ```roslaunch qingzhou_nav qingzhou_move_base_teb.launch``` 3. 将/muti_car/src/gettraj/yaml 中 record_path.yaml 所有与车辆id有关的量修改为car1，在 Rviz 中 使用 __2D Pose Estimate__ 进行位置初始化，如稍有偏差使用遥控手柄前后移动，使得轻舟自行匹配地图。 4. 启动轨迹记录节点记录车辆轨迹： ``` rosrun gettraj recordpath /home/learningx/muti_car/src/gettraj/yaml/record_path.yaml ``` 5. 在 Rviz 中观察轨迹实现闭环后结束录制节点，录制的轨迹在/muti_car/src/gettraj/path_txt/car1_path_tmp.txt 中。 __注:__ 其他车辆需要依次录制轨迹，操作相同，注意要修改 record_path.yaml 车辆 ID ,生成的路径文件对应在 path_txt 文件夹中。 初始位置的准确性直接影响后续路径跟踪的准确性。如若出现录制轨迹存在毛刺、不够光滑，可适当调整 record_path.yaml 文件中的 dt 以及 step 参数。 对于录制轨迹与实际轨迹稍有偏差，原因有三：（1）栅歌地图与实际地图存在偏差，导致匹配定位不准确，对此可通过PS对栅格地图进行适当调整；（2）传感器精度有限，可能存在漂移以及匹配不精准；（3）作者能力不足，对此，深感抱歉。 ### （3）多车路径跟踪（#1，#2，#4车） 车辆路径跟踪已封装在 launch 文件中，启动路径跟踪大致步骤如下：（1）主机启动 roscore (2)主机启动地图服务（3） 启动轻舟底盘和路径规划节点 （4）启动目标点发布、避让决策节点。 详细步骤如下： 1. 主机启动 roscore 2. 主机启动地图服务: ``` roslaunch gettraj mapserver.launch``` 3. 远程登陆轻舟机器人，启动底盘和导航文件： ```roslaunch qingzhou_nav qingzhou_bringup.launch``` ```roslaunch qingzhou_nav qingzhou_move_base_teb.launch``` 4. 配置 yaml 文件 ,path_txt 文件夹中各车辆轨迹txt文件的第一行路径点复制，拷贝到 yaml 文件夹下各车辆 yaml 文件第二行 "initpose"中。 __注:__ yaml 文件中不能存在 tab 和 空格，各个数据之间用 ',' 隔开。 5. 启动目标点发布、决策避让节点： - 在启动多车之前应当逐一检查单车路径跟踪是否能达到精度需求，启动单车路径跟踪方法为 ```roslaunch gettraj judgement1.launch ``` __注:__ judgement1 代表 #1车 路径跟踪。在执行多车前，应当逐一启动对应 launch 文件进行路径检查。若在Rviz中观察到路径跟踪效果不佳，原因有三：（1） 规划算法参数不合适，对此，可以调整轻舟中 qingzhou_ws/src/qingzhou_nav/config 中关于teb路径规划算法的参数以及主机中 muti_car/src/gettraj/yaml/ 中各个yaml 文件中 "goal_shift_dist" 和 "look_ahead_dist"，或者自行设计规划或纯跟踪算法；（2）传感器精度有限定位或存在少量漂移；（3）作者能力不足。 - 检查无误后，将各车辆放归起点，可以使用逐一启动 launch 文件或一次性全部启动的方式启动多车系统，启动命令如下： ``` roslaunch gettraj judgement.launch ``` ## 4、系统拓展 如若需要拓展系统至五车，需要 #5轻舟以及主机进行配置 - 轻舟配置： 1. 修改5# 车用户名，不能与系统内设备相同。将 1#车中 socket_ws 和 qingzhou_ws 功能包复制到 5#内，__注：__ 不需要复制编译的生成文件。 2. 将 5#车 qingzhou_ws/src 中 qingzhou_nav以及 qingzhou_odom 中所有文件(.launch、.yaml、.cpp )中出现的"car1" 查找替换为 "car5"。 3. 使用 ```catkin_make``` 编译 socket_ws 和 qingzhou_ws 工作空间，若存在依赖问题，请按照报错提示补齐依赖。 4. 将muti_car/src/gettraj 中 launch 、 yaml 文件夹中仿照其他车辆文件进行添加文件以及修改参数。 5. 对于judgement.cpp 进行如下修改： - 22 行 ：__vector carlist__ 增加 car5_base_link - 40-60 行附近仿造添加目标点回调函数void car5GoalCB - 60-77 行附近仿造添加让行回调函数 avoidcar5CB - 680-700 行附近仿造对 sub_car5_goal 和 avodi_sub4 进行申明 修改后请请重新编译 muti_car 工作空间 6. 对 5# 车辆进行同步对时，ros分布式配置（主机 /etc/hosts 文件需要添加 5#车 IP 以及用户名），后续录制轨迹与发布轨迹与前文方法相同。 ## 5、交通的灯识别 交通灯使用 Opencv 霍夫圆以及 HSV 色域双重判定检测，为避免误识别，采用划定区域的方法进行，当轻舟机器人在交通灯附近才开始识别。由于场景光线不同，交通等识别需要现场调试，调试文件为两个: - qingzhou_nav/config/trafficlight.yaml 文件中填写交通灯坐标，__注__ 四个值代表一个区域，车辆轨迹经过两个红绿灯，填写格式为[x1min,y1min,x1max,y1max,x2min,y2min,x2max,y2max] 多个交通灯以此类推。 - qingzhou_nav/srcipts/trafficlight.py 73行附近请适当调节色域值 。