![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/12\/ob.png\")
<\/a><\/p>\n\u4e00\u3001\u5730\u56fe\u5b9a\u4f4d<\/h1>\n1. \u542f\u52a8\u5e95\u76d8+\u4f20\u611f\u5668+Autoware<\/span><\/h4>\n# 1. \u542f\u52a8\u5e95\u76d8+\u4f20\u611f\u5668<\/span>\r\nroslaunch turn_on_wheeltec_robot open_autoware.launch\r\n\r\n# \u68c0\u67e5topic\u5e27\u7387<\/span>\r\nrostopic hz \/imu_raw \/points_raw \/vehicle\/<\/span>odom \/gps\/<\/span>fix\r\n# \u68c0\u67e5topic\u65f6\u95f4\u6233\u662f\u5426\u6b63\u5e38<\/span>\r\nrostopic echo \/imu_raw \/gps\/<\/span>fix -p\r\n\r\n# 2. \u542f\u52a8Autoware<\/span>\r\ncd ~\/autoware.ai.x86\/\r\n.\/open.sh\r\nsource ~\/Autoware\/install\/<\/span>setup.bash\r\nroslaunch runtime_manager runtime_manager.launch<\/code><\/pre>\n2. \u52a0\u8f7d\u4ecebase_link\u5230Lidar\u5750\u6807\u7cfb\u7684TF\u3001\u5c0f\u8f66\u6a21\u578b<\/span><\/h4>\n\u70b9\u51fb\u8fdb\u5165\u3010Setup\u3011\u83dc\u5355\uff0c\u5206\u522b\u8bbe\u7f6eLocalizer\u3001TF\u3001Vehicle Model\uff1a<\/p>\n
\n- \u5728\u3010Localizer\u3011\u4e2d\u9009\u62e9Velodyne\u3002<\/li>\n
- \u5728\u3010Baselink to Localizer\u3011\u4e2d\u8bbe\u7f6e\u597d\u5404\u4e2a\u53c2\u6570\uff0c\u5176\u4e2d x\u3001y\u3001z\u3001yaw\u3001pitch\u3001roll\u8868\u793a\u771f\u8f66\u96f7\u8fbe\u4e2d\u5fc3\u70b9\u4e0e\u8f66\u8eab\u540e\u8f74\u4e2d\u5fc3\u70b9\u7684 TF \u4f4d\u7f6e\u5173\u7cfb\uff0c\u53ef\u6839\u636e\u96f7\u8fbe\u5b9e\u9645\u4f4d\u7f6e\u8fdb\u884c\u4fee\u6539\uff08\u5bf9\u4e8e\u956d\u795e 16 \u7ebf\u96f7\u8fbez\u53c2\u6570\u8bbe\u7f6e\u4e3a0.38\uff0cyaw \u53c2\u6570\u8bbe\u7f6e\u6210-1.57\uff09\uff0c\u8bbe\u7f6e\u597d\u4e4b\u540e\u70b9\u51fb \u3010TF\u3011 \u6309\u94ae\u3002<\/li>\n
- docker\u7248\u672c\u7684urdf\u5c0f\u8f66\u6a21\u578b\u5728 autoware_shared_dir\/urdf \u76ee\u5f55\u4e0b\u9762\uff0c\u9009\u62e9\u540e\u70b9\u51fb\u3010Vehicle Model\u3011\u52a0\u8f7d\u6a21\u578b\uff08\u5982\u679cVehicle Model\u4e3a\u7a7a\uff0c\u90a3\u4e48\u4f1a\u52a0\u8f7d\u4e00\u4e2a\u9ed8\u8ba4\u6a21\u578b\uff0c\u5728 rviz \u663e\u793a\u65f6\uff0c\u5982\u679c\u6709\u6fc0\u5149\u96f7\u8fbe\u6570\u636e\uff0c\u8f66\u8f86\u4f1a\u663e\u793a\u4e3a\u9ed1\u8272\uff09\u3002<\/li>\n<\/ul>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/11\/autoware-setup-1024x644.png\")
<\/a><\/p>\n3. \u8bbe\u7f6e\u4ece world \u5230 map \u8f6c\u6362<\/span><\/h4>\n\u70b9\u51fb\u3010Map\u3011\u83dc\u5355\uff0c\u52a0\u8f7d world \u5230 map \u7684\u5750\u6807\u8f6c\u6362\uff1a<\/p>\n
\n- \u70b9\u51fb\u3010TF\u3011\u53f3\u4fa7\u7684\u3010ref\u3011\u6309\u94ae\uff0c\u9009\u62e9~\/autoware_shared_dir\/lgsvl-tf.launch \u6587\u4ef6\u540e<\/li>\n
- \u70b9\u51fb\u3010TF\u3011\u6309\u94ae\u52a0\u8f7d\u8be5\u6587\u4ef6\uff0c\u4ee5\u52a0\u8f7d world \u5230 map \u7684\u5750\u6807\u8f6c\u6362
<\/code><\/li>\n<\/ul>\n![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/11\/autoware-map-tf-1024x644.png\")
<\/a><\/h4>\n4. \u52a0\u8f7d\u70b9\u4e91\u5730\u56fe PCD \u6587\u4ef6<\/span><\/h4>\n\n- \u6253\u5f00\u3010map\u3011\u9875\u9762\uff0c\u70b9\u51fb\u3010point cloud\u3011\u53f3\u4fa7\u7684\u3010ref\u3011\u6309\u94ae\uff0c\u9009\u62e9\u4e0a\u4e00\u6b65\u5efa\u56fe\u4fdd\u5b58\u76843D\u70b9\u4e91\u5730\u56fe.pcd\u6587\u4ef6<\/li>\n
- \u70b9\u51fb\u3010point cloud\u3011\u6309\u94ae\uff0c\u6b64\u65f6\u4e0b\u65b9\u4f1a\u51fa\u73b0\u4e00\u4e2a\u8fdb\u5ea6\u6761\uff0c\u5f53\u8fdb\u5ea6\u6761\u663e\u793a\u52a0\u8f7d\u4e86 100\uff05\u5e76\u51fa\u73b0[OK]\u5b57\u6837\u65f6\uff0c\u8bc1\u660e 3D \u70b9\u4e91\u5730\u56fe\u5df2\u7ecf\u6210\u529f\u52a0\u8f7d\u5b8c\u6bd5<\/li>\n<\/ul>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/11\/autoware-pointcloud-load-1024x630.png\")
<\/a><\/p>\n5. \u70b9\u4e91\u964d\u91c7\u6837\u8fc7\u6ee4<\/span><\/h4>\n\u6253\u5f00\u3010sensing\u3011\u9875\u9762\uff0c\u627e\u5230\u3010points_downsampler\u3011\u4e0b\u7684\u3010voxel_grid_filter\u3011\u9009\u9879\uff0c\u8bbe\u7f6e[app]\u7684\u4e00\u4e9b\u53c2\u6570\u540e\uff0c\u52fe\u9009[voxel_grid_filter]\u3002 [voxel_grid_filter]\u6709\u4e00\u4e9b\u53c2\u6570\u8bbe\u7f6e\u5982\u4e0b\u56fe\uff1a<\/p>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/11\/Screenshot-from-2024-11-24-02-22-23-1024x628.png\")
<\/a><\/p>\n\n- Voxel Leaf size\uff1a\u4f53\u7d20\u964d\u91c7\u6837\u7b97\u6cd5\u4e2d\u7684\u4f53\u7d20\u53f6\u5927\u5c0f\uff0c\u5ba4\u5185\u53ef\u8bbe\u7f6e\u5c0f\u70b9\uff0c\u5ba4\u5916\u53ef\u8bbe\u7f6e\u5927\u70b9<\/li>\n<\/ul>\n
6. \u5730\u9762\u8fc7\u6ee4<\/h4>\n
\u6253\u5f00\u3010sensing\u3011\u9875\u9762\uff0c\u627e\u5230\u3010points_preprocessor\u3011\u4e0b\u7684\u3010ring_ground_filter\u3011\u9009\u9879\uff0c\u8bbe\u7f6e\u3010app\u3011\u7684\u4e00\u4e9b\u53c2\u6570\u540e\uff0c\u5e76\u52fe\u9009\u3002\u3010ring_ground_filter\u3011\u6709\u4e00\u4e9b\u53c2\u6570\u8bbe\u7f6e\u5982\u4e0b\u56fe\uff1a<\/p>\n
\n- Sensor model\uff1a16\u300132\u300164 \u7ebf\u96f7\u8fbe\uff0c\u8fd9\u91cc\u5982\u679c\u4f7f\u7528\u956d\u795e 16 \u7ebf\u96f7\u8fbe\u52fe\u9009[16]<\/li>\n
- Sensor height\uff1a\u96f7\u8fbe\u8ddd\u79bb\u5730\u9762\u9ad8\u5ea6\uff0c\u6211\u8fd9\u91cc\u662f0.438<\/li>\n
- Max slope\uff1a\u5730\u9762\u6700\u5927\u5761\u5ea6\uff0c\u5355\u4f4d\u00b0\uff0c\u5982\u679c\u4e0d\u7ed9\u51fa\u5761\u5ea6\uff0c\u5bb9\u6613\u5c06\u5761\u8bc6\u522b\u4e3a\u969c\u788d\u7269<\/li>\n
- Vertical Thres\uff1a\u8868\u793a\u969c\u788d\u7269\u548c\u5730\u9762\u7684\u5dee\u5f02\u5ea6\uff0c\u5982\u679c\u5927\u4e8e\u8fd9\u4e2a\u503c\u5219\u88ab\u8ba4\u4e3a\u662f\u969c\u788d\u7269<\/li>\n<\/ul>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/12\/ring_ground_filter-0.438.png\")
<\/a><\/p>\n7. \u542f\u52a8ndt_matching\uff08<\/span>\u4ece map \u5230 base_link \u7684\u8f6c\u6362\uff09<\/span><\/h4>\n\u627e\u5230\u3010Computing\u3011\u5de6\u83dc\u5355\u680f\u4e0b\u7684\u3010ndt_matching\u3011\u9009\u9879\uff0c\u8bbe\u7f6e\u3010app\u3011\u91cc\u9762\u7684\u53c2\u6570\u540e\uff0c\u52fe\u9009\u3010ndt_matching\u3011\uff0c\u4e00\u4e9b\u53c2\u6570\u8bbe\u7f6e\u5982\u4e0b\u6240\u793a\uff1a<\/p>\n
\n- \u4f7f\u7528RTK\u65f6\uff0c\u3010topic:\/config\/ndt\u3011\u00a0\u52fe\u9009\u3010GNSS\u3011\u3002\u6ca1\u6709\u4f7f\u7528 GNSS\u65f6\uff0c\u3010topic:\/config\/ndt\u3011\u52fe\u9009 \u3010Initial_Pose\u3011\uff0cx\uff0cy\uff0cz\uff0croll\uff0cpitch\uff0cyaw \u7684\u503c\u8868\u793a\u5c0f\u8f66\u7684\u521d\u59cb\u4f4d\u7f6e\uff08\u9ed8\u8ba4\u5efa\u56fe\u8d77\u70b9\u4f4d\u7f6e\uff09\u3002<\/li>\n
- \u5728[method type]\u680f\u4e2d\uff0c\u65e0 GPU \u52a0\u901f\u7684\u9009\u3010pcl_generic\u3011\uff0c\u6709 GPU \u52a0\u901f\u7684\u9009[pcl_anh_gpu]\u3002<\/li>\n
- \u7528\u5230 imu \u8f85\u52a9\u5b9a\u4f4d\uff0c\u9700\u8981\u628a\u3010useimu\u3011\u9009\u9879\u52fe\u9009\u4e0a\uff0c\u5176\u4f59\u53c2\u6570\u53c2\u8003\u9ed8\u8ba4\u503c\u5c31\u53ef\u4ee5\u4e86\u3002<\/li>\n<\/ul>\n
\u5728[Compulting]\u9875\u9762\u627e\u5230[ndt_matching_monitor]\u9009\u9879\uff0c\u52fe\u9009\u8282\u70b9<\/p>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/11\/ndt-matching.png\")
<\/a><\/p>\n![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/11\/fix2tfpose.png\")
<\/a><\/p>\n![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/11\/ndt_matching-step-0.1-7.png\")
<\/a><\/p>\nndt_matching\u53c2\u6570\u5728autoware\u4e2d\u662f\u901a\u8fc7runtime manager\u4ee5\/config\/ndt\u8bdd\u9898\u7684\u65b9\u5f0f\u53d1\u5e03\u7684\uff0c\u8bdd\u9898\u901a\u8fc7\u754c\u9762\u89e6\u53d1\uff0c\u6bcf\u5207\u6362\u754c\u9762\u6309\u94ae\u3001\u4fee\u6539\u53c2\u6570\u5e76\u786e\u5b9a\u5c31\u89e6\u53d1\u4e00\u6b21\u8bdd\u9898\u53d1\u5e03\uff0c\u6765\u5b9e\u65f6\u6539\u53d8\u7a0b\u5e8f\u4e2d\u7684NDT\u914d\u7f6e\u53c2\u6570\uff0c\u7c7b\u4f3c\u4e8e\u52a8\u6001\u53c2\u6570\u914d\u7f6e\uff0c\u540e\u7eed\u53ef\u6539\u4e3aros\u53c2\u6570\u5728\u7a0b\u5e8f\u5f00\u59cb\u65f6\u52a0\u8f7d\u3002<\/p>\n
rostopic echo \/config\/ndt<\/code><\/pre>\n\u53c2\u6570\u8bf4\u660e\uff1a<\/p>\n
\n- init_pos_gnss\uff1a1\u901a\u8fc7GNSS\u6765\u786e\u5b9a\u521d\u503c\uff0c0\u901a\u8fc7\u521d\u59cb\u4f4d\u59ff\u786e\u5b9a\u521d\u503c\uff1b\u5982\u679c\u4f7f\u7528GNSS\u53ef\u53c2\u8003GNSS+NDT\u8054\u5408\u5b9a\u4f4d<\/a><\/li>\n
- use_predict_pose\uff1a1\u4f7f\u7528\u9884\u6d4b\u4f4d\u59ff\uff0c0\u4e0d\u4f7f\u7528\u9884\u6d4b\u4f4d\u59ff\uff1b<\/li>\n
- error_threshold: \uff081.0\uff09\u8be5\u53c2\u6570\u4e0d\u77e5\u9053\u5e72\u561b\u7684\uff0c\u597d\u50cf\u6ca1\u7528\uff1b<\/li>\n
- resolution: \uff081.0\uff09\u7f51\u683c\u5927\u5c0f\u8bbe\u7f6e\uff1b<\/li>\n
- step_size: (0.1) \u8fed\u4ee3\u6b65\u957f\uff1b<\/li>\n
- trans_epsilon: \uff080.01\uff09\u7b97\u6cd5\u6536\u655b\u6761\u4ef6\uff1b<\/li>\n
- max_iterations: \uff0830\uff09\u6700\u5927\u8fed\u4ee3\u6b21\u6570\uff1b<\/li>\n<\/ul>\n
8. \u542f\u52a8\u5b9a\u4f4dvel_pose_connect<\/span><\/h4>\n\n- \u627e\u5230\u3010Computing\u3011 \u5de6\u83dc\u5355\u680f\u4e0b\u7684 \u3010vel_pose_connect\u3011\u3002<\/li>\n
- \u6253\u5f00 \u3010app\u3011 \u5e76\u786e\u4fdd\u9009\u9879 [Simulation_Mode] \u6ca1\u6709\u88ab\u52fe\u9009\u3002<\/li>\n
- \u9000\u51fa\u5e76\u52fe\u9009 \u3010vel_pose_connect\u3011\u3002<\/li>\n<\/ul>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/11\/autoware-vel-pose-connect-1024x674.png\")
<\/a><\/p>\n9. \u6253\u5f00rviz<\/span><\/h4>\n\u70b9\u51fb\u754c\u9762\u53f3\u4e0b\u89d2\u7684[rviz] \u6253\u5f00rviz\uff0c\u76f8\u5173\u914d\u7f6e\u5728~\/autoware_shared_dir\/default.rviz \u3002<\/p>\n
\u82e5\u662f\u5c0f\u8f66\u4e0d\u5728\u5efa\u56fe\u8d77\u70b9\u4f4d\u7f6e\uff0c\u6216\u8005\u662f\u5b9a\u4f4d\u4e0d\u51c6\u786e\uff0c\u53ef\u4ee5\u70b9\u51fb rviz \u83dc\u5355\u680f\u4e2d\u7684\u30102D pose estimate\u3011\uff0c\u7136\u540e\u5728\u5730\u56fe\u4e0a\u9f20\u6807\u5de6\u51fb\u62d6\u52a8\uff0c\u91cd\u65b0\u8bbe\u7f6e\u5c0f\u8f66\u7684\u4f4d\u7f6e\u3002<\/p>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/11\/autoware-ndt-rviz-1024x604.png\")
<\/a><\/p>\n <\/p>\n
\u4e8c\u3001\u96f7\u8fbe\u76ee\u6807\u8bc6\u522b<\/h1>\n1. \u5b8c\u6210\u5730\u56fe\u5b9a\u4f4d<\/h4>\n
\u5b8c\u6210\u4e0a\u4e00\u6b65\u5730\u56fe\u5b9a\u4f4d\u6b65\u9aa4\u3002<\/p>\n
2. \u6b27\u51e0\u91cc\u5f97\u805a\u7c7b<\/h4>\n
\u70b9\u4e91\u805a\u7c7b\u662f\u5c06\u4e09\u7ef4\u70b9\u4e91\u6570\u636e\u8fdb\u884c\u5206\u7ec4\u6216\u5206\u7c7b\uff0c\u5c06\u8ddd\u79bb\u8f83\u8fd1\u7684\u70b9\u5f52\u4e3a\u4e00\u7c7b\u7684\u8fc7\u7a0b\u3002<\/p>\n
\n- \u969c\u788d\u7269\u68c0\u6d4b\u4e0e\u8ddf\u8e2a\uff1a\u70b9\u4e91\u805a\u7c7b\u53ef\u4ee5\u5e2e\u52a9\u8bc6\u522b\u548c\u533a\u5206\u73af\u5883\u4e2d\u7684\u4e0d\u540c\u7269\u4f53\uff0c\u7279\u522b\u662f\u969c\u788d\u7269\u3002\u901a\u8fc7\u805a\u7c7b\uff0c\u53ef\u4ee5\u5c06\u540c\u4e00\u4e2a\u969c\u788d\u7269\u7684\u70b9\u5f52\u4e3a\u4e00\u7c7b\uff0c\u5e76\u4f30\u8ba1\u5176\u4f4d\u7f6e\u3001\u5f62\u72b6\u548c\u5927\u5c0f\uff0c\u4ece\u800c\u5b9e\u73b0\u5bf9\u969c\u788d\u7269\u7684\u68c0\u6d4b\u548c\u8ddf\u8e2a\u3002<\/li>\n
- \u5efa\u7acb\u73af\u5883\u6a21\u578b\uff1a\u901a\u8fc7\u70b9\u4e91\u805a\u7c7b\uff0c\u53ef\u4ee5\u5c06\u70b9\u4e91\u6570\u636e\u5206\u5272\u4e3a\u4e0d\u540c\u7684\u7269\u4f53\u7fa4\u7ec4\uff0c\u5e76\u5c06\u8fd9\u4e9b\u7fa4\u7ec4\u7684\u7279\u5f81\u63d0\u53d6\u51fa\u6765\u3002\u8fd9\u4e9b\u7279\u5f81\u53ef\u4ee5\u7528\u4e8e\u5efa\u7acb\u73af\u5883\u6a21\u578b\uff0c\u5305\u62ec\u9053\u8def\u3001\u5efa\u7b51\u7269\u3001\u4ea4\u901a\u6807\u5fd7\u7b49\uff0c\u4e3a\u81ea\u52a8\u9a7e\u9a76\u8f66\u8f86\u63d0\u4f9b\u66f4\u51c6\u786e\u548c\u8be6\u7ec6\u7684\u73af\u5883\u4fe1\u606f\u3002<\/li>\n
- \u52a8\u6001\u7269\u4f53\u68c0\u6d4b\uff1a\u70b9\u4e91\u805a\u7c7b\u53ef\u4ee5\u5e2e\u52a9\u8bc6\u522b\u548c\u533a\u5206\u7a33\u5b9a\u7269\u4f53\u548c\u52a8\u6001\u7269\u4f53\u3002\u901a\u8fc7\u5c06\u70b9\u4e91\u6570\u636e\u8fdb\u884c\u805a\u7c7b\uff0c\u53ef\u4ee5\u53d1\u73b0\u79fb\u52a8\u7684\u7269\u4f53\uff0c\u5e76\u5c06\u5176\u4e0e\u7a33\u5b9a\u7684\u73af\u5883\u8fdb\u884c\u533a\u5206\u3002\u8fd9\u5bf9\u4e8e\u81ea\u52a8\u9a7e\u9a76\u6765\u8bf4\u81f3\u5173\u91cd\u8981\uff0c\u56e0\u4e3a\u5b83\u53ef\u4ee5\u5e2e\u52a9\u8f66\u8f86\u9884\u6d4b\u548c\u9002\u5e94\u52a8\u6001\u7269\u4f53\u7684\u884c\u4e3a\u3002<\/li>\n
- \u4f20\u611f\u5668\u878d\u5408\uff1a\u81ea\u52a8\u9a7e\u9a76\u7cfb\u7edf\u901a\u5e38\u4f1a\u4f7f\u7528\u591a\u79cd\u4e0d\u540c\u7c7b\u578b\u7684\u4f20\u611f\u5668\uff0c\u5982\u6fc0\u5149\u96f7\u8fbe\u3001\u6444\u50cf\u5934\u7b49\u3002\u70b9\u4e91\u805a\u7c7b\u53ef\u4ee5\u5c06\u591a\u4e2a\u4f20\u611f\u5668\u83b7\u53d6\u7684\u70b9\u4e91\u6570\u636e\u8fdb\u884c\u878d\u5408\uff0c\u63d0\u4f9b\u66f4\u5168\u9762\u548c\u4e00\u81f4\u7684\u73af\u5883\u611f\u77e5\u3002<\/li>\n<\/ul>\n
\u603b\u7684\u6765\u8bf4\uff0c\u70b9\u4e91\u805a\u7c7b\u5728\u81ea\u52a8\u9a7e\u9a76\u611f\u77e5\u5c42\u4e2d\u7684\u610f\u4e49\u5728\u4e8e\u5e2e\u52a9\u5c06\u4e09\u7ef4\u70b9\u4e91\u6570\u636e\u8f6c\u5316\u4e3a\u66f4\u6613\u5904\u7406\u7684\u7269\u4f53\u4fe1\u606f\uff0c\u5e76\u4e3a\u969c\u788d\u7269\u68c0\u6d4b\u3001\u73af\u5883\u5efa\u6a21\u3001\u52a8\u6001\u7269\u4f53\u68c0\u6d4b\u548c\u4f20\u611f\u5668\u878d\u5408\u7b49\u4efb\u52a1\u63d0\u4f9b\u57fa\u7840\u6570\u636e\u548c\u51c6\u786e\u6027\u3002\u5b83\u662f\u5b9e\u73b0\u81ea\u52a8\u9a7e\u9a76\u73af\u5883\u611f\u77e5\u4e0e\u51b3\u7b56\u7684\u5173\u952e\u6b65\u9aa4\u4e4b\u4e00\u3002<\/p>\n
\u6b27\u5f0f\u805a\u7c7b\u662f\u4e00\u79cd\u57fa\u4e8e\u6b27\u6c0f\u8ddd\u79bb\u5ea6\u91cf\u7684\u805a\u7c7b\u7b97\u6cd5\u3002\u91c7\u7528\u57fa\u4e8eKD-Tree\u7684\u8fd1\u90bb\u67e5\u8be2\u7b97\u6cd5\u662f\u52a0\u901f\u6b27\u5f0f\u805a\u7c7b\u3002<\/p>\n
\u6253\u5f00[Computing]\u9875\u9762\uff0c\u627e\u5230[lidar_detector]\u4e0b\u7684[lidar_euclidean_cluster_detect]\u9009\u9879\uff0c\u8bbe\u7f6e[app]\u7684\u4e00\u4e9b\u53c2\u6570\u540e\uff0c\u5e76\u52fe\u9009\u3002[lidar_euclidean_cluster_detect]\u6709\u4e00\u4e9b\u53c2\u6570\u8bbe\u7f6e\u5982\u4e0b\u56fe\uff1a<\/p>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/11\/lidar_euclidean_cluster_detect-1024x461.png\")
<\/a><\/p>\n\n- use_gpu\uff1a\u662f\u5426\u4f7f\u7528GPU\uff0c\u9009\u62e9\u4f7f\u7528\uff0c\u5426\u5219\u805a\u7c7b\u5341\u5206\u6162\uff1b<\/li>\n
- output_frame\uff1a\u8f93\u51fa\u5750\u6807\u7cfb\uff0c\u6539\u4e3amap\uff1b<\/li>\n
- pose_estimation\uff1a\u9700\u8981\u4f30\u8ba1\u805a\u7c7b\u70b9\u4e91\u6700\u5c0f\u9762\u79ef\u8fb9\u754c\u77e9\u5f62\u7684\u59ff\u6001\uff1b<\/li>\n
- downsample_cloud\uff1a\u70b9\u4e91\u901a\u8fc7VoxelGrid\u6ee4\u6ce2\u5668\u8fdb\u884c\u4e0b\u91c7\u6837\uff1b<\/li>\n
- input_points_node\uff1a\u8f93\u5165\u70b9\u4e91topic\uff0c\u8fd9\u91cc\u4f7f\u7528\u8fc7\u6ee4\u5730\u9762\u540e\u7684\u96f7\u8fbe\u8bdd\u9898\/points_no_ground\uff1b<\/li>\n
- leaf size\uff1a\u4e0b\u91c7\u6837\u4f53\u7d20\u7f51\u683c\u5927\u5c0f\uff1b<\/li>\n
- cluster size minimum\uff1a\u805a\u7c7b\u7684\u6700\u5c11\u70b9\u6570\uff08\u8c03\u5c0f\u4e00\u70b9\u80fd\u805a\u5230\u66f4\u8fdc\u7684\u7c7b\uff09\uff1b<\/li>\n
- cluster size maximum\uff1a\u805a\u7c7b\u7684\u6700\u591a\u70b9\u6570\uff1b<\/li>\n
- clustering distance\uff1a\u805a\u7c7b\u516c\u5dee\uff08\u540c\u4e00\u7c7b\u4e24\u70b9\u6700\u5927\u8ddd\u79bb\u9608\u503c\uff0c\u8c03\u5927\u4e00\u70b9\u80fd\u805a\u5230\u66f4\u8fdc\u7684\u7c7b\uff0c\u4f46\u662f\u592a\u5927\u4f1a\u9020\u6210\u660e\u663e\u7684\u4e0d\u540c\u7c7b\u805a\u5230\u4e00\u8d77m\uff09\uff1b<\/li>\n
- clip_min_height\uff1a\u88c1\u526a\u7684\u6700\u5c0f\u9ad8\u5ea6\uff1b<\/li>\n
- clip_max_height\uff1a\u88c1\u526a\u7684\u6700\u5927\u9ad8\u5ea6\uff08\u9ad8\u4e8e\u96f7\u8fbe\u591a\u5c11\uff09\uff1b<\/li>\n
- use_vector_map\uff1a\u662f\u5426\u4f7f\u7528\u77e2\u91cf\u5730\u56fe\uff1b<\/li>\n
- vectormap_frame\uff1a\u77e2\u91cf\u5730\u56fe\u5750\u6807\u7cfb\uff1b<\/li>\n
- remove_points_upto\uff1a\u8ddd\u79bb\u6fc0\u5149\u96f7\u8fbe\u8fd9\u4e2a\u8ddd\u79bb\u8fc7\u8fd1\u7684\u70b9\u5c06\u88ab\u79fb\u9664\uff08\u6700\u5927\u53ea\u80fd2.5\uff0c\u56e0\u6b64\u8f66\u8eab\u8fc7\u5927\u4e0d\u8981\u7528\uff0c\u9700\u8981\u8f93\u5165\u53bb\u9664\u5730\u9762\u70b9\u70b9\u4e91\uff09\uff1b<\/li>\n
- keep_only_lanes_points\uff1a\u884c\u9a76\u7ebf\u8fb9\uff08\u8fdc\uff09\u6ee4\u6ce2\uff1b<\/li>\n
- keep_lane_left_distance\uff1a\u5411\u5de6\u79fb\u9664\u8be5\u8ddd\u79bb\u4ee5\u5916\u7684\u70b9 (m)\uff08\u7b97\u529b\u5f3a\u53ef\u4ee5\u8bbe\u5927\u70b9\uff09\uff1b<\/li>\n
- keep_lane_right_distance\uff1a\u5411\u53f3\u79fb\u9664\u8be5\u8ddd\u79bb\u4ee5\u5916\u7684\u70b9 (m)\uff1b<\/li>\n
- cluster_merge_threshold\uff1a\u805a\u7c7b\u7c07\u95f4\u7684\u8ddd\u79bb\u9608\u503c(m)\uff1b<\/li>\n
- use_multiple_thres\uff1a\u662f\u5426\u4f7f\u7528\u591a\u7ea7\u9608\u503c\u8fdb\u884c\u805a\u7c7b\uff08\u4f7f\u7528\u4e0b\u9762\u4e24\u4e2a\u53c2\u6570\uff09\uff1b<\/li>\n
- clustering_ranges\uff1a\u4e0d\u540c\u7684\u8ddd\u79bb\u6709\u4e0d\u540c\u7684\u805a\u7c7b\u9608\u503c (m)\uff1b<\/li>\n
- clustering_distances\uff1a\u805a\u7c7b\u516c\u5dee(\u540c\u4e00\u7c7b\u4e24\u4e2a\u70b9\u7684\u6700\u5927\u8ddd\u79bb\u9608\u503c\uff0c\u4e0eclustering_ranges\u5bf9\u5e94)\uff1b<\/li>\n
- remove ground\uff1a\u5730\u5e73\u9762\u6ee4\u6ce2\uff08\u79fb\u9664\u5c5e\u4e8e\u5730\u9762\u7684\u70b9\uff09\uff0c\u63a8\u8350\u8bbe\u7f6e\u4e3a0\uff1b<\/li>\n
- use_diffnormals\uff1a\u91c7\u7528\u6b63\u6001\u5dee\u503c\u6ee4\u6ce2\u518d\u53bb\u9664\u4e00\u6b21\u5730\u9762\u70b9\u3002<\/li>\n<\/ul>\n
3. \u8ddf\u8e2a\u969c\u788d\u7269<\/h4>\n
\u591a\u7ebf\u6fc0\u5149\u96f7\u8fbe\u6570\u636e\u4ece\u4f20\u611f\u5668\u51fa\u6765\u540e\uff0c\u7ecf\u8fc7\u4f20\u7edf\u7684\u805a\u7c7b+\u9ad8\u7cbe\u5730\u56feobject filter\uff0c\u6216\u8005learning-based\u7684\u65b9\u6cd5\u4e4b\u540e\uff0c\u5f97\u5230\u4e00\u4e2a\u76ee\u6807\u5217\u8868\uff0c\u6b64\u76ee\u6807\u5217\u8868\u542b\u6709\u4e8c\u7ef4\u4f4d\u7f6e\u4fe1\u606f\u548c\u89d2\u5ea6\u4fe1\u606f\uff0c\u6ca1\u6709\u901f\u5ea6\u4fe1\u606f\uff0c\u540c\u65f6\u57fa\u4e8e\u6bcf\u4e00\u5e27\u7684\u68c0\u6d4b\u65b9\u6cd5\uff0c\u53ef\u80fd\u4f1a\u51fa\u73b0\u201c\u76ee\u6807\u6d88\u5931\u201d\u7684\u95ee\u9898\uff0c\u56e0\u6b64\u9700\u8981\u6ee4\u6ce2\u5668\u6765\u8ddf\u8e2a\u8fd9\u4e9b\u76ee\u6807\u3002<\/p>\n
\u6253\u5f00[Computing]\u9875\u9762\uff0c\u627e\u5230[lidar_tracker]\u4e0b\u7684[lidar_kf_contour_track]\u9009\u9879\uff0c\u8bbe\u7f6e[app]\u7684\u4e00\u4e9b\u53c2\u6570\u540e\uff0c\u52fe\u9009[lidar_kf_contour_track]\u3002[lidar_kf_contour_track]\u6709\u4e00\u4e9b\u53c2\u6570\u8bbe\u7f6e\u5982\u4e0b\u56fe\uff1a<\/p>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/11\/lidar_kf_contour_track-1024x471.png\")
<\/a><\/p>\n\n- Vector map distance\uff1a\u9700\u8981\u4fee\u6539\u4e3a 0\uff0c\u5176\u4ed6\u53c2\u6570\u9ed8\u8ba4\u5373\u53ef\u3002<\/li>\n<\/ul>\n
\u6211\u4eec\u53ef\u4ee5\u901a\u8fc7 rviz \u8ba2\u9605\/detection\/lidar_detector\/objects_markers \u8bdd\u9898\u53ef\u4ee5\u770b\u5230\u805a\u7c7b\u53ca\u8ddf\u8e2a\u969c\u788d\u7269\u5f97\u5230\u7684\u7ed3\u679c\uff0c\u4e5f\u53ef\u4ee5\u901a\u8fc7\u56fe\u7247\u4e2d\u7684\u6b65\u9aa4\u8ba2\u9605\/bounding_boxes\u8bdd\u9898\u4f7f\u7ed3\u679c\u4ee5\u65b9\u6846\u7684\u5f62\u5f0f\u663e\u793a\u51fa\u6765\u3002<\/p>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/11\/bounding_boxes.png\")
<\/a><\/p>\n<\/h1>\n\u4e09\u3001\u9ad8\u7cbe\u5730\u56fe\u7ed8\u5236<\/h1>\n
\u5728\u5f00\u59cb\u5bfc\u822a\u4e4b\u524d\uff0c\u5355\u9760\u70b9\u4e91\u5730\u56fe\u5df2\u7ecf\u4e0d\u591f\u4e86\uff0c\u6211\u4eec\u5fc5\u987b\u5148\u7ed8\u5236\u77e2\u91cf\u5730\u56fe\u518d\u7ee7\u7eed\u540e\u7eed\u7684\u5bfc\u822a\u73af\u8282\u3002<\/p>\n
\u5177\u4f53\u53c2\u8003\uff1a\u4f7f\u7528Unity+MapToolBox\u7ed8\u5236VectorMap\u5730\u56fe<\/a><\/p>\n <\/p>\n
\u56db\u3001open_planner\u5bfc\u822a<\/h1>\n
OpenPlanner\u662f\u540d\u53e4\u5c4b\u5927\u5b66\u5f00\u53d1\u7684\u5f00\u6e90\u81ea\u52a8\u9a7e\u9a76\u6846\u67b6Autoware\u4e2d\u4f7f\u7528\u7684\u8fd0\u52a8\u89c4\u5212\u7b97\u6cd5\u3002\u5b83\u662f\u901a\u8fc7\u5bf9\u5168\u5c40\u8def\u5f84\u8fdb\u884c\u91c7\u6837\u751f\u6210\u4e00\u7cfb\u5217\u7684\u5019\u9009\u8def\u5f84\uff0c\u7ed3\u5408\u77e2\u91cf\u5730\u56fe\u3001\u4f20\u611f\u5668\u4fe1\u606f\u4ee5\u53ca\u78b0\u649e\u3001\u4ea4\u901a\u89c4\u5219\u7b49\u7ea6\u675f\u548c\u4f18\u5316\u76ee\u6807\uff0c\u9009\u62e9\u51fa\u6700\u4f18\u7684\u8fd0\u52a8\u8f68\u8ff9\u3002OpenPlanner\u901a\u7528\u6027\u5f3a\uff0c\u53ea\u9700\u76f8\u5e94\u7684\u8c03\u6574\u53c2\u6570\u5373\u53ef\u4e0e\u4efb\u4f55\u7684\u79fb\u52a8\u673a\u5668\u4eba\u914d\u5408\u4f7f\u7528\u3002<\/p>\n
\u56fe\u662fOpenPlanner\u7684\u4e00\u822c\u4f53\u7cfb\u7ed3\u6784\uff0c\u5176\u4e3b\u8981\u4e09\u4e2a\u7ec4\u6210\u90e8\u5206\u662f\u5168\u5c40\u89c4\u5212\u5668\u3001\u884c\u4e3a\u72b6\u6001\u751f\u6210\u5668\u548c\u5c40\u90e8\u89c4\u5212\u5668\u3002<\/p>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/12\/OpenPlanner.png\")
<\/a><\/p>\nOpenPlanner\u65e8\u5728\u7528\u4e8e\u79fb\u52a8\u673a\u5668\u4eba\u7684\u81ea\u4e3b\u5bfc\u822a\uff0c\u66f4\u786e\u5207\u5730\u8bf4\u662f\u5e94\u7528\u4e8e\u81ea\u52a8\u9a7e\u9a76\u9886\u57df\u3002\u5b83\u7ed3\u5408\u4e86\u77e2\u91cf\u5730\u56fe\u6216\u9053\u8def\u7f51\u7edc\u5730\u56fe\u4e2d\u5305\u542b\u7684\u6240\u6709\u79bb\u6563\u4fe1\u606f\uff08\u4f8b\u5982\u4ea4\u901a\u4fe1\u53f7\u706f\uff0c\u4ea4\u901a\u6807\u5fd7\uff0c\u5341\u5b57\u8def\u53e3\uff0c\u505c\u8f66\u7ebf\u7b49\u7684\u4f4d\u7f6e\uff09\uff0c\u8fd9\u76f8\u5bf9\u4e8e\u5176\u4ed6\u7684\u5f00\u6e90\u7684\u5bfc\u822a\u7cfb\u7edfOMPL\u548cNavigation Stack\u6765\u8bf4\u662f\u4e3b\u8981\u7684\u4f18\u52bf\u3002 \u4f7f\u7528\u77e2\u91cf\u5730\u56fe\u53ef\u4ee5\u66f4\u52a0\u8f7b\u677e\u3001\u5feb\u901f\u5730\u8fdb\u884c\u5168\u5c40\u548c\u5c40\u90e8\u89c4\u5212\u3002\u5f53\u7136\uff0c\u50cfRRT * \u548c Hybrid A *\u8fd9\u4e9b\u81ea\u7531\u7a7a\u95f4\u4e2d\u5168\u5c40\u89c4\u5212\u5668\u5bf9\u4e8e\u505c\u8f66\u548c\u8d8a\u91ce\u60c5\u51b5\u4e5f\u662f\u5f88\u91cd\u8981\u7684\u3002\u5728\u8fd9\u79cd\u60c5\u51b5\u4e0b\uff0c\u6211\u4eec\u53ef\u4ee5\u5207\u6362\u5230\u81ea\u7531\u7a7a\u95f4\u89c4\u5212\u5668\u8fdb\u884c\u5168\u5c40\u89c4\u5212\uff0c\u4ecd\u9700\u8981\u4f7f\u7528Open Planner\u7684\u884c\u4e3a\u72b6\u6001\u673a\u548c\u672c\u5730\u89c4\u5212\u5668\u3002<\/p>\n
OpenPlanner\u9002\u5408\u4e8e\u9075\u5b88\u4ea4\u901a\u89c4\u5219\u7684\u81ea\u4e3b\u79fb\u52a8\u673a\u5668\u4eba\u5bfc\u822a\u7cfb\u7edf\uff0c\u5b83\u4ec5\u9700\u8981\u77e2\u91cf\u5730\u56fe\u548c\u76ee\u6807\u4f4d\u7f6e\u5373\u53ef\u8fdb\u884c\u5168\u5c40\u89c4\u5212\uff0c\u800c\u5bf9\u4e8e\u5c40\u90e8\u89c4\u5212\u548c\u884c\u4e3a\u72b6\u6001\u751f\u6210\uff0c\u5219\u4ec5\u9700\u8981\u5f53\u524d\u4f4d\u7f6e\u548c\u68c0\u6d4b\u5230\u7684\u969c\u788d\u5373\u53ef\u3002<\/p>\n
1. \u5b8c\u6210\u96f7\u8fbe\u76ee\u6807\u8bc6\u522b<\/h4>\n
\u5b8c\u6210\u4e0a\u4e00\u6b65\u96f7\u8fbe\u76ee\u6807\u8bc6\u522b\u6b65\u9aa4\u3002<\/p>\n
2. \u52a0\u8f7d\u9ad8\u7cbe\u5730\u56fe<\/h4>\n
\u9ad8\u7cbe\u5730\u56fe\u5730\u56fe\u5177\u4f53\u5236\u4f5c\u65b9\u6cd5\u53c2\u8003\u7b2c 3 \u7ae0\u5185\u5bb9\u3002<\/p>\n
\u6253\u5f00[map]\u9875\u9762\uff0c\u70b9\u51fb[vector map]\u7684[ref]\uff0c\u9009\u4e2d\u5168\u90e8\u7684\u9ad8\u7cbe\u5730\u56fe\u6587\u4ef6\uff0c\u70b9\u51fb[vector map]\u52a0\u8f7d\u9ad8\u7cbe\u5730\u56fe\uff0c\u6b64\u65f6\u53ef\u4ee5\u5728 rviz \u4e2d\u770b\u5230\u6211\u4eec\u753b\u7684\u9ad8\u7cbe\u5730\u56fe\u3002<\/p>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/11\/load_vector_map-1024x436.png\")
<\/a><\/p>\n3. \u5168\u5c40\u8def\u5f84\u89c4\u5212<\/h4>\n
\u6253\u5f00[Computing]\u9875\u9762\uff0c\u627e\u5230[openplanner_global planning]\u4e0b\u7684[op_global_planner]\u9009\u9879\uff0c\u8bbe\u7f6e[app]\u7684\u4e00\u4e9b\u53c2\u6570\u540e\uff0c\u5e76\u52fe\u9009\u3002\u7136\u540e\u5728 rviz \u4e2d\u70b9\u51fb[2Dnav goal]\uff0c\u9009\u62e9\u4f60\u6240\u8981\u5bfc\u822a\u7684\u76ee\u6807\u70b9\u4f4d\u7f6e\uff08\u5c0f\u8f66\u8d77\u70b9\u4f4d\u7f6e\u548c\u8bbe\u5b9a\u7684\u76ee\u6807\u70b9\u4f4d\u7f6e\u9700\u8981\u653e\u5728\u884c\u9a76\u7ebf lane \u9644\u8fd1\uff09rviz \u4e2d\u4f1a\u51fa\u73b0\u4e00\u6761\u84dd\u8272\u8def\u5f84\u5373\u4e3a\u5168\u5c40\u8def\u7ebf\u3002<\/p>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/12\/op_global_planner-1024x541.png\")
<\/a><\/p>\n![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/12\/op_global_planner-2.png\")
<\/a><\/p>\n\n- Replanning\uff1a\u7528\u4e8e\u5230\u8fbe\u76ee\u6807\u70b9\u540e\uff0c\u53ef\u4ee5\u91cd\u65b0\u89c4\u5212\u5230\u4e0b\u4e00\u4e2a\u76ee\u6807\u70b9\u3002\u5982\u679c\u6709\u4e86\u4e24\u4e2a\u76ee\u6807\u70b9\u4f1a\u5faa\u73af<\/li>\n
- Smooth\uff1a\u7528\u4e8e\u5e73\u6ed1\u5168\u5c40\u8def\u5f84<\/li>\n
- Path density\uff1a\u7531\u6bcf\u4e00\u4e2a\u5c0f\u70b9\u7ec4\u5408\u6210\u5168\u5c40\u8def\u5f84\uff0c\u6b64\u53c2\u6570\u4e3a\u6bcf\u4e00\u4e2a\u70b9\u7684\u95f4\u9694\u8ddd\u79bb\u3002<\/li>\n<\/ul>\n
4. \u5c40\u90e8\u8def\u5f84\u89c4\u5212<\/h4>\n
\u6253\u5f00[Computing]\u9875\u9762\uff0c\u627e\u5230[openplanner_local planning]\u680f\u76ee\uff0c\u8bbe\u7f6e[app]\u7684\u4e00\u4e9b\u53c2\u6570\u540e\uff0c\u9700\u8981\u4f9d\u6b21\u52fe\u9009[openplanner_local planning]\u680f\u76ee\u4e0b\u7684\u5168\u90e8\u8282\u70b9\u3002\u53ef\u4ee5\u5728rviz \u4e2d\u70b9\u51fb[add]\uff0c\u6dfb\u52a0[local trajectories_evel_rviz]\u8bdd\u9898\uff0c rviz \u4e2d\u4f1a\u51fa\u73b0\u51e0\u6761\u8def\u5f84\u5373\u4e3a\u53ef\u884c\u9a76\u7684\u51e0\u6761\u5c40\u90e8\u8def\u7ebf\uff0c\u7ea2\u8272\u4e3a\u4e0d\u53ef\u884c\u9a76\u7684\u8def\u7ebf\uff0c\u7c89\u7ea2\u8272\u4e3a\u5f53\u524d\u9009\u62e9\u884c\u9a76\u7684\u5c40\u90e8\u8def\u7ebf\uff0c\u9ec4\u8272\u4e3a\u53ef\u884c\u9a76\u7684\u8def\u7ebf\u3002\u76f8\u5173\u4e00\u4e9b\u53c2\u6570\u8bbe\u7f6e\u5982\u4e0b\u56fe\uff1a<\/p>\n
\u5c40\u90e8\u8def\u5f84\u89c4\u5212\u52fe\u9009\u8282\u70b9\uff1a<\/p>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/12\/openplanner_local-1024x598.png\")
<\/a><\/p>\nop_commom_params \u8282\u70b9\u76f8\u5173\u53c2\u6570\uff1a<\/p>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/12\/op_commom_params.png\")
<\/a><\/p>\n\n- Path distance\uff1a\u7528\u4e8e\u8bbe\u7f6e\u4e3b\u8def\u5f84\u4e24\u4fa7\u7684\u884d\u751f\u51fa\u7684\u5c40\u90e8\u8f68\u8ff9\u7684\u957f\u5ea6<\/li>\n
- Path Density \u7528\u4e8e\u8bbe\u7f6e\u4e00\u6761\u5c40\u90e8\u8f68\u8ff9\u4e0a\u4e24\u4e2a\u8f68\u8ff9\u70b9\u7684\u8ddd\u79bb<\/li>\n
- Horizontal Density\uff1a\u7528\u4e8e\u8bbe\u7f6e\u4e24\u4e2a\u5c40\u90e8\u8f68\u8ff9\u7684\u95f4\u8ddd<\/li>\n
- Rollouts Number\uff1a\u7528\u4e8e\u8bbe\u7f6e\u5c40\u90e8\u8f68\u8ff9\u7684\u6570\u91cf<\/li>\n
- Max velocity\uff1a\u5c0f\u8f66\u7684\u884c\u9a76\u901f\u5ea6\uff0c\u4e0d\u5728\u4e2d\u5fc3\u8f68\u8ff9\u4e0a\u7684\u5c0f\u8f66\u884c\u9a76\u901f\u5ea6\u51cf\u534a\uff0c\u5355\u4f4d m\/s<\/li>\n
- Accelration\/deceleration\uff1a\u52a0\u901f\u5ea6\/\u51cf\u52a0\u901f\u5ea6<\/li>\n
- Avoiding distance\uff1a\u8ddd\u79bb\u969c\u788d\u7269\u591a\u8fdc\u624d\u5f00\u59cb\u907f\u969c<\/li>\n
- Avoidance Limit\uff1a\u505c\u8f66\u65f6\uff0c\u969c\u788d\u7269\u8ddd\u79bb\u591a\u8fdc\u53ef\u4ee5\u5f00\u8f66<\/li>\n
- Lateral Safety \u4e0e Longitudinal Safty\uff1a\u8bbe\u7f6e\u5b89\u5168\u6846\u5c3a\u5bf8\uff0c\u5206\u522b\u4ee3\u8868\u8f66\u8f86\u5b89\u5168\u6846\u5bbd\u4e0e\u957f\uff0c\u969c\u788d\u7269\u5728\u5c0f\u8f66\u5b89\u5168\u6846\u5185\u65f6\uff0c\u5c0f\u8f66\u505c\u6b62\u8fd0\u52a8<\/li>\n
- Enable following \u548c enable avoidance\uff1a\u9700\u8981\u52fe\u9009\u4e0a\uff0c\u6253\u5f00\u8ddf\u968f\u548c\u907f\u969c\u529f\u80fd<\/li>\n<\/ul>\n
op_trajectory_generator \u76f8\u5173\u53c2\u6570\uff1a<\/p>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/12\/op_trajectory_generator.png\")
<\/a><\/p>\nTip Margin \u4e0e Roll In Margin \u4ee3\u8868\u610f\u4e49\uff1a<\/p>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/12\/cartip-rollin.png\")
<\/a><\/p>\n\n- Tip Margin\uff1a\u7528\u4e8e\u8bbe\u7f6e\u4ece\u8f66\u8f86\u4e2d\u5fc3\u5230\u5f00\u59cb\u8def\u5f84\u5206\u53c9\u70b9\u6a2a\u5411\u91c7\u6837\u70b9\u7684\u8ddd\u79bb\uff0c\u5176\u957f\u5ea6\u51b3\u5b9a\u4e86\u8f66\u8f86\u5207\u6362\u5230\u4e0d\u540c\u8f68\u8ff9\u7684\u5e73\u6ed1\u5ea6\u3002\uff08\u5efa\u8bae\u8bbe\u7f6e\u4e3a\u8f66\u8f86\u4e2d\u5fc3\u5230\u8f66\u8f86\u6700\u524d\u65b9\u7684\u8ddd\u79bb+\u8f6e\u5b50\u8f6c\u5f2f\u534a\u5f84\u53ef\u4ee5\u6ee1\u8db3\u8def\u7ebf\u5207\u6362\u7684\u8ddd\u79bb\uff0c\u7b80\u5355\u7c97\u66b4\u70b9\u5c31\u76f4\u63a5\u8bbe\u7f6e\u4e3a\u8f66\u8f86\u957f\u5ea6*0.8\uff0c\u6211\u8fd9\u91cc\u8bbe\u7f6e\u7684\u8f66\u957f0.45*0.8=0.36<\/span>\uff09<\/li>\n
- Roll In Margin\uff1a\u6c34\u5e73\u6a2a\u5411\u91c7\u6837\u70b9\u5230\u5e73\u884c\u6a2a\u5411\u91c7\u6837\u70b9\u7684\u8ddd\u79bb\uff0c\u5176\u957f\u5ea6\u4e0e\u8f66\u8f86\u901f\u5ea6\u6210\u6b63\u6bd4\u3002\u8f66\u8f86\u884c\u9a76\u7684\u901f\u5ea6\u8d8a\u5feb\uff0c\u6b64\u533a\u57df\u4ea7\u751f\u5e73\u6ed1\u53d8\u5316\u7684\u65f6\u95f4\u5c31\u8d8a\u957f\u3002\uff08\u7531\u4e8e\u9ed8\u8ba4\u901f\u5ea6\u56e0\u5b50\u4e3a0.25\uff0c\u8fd9\u91cc\u53ef\u4ee5\u8bbe\u7f6e\u4e3aTipMargin\u76844\u500d\uff0c\u6211\u8fd9\u91cc\u8bbe\u7f6e\u76841.44<\/span>\u3002\u6ce8\u610f\u5b83\u5bf9\u5e94\u5230\u53c2\u6570\u91cc\u662fsamplingOutMargin\uff09<\/li>\n
- Roll out\uff1a\u5e73\u884c\u6a2a\u5411\u91c7\u6837\u70b9\u5230\u6700\u5927\u89c4\u5212\u8ddd\u79bb\uff0c\u901a\u8fc7\u4ece\u5168\u5c40\u8def\u5f84\u5782\u76f4\u79fb\u52a8\u56fa\u5b9a\u7684\u8ddd\u79bb\uff08\u79f0\u4e3aRoll out\u5bc6\u5ea6\uff09\u6765\u6267\u884c\u76f4\u63a5\u6a2a\u5411\u91c7\u6837\u3002<\/span><\/li>\n<\/ul>\n
op_motion_predictor \u53c2\u6570<\/p>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/12\/op_motion_predictor.png\")
<\/a><\/p>\n\n- Detect curbs from map\uff1a\u7528\u4e8e\u5c06\u8def\u6cbf\u5224\u5b9a\u4e3a\u969c\u788d\u7269<\/li>\n<\/ul>\n
op_trajectory_evaluator \u76f8\u5173\u53c2\u6570\uff1a<\/p>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/12\/op_trajectory_evaluator.png\")
<\/a><\/p>\n\n- Enable prediction\uff1a\u6253\u5f00\u9884\u6d4b\u529f\u80fd<\/li>\n<\/ul>\n
op_behavior_selector\u884c\u4e3a\u9009\u62e9\u5668\u72b6\u6001\u673a\uff1a\uff08\u72b6\u6001\u673a\u8f93\u51fatopic\uff1a\/current_behavior\uff09<\/p>\n
\n- \u505c\u6b62\u72b6\u6001:[#\u505c\u6b62\u72b6\u6001]<\/li>\n
- \u4efb\u52a1\u5b8c\u6210\u72b6\u6001:[#\u4efb\u52a1\u5b8c\u6210\u72b6\u6001]<\/li>\n
- \u8f6c\u5411\u72b6\u6001:[*\u524d\u8fdb\u72b6\u6001,#\u8f6c\u5411\u72b6\u6001]<\/li>\n
- \u505c\u6b62\u4fe1\u53f7\u505c\u6b62\u72b6\u6001:[*\u505c\u6b62\u4fe1\u53f7\u7b49\u5f85\u72b6\u6001,#\u505c\u6b62\u4fe1\u53f7\u505c\u6b62\u72b6\u6001]<\/li>\n
- \u524d\u8fdb\u72b6\u6001 :[*\u76ee\u6807\u72b6\u6001, #\u524d\u8fdb\u72b6\u6001, \u8f6c\u5411\u72b6\u6001, \u505c\u6b62\u4fe1\u53f7\u505c\u6b62\u72b6\u6001, \u4ea4\u901a\u706f\u505c\u6b62\u72b6\u6001, \u8ddf\u968f\u72b6\u6001, ]<\/li>\n
- \u76ee\u6807\u72b6\u6001 :[*\u4efb\u52a1\u5b8c\u6210\u72b6\u6001,#\u76ee\u6807\u72b6\u6001, \u524d\u8fdb\u72b6\u6001]<\/li>\n
- \u505c\u6b62\u4fe1\u53f7\u7b49\u5f85\u72b6\u6001:[*\u524d\u8fdb\u72b6\u6001,#\u505c\u6b62\u4fe1\u53f7\u7b49\u5f85\u72b6\u6001,\u505c\u6b62\u4fe1\u53f7\u505c\u6b62\u72b6\u6001, \u76ee\u6807\u72b6\u6001]<\/li>\n
- \u4ea4\u901a\u706f\u505c\u6b62\u72b6\u6001:[*\u524d\u8fdb\u72b6\u6001, #\u4ea4\u901a\u706f\u505c\u6b62\u72b6\u6001, \u4ea4\u901a\u706f\u7b49\u5f85\u72b6\u6001]<\/li>\n
- \u4ea4\u901a\u706f\u7b49\u5f85\u72b6\u6001:[*\u524d\u8fdb\u72b6\u6001, #\u4ea4\u901a\u706f\u7b49\u5f85\u72b6\u6001, \u4ea4\u901a\u706f\u505c\u6b62\u72b6\u6001, \u76ee\u6807\u72b6\u6001]<\/li>\n
- \u8ddf\u968f\u72b6\u6001:[*\u524d\u8fdb\u72b6\u6001, #\u8ddf\u968f\u72b6\u6001, \u8f6c\u5411\u72b6\u6001, \u505c\u6b62\u4fe1\u53f7\u505c\u6b62\u72b6\u6001, \u4ea4\u901a\u706f\u505c\u6b62\u72b6\u6001, \u76ee\u6807\u72b6\u6001]<\/li>\n
- \u521d\u59cb\u72b6\u6001:[*\u524d\u8fdb\u72b6\u6001, #\u521d\u59cb\u72b6\u6001]<\/li>\n<\/ul>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/12\/op_behavior_selector.png\")
<\/a><\/p>\n5. \u901f\u5ea6\u8f93\u51fa\uff0c\u5c0f\u8f66\u5f00\u59cb\u5bfc\u822a<\/h4>\n
\u6253\u5f00[Computing]\u9875\u9762\uff0c\u627e\u5230 [waypoint_follower] \u4e0b [pure_pursuit] \u4e0e[twist_filter]\uff0c\u8bbe\u7f6e[app]\u7684\u4e00\u4e9b\u53c2\u6570\u540e\uff0c\u52fe\u9009 [pure_pursuit] \u4e0e [twist_filter] \u3002\u76f8\u5173\u4e00\u4e9b\u53c2\u6570\u8bbe\u7f6e\u5982\u4e0b\u56fe\uff1a<\/p>\n
\u901f\u5ea6\u8f93\u51fa\u52fe\u9009\u8282\u70b9\uff1a<\/p>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/12\/waypoint_follower.png\")
<\/a><\/p>\n5.1 pure_pursuit \u5de1\u8ff9\u8282\u70b9<\/h6>\n
\u4e3b\u8981\u4f5c\u7528\u662f\u63a5\u6536op_behavior_select\u8282\u70b9\u53d1\u5e03\u7684\u201c\/final_waypoints\u201d\u76ee\u6807\u8def\u5f84\uff0c\u6839\u636e\u7eaf\u8ddf\u8e2a\u7b97\u6cd5\uff0c\u8ba1\u7b97\u81ea\u8f66\u4e0b\u4e00\u65f6\u523b\u7684\u901f\u5ea6\u3001\u89d2\u901f\u5ea6\u6307\u4ee4\uff0c\u5e76\u53d1\u5e03\u5230\u201c\/twist_raw\u201dx\u8f74\u7ebf\u901f\u5ea6\u548cz\u8f74\u89d2\u901f\u5ea6\u3001”\/ctrl_raw”\u52a0\u901f\u5ea6\u4ee5\u53ca\u8f6c\u89d2topic\uff1a<\/p>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/12\/pure_pursult-1024x181.png\")
<\/a><\/p>\n\u76f8\u5173\u53c2\u6570\u5982\u4e0b\uff1a<\/p>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/12\/pure_pursuit.png\")
<\/a><\/p>\n\n- \u52fe\u9009 waypoint \u9009\u9879\uff0c\u4f7f\u7528\u8def\u5f84\u70b9\u7684\u901f\u5ea6\u8fdb\u884c\u8f93\u51fa<\/li>\n
- Lookahead_ratio\uff1a\u524d\u89c6\u8ddd\u79bb\u7cfb\u6570<\/li>\n
- Minimum_lookahead_distance\uff1a\u6700\u5c0f\u524d\u89c6\u8ddd\u79bb\u9650\u5236\uff0c\u8fd9\u91cc\u8bbe\u7f6e\u4e3a\u5c0f\u8f66\u7684\u6700\u5c0f\u8f6c\u5f2f\u534a\u5f84<\/li>\n
- Linear interpolation\uff1a\u662f\u5426\u7ebf\u6027\u63d2\u503c<\/li>\n
- \u5c0f\u8f66\u7684\u9884\u7784\u70b9\u8ddd\u79bb\u5c0f\u8f66\u7684\u8ddd\u79bb\u662f\u5c0f\u8f66\u7684\u524d\u89c6\u8ddd\u79bb\uff0c\u9884\u7784\u70b9\u662f\u5c0f\u8f66\u5728\u5bfc\u822a\u4e2d\u4e00\u76f4\u5728\u201c\u8ffd\u201d\u7684\u70b9\uff0c\u5f53\u524d\u884c\u9a76\u901f\u5ea6\u4e58\u4ee5\u7cfb\u6570 Lookahead_ratio \u5373\u4e3a\u5c0f\u8f66\u7684\u524d\u89c6\u8ddd\u79bb\u3002<\/li>\n<\/ul>\n
5.2 twist_filter\u5e73\u6ed1\u8282\u70b9<\/h6>\n
\u4e3b\u8981\u529f\u80fd\u662f\u5bf9pure_suit\u8282\u70b9\u8f93\u51fa\u7684\u6c7d\u8f66\u8fd0\u52a8\u901f\u5ea6\u8fdb\u884c\u4f4e\u901a\u6ee4\u6ce2\uff0c\u6d88\u9664\u6742\u6ce2\uff0c\u4f7f\u901f\u5ea6\u66f4\u52a0\u5e73\u6ed1\u3002<\/p>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/12\/twist_filter-1-1024x314.png\")
<\/a><\/p>\n\u53c2\u6570\u5982\u4e0b\uff1a<\/p>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/12\/twist_filter.png\")
<\/a><\/p>\n\n- Lateral_accel_limit \u548c Lateral_jerk_limit\uff1a\u6a2a\u5411\u52a0\u51cf\u901f\u9650\u5236\uff0c\u9ed8\u8ba4\u5373\u53ef<\/li>\n<\/ul>\n
5.3 twist_gate \u63a7\u8f66\u8282\u70b9<\/h6>\n
\u8fd9\u91cc\u5355\u72ec\u8bf4\u4e0b\u8fd9\u4e2a\u8282\u70b9\uff0c\u5b83\u5176\u5b9e\u662f\u5728twist_filter\u5e73\u6ed1\u6ee4\u6ce2\u8282\u70b9\u542f\u52a8\u7684\uff0c\u4f46\u5b83\u5b9e\u9645\u8d1f\u8d23\u4e86\u8ddf\u5e95\u76d8\u7684\u63a7\u8f66\u901a\u4fe1\u3002\u5b83\u901a\u8fc7\u8ba2\u9605\u5e73\u6ed1\u540e\u7684\u63a7\u8f66\/twist_cmd\u3001\/ctrl_cmd topic\uff0c\u5c06\u6700\u7ec8\u7684\u63a7\u8f66\u6d88\u606f\u901a\u8fc7timerCallback\u53d1\u9001\u5230\u4e86\/vehicle_cmd\u3002<\/p>\n
*\u6ce8\uff1a\u82e5\u662f\u5bfc\u822a\u4e2d\u5c0f\u8f66\u5b9a\u4f4d\u5bb9\u6613\u4e71\uff0c\u8bf7\u8fd0\u884c rostopic hz \/points_raw \u89c2\u5bdf\u5c0f\u8f66\u96f7\u8fbe\u9891\u7387\u591a\u5c11\uff0c\u63a8\u8350\u96f7\u8fbe\u9891\u7387\u4e3a 20hz \u5de6\u53f3\uff0c\u592a\u5c0f\u5982 10hz \u5c0f\u8f66\u8fd0\u52a8\u65f6\u5bb9\u6613\u5b9a\u4f4d\u504f\u79fb\u3002<\/p>\n
<\/h6>\n
<\/p>\n
\u4e94\u3001\u7ed5\u969c<\/h1>\n
\u6309\u7167\u4e0a\u8ff0\u65b9\u6cd5\u5bfc\u822a\u65f6\uff0c\u9047\u5230\u969c\u788d\u7269\u4f1a\u505c\u6b62\u4e0b\u6765\uff0c\u4f46\u662f\u4e0d\u4f1a\u7ed5\u8fc7\u53bb\uff0c\u9700\u8981\u8c03\u6574\u4e9b\u5730\u65b9\u624d\u53ef\u4ee5\u505a\u5230<\/p>\n
1. \u969c\u788d\u7269\u8ddf\u8e2a\u4e0e\u805a\u7c7b\u7684\u5173\u8054<\/h4>\n# \u8ba2\u9605\u7684cloud_clusters topic\u540d\u9700\u8981\u4e0e\u805a\u7c7btopic\u5bf9\u5e94\u4e0a\r\nvim Autoware\/src\/autoware\/core_perception\/lidar_kf_contour_track\/nodes\/lidar_kf_contour_track\/lidar_kf_contour_track_core.cpp\r\n sub_cloud_clusters = nh.subscribe(\"\/detection\/lidar_detector\/cloud_clusters<\/span>\", 1, &ContourTracker::callbackGetCloudClusters, this);\r\n\r\n# \u6b27\u51e0\u91cc\u5f97\u805a\u7c7b\u8f93\u51fa\u7684topic\r\nvim Autoware\/src\/autoware\/core_perception\/lidar_euclidean_cluster_detect\/nodes\/lidar_euclidean_cluster_detect\/lidar_euclidean_cluster_detect.cpp\r\n _pub_clusters_message = h.advertise<autoware_msgs::CloudClusterArray>(\"\/detection\/lidar_detector\/cloud_clusters<\/span>\", 1);\r\n\r\n# \u91cd\u65b0\u7f16\u8bd1\r\ncolcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select=lidar_kf_contour_track\r\n\r\n# \u68c0\u67e5\u8282\u70b9\u8ba2\u9605\u5173\u7cfb\r\nrosrun rqt_graph rqt_graph<\/code><\/pre>\n\u786e\u4fddlidar_kf_contour_track\u8ddf\u8e2a\u8282\u70b9\u8ba2\u9605\u4e86lidar_euclidean_cluster_detect\u805a\u7c7b\u8282\u70b9\u8f93\u51fa\u7684\/detection\/lidar_detector\/cloud_clusters topic\uff1a<\/p>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/12\/rosgraph-1-1024x425.png\")
<\/a><\/p>\n2. \u8c03\u6574\u53d1\u5e03frame<\/h4>\n
\u6b64\u65f6\u969c\u788d\u7269\u53ef\u4ee5\u88abop_planer\u8bc6\u522b\u548c\u8ddf\u8e2a\uff0c\u4f46\u662f\u663e\u793a\u7684\u4f4d\u7f6e\u5728\u5730\u56fe\u5750\u6807\u539f\u70b9\uff0c\u539f\u56e0\u662f\u4ece\u6e90\u5934\u4e0alidar\u68c0\u6d4b\u7684\u969c\u788d\u7269\u5c31\u662f\u57fa\u4e8e\u96f7\u8fbe\u5750\u6807\u7cfb\u7684\uff0c\u9700\u8981\u8c03\u6574\u4e3amap\u5750\u6807\u7cfb\uff0c\u5e76\u5c06\u805a\u7c7b\u7ed3\u679c\u8fdb\u884c\u5750\u6807\u7cfb\u7684\u540c\u6b65<\/p>\n
# \u68c0\u67e5\u805a\u7c7b\u7684frame\r\nrostopic echo \/detection\/lidar_detector\/cloud_clusters |grep frame_id\r\n frame_id: \"map<\/span>\"\r\n\r\n# \u68c0\u67e5\u969c\u788d\u7269\u8ddf\u8e2a\u7684frame\r\nrostopic echo \/tracked_objects |grep frame_id\r\n frame_id: \"map<\/span>\"\r\n\r\n# \u4fee\u6539\u969c\u788d\u7269\u8ddf\u8e2a\u7684frame\r\nvim Autoware\/src\/autolaunch\/launch\/detection.launch\r\n <arg name=\"output_frame\" default=\"map<\/span>\" \/>\r\n <arg name=\"sync\" default=\"true<\/span>\" \/> <!-- \u5c06\u805a\u7c7b\u7ed3\u679c\u4ecevelodyne\u5750\u6807\u7cfb\u540c\u6b65\u5230map\u5750\u6807\u7cfb -->\r\n\r\n# \u4fee\u6539\u5f71\u54cd\u907f\u969c\u7684\u51e0\u4e2a\u53c2\u6570\r\nvim Autoware\/src\/autolaunch\/launch\/planner.launch\r\n <include file=\"$(find op_local_planner)\/launch\/op_common_params.launch\">\r\n <arg name=\"horizontalSafetyDistance\" default=\"0.1\" \/> <!-- \u8f66\u8f86\u5b89\u5168\u6846-\u6a2a\u5411 -->\r\n <arg name=\"verticalSafetyDistance\" default=\"0.3\" \/> <!-- \u8f66\u8f86\u5b89\u5168\u6846-\u7eb5\u5411 -->\r\n <arg name=\"width\" default=\"0.35\" \/> <!-- \u8f66\u5bbd -->\r\n <arg name=\"length\" default=\"0.45\" \/> <!-- \u8f66\u957f -->\r\n <arg name=\"wheelBaseLength\" default=\"0.322\" \/> <!-- \u8f66\u8f86\u8f74\u8ddd -->\r\n <arg name=\"turningRadius\" default=\"0.75\" \/> <!-- \u8f66\u8f86\u8f6c\u5f2f\u534a\u5f84 -->\r\n \r\n <arg name=\"enableSwerving\" default=\"true\" \/> <!-- \u6253\u5f00\u907f\u969c\u529f\u80fd -->\r\n <arg name=\"enableFollowing\" default=\"true\" \/> <!-- \u6253\u5f00\u8ddf\u968f\u529f\u80fd -->\r\n <arg name=\"minFollowingDistance\" default=\"3.0\" \/> <!-- \u9700\u8981\u5927\u4e8e\u8ddf\u968f\u8ddd\u79bb -->\r\n <arg name=\"minDistanceToAvoid\" default=\"2.0\" \/> <!-- \u8ddd\u79bb\u969c\u788d\u7269\u591a\u8fdc\u624d\u5f00\u59cb\u907f\u969c\uff0c\u9700\u8981 <minFollowingDistance \u5e76\u4e14 >maxDistanceToAvoid -->\r\n <arg name=\"maxDistanceToAvoid\" default=\"0.8\" \/> <!-- \u505c\u8f66\u65f6\uff0c\u969c\u788d\u7269\u8ddd\u79bb\u591a\u8fdc\u53ef\u4ee5\u5f00\u8f66 -->\r\n <\/include>\r\n <include file=\"$(find op_local_planner)\/launch\/op_trajectory_evaluator.launch\">\r\n <arg name=\"enablePrediction\" default=\"true\" \/> <!-- \u6253\u5f00\u9884\u6d4b\u529f\u80fd -->\r\n <arg name=\"horizontalSafetyDistance\" default=\"0.1\" \/>\r\n <arg name=\"verticalSafetyDistance\" default=\"0.3\" \/>\r\n <\/include><\/code><\/pre>\nrviz\u6dfb\u52a0\/detected_polygons\u3001\/op_planner_tracked_boxes topic\u7684\u8f93\u51fa\uff0c\u53ef\u4ee5\u770b\u5230\u969c\u788d\u7269\u88ab\u8ddf\u8e2a\uff0c\u4f4d\u7f6e\u6b63\u5e38\uff0c\u5e76\u4e14\u5f00\u59cb\u5e72\u9884\u5c40\u90e8\u8def\u5f84\u89c4\u5212\uff1a<\/p>\n
\n- \u7ea2\u8272\uff1a\u4e0d\u53ef\u884c\u9a76\u7684\u8def\u7ebf<\/li>\n
- \u9ec4\u8272\uff1a\u53ef\u884c\u9a76\u7684\u8def\u7ebf<\/li>\n
- \u7d2b\u8272\uff1a\u4e3a\u5f53\u524d\u9009\u62e9\u884c\u9a76\u7684\u5c40\u90e8\u8def\u7ebf<\/li>\n<\/ul>\n
<\/a><\/p>\n3. \u5176\u4ed6\u7ed5\u969c\u6548\u679c\u4f18\u5316<\/h4>\n
\u5b9e\u8f66\u6d4b\u8bd5\uff0c\u53ef\u4ee5\u907f\u969c\u548c\u7ed5\u969c\u4e86\uff0c\u4f46\u662f\u8fd8\u6709\u51e0\u4e2a\u95ee\u9898\u9700\u8981\u89e3\u51b3\uff1a<\/p>\n
\n- \u5230\u7ec8\u70b9\u65f6\uff0c\u603b\u662f\u4f1a\u4f7f\u52b2\u9760\u5f80\u8def\u8fb9\u8d70\uff0c\u5373\u4f7f\u6709\u8f66\u6216\u8005\u6811\u4e1b\u4e5f\u4f7f\u52b2\u4e0a\u649e\uff1a\u611f\u89c9\u50cf\u662fop_planer\u5230\u7ec8\u70b9\u65f6\u6709\u7279\u6b8a\u7684\u9760\u8fb9\u505c\u8f66\u7b56\u7565\uff0c\u5e76\u4e14\u6b64\u65f6\u907f\u969c\/\u7ed5\u969c\u662f\u4e0d\u751f\u6548\u7684\uff0c\u9700\u8981\u6392\u67e5\u4e0b\uff1b\n
\n- \u969c\u788d\u7269\u8fd9\u5757\u53ef\u4ee5\u5148\u589e\u52a0\u5b89\u5168\u6846\u6765\u9650\u5236\u8f66\u8f86\u78b0\u649e\uff0c\u5e76\u5f3a\u8feb\u8f66\u8f86\u8d70\u9009\u62e9\u5176\u4ed6\u5c40\u90e8\u8def\u7ebf\u7ed5\u884c<\/li>\n
- \u4fee\u6539\u540e\u6b64\u95ee\u9898\u6d88\u5931\uff0c\u53ef\u80fd\u8ddf\u4e0b\u4e00\u4e2a\u95ee\u9898\u91cc\u7684imu\u504f\u79fb\u6709\u5173<\/li>\n<\/ul>\n<\/li>\n
- \u8f66\u8d70\u7684\u65f6\u5019\uff0c\u4ece\u5de6\u4fa7\u7ed5\u884c\u65f6\uff0c\u559c\u6b22\u8d34\u7740\u969c\u788d\u7269\u8d70\uff0c\u4ece\u53f3\u4fa7\u7ed5\u884c\u65f6\uff0c\u559c\u6b22\u79bb\u969c\u788d\u7269\u5f88\u8fdc\uff1a\u50cf\u662f\u8f66\u81ea\u8eab\u5de6\u53f3\u65b9\u5411\u6709\u70b9\u504f\u4f3c\u7684\uff0c\u9700\u8981\u6392\u67e5\u4e0b\u8f66\u8eab\u548c\u969c\u788d\u7269\u7269\u7684\u504f\u79fb\u5173\u7cfb\u662f\u4e0d\u662f\u54ea\u91cc\u4e0d\u5bf9\n
\n- \u5e94\u8be5\u662fimu\u76f8\u5bf9\u8f66\u540e\u8f74\u4e2d\u5fc3\u7684\u4f4d\u7f6e\u4e0d\u5bf9\uff0c\u8fd9\u4e2a\u8f66imu\u5b89\u88c5\u5728\u5de6\u4fa7\uff0c\u76f8\u5bf9\u8f66\u8eabtf\u8bbe\u7f6e\u4e3a0\uff0c\u90a3\u4e48\u8f66\u8f86\u5c31\u4f1a\u4ee5\u4e3aimu\u7684\u4f4d\u7f6e\u624d\u662f\u8f66\u8f86\u4e2d\u5fc3\uff0c\u4ece\u800c\u5bfc\u81f4\u4e24\u4fa7\u7684\u7ed5\u884c\u8ddd\u79bb\u51fa\u73b0\u5dee\u5f02<\/li>\n
- \u8c03\u6574imu\u4e0ebase_link\u7684tf\uff0c\u53d1\u5e03\u7684tf\u91cc\u5373\u53ef<\/li>\n<\/ul>\n<\/li>\n
- \u79bb\u8f66\u8f86\u5f88\u8fd1\u65f6\u7a81\u7136\u963b\u6321\u8f66\u8f86\uff0c\u8f66\u8f86\u4e0d\u4f1a\u907f\u969c\/\u7ed5\u969c\uff0c\u4f46\u662f\u7ad9\u57283\u7c73\u4ee5\u5916\u5c31\u80fd\u6b63\u5e38\u7ed5\n
\n- \u7cbe\u7ec6\u6d4b\u91cf\u5e76\u8c03\u6574\u4e0b\u8f66\u8f86\u7684\u51c6\u786e\u957f\u5bbd\u3001\u8f74\u8ddd\u3001\u8f6c\u5f2f\u534a\u5f84\u3001\u6700\u5927\u8f6c\u5411\u89d2\u7b49\u8f66\u8f86\u53c2\u6570<\/li>\n
- \u8c03\u6574\u5c40\u90e8\u8def\u5f84\u89c4\u5212tip\/in margin\u3001\u89c4\u5212\u8def\u5f84\u6761\u6570\/\u95f4\u8ddd\/\u8def\u5f84\u70b9\u8ddd\u79bb\u7b49\uff0c\u786e\u4fdd\u8986\u76d6\u53ef\u884c\u8fdb\u7684\u8def\u5bbd\u8db3\u4ee5\u89c4\u5212\u7ed5\u884c\u8def\u7ebf<\/li>\n
- \u8c03\u6574\u907f\u969c\u53c2\u6570\uff0c\u907f\u969c\u8ddd\u79bb\u3001\u5b89\u5168\u6846\u5927\u5c0f<\/li>\n<\/ul>\n<\/li>\n
- \n\u7528gps\u521d\u59cb\u5316\u5b9a\u4f4d\u5bb9\u6613\u98d8\uff0c\u9014\u4e2d\u98d8\u4e0d\u5bb9\u6613\u6062\u590d\u5b9a\u4f4d<\/div>\n
\n- \n\u7ecf\u8fc7\u4ed4\u7ec6\u89c2\u5bdf\uff0c\u5173\u952e\u5728gps\u7684\u822a\u5411\u4e0a\uff0c\u4e00\u65e6gps\u822a\u5411\u51c6\u786e\uff0c\u4fee\u6b63\u6210\u529f\u7684\u6982\u7387\u5c31\u4f1a\u5927\u5927\u589e\u52a0\uff1b<\/div>\n<\/li>\n
- \u521d\u59cb\u5316\u65f6\u589e\u52a0\u65cb\u8f6c\u822a\u5411\u5c1d\u8bd5match\u673a\u5236\uff0c\u6539\u9020\u540e\uff0c\u57fa\u672c\u79d2\u521d\u59cb\u5316\u5b9a\u4f4d\u6210\u529f\uff1b<\/li>\n
- \u884c\u8fdb\u9014\u4e2d\u589e\u52a0\u6a2a\u5411\u5e73\u79fb\u5c1d\u8bd5match\u673a\u5236\uff0c\u6539\u9020\u540e\uff0c\u5b9e\u8f66\u9a8c\u8bc1\u6548\u679c\u5c1a\u53ef\uff1b<\/li>\n
- \u5c3d\u91cf\u907f\u514d\u5728\u4fe1\u53f7\u4e0d\u597d\u65f6\u4e0a\u7535\uff0c\u6b64\u65f6\u8f66\u8f86\u79fb\u52a8\u5230\u4fe1\u53f7\u9010\u6e10\u53d8\u597d\u4f4d\u7f6e\u7684\u8fc7\u7a0b\u4e2d\uff0c\u5b9a\u4f4d\u4f1a\u504f\u79fb\uff0c\u5c3d\u91cf\u5728\u7a7a\u65f7\u4f4d\u7f6e\u91cd\u65b0\u4e0a\u7535<\/span>\uff0c\u8fd9\u6837\u521d\u59cb\u5b9a\u4f4d\u4f4d\u7f6e\u4f1a\u66f4\u52a0\u51c6\u786e\uff1b<\/li>\n
- \u540e\u7eed\u53ef\u8003\u8651<\/span>\u66f4\u6362\u5177\u6709\u51c6\u786e\u822a\u5411<\/span>\u7684imu<\/span>\u4ee5\u65b9\u4fbf\u786e\u5b9a\u521d\u59cb\u822a\u5411\uff1b<\/span>\u589e\u52a0\u6fc0\u5149\u96f7\u8fbe\u7ebf\u6570<\/span>\uff0c\u4ece\u800c\u63d0\u9ad8match\u7279\u5f81\u70b9<\/span>\u3002<\/span><\/li>\n<\/ul>\n<\/li>\n
- \u5c40\u90e8\u89c4\u5212\u7684\u8def\u5f84\uff0c\u96c6\u4e2d\u5728\u72ed\u7a84\u7684\u884c\u9a76\u7ebf\u4e0a\n
\n- \u5c40\u90e8\u89c4\u5212\u8def\u7ebf\u8bbe\u7f6e\u592a\u5c11\uff0c\u6216\u8005\u8def\u7ebf\u95f4\u95f4\u8ddd\u8fc7\u5c0f\uff0c\u8c03\u5927\u8986\u76d6\u53ef\u884c\u8fdb\u7684\u6574\u4e2a\u8def\u5bbd\u5c31\u597d\u4e86<\/li>\n<\/ul>\n<\/li>\n
- \u8def\u5f84\u89c4\u5212\u4f1a\u89c4\u5212\u5230\u975e\u9053\u8def\u4e0a\n
\n- \u4fee\u6539\u5730\u56fe\uff0c\u589e\u52a0\u8def\u6cbf<\/span>\u9650\u5236<\/span><\/li>\n<\/ul>\n<\/li>\n
- \u8fc7\u51cf\u901f\u5e26\u65f6\uff0c\u5bb9\u6613match\u5b9a\u4f4d\u5931\u8d25\n
\n- \u4fee\u6539\u5730\u56fe\uff0c\u589e\u52a0\u51cf\u901f<\/span>\u5e26\u6807\u8bb0<\/span>\uff0c\u964d\u4f4e\u98a0\u7c38<\/li>\n<\/ul>\n<\/li>\n
- \u4e2d\u9014\u5bb9\u6613\u5728\u5b89\u5168\u7684\u60c5\u51b5\u4e0b\u51cf\u901f\u6216\u6682\u505c\n
\n- \u76ee\u524d\u770b\u4ee5\u4e0a\u4fee\u6539\u8c03\u6574\u540e\u5b9e\u8f66\u9a8c\u8bc1\uff0c\u6b64\u95ee\u9898\u57fa\u672c\u6d88\u5931\uff0c\u5e94\u8be5\u8ddfimu tf\u504f\u79fb\u5bfc\u81f4\u969c\u788d\u7269\u8ddd\u79bb\u504f\u79fb\u6709\u5173<\/li>\n<\/ul>\n<\/li>\n<\/ol>\n
<\/p>\n
\u516d\u3001\u591a\u70b9\u8fde\u7eed\u5bfc\u822a<\/h1>\n
\u76ee\u524d\u5b9e\u8f66\u6d4b\u8bd5\u770b\uff0copen planner\u5bfc\u822a\u5e76\u4e0d\u652f\u6301\u8fde\u7eed\u5bfc\u822a\uff0c\u5728\u5bfc\u822a\u5230\u7b2c\u4e00\u4e2a\u76ee\u6807\u70b9\u540e\uff0c\u9700\u8981\u91cd\u65b0\u8fd0\u884c\u5168\u5c40\u8def\u5f84\u89c4\u5212\u3001\u5c40\u90e8\u8def\u5f84\u89c4\u5212\u3001\u4ee5\u53capure persuit\u63a7\u5236\u8282\u70b9\u624d\u80fd\u5bfc\u822a\u5230\u4e0b\u4e00\u4e2a\u70b9\u3002\u6211\u4eec\u8fd9\u91cc\u53ef\u4ee5\u4fee\u6539\u90e8\u5206\u4ee3\u7801\uff0c\u8ba9\u5b83\u80fd\u591f\u5728\u4e0a\u4e00\u5bfc\u822a\u7ed3\u675f\u540e\uff0c\u76f4\u63a5\u8bbe\u5b9a\u5bfc\u822a\u5230\u4e0b\u4e00\u4e2a\u70b9\uff0c\u800c\u4e0d\u9700\u8981\u91cd\u542f\u4efb\u4f55\u8282\u70b9\u3002<\/p>\n
0. op_global_planner_core.cpp\u7684callbackGetGoalPose()<\/h4>\n
\u6253\u5370m_GoalsPos\u76ee\u6807\u70b9\u5217\u8868\u7684\u6dfb\u52a0\u8bb0\u5f55<\/p>\n
void GlobalPlanner::callbackGetGoalPose(const geometry_msgs::PoseStampedConstPtr &msg)\r\n{\r\n PlannerHNS::WayPoint wp = PlannerHNS::WayPoint(msg->pose.position.x+m_OriginPos.position.x, msg->pose.position.y+m_OriginPos.position.y, msg->pose.position.z+m_OriginPos.position.z, tf::getYaw(msg->pose.orientation));\r\n m_GoalsPos.push_back(wp);\r\n ROS_INFO(\"Received Goal Pose<\/span>: GoalsPosSize=%ld NewPose=%f,%f,%f OrignPos=%f,%f,%f\", m_GoalsPos.size(), msg->pose.position.x, msg->pose.position.y, msg->pose.position.z, m_OriginPos.position.x, m_OriginPos.position.y, m_OriginPos.position.z);\r\n}<\/code><\/pre>\n1. op_global_planner_core.cpp\u7684MainLoop()<\/h4>\n
\u63a5\u8fd1\u5f53\u524d\u76ee\u6807\u70b9\u8ddd\u79bb\u65f6\uff0c\u5f00\u59cb\u4e0b\u4e00\u4e2a\u76ee\u6807\u70b9\u7684\u5168\u5c40\u8def\u5f84\u89c4\u5212\uff1a<\/p>\n
void GlobalPlanner::MainLoop()\r\n{\r\n \r\n if(m_GoalsPos.size() > 0)\r\n {\r\n if(m_GeneratedTotalPaths.size() > 0 && m_GeneratedTotalPaths.at(0).size() > 3)\r\n {\r\n \/\/ \u652f\u6301\u5728\u5bfc\u822a\u5230\u7b2c\u4e00\u4e2a\u76ee\u6807\u70b9\u540e\uff0c\u4e0d\u91cd\u542f\u7684\u60c5\u51b5\u4e0b\u652f\u6301\u76f4\u63a5\u5bfc\u822a\u5230\u4e0b\u4e00\u4e2a\u70b9<\/span> yan 24.12.10\r\n if(m_params.bEnableReplanning && m_iCurrentGoalIndex<m_GoalsPos.size()-1)\r\n {\r\n PlannerHNS::RelativeInfo info;\r\n \/\/ \u8f66\u8f86\u662f\u5426\u5728\u5f53\u524d\u76ee\u7684\u5730\u89c4\u5212\u8def\u5f84\u4e0a\uff0c\u5982\u5728\u7684\u8bdd\uff0c\u83b7\u5f97\u6240\u5728link\u4f4d\u7f6e\u4fe1\u606f<\/span>\r\n bool ret = PlannerHNS::PlanningHelpers::GetRelativeInfoRange(m_GeneratedTotalPaths, m_CurrentPose, 0.75, info); \r\n ROS_INFO(\"Goal Pose Enable Replanning Check: GetRelativeInfoRange=%d iGlobalPath=%d m_GeneratedTotalPaths.size=%ld info.iFront=%d m_GeneratedTotalPaths.at(info.iGlobalPath).size=%ld\", ret, info.iGlobalPath, m_GeneratedTotalPaths.size(), info.iFront, m_GeneratedTotalPaths.at(info.iGlobalPath).size());\r\n if(ret == true && info.iGlobalPath >= 0 && info.iGlobalPath < m_GeneratedTotalPaths.size() && info.iFront > 0 && info.iFront < m_GeneratedTotalPaths.at(info.iGlobalPath).size())\r\n {\r\n \/\/ \u8f66\u8f86\u5f53\u524d\u4f4d\u7f6e\u8ddd\u79bb\u5f53\u524d\u76ee\u7684\u5730\u7684\u8ddd\u79bb\uff08<=30\u7c73\u65f6\uff0c\u81ea\u52a8\u5f00\u59cb\u4e0b\u4e00\u76ee\u7684\u5730\u7684\u5168\u5c40\u8def\u5f84\u89c4\u5212\uff09<\/span>\r\n double remaining_distance = m_GeneratedTotalPaths.at(info.iGlobalPath).at(m_GeneratedTotalPaths.at(info.iGlobalPath).size()-1).cost - (m_GeneratedTotalPaths.at(info.iGlobalPath).at(info.iFront).cost + info.to_front_distance);\r\n ROS_INFO(\"Goal Pose Remaining Distance<\/span> %f <= %d \", remaining_distance, REPLANNING_DISTANCE);\r\n if(remaining_distance <= REPLANNING_DISTANCE<\/span>)\r\n {\r\n bMakeNewPlan = true;\r\n if(m_GoalsPos.size() > 0){\r\n ROS_WARN(\"m_iCurrentGoalIndex = %d\", m_iCurrentGoalIndex);\r\n m_iCurrentGoalIndex = (m_iCurrentGoalIndex + 1) % m_GoalsPos.size();\r\n }\r\n ROS_INFO(\"Change Goal Pose<\/span>: Index=%d, GoalsPosSize=%ld \", m_iCurrentGoalIndex, m_GoalsPos.size());\r\n }\r\n }\r\n }\r\n }\r\n \r\n}<\/code><\/pre>\n2. BehaviorStateMachine.cpp\u7684MissionAccomplishedStateII::GetNextState()<\/h4>\nBehaviorStateMachine* MissionAccomplishedStateII::GetNextState()\r\n{\r\n \/\/ \u6ce8\u91ca\u539f\u6709\u4ee3\u7801\r\n \/\/ return FindBehaviorState(this->m_Behavior);\r\n \/\/ \u6539\u6210\u4ee5\u4e0b\u7684\u4ee3\u7801\r\n PreCalculatedConditions* pCParams = GetCalcParams();\r\n if(pCParams->currentGoalID == -1) {\r\n return FindBehaviorState(this->m_Behavior);\r\n } else {\r\n pCParams->prevGoalID = pCParams->currentGoalID;\r\n return FindBehaviorState(FORWARD_STATE);\r\n }\r\n}<\/code><\/pre>\n3. pure_pursuit\u6a21\u5757\u5728final_waypoints\u4e3a\u7a7a\u65f6\u4f1a\u51fa\u5f02\u5e38<\/h4>\n
\u5148\u5728pure_pursuit_core.h\u4e2d\u6dfb\u52a0 ros::Subscriber sub_ndt_stat\u5b9a\u4e49\uff0c\u5e76\u589e\u52a0\u56de\u8c03\u51fd\u6570\u58f0\u660e\uff1a<\/p>\n
private:\r\n ros::Subscriber sub_ndt_stat;\r\n ...\r\n \/\/ callbacks\r\n void loopWaypointsCallback(const autoware_msgs::LaneConstPtr &msg);<\/code><\/pre>\npure_pursuit_core.cpp\u6dfb\u52a0sub_ndt_stat\u7684\u5b9a\u4e49\uff1a<\/p>\n
void PurePursuitNode::initForROS()\r\n{\r\n \/\/ setup subscriber\r\n sub_ndt_stat = nh_.subscribe(\"loop_waypoints\", 10, &PurePursuitNode::loopWaypointsCallback, this); \/\/add\r\n}<\/code><\/pre>\npure_pursuit_core.cpp \u4e2d\u6dfb\u52a0sub_ndt_stat<\/code>\u7684\u56de\u8c03\u51fd\u6570\uff1a<\/p>\nstatic autoware_msgs::Lane loopWaypoints;\r\nvoid PurePursuitNode::loopWaypointsCallback(const autoware_msgs::LaneConstPtr &msg)\r\n{\r\n loopWaypoints = *msg;\r\n}<\/code><\/pre>\npure_pursuit_core.cpp \u4e2d\u4fee\u6539callbackFromWayPoints\u51fd\u6570\uff0c\u6539\u6210\u4ee5\u4e0b\u6837\u5b50\uff1a<\/p>\n
void PurePursuitNode::callbackFromWayPoints(\r\n const autoware_msgs::LaneConstPtr& msg)\r\n{\r\n autoware_msgs::Lane::Ptr msg_output(new autoware_msgs::Lane);\r\n *msg_output = *msg;\r\n if (!loopWaypoints.waypoints.empty())\r\n {\r\n msg_output->waypoints.insert(msg_output->waypoints.end(), loopWaypoints.waypoints.begin(), loopWaypoints.waypoints.end());\r\n loopWaypoints.waypoints.clear();\r\n }\r\n if (msg_output->waypoints.empty()) return; \/\/ \u5982\u679cwaypoints\u4e3a\u7a7a\uff0cpp_\u4f1a\u51fa\u5f02\u5e38 \r\n \r\n if (!msg_output->waypoints.empty())\r\n command_linear_velocity_ = msg_output->waypoints.at(0).twist.twist.linear.x; \/\/ \u4ece\u822a\u70b9\u4e2d\u5f97\u5230\u901f\u5ea6\r\n else\r\n command_linear_velocity_ = 0;\r\n \r\n pp_.setCurrentWaypoints(msg_output->waypoints);\r\n is_waypoint_set_ = true;\r\n}\r\n <\/code><\/pre>\n4. \u81ea\u52a8\u8bbe\u7f6e\u591a\u4e2a\u8fde\u7eed\u7684\u76ee\u6807\u70b9<\/h4>\nvim Robot\/goals.py\r\n\r\n#! \/usr\/bin\/env python3\r\n\r\n# \u53d1\u9001\u76ee\u6807\u70b9\r\n# python3 goals.py\r\n\r\n\r\nimport rospy\r\nimport time\r\nimport math\r\nfrom geometry_msgs.msg import PoseStamped\r\n\r\n\r\n\r\ndef InitXiangyansiGoals():\r\n \"\"\"\u521d\u59cb\u5316\u76ee\u6807\u4f4d\u7f6e\u5217\u8868\"\"\"\r\n goals = []\r\n\r\n t = time.time()\r\n secs = int(t)\r\n nsecs = int((t - secs) * 1000000000)\r\n # print(t, secs, nsecs)\r\n\r\n # \u897f\u5357\u89d2\r\n pose_westsourth = PoseStamped()\r\n pose_westsourth.header.frame_id = 'world'\r\n pose_westsourth.header.stamp.secs = secs\r\n pose_westsourth.header.stamp.nsecs = nsecs\r\n pose_westsourth.pose.position.x = -36.194183349609375\r\n pose_westsourth.pose.position.y = -3.115070343017578\r\n pose_westsourth.pose.position.z = 0.0\r\n pose_westsourth.pose.orientation.x = 0.0\r\n pose_westsourth.pose.orientation.y = 0.0\r\n pose_westsourth.pose.orientation.z = 0.9140234511288408\r\n pose_westsourth.pose.orientation.w = 0.4056613498800741\r\n goals.append(pose_westsourth)\r\n\r\n\r\n # \u4e2d\u95f4\u9053\u8def\r\n pose_middlerow = PoseStamped()\r\n pose_middlerow.header.frame_id = 'world'\r\n pose_middlerow.header.stamp.secs = secs\r\n pose_middlerow.header.stamp.nsecs = nsecs\r\n pose_middlerow.pose.position.x = -1.769686222076416\r\n pose_middlerow.pose.position.y = 52.081050872802734\r\n pose_middlerow.pose.position.z = 0.0\r\n pose_middlerow.pose.orientation.x = 0.0\r\n pose_middlerow.pose.orientation.y = 0.0\r\n pose_middlerow.pose.orientation.z = 0.3816013240128726\r\n pose_middlerow.pose.orientation.w = 0.9243270143794471\r\n goals.append(pose_middlerow)\r\n\r\n # \u516c\u5bd3\u697c\r\n pose_apartment = PoseStamped()\r\n pose_apartment.header.frame_id = 'world'\r\n pose_apartment.header.stamp.secs = secs\r\n pose_apartment.header.stamp.nsecs = nsecs\r\n pose_apartment.pose.position.x = 36.168060302734375\r\n pose_apartment.pose.position.y = 35.61585998535156\r\n pose_apartment.pose.position.z = 0.0\r\n pose_apartment.pose.orientation.x = 0.0\r\n pose_apartment.pose.orientation.y = 0.0\r\n pose_apartment.pose.orientation.z = 0.96506640968254\r\n pose_apartment.pose.orientation.w = -0.26200539097975056\r\n goals.append(pose_apartment)\r\n\r\n # # \u505c\u8f66\u573a\r\n # pose_parkinglot = PoseStamped()\r\n # pose_parkinglot.header.frame_id = 'world'\r\n # pose_parkinglot.header.stamp.secs = secs\r\n # pose_parkinglot.header.stamp.nsecs = nsecs\r\n # pose_parkinglot.pose.position.x = -14.472017288208008\r\n # pose_parkinglot.pose.position.y = 79.07386779785156\r\n # pose_parkinglot.pose.position.z = 0.0\r\n # pose_parkinglot.pose.orientation.x = 0.0\r\n # pose_parkinglot.pose.orientation.y = 0.0\r\n # pose_parkinglot.pose.orientation.z = -0.4085631512503463\r\n # pose_parkinglot.pose.orientation.w = 0.9127300539811246\r\n # goals.append(pose_parkinglot)\r\n\r\n\r\n # \u897f\u5317\u89d2\u9053\u8def\r\n pose_westnorth = PoseStamped()\r\n pose_westnorth.header.frame_id = 'world'\r\n pose_westnorth.header.stamp.secs = secs\r\n pose_westnorth.header.stamp.nsecs = nsecs\r\n pose_westnorth.pose.position.x = -41.374229431152344\r\n pose_westnorth.pose.position.y = 85.13764953613281\r\n pose_westnorth.pose.position.z = 0.0\r\n pose_westnorth.pose.orientation.x = 0.0\r\n pose_westnorth.pose.orientation.y = 0.0\r\n pose_westnorth.pose.orientation.z = 0.3484739589582032\r\n pose_westnorth.pose.orientation.w = 0.9373184623851152\r\n goals.append(pose_westnorth)\r\n\r\n # \u5bfa\u5e99\u95e8\u53e3\u5de6\u4fa7\r\n pose_templeleft = PoseStamped()\r\n pose_templeleft.header.frame_id = 'world'\r\n pose_templeleft.header.stamp.secs = secs\r\n pose_templeleft.header.stamp.nsecs = nsecs\r\n pose_templeleft.pose.position.x = 83.72945404052734\r\n pose_templeleft.pose.position.y = 171.56243896484375\r\n pose_templeleft.pose.position.z = 0.0\r\n pose_templeleft.pose.orientation.x = 0.0\r\n pose_templeleft.pose.orientation.y = 0.0\r\n pose_templeleft.pose.orientation.z = 0.3451665329359535\r\n pose_templeleft.pose.orientation.w = 0.9385414559522522\r\n goals.append(pose_templeleft)\r\n\r\n # \u5c0f\u5356\u90e8\r\n pose_store = PoseStamped()\r\n pose_store.header.frame_id = 'world'\r\n pose_store.header.stamp.secs = secs\r\n pose_store.header.stamp.nsecs = nsecs\r\n pose_store.pose.position.x = 315.1929626464844\r\n pose_store.pose.position.y = 318.74285888671875\r\n pose_store.pose.position.z = 0.0\r\n pose_store.pose.orientation.x = 0.0\r\n pose_store.pose.orientation.y = 0.0\r\n pose_store.pose.orientation.z = 0.7824470064057306\r\n pose_store.pose.orientation.w = 0.622717176707621\r\n goals.append(pose_store)\r\n\r\n # \u5bfa\u5e99\u5927\u95e8\r\n pose_temple = PoseStamped()\r\n pose_temple.header.frame_id = 'world'\r\n pose_temple.header.stamp.secs = secs\r\n pose_temple.header.stamp.nsecs = nsecs\r\n pose_temple.pose.position.x = 102.62203979492188\r\n pose_temple.pose.position.y = 163.55311584472656\r\n pose_temple.pose.position.z = 0.0\r\n pose_temple.pose.orientation.x = 0.0\r\n pose_temple.pose.orientation.y = 0.0\r\n pose_temple.pose.orientation.z = -0.47270840074021886\r\n pose_temple.pose.orientation.w = 0.8812189102996058\r\n goals.append(pose_temple)\r\n\r\n # \u516c\u5bd3\u697c\r\n pose_apartment.header.stamp.secs = secs\r\n pose_apartment.header.stamp.nsecs = nsecs\r\n goals.append(pose_apartment)\r\n\r\n return goals\r\n\r\n\r\nif __name__ == '__main__':\r\n \"\"\"main\"\"\"\r\n # init\r\n rospy.init_node('goal_pose')\r\n pub_goal = rospy.Publisher('\/move_base_simple\/goal', PoseStamped, queue_size = 1)\r\n \r\n pose = PoseStamped()\r\n pub_goal.publish(pose)\r\n time.sleep(1)\r\n\r\n # send goal pose\r\n goals = InitXiangyansiGoals()\r\n for goal in goals:\r\n pub_goal.publish(goal)\r\n time.sleep(5)\r\n<\/code><\/pre>\n5. \u7f16\u8bd1\u9a8c\u8bc1<\/h4>\n
\u91cd\u65b0\u7f16\u8bd1\uff0c\u5f53\u4e0a\u8f66\u5230\u8fbe\u4e0a\u4e00\u76ee\u6807\u70b9\u540e\uff0c\u76f4\u63a5\u8bbe\u7f6e\u4e0b\u4e00\u76ee\u6807\u70b9\uff0c\u8f66\u8f86\u5373\u53ef\u81ea\u52a8\u8fd0\u884c\uff1a<\/p>\n
cd ~\/Autoware\r\nAUTOWARE_COMPILE_WITH_CUDA=1 colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select=op_global_planner\r\nor\r\n.\/build.sh op_global_planner<\/code><\/pre>\n\u4e5f\u53ef\u4ee5\u76f4\u63a5\u6839\u636e\u9884\u5148\u8bbe\u5b9a\u597d\u7684\u4e00\u7ec4\u9014\u5f84\u70b9\uff0c\u4e00\u952e\u81ea\u52a8\u8bbe\u5b9a\u76ee\u6807\uff08\u7528\u4e8e\u56fa\u5b9a\u5de1\u903b\u70b9\u8def\u7ebf\u5de1\u903b\uff09\uff1a<\/p>\n
cd Robot\/\r\npython3 goals.py<\/code><\/pre>\n <\/p>\n
\u4e03\u3001\u53d8\u9053<\/h1>\n
OpenPlanner\u89c4\u5212\u5168\u5c40\u8def\u5f84\uff0c\u5982\u679c\u8981\u652f\u6301\u5b9e\u65f6\u6839\u636e\u73af\u5883\u53d8\u9053\u548c\u6539\u53d8\u8def\u5f84\uff0c\u9700\u8981\u5728op_global_planner<\/strong>\u4e2d\u542f\u7528enableLaneChange\u3001<\/span>enableReplan<\/span>\u3001<\/span>enableDynamicMapUpdate\uff0c\u540c\u65f6\u5728vector_map\u7ed8\u5236\u591a\u6761\u5e76\u884cLane\u884c\u9a76\u7ebf\uff08\u9700\u8981\u7528golbal planner\u9a8c\u8bc1\u4e0b\u6240\u6709\u7684\u8def\u53e3\u8f6c\u5f2f\uff0c\u540c\u65f6\u6ce8\u610f\u6240\u6709\u4ea4\u53c9\u8def\u53e3\u7684\u5730\u65b9Lane\u5fc5\u987b\u5207\u65ad\uff0c\u7528\u72ec\u7acb\u7684\u8854\u63a5Lane\u8fde\u63a5\u4e0d\u540c\u7684\u8def\uff0c\u5426\u5219\u5207\u6362\u8def\u7ebf\u4f1a\u6709\u95ee\u9898\uff09<\/span>\u3002<\/p>\n\u6574\u4e2a\u53d8\u9053\u7684\u6d41\u7a0b\u5982\u4e0b\uff1a<\/p>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/12\/LaneChanging-1024x737.png\")
<\/a><\/p>\n\u64ad\u5305\u5355\u72ec\u9a8c\u8bc1\u8def\u7ebf\u5207\u6362\u548c\u5404\u8def\u7ebf\u7684\u8fde\u901a\u578b\uff1a<\/p>\n
# \u52a0\u8f7d\u5730\u56fe\u7b49\u5176\u4ed6\u6a21\u5757\r\nroslaunch autolaunch run.launch use_gnss:=1 rviz:=true map_name:=jingheyuan bag_file:=\/home\/autoware\/shared_dir\/bag\/run-4-jhy-bz\/2024-12-11-22-26-17.bag planner:=false\r\n\r\n# \u5355\u72ecdebug\u8def\u5f84\u89c4\u5212\r\nroslaunch autolaunch planner.launch<\/code><\/pre>\n\u4e0a\u8f66\u9a8c\u8bc1\uff0c\u5728\u539f\u6709\u884c\u9a76\u8def\u7ebf\u4e0a\u653e\u7f6e\u969c\u788d\u7269\uff0c\u5373\u53ef\u81ea\u52a8\u5207\u6362\u5e76\u884c\u7684\u5176\u4ed6\u884c\u9a76\u7ebf\u3002<\/p>\n
![\"\"](\"https:\/\/yanjingang.com\/blog\/wp-content\/uploads\/2024\/12\/op-planner-change-line-1024x683.png\")
<\/a><\/p>\n\u516b\u3001\u5b89\u5168\u505c\u8f66<\/h1>\n1. \u5b9a\u4f4d\u8d28\u91cf\u5dee\u573a\u666f<\/h4>\n
\u524d\u51e0\u5929\u8c03\u8f66\u65f6\uff0c\u9047\u5230\u4e00\u6b21\u5b9a\u4f4d\u98d8\u540e\u8f66\u8f86\u4f9d\u7136\u5728\u79fb\u52a8\u5bfc\u81f4\u78b0\u649e\u7684\u60c5\u51b5\uff0c\u6211\u4eec\u8fd9\u91cc\u5728control\u903b\u8f91\u91cc\u6dfb\u52a0\u4e00 \u4e2a\u5b89\u5168\u4fdd\u62a4\uff1a\u5f53\u5b9a\u4f4d\u8d28\u91cf\u5dee\u65f6\uff0c\u505c\u6b62\u63a7\u8f66\u6307\u4ee4\u6307\u4ee4\u7684\u8f93\u51fa\u3002<\/p>\n
\u8003\u8651\u5230\u53d1\u51fa\u79fb\u52a8\u6307\u4ee4\u4f46\u8f66\u8f86\u672a\u79fb\u52a8\u65f6\uff0cpnc\u4f1a\u589e\u52a0\u8f6c\u901f\uff0c\u4e3a\u907f\u514d\u53d1\u751f\u610f\u5916\uff0c\u6211\u4eec\u76f4\u63a5\u5728\u63a7\u5236\u7684\u6e90\u5934\u62e6\u622a\uff0c\u5373pure_pursuit \u8def\u5f84\u8ddf\u8e2a\u8282\u70b9\uff0c\u62e6\u622a\u539f\u59cb\u63a7\u5236\u6307\u4ee4\/twist_raw\u3001\/ctrl_raw\u7684\u53d1\u51fa\u3002<\/p>\n
vim Autoware\/src\/autoware\/core_planning\/pure_pursuit\/src\/pure_pursuit_core.cpp\r\n #include \"autoware_msgs\/NDTStat.h\"\r\n\r\n void PurePursuitNode::initForROS() {\r\n ...\r\n sub6_ = nh_.subscribe(\"ndt_stat\", 10, &PurePursuitNode::callbackFromNdtStat, this);\r\n ...\r\n }\r\n\r\n \/\/ \u8ba2\u9605ndt_stat\u72b6\u6001\uff0c\u7528\u4e8e\u6839\u636escore\u51b3\u5b9a\u662f\u5426\u505c\u4e0b\u8f66\u8f86 yan 24.12.25\r\n void PurePursuitNode::callbackFromNdtStat(const autoware_msgs::NDTStatConstPtr &msg) {\r\n if (msg->score != 0.0 && msg->score < 1.0) {\r\n good_location_quality_ = true;\r\n } else { \/\/ \u51fa\u73b01\u5e27\u5b9a\u4f4d\u8d28\u91cf\u98d8\uff0c\u5373\u7acb\u5373\u505c\u6b62\u79fb\u52a8\u8f66\u8f86\r\n good_location_quality_ = false;\r\n }\r\n }\r\n\r\n void PurePursuitNode::run() {\r\n ...\r\n bool can_get_curvature = pp_.canGetCurvature(&kappa);\r\n if (!good_location_quality_) \/\/ \u5f53\u5b9a\u4f4d\u8d28\u91cf\u4e0d\u597d\u65f6\uff0c\u4e0d\u5141\u8bb8\u63a7\u8f66\r\n can_get_curvature = false;\r\n ...\r\n }\r\n<\/code><\/pre>\n\u4fee\u6539\u540e\u7f16\u8bd1\u5e76\u8fd0\u884c\uff0c\u76d1\u63a7\u5728\u5b9a\u4f4d\u98d8\u65f6\uff0c\u63a7\u8f66\u901f\u5ea6\u4ee5\u53ca\u8f6c\u89d2topic\u662f\u5426\u53d8\u4e3a0\uff0c\u5e76\u5728\u5b9a\u4f4d\u6062\u590d\u540e\u7ee7\u7eed\u63a7\u8f66\uff1a<\/p>\n
# build\r\ncolcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select=pure_pursuit\r\n\r\n# check\r\nrostopic echo \/ctrl_raw<\/code><\/pre>\n2. \u5b9a\u9519\u4f4d\u7f6e\u573a\u666f<\/h4>\n
\u9664\u4e86\u4e0a\u8ff0\u573a\u666f\u65f6\uff0c\u8fd8\u5b58\u5728\u4e00\u79cd\u60c5\u51b5\uff0c\u5c31\u662f\u5b9a\u4f4d\u8d28\u91cf\u663e\u793a\u5f88\u597d\uff0c\u4f46\u5b9e\u9645\u8ba9\u5b83\u5b9a\u9519\u4f4d\u7f6e\u4e86\uff0c\u6b64\u65f6\u5b83\u4f1a\u57fa\u4e8e\u9519\u8bef\u7684\u4f4d\u7f6e\u89c4\u5212\u8def\u7ebf\uff0c\u800c\u969c\u788d\u7269\u57fa\u4e8emap\u5750\u6807\u7cfb\u8f6c\u6362\u540e\uff0c\u4e5f\u4f1a\u51fa\u73b0\u5728\u9519\u8bef\u7684\u4f4d\u7f6e\u4e0a\uff0c\u5bfc\u81f4\u6b64\u65f6\u8f66\u8f86\u78b0\u649e\u95ee\u9898\u3002\u8fd9\u91cc\u6709\u4e2a\u7b80\u5355\u7c97\u66b4\u7684\u65b9\u6cd5\uff0c\u5c31\u662f\u589e\u52a0lidar\u8fd1\u8ddd\u79bb\u70b9\u4e91\u68c0\u6d4b\uff0c\u5373lidar\u68c0\u6d4b\u5230\u81ea\u8eab\u5750\u6807\u7cfb\u9644\u8fd1\u8ddd\u79bb\u6709\u969c\u788d\u7269\uff0c\u90a3\u4e48\u5c31\u5fc5\u987b\u5b89\u5168\u505c\u8f66\u3002<\/p>\n
\u540c\u4e0a\uff0c\u6211\u4eec\u76f4\u63a5\u5728\u63a7\u5236\u7684\u6e90\u5934\u62e6\u622a\u539f\u59cb\u63a7\u5236\u6307\u4ee4\/twist_raw\u3001\/ctrl_raw\u7684\u53d1\u51fa\u3002<\/p>\n
vim Autoware\/src\/autoware\/core_planning\/pure_pursuit\/launch\/pure_pursuit.launch\r\n <arg name=\"safety_distance\" default=\"0.5\" \/> <!-- \u8f66\u8f86\u5b89\u5168\u8ddd\u79bb\uff08\u8f66\u8eab\u5750\u6807\u7cfb\u4e0b\uff0clidar\u5982\u679c\u63a2\u6d4b\u5230\u6b64\u8ddd\u79bb\u5185\u7684\u969c\u788d\u7269\uff0c\u5219\u8ba4\u4e3a\u8f66\u8f86\u4e0d\u5b89\u5168\uff0c\u6682\u65f6\u505c\u6b62\u63a7\u8f66\uff09 -->\r\n <arg name=\"points_topic\" default=\"\/points_no_ground\" \/> <!-- \uff0c\u8f66\u8f86\u5b89\u5168\u68c0\u67e5\u8f93\u5165\u70b9\u4e91topic\uff0c\u8fd9\u91cc\u4f7f\u7528\u8fc7\u6ee4\u5730\u9762\u540e\u7684\u96f7\u8fbe\u8bdd\u9898\/points_no_ground -->\r\nvim Autoware\/src\/autoware\/core_planning\/pure_pursuit\/include\/pure_pursuit\/pure_pursuit_core.h\r\n #include <sensor_msgs\/PointCloud2.h>\r\n #include <pcl_conversions\/pcl_conversions.h>\r\n #include <pcl\/PCLPointCloud2.h>\r\n #include <pcl\/conversions.h>\r\n #include <pcl_ros\/transforms.h>\r\n #include <pcl_ros\/point_cloud.h>\r\n ros::Subscriber sub_lidar_points;\r\n ...\r\n \/\/ \u8f66\u8eab\u5468\u56f4\u969c\u788d\u7269\u68c0\u67e5 yan 25.9.23\r\n bool safe_near_vehicle_ = false; \/\/ \u8f66\u8eab\u5750\u6807\u7cfb\u9644\u8fd1\u662f\u5426\u5b89\u5168\uff08\u65e0\u8fd1\u8ddd\u79bb\u969c\u788d\u7269\uff09\r\n double safety_distance_ = 0.5; \/\/ \u8f66\u8f86\u5b89\u5168\u8ddd\u79bb\uff08\u8f66\u8eab\u5750\u6807\u7cfb\u4e0b\uff0clidar\u5982\u679c\u63a2\u6d4b\u5230\u6b64\u8ddd\u79bb\u5185\u7684\u969c\u788d\u7269\uff0c\u5219\u8ba4\u4e3a\u8f66\u8f86\u4e0d\u5b89\u5168\uff0c\u6682\u65f6\u505c\u6b62\u63a7\u8f66\uff09\r\n std::string points_topic_ = \"\/points_no_ground\"; \/\/\u5b89\u5168\u68c0\u67e5\u8f93\u5165\u70b9\u4e91topic\uff0c\u8fd9\u91cc\u4f7f\u7528\u8fc7\u6ee4\u5730\u9762\u540e\u7684\u96f7\u8fbe\u8bdd\u9898\r\n void callbackLidarPoints(const sensor_msgs::PointCloud2ConstPtr& msg);\r\n\r\nvim Autoware\/src\/autoware\/core_planning\/pure_pursuit\/src\/pure_pursuit_core.cpp\r\n void PurePursuitNode::initForROS() {\r\n private_nh_.param(\"points_topic\", points_topic_);\r\n sub_lidar_points = nh_.subscribe(points_topic_, 10, &PurePursuitNode::callbackLidarPoints, this);\r\n }\r\n\r\n void PurePursuitNode::callbackLidarPoints(const sensor_msgs::PointCloud2ConstPtr& msg)\r\n {\r\n pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);\r\n pcl::fromROSMsg(*msg, *cloud_ptr);\r\n \/\/ \u68c0\u67e5\u8fc7\u8fd1\u7684\u70b9\u4e91\uff08\u9ed8\u8ba4\u4e0d\u79fb\u9664\uff09\r\n int near_points = 0;\r\n for (unsigned int i = 0; i < cloud_ptr->points.size(); i++) {\r\n double origin_distance = sqrt(pow(cloud_ptr->points[i].x, 2) + pow(cloud_ptr->points[i].y, 2));\r\n if (origin_distance < safety_distance_) { \/\/ \u5c0f\u4e8e\u8ddd\u79bb\u9608\u503c\u7684\u6280\u672f\r\n near_points++;\r\n }\r\n }\r\n if (near_points == 0) { \/\/ \u8f66\u8eab\u9644\u8fd1\u8fd1\u8ddd\u79bb\u6ca1\u6709\u969c\u788d\u7269\u70b9\u4e91\uff0c\u5141\u8bb8\u79fb\u52a8\u8f66\u8f86\r\n safe_near_vehicle_ = true;\r\n } else { \/\/ \u51fa\u73b0\u975e\u5e38\u8fd1\u8ddd\u79bb\u7684\u969c\u788d\u7269\u65f6\uff0c\u7acb\u5373\u505c\u6b62\u79fb\u52a8\u8f66\u8f86\r\n ROS_WARN(\"Unsafe! Lidar detect %d points neer vehicle!\", near_points);\r\n safe_near_vehicle_ = false;\r\n }\r\n }\r\n\r\n void PurePursuitNode::run(){\r\n if (!good_location_quality_ || !safe_near_vehicle_){ \/\/ \u5f53\u5b9a\u4f4d\u8d28\u91cf\u4e0d\u597d\u6216\u8f66\u8eab\u9644\u8fd1\u6709\u969c\u788d\u7269\u65f6\uff0c\u4e0d\u5141\u8bb8\u63a7\u8f66 yan 24.12.25\r\n can_get_curvature = false;\r\n ROS_WARN(\"Unsafe! Pause moving Vehicle! %d %d\", good_location_quality_, safe_near_vehicle_);\r\n }\r\n }\r\n }<\/code><\/pre>\n\u4fee\u6539\u540e\u7f16\u8bd1\u5e76\u8fd0\u884c\uff0c\u76d1\u63a7\u5728\u9760\u8fd1\u6fc0\u5149\u96f7\u8fbe1\u7c73\u5185\u65f6\uff0c\u63a7\u8f66\u901f\u5ea6\u4ee5\u53ca\u8f6c\u89d2topic\u662f\u5426\u53d8\u4e3a0\uff0c\u5e76\u5728\u8fdc\u79bb\u540e\u7ee7\u7eed\u63a7\u8f66\uff1a<\/p>\n
# build\r\ncd ~\/Autoware\r\n.\/build.sh pure_pursuit\r\n\r\n# check\r\nrostopic echo \/ctrl_raw\r\n cmd: \r\n linear_velocity: 0.0\r\n linear_acceleration: 0.0\r\n steering_angle: 0.0<\/code><\/pre>\n <\/p>\n
\u53c2\u8003\uff1a<\/p>\n
\u57fa\u4e8e\u5df2\u77e5\u70b9\u4e91\u5730\u56fe\u7684NDT\u7684\u6fc0\u5149SLAM\u5b9a\u4f4d<\/a><\/p>\nAutoware\u81ea\u52a8\u9a7e\u9a76\u4f7f\u7528\u6559\u7a0bv1.3<\/a><\/p>\n\u57fa\u4e8eSC-LeGO-LOAM\u7684\u5efa\u56fe\u548cndt_localizer\u7684\u5b9a\u4f4d<\/a><\/p>\nautoware\u4e0bndt_mapping\u8282\u70b9\u89e3\u8bfb<\/a><\/p>\n
# 1. \u542f\u52a8\u5e95\u76d8+\u4f20\u611f\u5668<\/span>\r\nroslaunch turn_on_wheeltec_robot open_autoware.launch\r\n\r\n# \u68c0\u67e5topic\u5e27\u7387<\/span>\r\nrostopic hz \/imu_raw \/points_raw \/vehicle\/<\/span>odom \/gps\/<\/span>fix\r\n# \u68c0\u67e5topic\u65f6\u95f4\u6233\u662f\u5426\u6b63\u5e38<\/span>\r\nrostopic echo \/imu_raw \/gps\/<\/span>fix -p\r\n\r\n# 2. \u542f\u52a8Autoware<\/span>\r\ncd ~\/autoware.ai.x86\/\r\n.\/open.sh\r\nsource ~\/Autoware\/install\/<\/span>setup.bash\r\nroslaunch runtime_manager runtime_manager.launch<\/code><\/pre>\n2. \u52a0\u8f7d\u4ecebase_link\u5230Lidar\u5750\u6807\u7cfb\u7684TF\u3001\u5c0f\u8f66\u6a21\u578b<\/span><\/h4>\n\u70b9\u51fb\u8fdb\u5165\u3010Setup\u3011\u83dc\u5355\uff0c\u5206\u522b\u8bbe\u7f6eLocalizer\u3001TF\u3001Vehicle Model\uff1a<\/p>\n
\n- \u5728\u3010Localizer\u3011\u4e2d\u9009\u62e9Velodyne\u3002<\/li>\n
- \u5728\u3010Baselink to Localizer\u3011\u4e2d\u8bbe\u7f6e\u597d\u5404\u4e2a\u53c2\u6570\uff0c\u5176\u4e2d x\u3001y\u3001z\u3001yaw\u3001pitch\u3001roll\u8868\u793a\u771f\u8f66\u96f7\u8fbe\u4e2d\u5fc3\u70b9\u4e0e\u8f66\u8eab\u540e\u8f74\u4e2d\u5fc3\u70b9\u7684 TF \u4f4d\u7f6e\u5173\u7cfb\uff0c\u53ef\u6839\u636e\u96f7\u8fbe\u5b9e\u9645\u4f4d\u7f6e\u8fdb\u884c\u4fee\u6539\uff08\u5bf9\u4e8e\u956d\u795e 16 \u7ebf\u96f7\u8fbez\u53c2\u6570\u8bbe\u7f6e\u4e3a0.38\uff0cyaw \u53c2\u6570\u8bbe\u7f6e\u6210-1.57\uff09\uff0c\u8bbe\u7f6e\u597d\u4e4b\u540e\u70b9\u51fb \u3010TF\u3011 \u6309\u94ae\u3002<\/li>\n
- docker\u7248\u672c\u7684urdf\u5c0f\u8f66\u6a21\u578b\u5728 autoware_shared_dir\/urdf \u76ee\u5f55\u4e0b\u9762\uff0c\u9009\u62e9\u540e\u70b9\u51fb\u3010Vehicle Model\u3011\u52a0\u8f7d\u6a21\u578b\uff08\u5982\u679cVehicle Model\u4e3a\u7a7a\uff0c\u90a3\u4e48\u4f1a\u52a0\u8f7d\u4e00\u4e2a\u9ed8\u8ba4\u6a21\u578b\uff0c\u5728 rviz \u663e\u793a\u65f6\uff0c\u5982\u679c\u6709\u6fc0\u5149\u96f7\u8fbe\u6570\u636e\uff0c\u8f66\u8f86\u4f1a\u663e\u793a\u4e3a\u9ed1\u8272\uff09\u3002<\/li>\n<\/ul>\n
<\/a><\/p>\n3. \u8bbe\u7f6e\u4ece world \u5230 map \u8f6c\u6362<\/span><\/h4>\n\u70b9\u51fb\u3010Map\u3011\u83dc\u5355\uff0c\u52a0\u8f7d world \u5230 map \u7684\u5750\u6807\u8f6c\u6362\uff1a<\/p>\n
\n- \u70b9\u51fb\u3010TF\u3011\u53f3\u4fa7\u7684\u3010ref\u3011\u6309\u94ae\uff0c\u9009\u62e9~\/autoware_shared_dir\/lgsvl-tf.launch \u6587\u4ef6\u540e<\/li>\n
- \u70b9\u51fb\u3010TF\u3011\u6309\u94ae\u52a0\u8f7d\u8be5\u6587\u4ef6\uff0c\u4ee5\u52a0\u8f7d world \u5230 map \u7684\u5750\u6807\u8f6c\u6362
<\/code><\/li>\n<\/ul>\n<\/a><\/h4>\n4. \u52a0\u8f7d\u70b9\u4e91\u5730\u56fe PCD \u6587\u4ef6<\/span><\/h4>\n\n- \u6253\u5f00\u3010map\u3011\u9875\u9762\uff0c\u70b9\u51fb\u3010point cloud\u3011\u53f3\u4fa7\u7684\u3010ref\u3011\u6309\u94ae\uff0c\u9009\u62e9\u4e0a\u4e00\u6b65\u5efa\u56fe\u4fdd\u5b58\u76843D\u70b9\u4e91\u5730\u56fe.pcd\u6587\u4ef6<\/li>\n
- \u70b9\u51fb\u3010point cloud\u3011\u6309\u94ae\uff0c\u6b64\u65f6\u4e0b\u65b9\u4f1a\u51fa\u73b0\u4e00\u4e2a\u8fdb\u5ea6\u6761\uff0c\u5f53\u8fdb\u5ea6\u6761\u663e\u793a\u52a0\u8f7d\u4e86 100\uff05\u5e76\u51fa\u73b0[OK]\u5b57\u6837\u65f6\uff0c\u8bc1\u660e 3D \u70b9\u4e91\u5730\u56fe\u5df2\u7ecf\u6210\u529f\u52a0\u8f7d\u5b8c\u6bd5<\/li>\n<\/ul>\n
<\/a><\/p>\n5. \u70b9\u4e91\u964d\u91c7\u6837\u8fc7\u6ee4<\/span><\/h4>\n\u6253\u5f00\u3010sensing\u3011\u9875\u9762\uff0c\u627e\u5230\u3010points_downsampler\u3011\u4e0b\u7684\u3010voxel_grid_filter\u3011\u9009\u9879\uff0c\u8bbe\u7f6e[app]\u7684\u4e00\u4e9b\u53c2\u6570\u540e\uff0c\u52fe\u9009[voxel_grid_filter]\u3002 [voxel_grid_filter]\u6709\u4e00\u4e9b\u53c2\u6570\u8bbe\u7f6e\u5982\u4e0b\u56fe\uff1a<\/p>\n
<\/a><\/p>\n\n- Voxel Leaf size\uff1a\u4f53\u7d20\u964d\u91c7\u6837\u7b97\u6cd5\u4e2d\u7684\u4f53\u7d20\u53f6\u5927\u5c0f\uff0c\u5ba4\u5185\u53ef\u8bbe\u7f6e\u5c0f\u70b9\uff0c\u5ba4\u5916\u53ef\u8bbe\u7f6e\u5927\u70b9<\/li>\n<\/ul>\n
6. \u5730\u9762\u8fc7\u6ee4<\/h4>\n
\u6253\u5f00\u3010sensing\u3011\u9875\u9762\uff0c\u627e\u5230\u3010points_preprocessor\u3011\u4e0b\u7684\u3010ring_ground_filter\u3011\u9009\u9879\uff0c\u8bbe\u7f6e\u3010app\u3011\u7684\u4e00\u4e9b\u53c2\u6570\u540e\uff0c\u5e76\u52fe\u9009\u3002\u3010ring_ground_filter\u3011\u6709\u4e00\u4e9b\u53c2\u6570\u8bbe\u7f6e\u5982\u4e0b\u56fe\uff1a<\/p>\n
\n- Sensor model\uff1a16\u300132\u300164 \u7ebf\u96f7\u8fbe\uff0c\u8fd9\u91cc\u5982\u679c\u4f7f\u7528\u956d\u795e 16 \u7ebf\u96f7\u8fbe\u52fe\u9009[16]<\/li>\n
- Sensor height\uff1a\u96f7\u8fbe\u8ddd\u79bb\u5730\u9762\u9ad8\u5ea6\uff0c\u6211\u8fd9\u91cc\u662f0.438<\/li>\n
- Max slope\uff1a\u5730\u9762\u6700\u5927\u5761\u5ea6\uff0c\u5355\u4f4d\u00b0\uff0c\u5982\u679c\u4e0d\u7ed9\u51fa\u5761\u5ea6\uff0c\u5bb9\u6613\u5c06\u5761\u8bc6\u522b\u4e3a\u969c\u788d\u7269<\/li>\n
- Vertical Thres\uff1a\u8868\u793a\u969c\u788d\u7269\u548c\u5730\u9762\u7684\u5dee\u5f02\u5ea6\uff0c\u5982\u679c\u5927\u4e8e\u8fd9\u4e2a\u503c\u5219\u88ab\u8ba4\u4e3a\u662f\u969c\u788d\u7269<\/li>\n<\/ul>\n
<\/a><\/p>\n7. \u542f\u52a8ndt_matching\uff08<\/span>\u4ece map \u5230 base_link \u7684\u8f6c\u6362\uff09<\/span><\/h4>\n\u627e\u5230\u3010Computing\u3011\u5de6\u83dc\u5355\u680f\u4e0b\u7684\u3010ndt_matching\u3011\u9009\u9879\uff0c\u8bbe\u7f6e\u3010app\u3011\u91cc\u9762\u7684\u53c2\u6570\u540e\uff0c\u52fe\u9009\u3010ndt_matching\u3011\uff0c\u4e00\u4e9b\u53c2\u6570\u8bbe\u7f6e\u5982\u4e0b\u6240\u793a\uff1a<\/p>\n
\n- \u4f7f\u7528RTK\u65f6\uff0c\u3010topic:\/config\/ndt\u3011\u00a0\u52fe\u9009\u3010GNSS\u3011\u3002\u6ca1\u6709\u4f7f\u7528 GNSS\u65f6\uff0c\u3010topic:\/config\/ndt\u3011\u52fe\u9009 \u3010Initial_Pose\u3011\uff0cx\uff0cy\uff0cz\uff0croll\uff0cpitch\uff0cyaw \u7684\u503c\u8868\u793a\u5c0f\u8f66\u7684\u521d\u59cb\u4f4d\u7f6e\uff08\u9ed8\u8ba4\u5efa\u56fe\u8d77\u70b9\u4f4d\u7f6e\uff09\u3002<\/li>\n
- \u5728[method type]\u680f\u4e2d\uff0c\u65e0 GPU \u52a0\u901f\u7684\u9009\u3010pcl_generic\u3011\uff0c\u6709 GPU \u52a0\u901f\u7684\u9009[pcl_anh_gpu]\u3002<\/li>\n
- \u7528\u5230 imu \u8f85\u52a9\u5b9a\u4f4d\uff0c\u9700\u8981\u628a\u3010useimu\u3011\u9009\u9879\u52fe\u9009\u4e0a\uff0c\u5176\u4f59\u53c2\u6570\u53c2\u8003\u9ed8\u8ba4\u503c\u5c31\u53ef\u4ee5\u4e86\u3002<\/li>\n<\/ul>\n
\u5728[Compulting]\u9875\u9762\u627e\u5230[ndt_matching_monitor]\u9009\u9879\uff0c\u52fe\u9009\u8282\u70b9<\/p>\n
<\/a><\/p>\n<\/a><\/p>\n<\/a><\/p>\nndt_matching\u53c2\u6570\u5728autoware\u4e2d\u662f\u901a\u8fc7runtime manager\u4ee5\/config\/ndt\u8bdd\u9898\u7684\u65b9\u5f0f\u53d1\u5e03\u7684\uff0c\u8bdd\u9898\u901a\u8fc7\u754c\u9762\u89e6\u53d1\uff0c\u6bcf\u5207\u6362\u754c\u9762\u6309\u94ae\u3001\u4fee\u6539\u53c2\u6570\u5e76\u786e\u5b9a\u5c31\u89e6\u53d1\u4e00\u6b21\u8bdd\u9898\u53d1\u5e03\uff0c\u6765\u5b9e\u65f6\u6539\u53d8\u7a0b\u5e8f\u4e2d\u7684NDT\u914d\u7f6e\u53c2\u6570\uff0c\u7c7b\u4f3c\u4e8e\u52a8\u6001\u53c2\u6570\u914d\u7f6e\uff0c\u540e\u7eed\u53ef\u6539\u4e3aros\u53c2\u6570\u5728\u7a0b\u5e8f\u5f00\u59cb\u65f6\u52a0\u8f7d\u3002<\/p>\n
rostopic echo \/config\/ndt<\/code><\/pre>\n\u53c2\u6570\u8bf4\u660e\uff1a<\/p>\n
\n- init_pos_gnss\uff1a1\u901a\u8fc7GNSS\u6765\u786e\u5b9a\u521d\u503c\uff0c0\u901a\u8fc7\u521d\u59cb\u4f4d\u59ff\u786e\u5b9a\u521d\u503c\uff1b\u5982\u679c\u4f7f\u7528GNSS\u53ef\u53c2\u8003GNSS+NDT\u8054\u5408\u5b9a\u4f4d<\/a><\/li>\n
- use_predict_pose\uff1a1\u4f7f\u7528\u9884\u6d4b\u4f4d\u59ff\uff0c0\u4e0d\u4f7f\u7528\u9884\u6d4b\u4f4d\u59ff\uff1b<\/li>\n
- error_threshold: \uff081.0\uff09\u8be5\u53c2\u6570\u4e0d\u77e5\u9053\u5e72\u561b\u7684\uff0c\u597d\u50cf\u6ca1\u7528\uff1b<\/li>\n
- resolution: \uff081.0\uff09\u7f51\u683c\u5927\u5c0f\u8bbe\u7f6e\uff1b<\/li>\n
- step_size: (0.1) \u8fed\u4ee3\u6b65\u957f\uff1b<\/li>\n
- trans_epsilon: \uff080.01\uff09\u7b97\u6cd5\u6536\u655b\u6761\u4ef6\uff1b<\/li>\n
- max_iterations: \uff0830\uff09\u6700\u5927\u8fed\u4ee3\u6b21\u6570\uff1b<\/li>\n<\/ul>\n
8. \u542f\u52a8\u5b9a\u4f4dvel_pose_connect<\/span><\/h4>\n\n- \u627e\u5230\u3010Computing\u3011 \u5de6\u83dc\u5355\u680f\u4e0b\u7684 \u3010vel_pose_connect\u3011\u3002<\/li>\n
- \u6253\u5f00 \u3010app\u3011 \u5e76\u786e\u4fdd\u9009\u9879 [Simulation_Mode] \u6ca1\u6709\u88ab\u52fe\u9009\u3002<\/li>\n
- \u9000\u51fa\u5e76\u52fe\u9009 \u3010vel_pose_connect\u3011\u3002<\/li>\n<\/ul>\n
<\/a><\/p>\n9. \u6253\u5f00rviz<\/span><\/h4>\n\u70b9\u51fb\u754c\u9762\u53f3\u4e0b\u89d2\u7684[rviz] \u6253\u5f00rviz\uff0c\u76f8\u5173\u914d\u7f6e\u5728~\/autoware_shared_dir\/default.rviz \u3002<\/p>\n
\u82e5\u662f\u5c0f\u8f66\u4e0d\u5728\u5efa\u56fe\u8d77\u70b9\u4f4d\u7f6e\uff0c\u6216\u8005\u662f\u5b9a\u4f4d\u4e0d\u51c6\u786e\uff0c\u53ef\u4ee5\u70b9\u51fb rviz \u83dc\u5355\u680f\u4e2d\u7684\u30102D pose estimate\u3011\uff0c\u7136\u540e\u5728\u5730\u56fe\u4e0a\u9f20\u6807\u5de6\u51fb\u62d6\u52a8\uff0c\u91cd\u65b0\u8bbe\u7f6e\u5c0f\u8f66\u7684\u4f4d\u7f6e\u3002<\/p>\n
<\/a><\/p>\n <\/p>\n
\u4e8c\u3001\u96f7\u8fbe\u76ee\u6807\u8bc6\u522b<\/h1>\n1. \u5b8c\u6210\u5730\u56fe\u5b9a\u4f4d<\/h4>\n
\u5b8c\u6210\u4e0a\u4e00\u6b65\u5730\u56fe\u5b9a\u4f4d\u6b65\u9aa4\u3002<\/p>\n
2. \u6b27\u51e0\u91cc\u5f97\u805a\u7c7b<\/h4>\n
\u70b9\u4e91\u805a\u7c7b\u662f\u5c06\u4e09\u7ef4\u70b9\u4e91\u6570\u636e\u8fdb\u884c\u5206\u7ec4\u6216\u5206\u7c7b\uff0c\u5c06\u8ddd\u79bb\u8f83\u8fd1\u7684\u70b9\u5f52\u4e3a\u4e00\u7c7b\u7684\u8fc7\u7a0b\u3002<\/p>\n
\n- \u969c\u788d\u7269\u68c0\u6d4b\u4e0e\u8ddf\u8e2a\uff1a\u70b9\u4e91\u805a\u7c7b\u53ef\u4ee5\u5e2e\u52a9\u8bc6\u522b\u548c\u533a\u5206\u73af\u5883\u4e2d\u7684\u4e0d\u540c\u7269\u4f53\uff0c\u7279\u522b\u662f\u969c\u788d\u7269\u3002\u901a\u8fc7\u805a\u7c7b\uff0c\u53ef\u4ee5\u5c06\u540c\u4e00\u4e2a\u969c\u788d\u7269\u7684\u70b9\u5f52\u4e3a\u4e00\u7c7b\uff0c\u5e76\u4f30\u8ba1\u5176\u4f4d\u7f6e\u3001\u5f62\u72b6\u548c\u5927\u5c0f\uff0c\u4ece\u800c\u5b9e\u73b0\u5bf9\u969c\u788d\u7269\u7684\u68c0\u6d4b\u548c\u8ddf\u8e2a\u3002<\/li>\n
- \u5efa\u7acb\u73af\u5883\u6a21\u578b\uff1a\u901a\u8fc7\u70b9\u4e91\u805a\u7c7b\uff0c\u53ef\u4ee5\u5c06\u70b9\u4e91\u6570\u636e\u5206\u5272\u4e3a\u4e0d\u540c\u7684\u7269\u4f53\u7fa4\u7ec4\uff0c\u5e76\u5c06\u8fd9\u4e9b\u7fa4\u7ec4\u7684\u7279\u5f81\u63d0\u53d6\u51fa\u6765\u3002\u8fd9\u4e9b\u7279\u5f81\u53ef\u4ee5\u7528\u4e8e\u5efa\u7acb\u73af\u5883\u6a21\u578b\uff0c\u5305\u62ec\u9053\u8def\u3001\u5efa\u7b51\u7269\u3001\u4ea4\u901a\u6807\u5fd7\u7b49\uff0c\u4e3a\u81ea\u52a8\u9a7e\u9a76\u8f66\u8f86\u63d0\u4f9b\u66f4\u51c6\u786e\u548c\u8be6\u7ec6\u7684\u73af\u5883\u4fe1\u606f\u3002<\/li>\n
- \u52a8\u6001\u7269\u4f53\u68c0\u6d4b\uff1a\u70b9\u4e91\u805a\u7c7b\u53ef\u4ee5\u5e2e\u52a9\u8bc6\u522b\u548c\u533a\u5206\u7a33\u5b9a\u7269\u4f53\u548c\u52a8\u6001\u7269\u4f53\u3002\u901a\u8fc7\u5c06\u70b9\u4e91\u6570\u636e\u8fdb\u884c\u805a\u7c7b\uff0c\u53ef\u4ee5\u53d1\u73b0\u79fb\u52a8\u7684\u7269\u4f53\uff0c\u5e76\u5c06\u5176\u4e0e\u7a33\u5b9a\u7684\u73af\u5883\u8fdb\u884c\u533a\u5206\u3002\u8fd9\u5bf9\u4e8e\u81ea\u52a8\u9a7e\u9a76\u6765\u8bf4\u81f3\u5173\u91cd\u8981\uff0c\u56e0\u4e3a\u5b83\u53ef\u4ee5\u5e2e\u52a9\u8f66\u8f86\u9884\u6d4b\u548c\u9002\u5e94\u52a8\u6001\u7269\u4f53\u7684\u884c\u4e3a\u3002<\/li>\n
- \u4f20\u611f\u5668\u878d\u5408\uff1a\u81ea\u52a8\u9a7e\u9a76\u7cfb\u7edf\u901a\u5e38\u4f1a\u4f7f\u7528\u591a\u79cd\u4e0d\u540c\u7c7b\u578b\u7684\u4f20\u611f\u5668\uff0c\u5982\u6fc0\u5149\u96f7\u8fbe\u3001\u6444\u50cf\u5934\u7b49\u3002\u70b9\u4e91\u805a\u7c7b\u53ef\u4ee5\u5c06\u591a\u4e2a\u4f20\u611f\u5668\u83b7\u53d6\u7684\u70b9\u4e91\u6570\u636e\u8fdb\u884c\u878d\u5408\uff0c\u63d0\u4f9b\u66f4\u5168\u9762\u548c\u4e00\u81f4\u7684\u73af\u5883\u611f\u77e5\u3002<\/li>\n<\/ul>\n
\u603b\u7684\u6765\u8bf4\uff0c\u70b9\u4e91\u805a\u7c7b\u5728\u81ea\u52a8\u9a7e\u9a76\u611f\u77e5\u5c42\u4e2d\u7684\u610f\u4e49\u5728\u4e8e\u5e2e\u52a9\u5c06\u4e09\u7ef4\u70b9\u4e91\u6570\u636e\u8f6c\u5316\u4e3a\u66f4\u6613\u5904\u7406\u7684\u7269\u4f53\u4fe1\u606f\uff0c\u5e76\u4e3a\u969c\u788d\u7269\u68c0\u6d4b\u3001\u73af\u5883\u5efa\u6a21\u3001\u52a8\u6001\u7269\u4f53\u68c0\u6d4b\u548c\u4f20\u611f\u5668\u878d\u5408\u7b49\u4efb\u52a1\u63d0\u4f9b\u57fa\u7840\u6570\u636e\u548c\u51c6\u786e\u6027\u3002\u5b83\u662f\u5b9e\u73b0\u81ea\u52a8\u9a7e\u9a76\u73af\u5883\u611f\u77e5\u4e0e\u51b3\u7b56\u7684\u5173\u952e\u6b65\u9aa4\u4e4b\u4e00\u3002<\/p>\n
\u6b27\u5f0f\u805a\u7c7b\u662f\u4e00\u79cd\u57fa\u4e8e\u6b27\u6c0f\u8ddd\u79bb\u5ea6\u91cf\u7684\u805a\u7c7b\u7b97\u6cd5\u3002\u91c7\u7528\u57fa\u4e8eKD-Tree\u7684\u8fd1\u90bb\u67e5\u8be2\u7b97\u6cd5\u662f\u52a0\u901f\u6b27\u5f0f\u805a\u7c7b\u3002<\/p>\n
\u6253\u5f00[Computing]\u9875\u9762\uff0c\u627e\u5230[lidar_detector]\u4e0b\u7684[lidar_euclidean_cluster_detect]\u9009\u9879\uff0c\u8bbe\u7f6e[app]\u7684\u4e00\u4e9b\u53c2\u6570\u540e\uff0c\u5e76\u52fe\u9009\u3002[lidar_euclidean_cluster_detect]\u6709\u4e00\u4e9b\u53c2\u6570\u8bbe\u7f6e\u5982\u4e0b\u56fe\uff1a<\/p>\n
<\/a><\/p>\n\n- use_gpu\uff1a\u662f\u5426\u4f7f\u7528GPU\uff0c\u9009\u62e9\u4f7f\u7528\uff0c\u5426\u5219\u805a\u7c7b\u5341\u5206\u6162\uff1b<\/li>\n
- output_frame\uff1a\u8f93\u51fa\u5750\u6807\u7cfb\uff0c\u6539\u4e3amap\uff1b<\/li>\n
- pose_estimation\uff1a\u9700\u8981\u4f30\u8ba1\u805a\u7c7b\u70b9\u4e91\u6700\u5c0f\u9762\u79ef\u8fb9\u754c\u77e9\u5f62\u7684\u59ff\u6001\uff1b<\/li>\n
- downsample_cloud\uff1a\u70b9\u4e91\u901a\u8fc7VoxelGrid\u6ee4\u6ce2\u5668\u8fdb\u884c\u4e0b\u91c7\u6837\uff1b<\/li>\n
- input_points_node\uff1a\u8f93\u5165\u70b9\u4e91topic\uff0c\u8fd9\u91cc\u4f7f\u7528\u8fc7\u6ee4\u5730\u9762\u540e\u7684\u96f7\u8fbe\u8bdd\u9898\/points_no_ground\uff1b<\/li>\n
- leaf size\uff1a\u4e0b\u91c7\u6837\u4f53\u7d20\u7f51\u683c\u5927\u5c0f\uff1b<\/li>\n
- cluster size minimum\uff1a\u805a\u7c7b\u7684\u6700\u5c11\u70b9\u6570\uff08\u8c03\u5c0f\u4e00\u70b9\u80fd\u805a\u5230\u66f4\u8fdc\u7684\u7c7b\uff09\uff1b<\/li>\n
- cluster size maximum\uff1a\u805a\u7c7b\u7684\u6700\u591a\u70b9\u6570\uff1b<\/li>\n
- clustering distance\uff1a\u805a\u7c7b\u516c\u5dee\uff08\u540c\u4e00\u7c7b\u4e24\u70b9\u6700\u5927\u8ddd\u79bb\u9608\u503c\uff0c\u8c03\u5927\u4e00\u70b9\u80fd\u805a\u5230\u66f4\u8fdc\u7684\u7c7b\uff0c\u4f46\u662f\u592a\u5927\u4f1a\u9020\u6210\u660e\u663e\u7684\u4e0d\u540c\u7c7b\u805a\u5230\u4e00\u8d77m\uff09\uff1b<\/li>\n
- clip_min_height\uff1a\u88c1\u526a\u7684\u6700\u5c0f\u9ad8\u5ea6\uff1b<\/li>\n
- clip_max_height\uff1a\u88c1\u526a\u7684\u6700\u5927\u9ad8\u5ea6\uff08\u9ad8\u4e8e\u96f7\u8fbe\u591a\u5c11\uff09\uff1b<\/li>\n
- use_vector_map\uff1a\u662f\u5426\u4f7f\u7528\u77e2\u91cf\u5730\u56fe\uff1b<\/li>\n
- vectormap_frame\uff1a\u77e2\u91cf\u5730\u56fe\u5750\u6807\u7cfb\uff1b<\/li>\n
- remove_points_upto\uff1a\u8ddd\u79bb\u6fc0\u5149\u96f7\u8fbe\u8fd9\u4e2a\u8ddd\u79bb\u8fc7\u8fd1\u7684\u70b9\u5c06\u88ab\u79fb\u9664\uff08\u6700\u5927\u53ea\u80fd2.5\uff0c\u56e0\u6b64\u8f66\u8eab\u8fc7\u5927\u4e0d\u8981\u7528\uff0c\u9700\u8981\u8f93\u5165\u53bb\u9664\u5730\u9762\u70b9\u70b9\u4e91\uff09\uff1b<\/li>\n
- keep_only_lanes_points\uff1a\u884c\u9a76\u7ebf\u8fb9\uff08\u8fdc\uff09\u6ee4\u6ce2\uff1b<\/li>\n
- keep_lane_left_distance\uff1a\u5411\u5de6\u79fb\u9664\u8be5\u8ddd\u79bb\u4ee5\u5916\u7684\u70b9 (m)\uff08\u7b97\u529b\u5f3a\u53ef\u4ee5\u8bbe\u5927\u70b9\uff09\uff1b<\/li>\n
- keep_lane_right_distance\uff1a\u5411\u53f3\u79fb\u9664\u8be5\u8ddd\u79bb\u4ee5\u5916\u7684\u70b9 (m)\uff1b<\/li>\n
- cluster_merge_threshold\uff1a\u805a\u7c7b\u7c07\u95f4\u7684\u8ddd\u79bb\u9608\u503c(m)\uff1b<\/li>\n
- use_multiple_thres\uff1a\u662f\u5426\u4f7f\u7528\u591a\u7ea7\u9608\u503c\u8fdb\u884c\u805a\u7c7b\uff08\u4f7f\u7528\u4e0b\u9762\u4e24\u4e2a\u53c2\u6570\uff09\uff1b<\/li>\n
- clustering_ranges\uff1a\u4e0d\u540c\u7684\u8ddd\u79bb\u6709\u4e0d\u540c\u7684\u805a\u7c7b\u9608\u503c (m)\uff1b<\/li>\n
- clustering_distances\uff1a\u805a\u7c7b\u516c\u5dee(\u540c\u4e00\u7c7b\u4e24\u4e2a\u70b9\u7684\u6700\u5927\u8ddd\u79bb\u9608\u503c\uff0c\u4e0eclustering_ranges\u5bf9\u5e94)\uff1b<\/li>\n
- remove ground\uff1a\u5730\u5e73\u9762\u6ee4\u6ce2\uff08\u79fb\u9664\u5c5e\u4e8e\u5730\u9762\u7684\u70b9\uff09\uff0c\u63a8\u8350\u8bbe\u7f6e\u4e3a0\uff1b<\/li>\n
- use_diffnormals\uff1a\u91c7\u7528\u6b63\u6001\u5dee\u503c\u6ee4\u6ce2\u518d\u53bb\u9664\u4e00\u6b21\u5730\u9762\u70b9\u3002<\/li>\n<\/ul>\n
3. \u8ddf\u8e2a\u969c\u788d\u7269<\/h4>\n
\u591a\u7ebf\u6fc0\u5149\u96f7\u8fbe\u6570\u636e\u4ece\u4f20\u611f\u5668\u51fa\u6765\u540e\uff0c\u7ecf\u8fc7\u4f20\u7edf\u7684\u805a\u7c7b+\u9ad8\u7cbe\u5730\u56feobject filter\uff0c\u6216\u8005learning-based\u7684\u65b9\u6cd5\u4e4b\u540e\uff0c\u5f97\u5230\u4e00\u4e2a\u76ee\u6807\u5217\u8868\uff0c\u6b64\u76ee\u6807\u5217\u8868\u542b\u6709\u4e8c\u7ef4\u4f4d\u7f6e\u4fe1\u606f\u548c\u89d2\u5ea6\u4fe1\u606f\uff0c\u6ca1\u6709\u901f\u5ea6\u4fe1\u606f\uff0c\u540c\u65f6\u57fa\u4e8e\u6bcf\u4e00\u5e27\u7684\u68c0\u6d4b\u65b9\u6cd5\uff0c\u53ef\u80fd\u4f1a\u51fa\u73b0\u201c\u76ee\u6807\u6d88\u5931\u201d\u7684\u95ee\u9898\uff0c\u56e0\u6b64\u9700\u8981\u6ee4\u6ce2\u5668\u6765\u8ddf\u8e2a\u8fd9\u4e9b\u76ee\u6807\u3002<\/p>\n
\u6253\u5f00[Computing]\u9875\u9762\uff0c\u627e\u5230[lidar_tracker]\u4e0b\u7684[lidar_kf_contour_track]\u9009\u9879\uff0c\u8bbe\u7f6e[app]\u7684\u4e00\u4e9b\u53c2\u6570\u540e\uff0c\u52fe\u9009[lidar_kf_contour_track]\u3002[lidar_kf_contour_track]\u6709\u4e00\u4e9b\u53c2\u6570\u8bbe\u7f6e\u5982\u4e0b\u56fe\uff1a<\/p>\n
<\/a><\/p>\n\n- Vector map distance\uff1a\u9700\u8981\u4fee\u6539\u4e3a 0\uff0c\u5176\u4ed6\u53c2\u6570\u9ed8\u8ba4\u5373\u53ef\u3002<\/li>\n<\/ul>\n
\u6211\u4eec\u53ef\u4ee5\u901a\u8fc7 rviz \u8ba2\u9605\/detection\/lidar_detector\/objects_markers \u8bdd\u9898\u53ef\u4ee5\u770b\u5230\u805a\u7c7b\u53ca\u8ddf\u8e2a\u969c\u788d\u7269\u5f97\u5230\u7684\u7ed3\u679c\uff0c\u4e5f\u53ef\u4ee5\u901a\u8fc7\u56fe\u7247\u4e2d\u7684\u6b65\u9aa4\u8ba2\u9605\/bounding_boxes\u8bdd\u9898\u4f7f\u7ed3\u679c\u4ee5\u65b9\u6846\u7684\u5f62\u5f0f\u663e\u793a\u51fa\u6765\u3002<\/p>\n
<\/a><\/p>\n<\/h1>\n\u4e09\u3001\u9ad8\u7cbe\u5730\u56fe\u7ed8\u5236<\/h1>\n
\u5728\u5f00\u59cb\u5bfc\u822a\u4e4b\u524d\uff0c\u5355\u9760\u70b9\u4e91\u5730\u56fe\u5df2\u7ecf\u4e0d\u591f\u4e86\uff0c\u6211\u4eec\u5fc5\u987b\u5148\u7ed8\u5236\u77e2\u91cf\u5730\u56fe\u518d\u7ee7\u7eed\u540e\u7eed\u7684\u5bfc\u822a\u73af\u8282\u3002<\/p>\n
\u5177\u4f53\u53c2\u8003\uff1a\u4f7f\u7528Unity+MapToolBox\u7ed8\u5236VectorMap\u5730\u56fe<\/a><\/p>\n <\/p>\n
\u56db\u3001open_planner\u5bfc\u822a<\/h1>\n
OpenPlanner\u662f\u540d\u53e4\u5c4b\u5927\u5b66\u5f00\u53d1\u7684\u5f00\u6e90\u81ea\u52a8\u9a7e\u9a76\u6846\u67b6Autoware\u4e2d\u4f7f\u7528\u7684\u8fd0\u52a8\u89c4\u5212\u7b97\u6cd5\u3002\u5b83\u662f\u901a\u8fc7\u5bf9\u5168\u5c40\u8def\u5f84\u8fdb\u884c\u91c7\u6837\u751f\u6210\u4e00\u7cfb\u5217\u7684\u5019\u9009\u8def\u5f84\uff0c\u7ed3\u5408\u77e2\u91cf\u5730\u56fe\u3001\u4f20\u611f\u5668\u4fe1\u606f\u4ee5\u53ca\u78b0\u649e\u3001\u4ea4\u901a\u89c4\u5219\u7b49\u7ea6\u675f\u548c\u4f18\u5316\u76ee\u6807\uff0c\u9009\u62e9\u51fa\u6700\u4f18\u7684\u8fd0\u52a8\u8f68\u8ff9\u3002OpenPlanner\u901a\u7528\u6027\u5f3a\uff0c\u53ea\u9700\u76f8\u5e94\u7684\u8c03\u6574\u53c2\u6570\u5373\u53ef\u4e0e\u4efb\u4f55\u7684\u79fb\u52a8\u673a\u5668\u4eba\u914d\u5408\u4f7f\u7528\u3002<\/p>\n
\u56fe\u662fOpenPlanner\u7684\u4e00\u822c\u4f53\u7cfb\u7ed3\u6784\uff0c\u5176\u4e3b\u8981\u4e09\u4e2a\u7ec4\u6210\u90e8\u5206\u662f\u5168\u5c40\u89c4\u5212\u5668\u3001\u884c\u4e3a\u72b6\u6001\u751f\u6210\u5668\u548c\u5c40\u90e8\u89c4\u5212\u5668\u3002<\/p>\n
<\/a><\/p>\nOpenPlanner\u65e8\u5728\u7528\u4e8e\u79fb\u52a8\u673a\u5668\u4eba\u7684\u81ea\u4e3b\u5bfc\u822a\uff0c\u66f4\u786e\u5207\u5730\u8bf4\u662f\u5e94\u7528\u4e8e\u81ea\u52a8\u9a7e\u9a76\u9886\u57df\u3002\u5b83\u7ed3\u5408\u4e86\u77e2\u91cf\u5730\u56fe\u6216\u9053\u8def\u7f51\u7edc\u5730\u56fe\u4e2d\u5305\u542b\u7684\u6240\u6709\u79bb\u6563\u4fe1\u606f\uff08\u4f8b\u5982\u4ea4\u901a\u4fe1\u53f7\u706f\uff0c\u4ea4\u901a\u6807\u5fd7\uff0c\u5341\u5b57\u8def\u53e3\uff0c\u505c\u8f66\u7ebf\u7b49\u7684\u4f4d\u7f6e\uff09\uff0c\u8fd9\u76f8\u5bf9\u4e8e\u5176\u4ed6\u7684\u5f00\u6e90\u7684\u5bfc\u822a\u7cfb\u7edfOMPL\u548cNavigation Stack\u6765\u8bf4\u662f\u4e3b\u8981\u7684\u4f18\u52bf\u3002 \u4f7f\u7528\u77e2\u91cf\u5730\u56fe\u53ef\u4ee5\u66f4\u52a0\u8f7b\u677e\u3001\u5feb\u901f\u5730\u8fdb\u884c\u5168\u5c40\u548c\u5c40\u90e8\u89c4\u5212\u3002\u5f53\u7136\uff0c\u50cfRRT * \u548c Hybrid A *\u8fd9\u4e9b\u81ea\u7531\u7a7a\u95f4\u4e2d\u5168\u5c40\u89c4\u5212\u5668\u5bf9\u4e8e\u505c\u8f66\u548c\u8d8a\u91ce\u60c5\u51b5\u4e5f\u662f\u5f88\u91cd\u8981\u7684\u3002\u5728\u8fd9\u79cd\u60c5\u51b5\u4e0b\uff0c\u6211\u4eec\u53ef\u4ee5\u5207\u6362\u5230\u81ea\u7531\u7a7a\u95f4\u89c4\u5212\u5668\u8fdb\u884c\u5168\u5c40\u89c4\u5212\uff0c\u4ecd\u9700\u8981\u4f7f\u7528Open Planner\u7684\u884c\u4e3a\u72b6\u6001\u673a\u548c\u672c\u5730\u89c4\u5212\u5668\u3002<\/p>\n
OpenPlanner\u9002\u5408\u4e8e\u9075\u5b88\u4ea4\u901a\u89c4\u5219\u7684\u81ea\u4e3b\u79fb\u52a8\u673a\u5668\u4eba\u5bfc\u822a\u7cfb\u7edf\uff0c\u5b83\u4ec5\u9700\u8981\u77e2\u91cf\u5730\u56fe\u548c\u76ee\u6807\u4f4d\u7f6e\u5373\u53ef\u8fdb\u884c\u5168\u5c40\u89c4\u5212\uff0c\u800c\u5bf9\u4e8e\u5c40\u90e8\u89c4\u5212\u548c\u884c\u4e3a\u72b6\u6001\u751f\u6210\uff0c\u5219\u4ec5\u9700\u8981\u5f53\u524d\u4f4d\u7f6e\u548c\u68c0\u6d4b\u5230\u7684\u969c\u788d\u5373\u53ef\u3002<\/p>\n
1. \u5b8c\u6210\u96f7\u8fbe\u76ee\u6807\u8bc6\u522b<\/h4>\n
\u5b8c\u6210\u4e0a\u4e00\u6b65\u96f7\u8fbe\u76ee\u6807\u8bc6\u522b\u6b65\u9aa4\u3002<\/p>\n
2. \u52a0\u8f7d\u9ad8\u7cbe\u5730\u56fe<\/h4>\n
\u9ad8\u7cbe\u5730\u56fe\u5730\u56fe\u5177\u4f53\u5236\u4f5c\u65b9\u6cd5\u53c2\u8003\u7b2c 3 \u7ae0\u5185\u5bb9\u3002<\/p>\n
\u6253\u5f00[map]\u9875\u9762\uff0c\u70b9\u51fb[vector map]\u7684[ref]\uff0c\u9009\u4e2d\u5168\u90e8\u7684\u9ad8\u7cbe\u5730\u56fe\u6587\u4ef6\uff0c\u70b9\u51fb[vector map]\u52a0\u8f7d\u9ad8\u7cbe\u5730\u56fe\uff0c\u6b64\u65f6\u53ef\u4ee5\u5728 rviz \u4e2d\u770b\u5230\u6211\u4eec\u753b\u7684\u9ad8\u7cbe\u5730\u56fe\u3002<\/p>\n
<\/a><\/p>\n3. \u5168\u5c40\u8def\u5f84\u89c4\u5212<\/h4>\n
\u6253\u5f00[Computing]\u9875\u9762\uff0c\u627e\u5230[openplanner_global planning]\u4e0b\u7684[op_global_planner]\u9009\u9879\uff0c\u8bbe\u7f6e[app]\u7684\u4e00\u4e9b\u53c2\u6570\u540e\uff0c\u5e76\u52fe\u9009\u3002\u7136\u540e\u5728 rviz \u4e2d\u70b9\u51fb[2Dnav goal]\uff0c\u9009\u62e9\u4f60\u6240\u8981\u5bfc\u822a\u7684\u76ee\u6807\u70b9\u4f4d\u7f6e\uff08\u5c0f\u8f66\u8d77\u70b9\u4f4d\u7f6e\u548c\u8bbe\u5b9a\u7684\u76ee\u6807\u70b9\u4f4d\u7f6e\u9700\u8981\u653e\u5728\u884c\u9a76\u7ebf lane \u9644\u8fd1\uff09rviz \u4e2d\u4f1a\u51fa\u73b0\u4e00\u6761\u84dd\u8272\u8def\u5f84\u5373\u4e3a\u5168\u5c40\u8def\u7ebf\u3002<\/p>\n
<\/a><\/p>\n<\/a><\/p>\n\n- Replanning\uff1a\u7528\u4e8e\u5230\u8fbe\u76ee\u6807\u70b9\u540e\uff0c\u53ef\u4ee5\u91cd\u65b0\u89c4\u5212\u5230\u4e0b\u4e00\u4e2a\u76ee\u6807\u70b9\u3002\u5982\u679c\u6709\u4e86\u4e24\u4e2a\u76ee\u6807\u70b9\u4f1a\u5faa\u73af<\/li>\n
- Smooth\uff1a\u7528\u4e8e\u5e73\u6ed1\u5168\u5c40\u8def\u5f84<\/li>\n
- Path density\uff1a\u7531\u6bcf\u4e00\u4e2a\u5c0f\u70b9\u7ec4\u5408\u6210\u5168\u5c40\u8def\u5f84\uff0c\u6b64\u53c2\u6570\u4e3a\u6bcf\u4e00\u4e2a\u70b9\u7684\u95f4\u9694\u8ddd\u79bb\u3002<\/li>\n<\/ul>\n
4. \u5c40\u90e8\u8def\u5f84\u89c4\u5212<\/h4>\n
\u6253\u5f00[Computing]\u9875\u9762\uff0c\u627e\u5230[openplanner_local planning]\u680f\u76ee\uff0c\u8bbe\u7f6e[app]\u7684\u4e00\u4e9b\u53c2\u6570\u540e\uff0c\u9700\u8981\u4f9d\u6b21\u52fe\u9009[openplanner_local planning]\u680f\u76ee\u4e0b\u7684\u5168\u90e8\u8282\u70b9\u3002\u53ef\u4ee5\u5728rviz \u4e2d\u70b9\u51fb[add]\uff0c\u6dfb\u52a0[local trajectories_evel_rviz]\u8bdd\u9898\uff0c rviz \u4e2d\u4f1a\u51fa\u73b0\u51e0\u6761\u8def\u5f84\u5373\u4e3a\u53ef\u884c\u9a76\u7684\u51e0\u6761\u5c40\u90e8\u8def\u7ebf\uff0c\u7ea2\u8272\u4e3a\u4e0d\u53ef\u884c\u9a76\u7684\u8def\u7ebf\uff0c\u7c89\u7ea2\u8272\u4e3a\u5f53\u524d\u9009\u62e9\u884c\u9a76\u7684\u5c40\u90e8\u8def\u7ebf\uff0c\u9ec4\u8272\u4e3a\u53ef\u884c\u9a76\u7684\u8def\u7ebf\u3002\u76f8\u5173\u4e00\u4e9b\u53c2\u6570\u8bbe\u7f6e\u5982\u4e0b\u56fe\uff1a<\/p>\n
\u5c40\u90e8\u8def\u5f84\u89c4\u5212\u52fe\u9009\u8282\u70b9\uff1a<\/p>\n
<\/a><\/p>\nop_commom_params \u8282\u70b9\u76f8\u5173\u53c2\u6570\uff1a<\/p>\n
<\/a><\/p>\n\n- Path distance\uff1a\u7528\u4e8e\u8bbe\u7f6e\u4e3b\u8def\u5f84\u4e24\u4fa7\u7684\u884d\u751f\u51fa\u7684\u5c40\u90e8\u8f68\u8ff9\u7684\u957f\u5ea6<\/li>\n
- Path Density \u7528\u4e8e\u8bbe\u7f6e\u4e00\u6761\u5c40\u90e8\u8f68\u8ff9\u4e0a\u4e24\u4e2a\u8f68\u8ff9\u70b9\u7684\u8ddd\u79bb<\/li>\n
- Horizontal Density\uff1a\u7528\u4e8e\u8bbe\u7f6e\u4e24\u4e2a\u5c40\u90e8\u8f68\u8ff9\u7684\u95f4\u8ddd<\/li>\n
- Rollouts Number\uff1a\u7528\u4e8e\u8bbe\u7f6e\u5c40\u90e8\u8f68\u8ff9\u7684\u6570\u91cf<\/li>\n
- Max velocity\uff1a\u5c0f\u8f66\u7684\u884c\u9a76\u901f\u5ea6\uff0c\u4e0d\u5728\u4e2d\u5fc3\u8f68\u8ff9\u4e0a\u7684\u5c0f\u8f66\u884c\u9a76\u901f\u5ea6\u51cf\u534a\uff0c\u5355\u4f4d m\/s<\/li>\n
- Accelration\/deceleration\uff1a\u52a0\u901f\u5ea6\/\u51cf\u52a0\u901f\u5ea6<\/li>\n
- Avoiding distance\uff1a\u8ddd\u79bb\u969c\u788d\u7269\u591a\u8fdc\u624d\u5f00\u59cb\u907f\u969c<\/li>\n
- Avoidance Limit\uff1a\u505c\u8f66\u65f6\uff0c\u969c\u788d\u7269\u8ddd\u79bb\u591a\u8fdc\u53ef\u4ee5\u5f00\u8f66<\/li>\n
- Lateral Safety \u4e0e Longitudinal Safty\uff1a\u8bbe\u7f6e\u5b89\u5168\u6846\u5c3a\u5bf8\uff0c\u5206\u522b\u4ee3\u8868\u8f66\u8f86\u5b89\u5168\u6846\u5bbd\u4e0e\u957f\uff0c\u969c\u788d\u7269\u5728\u5c0f\u8f66\u5b89\u5168\u6846\u5185\u65f6\uff0c\u5c0f\u8f66\u505c\u6b62\u8fd0\u52a8<\/li>\n
- Enable following \u548c enable avoidance\uff1a\u9700\u8981\u52fe\u9009\u4e0a\uff0c\u6253\u5f00\u8ddf\u968f\u548c\u907f\u969c\u529f\u80fd<\/li>\n<\/ul>\n
op_trajectory_generator \u76f8\u5173\u53c2\u6570\uff1a<\/p>\n
<\/a><\/p>\nTip Margin \u4e0e Roll In Margin \u4ee3\u8868\u610f\u4e49\uff1a<\/p>\n
<\/a><\/p>\n\n- Tip Margin\uff1a\u7528\u4e8e\u8bbe\u7f6e\u4ece\u8f66\u8f86\u4e2d\u5fc3\u5230\u5f00\u59cb\u8def\u5f84\u5206\u53c9\u70b9\u6a2a\u5411\u91c7\u6837\u70b9\u7684\u8ddd\u79bb\uff0c\u5176\u957f\u5ea6\u51b3\u5b9a\u4e86\u8f66\u8f86\u5207\u6362\u5230\u4e0d\u540c\u8f68\u8ff9\u7684\u5e73\u6ed1\u5ea6\u3002\uff08\u5efa\u8bae\u8bbe\u7f6e\u4e3a\u8f66\u8f86\u4e2d\u5fc3\u5230\u8f66\u8f86\u6700\u524d\u65b9\u7684\u8ddd\u79bb+\u8f6e\u5b50\u8f6c\u5f2f\u534a\u5f84\u53ef\u4ee5\u6ee1\u8db3\u8def\u7ebf\u5207\u6362\u7684\u8ddd\u79bb\uff0c\u7b80\u5355\u7c97\u66b4\u70b9\u5c31\u76f4\u63a5\u8bbe\u7f6e\u4e3a\u8f66\u8f86\u957f\u5ea6*0.8\uff0c\u6211\u8fd9\u91cc\u8bbe\u7f6e\u7684\u8f66\u957f0.45*0.8=0.36<\/span>\uff09<\/li>\n
- Roll In Margin\uff1a\u6c34\u5e73\u6a2a\u5411\u91c7\u6837\u70b9\u5230\u5e73\u884c\u6a2a\u5411\u91c7\u6837\u70b9\u7684\u8ddd\u79bb\uff0c\u5176\u957f\u5ea6\u4e0e\u8f66\u8f86\u901f\u5ea6\u6210\u6b63\u6bd4\u3002\u8f66\u8f86\u884c\u9a76\u7684\u901f\u5ea6\u8d8a\u5feb\uff0c\u6b64\u533a\u57df\u4ea7\u751f\u5e73\u6ed1\u53d8\u5316\u7684\u65f6\u95f4\u5c31\u8d8a\u957f\u3002\uff08\u7531\u4e8e\u9ed8\u8ba4\u901f\u5ea6\u56e0\u5b50\u4e3a0.25\uff0c\u8fd9\u91cc\u53ef\u4ee5\u8bbe\u7f6e\u4e3aTipMargin\u76844\u500d\uff0c\u6211\u8fd9\u91cc\u8bbe\u7f6e\u76841.44<\/span>\u3002\u6ce8\u610f\u5b83\u5bf9\u5e94\u5230\u53c2\u6570\u91cc\u662fsamplingOutMargin\uff09<\/li>\n
- Roll out\uff1a\u5e73\u884c\u6a2a\u5411\u91c7\u6837\u70b9\u5230\u6700\u5927\u89c4\u5212\u8ddd\u79bb\uff0c\u901a\u8fc7\u4ece\u5168\u5c40\u8def\u5f84\u5782\u76f4\u79fb\u52a8\u56fa\u5b9a\u7684\u8ddd\u79bb\uff08\u79f0\u4e3aRoll out\u5bc6\u5ea6\uff09\u6765\u6267\u884c\u76f4\u63a5\u6a2a\u5411\u91c7\u6837\u3002<\/span><\/li>\n<\/ul>\n
op_motion_predictor \u53c2\u6570<\/p>\n
<\/a><\/p>\n\n- Detect curbs from map\uff1a\u7528\u4e8e\u5c06\u8def\u6cbf\u5224\u5b9a\u4e3a\u969c\u788d\u7269<\/li>\n<\/ul>\n
op_trajectory_evaluator \u76f8\u5173\u53c2\u6570\uff1a<\/p>\n
<\/a><\/p>\n\n- Enable prediction\uff1a\u6253\u5f00\u9884\u6d4b\u529f\u80fd<\/li>\n<\/ul>\n
op_behavior_selector\u884c\u4e3a\u9009\u62e9\u5668\u72b6\u6001\u673a\uff1a\uff08\u72b6\u6001\u673a\u8f93\u51fatopic\uff1a\/current_behavior\uff09<\/p>\n
\n- \u505c\u6b62\u72b6\u6001:[#\u505c\u6b62\u72b6\u6001]<\/li>\n
- \u4efb\u52a1\u5b8c\u6210\u72b6\u6001:[#\u4efb\u52a1\u5b8c\u6210\u72b6\u6001]<\/li>\n
- \u8f6c\u5411\u72b6\u6001:[*\u524d\u8fdb\u72b6\u6001,#\u8f6c\u5411\u72b6\u6001]<\/li>\n
- \u505c\u6b62\u4fe1\u53f7\u505c\u6b62\u72b6\u6001:[*\u505c\u6b62\u4fe1\u53f7\u7b49\u5f85\u72b6\u6001,#\u505c\u6b62\u4fe1\u53f7\u505c\u6b62\u72b6\u6001]<\/li>\n
- \u524d\u8fdb\u72b6\u6001 :[*\u76ee\u6807\u72b6\u6001, #\u524d\u8fdb\u72b6\u6001, \u8f6c\u5411\u72b6\u6001, \u505c\u6b62\u4fe1\u53f7\u505c\u6b62\u72b6\u6001, \u4ea4\u901a\u706f\u505c\u6b62\u72b6\u6001, \u8ddf\u968f\u72b6\u6001, ]<\/li>\n
- \u76ee\u6807\u72b6\u6001 :[*\u4efb\u52a1\u5b8c\u6210\u72b6\u6001,#\u76ee\u6807\u72b6\u6001, \u524d\u8fdb\u72b6\u6001]<\/li>\n
- \u505c\u6b62\u4fe1\u53f7\u7b49\u5f85\u72b6\u6001:[*\u524d\u8fdb\u72b6\u6001,#\u505c\u6b62\u4fe1\u53f7\u7b49\u5f85\u72b6\u6001,\u505c\u6b62\u4fe1\u53f7\u505c\u6b62\u72b6\u6001, \u76ee\u6807\u72b6\u6001]<\/li>\n
- \u4ea4\u901a\u706f\u505c\u6b62\u72b6\u6001:[*\u524d\u8fdb\u72b6\u6001, #\u4ea4\u901a\u706f\u505c\u6b62\u72b6\u6001, \u4ea4\u901a\u706f\u7b49\u5f85\u72b6\u6001]<\/li>\n
- \u4ea4\u901a\u706f\u7b49\u5f85\u72b6\u6001:[*\u524d\u8fdb\u72b6\u6001, #\u4ea4\u901a\u706f\u7b49\u5f85\u72b6\u6001, \u4ea4\u901a\u706f\u505c\u6b62\u72b6\u6001, \u76ee\u6807\u72b6\u6001]<\/li>\n
- \u8ddf\u968f\u72b6\u6001:[*\u524d\u8fdb\u72b6\u6001, #\u8ddf\u968f\u72b6\u6001, \u8f6c\u5411\u72b6\u6001, \u505c\u6b62\u4fe1\u53f7\u505c\u6b62\u72b6\u6001, \u4ea4\u901a\u706f\u505c\u6b62\u72b6\u6001, \u76ee\u6807\u72b6\u6001]<\/li>\n
- \u521d\u59cb\u72b6\u6001:[*\u524d\u8fdb\u72b6\u6001, #\u521d\u59cb\u72b6\u6001]<\/li>\n<\/ul>\n
<\/a><\/p>\n5. \u901f\u5ea6\u8f93\u51fa\uff0c\u5c0f\u8f66\u5f00\u59cb\u5bfc\u822a<\/h4>\n
\u6253\u5f00[Computing]\u9875\u9762\uff0c\u627e\u5230 [waypoint_follower] \u4e0b [pure_pursuit] \u4e0e[twist_filter]\uff0c\u8bbe\u7f6e[app]\u7684\u4e00\u4e9b\u53c2\u6570\u540e\uff0c\u52fe\u9009 [pure_pursuit] \u4e0e [twist_filter] \u3002\u76f8\u5173\u4e00\u4e9b\u53c2\u6570\u8bbe\u7f6e\u5982\u4e0b\u56fe\uff1a<\/p>\n
\u901f\u5ea6\u8f93\u51fa\u52fe\u9009\u8282\u70b9\uff1a<\/p>\n
<\/a><\/p>\n5.1 pure_pursuit \u5de1\u8ff9\u8282\u70b9<\/h6>\n
\u4e3b\u8981\u4f5c\u7528\u662f\u63a5\u6536op_behavior_select\u8282\u70b9\u53d1\u5e03\u7684\u201c\/final_waypoints\u201d\u76ee\u6807\u8def\u5f84\uff0c\u6839\u636e\u7eaf\u8ddf\u8e2a\u7b97\u6cd5\uff0c\u8ba1\u7b97\u81ea\u8f66\u4e0b\u4e00\u65f6\u523b\u7684\u901f\u5ea6\u3001\u89d2\u901f\u5ea6\u6307\u4ee4\uff0c\u5e76\u53d1\u5e03\u5230\u201c\/twist_raw\u201dx\u8f74\u7ebf\u901f\u5ea6\u548cz\u8f74\u89d2\u901f\u5ea6\u3001”\/ctrl_raw”\u52a0\u901f\u5ea6\u4ee5\u53ca\u8f6c\u89d2topic\uff1a<\/p>\n
<\/a><\/p>\n\u76f8\u5173\u53c2\u6570\u5982\u4e0b\uff1a<\/p>\n
<\/a><\/p>\n\n- \u52fe\u9009 waypoint \u9009\u9879\uff0c\u4f7f\u7528\u8def\u5f84\u70b9\u7684\u901f\u5ea6\u8fdb\u884c\u8f93\u51fa<\/li>\n
- Lookahead_ratio\uff1a\u524d\u89c6\u8ddd\u79bb\u7cfb\u6570<\/li>\n
- Minimum_lookahead_distance\uff1a\u6700\u5c0f\u524d\u89c6\u8ddd\u79bb\u9650\u5236\uff0c\u8fd9\u91cc\u8bbe\u7f6e\u4e3a\u5c0f\u8f66\u7684\u6700\u5c0f\u8f6c\u5f2f\u534a\u5f84<\/li>\n
- Linear interpolation\uff1a\u662f\u5426\u7ebf\u6027\u63d2\u503c<\/li>\n
- \u5c0f\u8f66\u7684\u9884\u7784\u70b9\u8ddd\u79bb\u5c0f\u8f66\u7684\u8ddd\u79bb\u662f\u5c0f\u8f66\u7684\u524d\u89c6\u8ddd\u79bb\uff0c\u9884\u7784\u70b9\u662f\u5c0f\u8f66\u5728\u5bfc\u822a\u4e2d\u4e00\u76f4\u5728\u201c\u8ffd\u201d\u7684\u70b9\uff0c\u5f53\u524d\u884c\u9a76\u901f\u5ea6\u4e58\u4ee5\u7cfb\u6570 Lookahead_ratio \u5373\u4e3a\u5c0f\u8f66\u7684\u524d\u89c6\u8ddd\u79bb\u3002<\/li>\n<\/ul>\n
5.2 twist_filter\u5e73\u6ed1\u8282\u70b9<\/h6>\n
\u4e3b\u8981\u529f\u80fd\u662f\u5bf9pure_suit\u8282\u70b9\u8f93\u51fa\u7684\u6c7d\u8f66\u8fd0\u52a8\u901f\u5ea6\u8fdb\u884c\u4f4e\u901a\u6ee4\u6ce2\uff0c\u6d88\u9664\u6742\u6ce2\uff0c\u4f7f\u901f\u5ea6\u66f4\u52a0\u5e73\u6ed1\u3002<\/p>\n
<\/a><\/p>\n\u53c2\u6570\u5982\u4e0b\uff1a<\/p>\n
<\/a><\/p>\n\n- Lateral_accel_limit \u548c Lateral_jerk_limit\uff1a\u6a2a\u5411\u52a0\u51cf\u901f\u9650\u5236\uff0c\u9ed8\u8ba4\u5373\u53ef<\/li>\n<\/ul>\n
5.3 twist_gate \u63a7\u8f66\u8282\u70b9<\/h6>\n
\u8fd9\u91cc\u5355\u72ec\u8bf4\u4e0b\u8fd9\u4e2a\u8282\u70b9\uff0c\u5b83\u5176\u5b9e\u662f\u5728twist_filter\u5e73\u6ed1\u6ee4\u6ce2\u8282\u70b9\u542f\u52a8\u7684\uff0c\u4f46\u5b83\u5b9e\u9645\u8d1f\u8d23\u4e86\u8ddf\u5e95\u76d8\u7684\u63a7\u8f66\u901a\u4fe1\u3002\u5b83\u901a\u8fc7\u8ba2\u9605\u5e73\u6ed1\u540e\u7684\u63a7\u8f66\/twist_cmd\u3001\/ctrl_cmd topic\uff0c\u5c06\u6700\u7ec8\u7684\u63a7\u8f66\u6d88\u606f\u901a\u8fc7timerCallback\u53d1\u9001\u5230\u4e86\/vehicle_cmd\u3002<\/p>\n
*\u6ce8\uff1a\u82e5\u662f\u5bfc\u822a\u4e2d\u5c0f\u8f66\u5b9a\u4f4d\u5bb9\u6613\u4e71\uff0c\u8bf7\u8fd0\u884c rostopic hz \/points_raw \u89c2\u5bdf\u5c0f\u8f66\u96f7\u8fbe\u9891\u7387\u591a\u5c11\uff0c\u63a8\u8350\u96f7\u8fbe\u9891\u7387\u4e3a 20hz \u5de6\u53f3\uff0c\u592a\u5c0f\u5982 10hz \u5c0f\u8f66\u8fd0\u52a8\u65f6\u5bb9\u6613\u5b9a\u4f4d\u504f\u79fb\u3002<\/p>\n
<\/h6>\n
<\/p>\n
\u4e94\u3001\u7ed5\u969c<\/h1>\n
\u6309\u7167\u4e0a\u8ff0\u65b9\u6cd5\u5bfc\u822a\u65f6\uff0c\u9047\u5230\u969c\u788d\u7269\u4f1a\u505c\u6b62\u4e0b\u6765\uff0c\u4f46\u662f\u4e0d\u4f1a\u7ed5\u8fc7\u53bb\uff0c\u9700\u8981\u8c03\u6574\u4e9b\u5730\u65b9\u624d\u53ef\u4ee5\u505a\u5230<\/p>\n
1. \u969c\u788d\u7269\u8ddf\u8e2a\u4e0e\u805a\u7c7b\u7684\u5173\u8054<\/h4>\n# \u8ba2\u9605\u7684cloud_clusters topic\u540d\u9700\u8981\u4e0e\u805a\u7c7btopic\u5bf9\u5e94\u4e0a\r\nvim Autoware\/src\/autoware\/core_perception\/lidar_kf_contour_track\/nodes\/lidar_kf_contour_track\/lidar_kf_contour_track_core.cpp\r\n sub_cloud_clusters = nh.subscribe(\"\/detection\/lidar_detector\/cloud_clusters<\/span>\", 1, &ContourTracker::callbackGetCloudClusters, this);\r\n\r\n# \u6b27\u51e0\u91cc\u5f97\u805a\u7c7b\u8f93\u51fa\u7684topic\r\nvim Autoware\/src\/autoware\/core_perception\/lidar_euclidean_cluster_detect\/nodes\/lidar_euclidean_cluster_detect\/lidar_euclidean_cluster_detect.cpp\r\n _pub_clusters_message = h.advertise<autoware_msgs::CloudClusterArray>(\"\/detection\/lidar_detector\/cloud_clusters<\/span>\", 1);\r\n\r\n# \u91cd\u65b0\u7f16\u8bd1\r\ncolcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select=lidar_kf_contour_track\r\n\r\n# \u68c0\u67e5\u8282\u70b9\u8ba2\u9605\u5173\u7cfb\r\nrosrun rqt_graph rqt_graph<\/code><\/pre>\n\u786e\u4fddlidar_kf_contour_track\u8ddf\u8e2a\u8282\u70b9\u8ba2\u9605\u4e86lidar_euclidean_cluster_detect\u805a\u7c7b\u8282\u70b9\u8f93\u51fa\u7684\/detection\/lidar_detector\/cloud_clusters topic\uff1a<\/p>\n
<\/a><\/p>\n2. \u8c03\u6574\u53d1\u5e03frame<\/h4>\n
\u6b64\u65f6\u969c\u788d\u7269\u53ef\u4ee5\u88abop_planer\u8bc6\u522b\u548c\u8ddf\u8e2a\uff0c\u4f46\u662f\u663e\u793a\u7684\u4f4d\u7f6e\u5728\u5730\u56fe\u5750\u6807\u539f\u70b9\uff0c\u539f\u56e0\u662f\u4ece\u6e90\u5934\u4e0alidar\u68c0\u6d4b\u7684\u969c\u788d\u7269\u5c31\u662f\u57fa\u4e8e\u96f7\u8fbe\u5750\u6807\u7cfb\u7684\uff0c\u9700\u8981\u8c03\u6574\u4e3amap\u5750\u6807\u7cfb\uff0c\u5e76\u5c06\u805a\u7c7b\u7ed3\u679c\u8fdb\u884c\u5750\u6807\u7cfb\u7684\u540c\u6b65<\/p>\n
# \u68c0\u67e5\u805a\u7c7b\u7684frame\r\nrostopic echo \/detection\/lidar_detector\/cloud_clusters |grep frame_id\r\n frame_id: \"map<\/span>\"\r\n\r\n# \u68c0\u67e5\u969c\u788d\u7269\u8ddf\u8e2a\u7684frame\r\nrostopic echo \/tracked_objects |grep frame_id\r\n frame_id: \"map<\/span>\"\r\n\r\n# \u4fee\u6539\u969c\u788d\u7269\u8ddf\u8e2a\u7684frame\r\nvim Autoware\/src\/autolaunch\/launch\/detection.launch\r\n <arg name=\"output_frame\" default=\"map<\/span>\" \/>\r\n <arg name=\"sync\" default=\"true<\/span>\" \/> <!-- \u5c06\u805a\u7c7b\u7ed3\u679c\u4ecevelodyne\u5750\u6807\u7cfb\u540c\u6b65\u5230map\u5750\u6807\u7cfb -->\r\n\r\n# \u4fee\u6539\u5f71\u54cd\u907f\u969c\u7684\u51e0\u4e2a\u53c2\u6570\r\nvim Autoware\/src\/autolaunch\/launch\/planner.launch\r\n <include file=\"$(find op_local_planner)\/launch\/op_common_params.launch\">\r\n <arg name=\"horizontalSafetyDistance\" default=\"0.1\" \/> <!-- \u8f66\u8f86\u5b89\u5168\u6846-\u6a2a\u5411 -->\r\n <arg name=\"verticalSafetyDistance\" default=\"0.3\" \/> <!-- \u8f66\u8f86\u5b89\u5168\u6846-\u7eb5\u5411 -->\r\n <arg name=\"width\" default=\"0.35\" \/> <!-- \u8f66\u5bbd -->\r\n <arg name=\"length\" default=\"0.45\" \/> <!-- \u8f66\u957f -->\r\n <arg name=\"wheelBaseLength\" default=\"0.322\" \/> <!-- \u8f66\u8f86\u8f74\u8ddd -->\r\n <arg name=\"turningRadius\" default=\"0.75\" \/> <!-- \u8f66\u8f86\u8f6c\u5f2f\u534a\u5f84 -->\r\n \r\n <arg name=\"enableSwerving\" default=\"true\" \/> <!-- \u6253\u5f00\u907f\u969c\u529f\u80fd -->\r\n <arg name=\"enableFollowing\" default=\"true\" \/> <!-- \u6253\u5f00\u8ddf\u968f\u529f\u80fd -->\r\n <arg name=\"minFollowingDistance\" default=\"3.0\" \/> <!-- \u9700\u8981\u5927\u4e8e\u8ddf\u968f\u8ddd\u79bb -->\r\n <arg name=\"minDistanceToAvoid\" default=\"2.0\" \/> <!-- \u8ddd\u79bb\u969c\u788d\u7269\u591a\u8fdc\u624d\u5f00\u59cb\u907f\u969c\uff0c\u9700\u8981 <minFollowingDistance \u5e76\u4e14 >maxDistanceToAvoid -->\r\n <arg name=\"maxDistanceToAvoid\" default=\"0.8\" \/> <!-- \u505c\u8f66\u65f6\uff0c\u969c\u788d\u7269\u8ddd\u79bb\u591a\u8fdc\u53ef\u4ee5\u5f00\u8f66 -->\r\n <\/include>\r\n <include file=\"$(find op_local_planner)\/launch\/op_trajectory_evaluator.launch\">\r\n <arg name=\"enablePrediction\" default=\"true\" \/> <!-- \u6253\u5f00\u9884\u6d4b\u529f\u80fd -->\r\n <arg name=\"horizontalSafetyDistance\" default=\"0.1\" \/>\r\n <arg name=\"verticalSafetyDistance\" default=\"0.3\" \/>\r\n <\/include><\/code><\/pre>\nrviz\u6dfb\u52a0\/detected_polygons\u3001\/op_planner_tracked_boxes topic\u7684\u8f93\u51fa\uff0c\u53ef\u4ee5\u770b\u5230\u969c\u788d\u7269\u88ab\u8ddf\u8e2a\uff0c\u4f4d\u7f6e\u6b63\u5e38\uff0c\u5e76\u4e14\u5f00\u59cb\u5e72\u9884\u5c40\u90e8\u8def\u5f84\u89c4\u5212\uff1a<\/p>\n
\n- \u7ea2\u8272\uff1a\u4e0d\u53ef\u884c\u9a76\u7684\u8def\u7ebf<\/li>\n
- \u9ec4\u8272\uff1a\u53ef\u884c\u9a76\u7684\u8def\u7ebf<\/li>\n
- \u7d2b\u8272\uff1a\u4e3a\u5f53\u524d\u9009\u62e9\u884c\u9a76\u7684\u5c40\u90e8\u8def\u7ebf<\/li>\n<\/ul>\n
<\/a><\/p>\n3. \u5176\u4ed6\u7ed5\u969c\u6548\u679c\u4f18\u5316<\/h4>\n
\u5b9e\u8f66\u6d4b\u8bd5\uff0c\u53ef\u4ee5\u907f\u969c\u548c\u7ed5\u969c\u4e86\uff0c\u4f46\u662f\u8fd8\u6709\u51e0\u4e2a\u95ee\u9898\u9700\u8981\u89e3\u51b3\uff1a<\/p>\n
\n- \u5230\u7ec8\u70b9\u65f6\uff0c\u603b\u662f\u4f1a\u4f7f\u52b2\u9760\u5f80\u8def\u8fb9\u8d70\uff0c\u5373\u4f7f\u6709\u8f66\u6216\u8005\u6811\u4e1b\u4e5f\u4f7f\u52b2\u4e0a\u649e\uff1a\u611f\u89c9\u50cf\u662fop_planer\u5230\u7ec8\u70b9\u65f6\u6709\u7279\u6b8a\u7684\u9760\u8fb9\u505c\u8f66\u7b56\u7565\uff0c\u5e76\u4e14\u6b64\u65f6\u907f\u969c\/\u7ed5\u969c\u662f\u4e0d\u751f\u6548\u7684\uff0c\u9700\u8981\u6392\u67e5\u4e0b\uff1b\n
\n- \u969c\u788d\u7269\u8fd9\u5757\u53ef\u4ee5\u5148\u589e\u52a0\u5b89\u5168\u6846\u6765\u9650\u5236\u8f66\u8f86\u78b0\u649e\uff0c\u5e76\u5f3a\u8feb\u8f66\u8f86\u8d70\u9009\u62e9\u5176\u4ed6\u5c40\u90e8\u8def\u7ebf\u7ed5\u884c<\/li>\n
- \u4fee\u6539\u540e\u6b64\u95ee\u9898\u6d88\u5931\uff0c\u53ef\u80fd\u8ddf\u4e0b\u4e00\u4e2a\u95ee\u9898\u91cc\u7684imu\u504f\u79fb\u6709\u5173<\/li>\n<\/ul>\n<\/li>\n
- \u8f66\u8d70\u7684\u65f6\u5019\uff0c\u4ece\u5de6\u4fa7\u7ed5\u884c\u65f6\uff0c\u559c\u6b22\u8d34\u7740\u969c\u788d\u7269\u8d70\uff0c\u4ece\u53f3\u4fa7\u7ed5\u884c\u65f6\uff0c\u559c\u6b22\u79bb\u969c\u788d\u7269\u5f88\u8fdc\uff1a\u50cf\u662f\u8f66\u81ea\u8eab\u5de6\u53f3\u65b9\u5411\u6709\u70b9\u504f\u4f3c\u7684\uff0c\u9700\u8981\u6392\u67e5\u4e0b\u8f66\u8eab\u548c\u969c\u788d\u7269\u7269\u7684\u504f\u79fb\u5173\u7cfb\u662f\u4e0d\u662f\u54ea\u91cc\u4e0d\u5bf9\n
\n- \u5e94\u8be5\u662fimu\u76f8\u5bf9\u8f66\u540e\u8f74\u4e2d\u5fc3\u7684\u4f4d\u7f6e\u4e0d\u5bf9\uff0c\u8fd9\u4e2a\u8f66imu\u5b89\u88c5\u5728\u5de6\u4fa7\uff0c\u76f8\u5bf9\u8f66\u8eabtf\u8bbe\u7f6e\u4e3a0\uff0c\u90a3\u4e48\u8f66\u8f86\u5c31\u4f1a\u4ee5\u4e3aimu\u7684\u4f4d\u7f6e\u624d\u662f\u8f66\u8f86\u4e2d\u5fc3\uff0c\u4ece\u800c\u5bfc\u81f4\u4e24\u4fa7\u7684\u7ed5\u884c\u8ddd\u79bb\u51fa\u73b0\u5dee\u5f02<\/li>\n
- \u8c03\u6574imu\u4e0ebase_link\u7684tf\uff0c\u53d1\u5e03\u7684tf\u91cc\u5373\u53ef<\/li>\n<\/ul>\n<\/li>\n
- \u79bb\u8f66\u8f86\u5f88\u8fd1\u65f6\u7a81\u7136\u963b\u6321\u8f66\u8f86\uff0c\u8f66\u8f86\u4e0d\u4f1a\u907f\u969c\/\u7ed5\u969c\uff0c\u4f46\u662f\u7ad9\u57283\u7c73\u4ee5\u5916\u5c31\u80fd\u6b63\u5e38\u7ed5\n
\n- \u7cbe\u7ec6\u6d4b\u91cf\u5e76\u8c03\u6574\u4e0b\u8f66\u8f86\u7684\u51c6\u786e\u957f\u5bbd\u3001\u8f74\u8ddd\u3001\u8f6c\u5f2f\u534a\u5f84\u3001\u6700\u5927\u8f6c\u5411\u89d2\u7b49\u8f66\u8f86\u53c2\u6570<\/li>\n
- \u8c03\u6574\u5c40\u90e8\u8def\u5f84\u89c4\u5212tip\/in margin\u3001\u89c4\u5212\u8def\u5f84\u6761\u6570\/\u95f4\u8ddd\/\u8def\u5f84\u70b9\u8ddd\u79bb\u7b49\uff0c\u786e\u4fdd\u8986\u76d6\u53ef\u884c\u8fdb\u7684\u8def\u5bbd\u8db3\u4ee5\u89c4\u5212\u7ed5\u884c\u8def\u7ebf<\/li>\n
- \u8c03\u6574\u907f\u969c\u53c2\u6570\uff0c\u907f\u969c\u8ddd\u79bb\u3001\u5b89\u5168\u6846\u5927\u5c0f<\/li>\n<\/ul>\n<\/li>\n
- \n\u7528gps\u521d\u59cb\u5316\u5b9a\u4f4d\u5bb9\u6613\u98d8\uff0c\u9014\u4e2d\u98d8\u4e0d\u5bb9\u6613\u6062\u590d\u5b9a\u4f4d<\/div>\n
\n- \n\u7ecf\u8fc7\u4ed4\u7ec6\u89c2\u5bdf\uff0c\u5173\u952e\u5728gps\u7684\u822a\u5411\u4e0a\uff0c\u4e00\u65e6gps\u822a\u5411\u51c6\u786e\uff0c\u4fee\u6b63\u6210\u529f\u7684\u6982\u7387\u5c31\u4f1a\u5927\u5927\u589e\u52a0\uff1b<\/div>\n<\/li>\n
- \u521d\u59cb\u5316\u65f6\u589e\u52a0\u65cb\u8f6c\u822a\u5411\u5c1d\u8bd5match\u673a\u5236\uff0c\u6539\u9020\u540e\uff0c\u57fa\u672c\u79d2\u521d\u59cb\u5316\u5b9a\u4f4d\u6210\u529f\uff1b<\/li>\n
- \u884c\u8fdb\u9014\u4e2d\u589e\u52a0\u6a2a\u5411\u5e73\u79fb\u5c1d\u8bd5match\u673a\u5236\uff0c\u6539\u9020\u540e\uff0c\u5b9e\u8f66\u9a8c\u8bc1\u6548\u679c\u5c1a\u53ef\uff1b<\/li>\n
- \u5c3d\u91cf\u907f\u514d\u5728\u4fe1\u53f7\u4e0d\u597d\u65f6\u4e0a\u7535\uff0c\u6b64\u65f6\u8f66\u8f86\u79fb\u52a8\u5230\u4fe1\u53f7\u9010\u6e10\u53d8\u597d\u4f4d\u7f6e\u7684\u8fc7\u7a0b\u4e2d\uff0c\u5b9a\u4f4d\u4f1a\u504f\u79fb\uff0c\u5c3d\u91cf\u5728\u7a7a\u65f7\u4f4d\u7f6e\u91cd\u65b0\u4e0a\u7535<\/span>\uff0c\u8fd9\u6837\u521d\u59cb\u5b9a\u4f4d\u4f4d\u7f6e\u4f1a\u66f4\u52a0\u51c6\u786e\uff1b<\/li>\n
- \u540e\u7eed\u53ef\u8003\u8651<\/span>\u66f4\u6362\u5177\u6709\u51c6\u786e\u822a\u5411<\/span>\u7684imu<\/span>\u4ee5\u65b9\u4fbf\u786e\u5b9a\u521d\u59cb\u822a\u5411\uff1b<\/span>\u589e\u52a0\u6fc0\u5149\u96f7\u8fbe\u7ebf\u6570<\/span>\uff0c\u4ece\u800c\u63d0\u9ad8match\u7279\u5f81\u70b9<\/span>\u3002<\/span><\/li>\n<\/ul>\n<\/li>\n
- \u5c40\u90e8\u89c4\u5212\u7684\u8def\u5f84\uff0c\u96c6\u4e2d\u5728\u72ed\u7a84\u7684\u884c\u9a76\u7ebf\u4e0a\n
\n- \u5c40\u90e8\u89c4\u5212\u8def\u7ebf\u8bbe\u7f6e\u592a\u5c11\uff0c\u6216\u8005\u8def\u7ebf\u95f4\u95f4\u8ddd\u8fc7\u5c0f\uff0c\u8c03\u5927\u8986\u76d6\u53ef\u884c\u8fdb\u7684\u6574\u4e2a\u8def\u5bbd\u5c31\u597d\u4e86<\/li>\n<\/ul>\n<\/li>\n
- \u8def\u5f84\u89c4\u5212\u4f1a\u89c4\u5212\u5230\u975e\u9053\u8def\u4e0a\n
\n- \u4fee\u6539\u5730\u56fe\uff0c\u589e\u52a0\u8def\u6cbf<\/span>\u9650\u5236<\/span><\/li>\n<\/ul>\n<\/li>\n
- \u8fc7\u51cf\u901f\u5e26\u65f6\uff0c\u5bb9\u6613match\u5b9a\u4f4d\u5931\u8d25\n
\n- \u4fee\u6539\u5730\u56fe\uff0c\u589e\u52a0\u51cf\u901f<\/span>\u5e26\u6807\u8bb0<\/span>\uff0c\u964d\u4f4e\u98a0\u7c38<\/li>\n<\/ul>\n<\/li>\n
- \u4e2d\u9014\u5bb9\u6613\u5728\u5b89\u5168\u7684\u60c5\u51b5\u4e0b\u51cf\u901f\u6216\u6682\u505c\n
\n- \u76ee\u524d\u770b\u4ee5\u4e0a\u4fee\u6539\u8c03\u6574\u540e\u5b9e\u8f66\u9a8c\u8bc1\uff0c\u6b64\u95ee\u9898\u57fa\u672c\u6d88\u5931\uff0c\u5e94\u8be5\u8ddfimu tf\u504f\u79fb\u5bfc\u81f4\u969c\u788d\u7269\u8ddd\u79bb\u504f\u79fb\u6709\u5173<\/li>\n<\/ul>\n<\/li>\n<\/ol>\n
<\/p>\n
\u516d\u3001\u591a\u70b9\u8fde\u7eed\u5bfc\u822a<\/h1>\n
\u76ee\u524d\u5b9e\u8f66\u6d4b\u8bd5\u770b\uff0copen planner\u5bfc\u822a\u5e76\u4e0d\u652f\u6301\u8fde\u7eed\u5bfc\u822a\uff0c\u5728\u5bfc\u822a\u5230\u7b2c\u4e00\u4e2a\u76ee\u6807\u70b9\u540e\uff0c\u9700\u8981\u91cd\u65b0\u8fd0\u884c\u5168\u5c40\u8def\u5f84\u89c4\u5212\u3001\u5c40\u90e8\u8def\u5f84\u89c4\u5212\u3001\u4ee5\u53capure persuit\u63a7\u5236\u8282\u70b9\u624d\u80fd\u5bfc\u822a\u5230\u4e0b\u4e00\u4e2a\u70b9\u3002\u6211\u4eec\u8fd9\u91cc\u53ef\u4ee5\u4fee\u6539\u90e8\u5206\u4ee3\u7801\uff0c\u8ba9\u5b83\u80fd\u591f\u5728\u4e0a\u4e00\u5bfc\u822a\u7ed3\u675f\u540e\uff0c\u76f4\u63a5\u8bbe\u5b9a\u5bfc\u822a\u5230\u4e0b\u4e00\u4e2a\u70b9\uff0c\u800c\u4e0d\u9700\u8981\u91cd\u542f\u4efb\u4f55\u8282\u70b9\u3002<\/p>\n
0. op_global_planner_core.cpp\u7684callbackGetGoalPose()<\/h4>\n
\u6253\u5370m_GoalsPos\u76ee\u6807\u70b9\u5217\u8868\u7684\u6dfb\u52a0\u8bb0\u5f55<\/p>\n
void GlobalPlanner::callbackGetGoalPose(const geometry_msgs::PoseStampedConstPtr &msg)\r\n{\r\n PlannerHNS::WayPoint wp = PlannerHNS::WayPoint(msg->pose.position.x+m_OriginPos.position.x, msg->pose.position.y+m_OriginPos.position.y, msg->pose.position.z+m_OriginPos.position.z, tf::getYaw(msg->pose.orientation));\r\n m_GoalsPos.push_back(wp);\r\n ROS_INFO(\"Received Goal Pose<\/span>: GoalsPosSize=%ld NewPose=%f,%f,%f OrignPos=%f,%f,%f\", m_GoalsPos.size(), msg->pose.position.x, msg->pose.position.y, msg->pose.position.z, m_OriginPos.position.x, m_OriginPos.position.y, m_OriginPos.position.z);\r\n}<\/code><\/pre>\n1. op_global_planner_core.cpp\u7684MainLoop()<\/h4>\n
\u63a5\u8fd1\u5f53\u524d\u76ee\u6807\u70b9\u8ddd\u79bb\u65f6\uff0c\u5f00\u59cb\u4e0b\u4e00\u4e2a\u76ee\u6807\u70b9\u7684\u5168\u5c40\u8def\u5f84\u89c4\u5212\uff1a<\/p>\n
void GlobalPlanner::MainLoop()\r\n{\r\n \r\n if(m_GoalsPos.size() > 0)\r\n {\r\n if(m_GeneratedTotalPaths.size() > 0 && m_GeneratedTotalPaths.at(0).size() > 3)\r\n {\r\n \/\/ \u652f\u6301\u5728\u5bfc\u822a\u5230\u7b2c\u4e00\u4e2a\u76ee\u6807\u70b9\u540e\uff0c\u4e0d\u91cd\u542f\u7684\u60c5\u51b5\u4e0b\u652f\u6301\u76f4\u63a5\u5bfc\u822a\u5230\u4e0b\u4e00\u4e2a\u70b9<\/span> yan 24.12.10\r\n if(m_params.bEnableReplanning && m_iCurrentGoalIndex<m_GoalsPos.size()-1)\r\n {\r\n PlannerHNS::RelativeInfo info;\r\n \/\/ \u8f66\u8f86\u662f\u5426\u5728\u5f53\u524d\u76ee\u7684\u5730\u89c4\u5212\u8def\u5f84\u4e0a\uff0c\u5982\u5728\u7684\u8bdd\uff0c\u83b7\u5f97\u6240\u5728link\u4f4d\u7f6e\u4fe1\u606f<\/span>\r\n bool ret = PlannerHNS::PlanningHelpers::GetRelativeInfoRange(m_GeneratedTotalPaths, m_CurrentPose, 0.75, info); \r\n ROS_INFO(\"Goal Pose Enable Replanning Check: GetRelativeInfoRange=%d iGlobalPath=%d m_GeneratedTotalPaths.size=%ld info.iFront=%d m_GeneratedTotalPaths.at(info.iGlobalPath).size=%ld\", ret, info.iGlobalPath, m_GeneratedTotalPaths.size(), info.iFront, m_GeneratedTotalPaths.at(info.iGlobalPath).size());\r\n if(ret == true && info.iGlobalPath >= 0 && info.iGlobalPath < m_GeneratedTotalPaths.size() && info.iFront > 0 && info.iFront < m_GeneratedTotalPaths.at(info.iGlobalPath).size())\r\n {\r\n \/\/ \u8f66\u8f86\u5f53\u524d\u4f4d\u7f6e\u8ddd\u79bb\u5f53\u524d\u76ee\u7684\u5730\u7684\u8ddd\u79bb\uff08<=30\u7c73\u65f6\uff0c\u81ea\u52a8\u5f00\u59cb\u4e0b\u4e00\u76ee\u7684\u5730\u7684\u5168\u5c40\u8def\u5f84\u89c4\u5212\uff09<\/span>\r\n double remaining_distance = m_GeneratedTotalPaths.at(info.iGlobalPath).at(m_GeneratedTotalPaths.at(info.iGlobalPath).size()-1).cost - (m_GeneratedTotalPaths.at(info.iGlobalPath).at(info.iFront).cost + info.to_front_distance);\r\n ROS_INFO(\"Goal Pose Remaining Distance<\/span> %f <= %d \", remaining_distance, REPLANNING_DISTANCE);\r\n if(remaining_distance <= REPLANNING_DISTANCE<\/span>)\r\n {\r\n bMakeNewPlan = true;\r\n if(m_GoalsPos.size() > 0){\r\n ROS_WARN(\"m_iCurrentGoalIndex = %d\", m_iCurrentGoalIndex);\r\n m_iCurrentGoalIndex = (m_iCurrentGoalIndex + 1) % m_GoalsPos.size();\r\n }\r\n ROS_INFO(\"Change Goal Pose<\/span>: Index=%d, GoalsPosSize=%ld \", m_iCurrentGoalIndex, m_GoalsPos.size());\r\n }\r\n }\r\n }\r\n }\r\n \r\n}<\/code><\/pre>\n2. BehaviorStateMachine.cpp\u7684MissionAccomplishedStateII::GetNextState()<\/h4>\nBehaviorStateMachine* MissionAccomplishedStateII::GetNextState()\r\n{\r\n \/\/ \u6ce8\u91ca\u539f\u6709\u4ee3\u7801\r\n \/\/ return FindBehaviorState(this->m_Behavior);\r\n \/\/ \u6539\u6210\u4ee5\u4e0b\u7684\u4ee3\u7801\r\n PreCalculatedConditions* pCParams = GetCalcParams();\r\n if(pCParams->currentGoalID == -1) {\r\n return FindBehaviorState(this->m_Behavior);\r\n } else {\r\n pCParams->prevGoalID = pCParams->currentGoalID;\r\n return FindBehaviorState(FORWARD_STATE);\r\n }\r\n}<\/code><\/pre>\n3. pure_pursuit\u6a21\u5757\u5728final_waypoints\u4e3a\u7a7a\u65f6\u4f1a\u51fa\u5f02\u5e38<\/h4>\n
\u5148\u5728pure_pursuit_core.h\u4e2d\u6dfb\u52a0 ros::Subscriber sub_ndt_stat\u5b9a\u4e49\uff0c\u5e76\u589e\u52a0\u56de\u8c03\u51fd\u6570\u58f0\u660e\uff1a<\/p>\n
private:\r\n ros::Subscriber sub_ndt_stat;\r\n ...\r\n \/\/ callbacks\r\n void loopWaypointsCallback(const autoware_msgs::LaneConstPtr &msg);<\/code><\/pre>\npure_pursuit_core.cpp\u6dfb\u52a0sub_ndt_stat\u7684\u5b9a\u4e49\uff1a<\/p>\n
void PurePursuitNode::initForROS()\r\n{\r\n \/\/ setup subscriber\r\n sub_ndt_stat = nh_.subscribe(\"loop_waypoints\", 10, &PurePursuitNode::loopWaypointsCallback, this); \/\/add\r\n}<\/code><\/pre>\npure_pursuit_core.cpp \u4e2d\u6dfb\u52a0sub_ndt_stat<\/code>\u7684\u56de\u8c03\u51fd\u6570\uff1a<\/p>\nstatic autoware_msgs::Lane loopWaypoints;\r\nvoid PurePursuitNode::loopWaypointsCallback(const autoware_msgs::LaneConstPtr &msg)\r\n{\r\n loopWaypoints = *msg;\r\n}<\/code><\/pre>\npure_pursuit_core.cpp \u4e2d\u4fee\u6539callbackFromWayPoints\u51fd\u6570\uff0c\u6539\u6210\u4ee5\u4e0b\u6837\u5b50\uff1a<\/p>\n
void PurePursuitNode::callbackFromWayPoints(\r\n const autoware_msgs::LaneConstPtr& msg)\r\n{\r\n autoware_msgs::Lane::Ptr msg_output(new autoware_msgs::Lane);\r\n *msg_output = *msg;\r\n if (!loopWaypoints.waypoints.empty())\r\n {\r\n msg_output->waypoints.insert(msg_output->waypoints.end(), loopWaypoints.waypoints.begin(), loopWaypoints.waypoints.end());\r\n loopWaypoints.waypoints.clear();\r\n }\r\n if (msg_output->waypoints.empty()) return; \/\/ \u5982\u679cwaypoints\u4e3a\u7a7a\uff0cpp_\u4f1a\u51fa\u5f02\u5e38 \r\n \r\n if (!msg_output->waypoints.empty())\r\n command_linear_velocity_ = msg_output->waypoints.at(0).twist.twist.linear.x; \/\/ \u4ece\u822a\u70b9\u4e2d\u5f97\u5230\u901f\u5ea6\r\n else\r\n command_linear_velocity_ = 0;\r\n \r\n pp_.setCurrentWaypoints(msg_output->waypoints);\r\n is_waypoint_set_ = true;\r\n}\r\n <\/code><\/pre>\n4. \u81ea\u52a8\u8bbe\u7f6e\u591a\u4e2a\u8fde\u7eed\u7684\u76ee\u6807\u70b9<\/h4>\nvim Robot\/goals.py\r\n\r\n#! \/usr\/bin\/env python3\r\n\r\n# \u53d1\u9001\u76ee\u6807\u70b9\r\n# python3 goals.py\r\n\r\n\r\nimport rospy\r\nimport time\r\nimport math\r\nfrom geometry_msgs.msg import PoseStamped\r\n\r\n\r\n\r\ndef InitXiangyansiGoals():\r\n \"\"\"\u521d\u59cb\u5316\u76ee\u6807\u4f4d\u7f6e\u5217\u8868\"\"\"\r\n goals = []\r\n\r\n t = time.time()\r\n secs = int(t)\r\n nsecs = int((t - secs) * 1000000000)\r\n # print(t, secs, nsecs)\r\n\r\n # \u897f\u5357\u89d2\r\n pose_westsourth = PoseStamped()\r\n pose_westsourth.header.frame_id = 'world'\r\n pose_westsourth.header.stamp.secs = secs\r\n pose_westsourth.header.stamp.nsecs = nsecs\r\n pose_westsourth.pose.position.x = -36.194183349609375\r\n pose_westsourth.pose.position.y = -3.115070343017578\r\n pose_westsourth.pose.position.z = 0.0\r\n pose_westsourth.pose.orientation.x = 0.0\r\n pose_westsourth.pose.orientation.y = 0.0\r\n pose_westsourth.pose.orientation.z = 0.9140234511288408\r\n pose_westsourth.pose.orientation.w = 0.4056613498800741\r\n goals.append(pose_westsourth)\r\n\r\n\r\n # \u4e2d\u95f4\u9053\u8def\r\n pose_middlerow = PoseStamped()\r\n pose_middlerow.header.frame_id = 'world'\r\n pose_middlerow.header.stamp.secs = secs\r\n pose_middlerow.header.stamp.nsecs = nsecs\r\n pose_middlerow.pose.position.x = -1.769686222076416\r\n pose_middlerow.pose.position.y = 52.081050872802734\r\n pose_middlerow.pose.position.z = 0.0\r\n pose_middlerow.pose.orientation.x = 0.0\r\n pose_middlerow.pose.orientation.y = 0.0\r\n pose_middlerow.pose.orientation.z = 0.3816013240128726\r\n pose_middlerow.pose.orientation.w = 0.9243270143794471\r\n goals.append(pose_middlerow)\r\n\r\n # \u516c\u5bd3\u697c\r\n pose_apartment = PoseStamped()\r\n pose_apartment.header.frame_id = 'world'\r\n pose_apartment.header.stamp.secs = secs\r\n pose_apartment.header.stamp.nsecs = nsecs\r\n pose_apartment.pose.position.x = 36.168060302734375\r\n pose_apartment.pose.position.y = 35.61585998535156\r\n pose_apartment.pose.position.z = 0.0\r\n pose_apartment.pose.orientation.x = 0.0\r\n pose_apartment.pose.orientation.y = 0.0\r\n pose_apartment.pose.orientation.z = 0.96506640968254\r\n pose_apartment.pose.orientation.w = -0.26200539097975056\r\n goals.append(pose_apartment)\r\n\r\n # # \u505c\u8f66\u573a\r\n # pose_parkinglot = PoseStamped()\r\n # pose_parkinglot.header.frame_id = 'world'\r\n # pose_parkinglot.header.stamp.secs = secs\r\n # pose_parkinglot.header.stamp.nsecs = nsecs\r\n # pose_parkinglot.pose.position.x = -14.472017288208008\r\n # pose_parkinglot.pose.position.y = 79.07386779785156\r\n # pose_parkinglot.pose.position.z = 0.0\r\n # pose_parkinglot.pose.orientation.x = 0.0\r\n # pose_parkinglot.pose.orientation.y = 0.0\r\n # pose_parkinglot.pose.orientation.z = -0.4085631512503463\r\n # pose_parkinglot.pose.orientation.w = 0.9127300539811246\r\n # goals.append(pose_parkinglot)\r\n\r\n\r\n # \u897f\u5317\u89d2\u9053\u8def\r\n pose_westnorth = PoseStamped()\r\n pose_westnorth.header.frame_id = 'world'\r\n pose_westnorth.header.stamp.secs = secs\r\n pose_westnorth.header.stamp.nsecs = nsecs\r\n pose_westnorth.pose.position.x = -41.374229431152344\r\n pose_westnorth.pose.position.y = 85.13764953613281\r\n pose_westnorth.pose.position.z = 0.0\r\n pose_westnorth.pose.orientation.x = 0.0\r\n pose_westnorth.pose.orientation.y = 0.0\r\n pose_westnorth.pose.orientation.z = 0.3484739589582032\r\n pose_westnorth.pose.orientation.w = 0.9373184623851152\r\n goals.append(pose_westnorth)\r\n\r\n # \u5bfa\u5e99\u95e8\u53e3\u5de6\u4fa7\r\n pose_templeleft = PoseStamped()\r\n pose_templeleft.header.frame_id = 'world'\r\n pose_templeleft.header.stamp.secs = secs\r\n pose_templeleft.header.stamp.nsecs = nsecs\r\n pose_templeleft.pose.position.x = 83.72945404052734\r\n pose_templeleft.pose.position.y = 171.56243896484375\r\n pose_templeleft.pose.position.z = 0.0\r\n pose_templeleft.pose.orientation.x = 0.0\r\n pose_templeleft.pose.orientation.y = 0.0\r\n pose_templeleft.pose.orientation.z = 0.3451665329359535\r\n pose_templeleft.pose.orientation.w = 0.9385414559522522\r\n goals.append(pose_templeleft)\r\n\r\n # \u5c0f\u5356\u90e8\r\n pose_store = PoseStamped()\r\n pose_store.header.frame_id = 'world'\r\n pose_store.header.stamp.secs = secs\r\n pose_store.header.stamp.nsecs = nsecs\r\n pose_store.pose.position.x = 315.1929626464844\r\n pose_store.pose.position.y = 318.74285888671875\r\n pose_store.pose.position.z = 0.0\r\n pose_store.pose.orientation.x = 0.0\r\n pose_store.pose.orientation.y = 0.0\r\n pose_store.pose.orientation.z = 0.7824470064057306\r\n pose_store.pose.orientation.w = 0.622717176707621\r\n goals.append(pose_store)\r\n\r\n # \u5bfa\u5e99\u5927\u95e8\r\n pose_temple = PoseStamped()\r\n pose_temple.header.frame_id = 'world'\r\n pose_temple.header.stamp.secs = secs\r\n pose_temple.header.stamp.nsecs = nsecs\r\n pose_temple.pose.position.x = 102.62203979492188\r\n pose_temple.pose.position.y = 163.55311584472656\r\n pose_temple.pose.position.z = 0.0\r\n pose_temple.pose.orientation.x = 0.0\r\n pose_temple.pose.orientation.y = 0.0\r\n pose_temple.pose.orientation.z = -0.47270840074021886\r\n pose_temple.pose.orientation.w = 0.8812189102996058\r\n goals.append(pose_temple)\r\n\r\n # \u516c\u5bd3\u697c\r\n pose_apartment.header.stamp.secs = secs\r\n pose_apartment.header.stamp.nsecs = nsecs\r\n goals.append(pose_apartment)\r\n\r\n return goals\r\n\r\n\r\nif __name__ == '__main__':\r\n \"\"\"main\"\"\"\r\n # init\r\n rospy.init_node('goal_pose')\r\n pub_goal = rospy.Publisher('\/move_base_simple\/goal', PoseStamped, queue_size = 1)\r\n \r\n pose = PoseStamped()\r\n pub_goal.publish(pose)\r\n time.sleep(1)\r\n\r\n # send goal pose\r\n goals = InitXiangyansiGoals()\r\n for goal in goals:\r\n pub_goal.publish(goal)\r\n time.sleep(5)\r\n<\/code><\/pre>\n5. \u7f16\u8bd1\u9a8c\u8bc1<\/h4>\n
\u91cd\u65b0\u7f16\u8bd1\uff0c\u5f53\u4e0a\u8f66\u5230\u8fbe\u4e0a\u4e00\u76ee\u6807\u70b9\u540e\uff0c\u76f4\u63a5\u8bbe\u7f6e\u4e0b\u4e00\u76ee\u6807\u70b9\uff0c\u8f66\u8f86\u5373\u53ef\u81ea\u52a8\u8fd0\u884c\uff1a<\/p>\n
cd ~\/Autoware\r\nAUTOWARE_COMPILE_WITH_CUDA=1 colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select=op_global_planner\r\nor\r\n.\/build.sh op_global_planner<\/code><\/pre>\n\u4e5f\u53ef\u4ee5\u76f4\u63a5\u6839\u636e\u9884\u5148\u8bbe\u5b9a\u597d\u7684\u4e00\u7ec4\u9014\u5f84\u70b9\uff0c\u4e00\u952e\u81ea\u52a8\u8bbe\u5b9a\u76ee\u6807\uff08\u7528\u4e8e\u56fa\u5b9a\u5de1\u903b\u70b9\u8def\u7ebf\u5de1\u903b\uff09\uff1a<\/p>\n
cd Robot\/\r\npython3 goals.py<\/code><\/pre>\n <\/p>\n
\u4e03\u3001\u53d8\u9053<\/h1>\n
OpenPlanner\u89c4\u5212\u5168\u5c40\u8def\u5f84\uff0c\u5982\u679c\u8981\u652f\u6301\u5b9e\u65f6\u6839\u636e\u73af\u5883\u53d8\u9053\u548c\u6539\u53d8\u8def\u5f84\uff0c\u9700\u8981\u5728op_global_planner<\/strong>\u4e2d\u542f\u7528enableLaneChange\u3001<\/span>enableReplan<\/span>\u3001<\/span>enableDynamicMapUpdate\uff0c\u540c\u65f6\u5728vector_map\u7ed8\u5236\u591a\u6761\u5e76\u884cLane\u884c\u9a76\u7ebf\uff08\u9700\u8981\u7528golbal planner\u9a8c\u8bc1\u4e0b\u6240\u6709\u7684\u8def\u53e3\u8f6c\u5f2f\uff0c\u540c\u65f6\u6ce8\u610f\u6240\u6709\u4ea4\u53c9\u8def\u53e3\u7684\u5730\u65b9Lane\u5fc5\u987b\u5207\u65ad\uff0c\u7528\u72ec\u7acb\u7684\u8854\u63a5Lane\u8fde\u63a5\u4e0d\u540c\u7684\u8def\uff0c\u5426\u5219\u5207\u6362\u8def\u7ebf\u4f1a\u6709\u95ee\u9898\uff09<\/span>\u3002<\/p>\n\u6574\u4e2a\u53d8\u9053\u7684\u6d41\u7a0b\u5982\u4e0b\uff1a<\/p>\n
<\/a><\/p>\n\u64ad\u5305\u5355\u72ec\u9a8c\u8bc1\u8def\u7ebf\u5207\u6362\u548c\u5404\u8def\u7ebf\u7684\u8fde\u901a\u578b\uff1a<\/p>\n
# \u52a0\u8f7d\u5730\u56fe\u7b49\u5176\u4ed6\u6a21\u5757\r\nroslaunch autolaunch run.launch use_gnss:=1 rviz:=true map_name:=jingheyuan bag_file:=\/home\/autoware\/shared_dir\/bag\/run-4-jhy-bz\/2024-12-11-22-26-17.bag planner:=false\r\n\r\n# \u5355\u72ecdebug\u8def\u5f84\u89c4\u5212\r\nroslaunch autolaunch planner.launch<\/code><\/pre>\n\u4e0a\u8f66\u9a8c\u8bc1\uff0c\u5728\u539f\u6709\u884c\u9a76\u8def\u7ebf\u4e0a\u653e\u7f6e\u969c\u788d\u7269\uff0c\u5373\u53ef\u81ea\u52a8\u5207\u6362\u5e76\u884c\u7684\u5176\u4ed6\u884c\u9a76\u7ebf\u3002<\/p>\n
<\/a><\/p>\n\u516b\u3001\u5b89\u5168\u505c\u8f66<\/h1>\n1. \u5b9a\u4f4d\u8d28\u91cf\u5dee\u573a\u666f<\/h4>\n
\u524d\u51e0\u5929\u8c03\u8f66\u65f6\uff0c\u9047\u5230\u4e00\u6b21\u5b9a\u4f4d\u98d8\u540e\u8f66\u8f86\u4f9d\u7136\u5728\u79fb\u52a8\u5bfc\u81f4\u78b0\u649e\u7684\u60c5\u51b5\uff0c\u6211\u4eec\u8fd9\u91cc\u5728control\u903b\u8f91\u91cc\u6dfb\u52a0\u4e00 \u4e2a\u5b89\u5168\u4fdd\u62a4\uff1a\u5f53\u5b9a\u4f4d\u8d28\u91cf\u5dee\u65f6\uff0c\u505c\u6b62\u63a7\u8f66\u6307\u4ee4\u6307\u4ee4\u7684\u8f93\u51fa\u3002<\/p>\n
\u8003\u8651\u5230\u53d1\u51fa\u79fb\u52a8\u6307\u4ee4\u4f46\u8f66\u8f86\u672a\u79fb\u52a8\u65f6\uff0cpnc\u4f1a\u589e\u52a0\u8f6c\u901f\uff0c\u4e3a\u907f\u514d\u53d1\u751f\u610f\u5916\uff0c\u6211\u4eec\u76f4\u63a5\u5728\u63a7\u5236\u7684\u6e90\u5934\u62e6\u622a\uff0c\u5373pure_pursuit \u8def\u5f84\u8ddf\u8e2a\u8282\u70b9\uff0c\u62e6\u622a\u539f\u59cb\u63a7\u5236\u6307\u4ee4\/twist_raw\u3001\/ctrl_raw\u7684\u53d1\u51fa\u3002<\/p>\n
vim Autoware\/src\/autoware\/core_planning\/pure_pursuit\/src\/pure_pursuit_core.cpp\r\n #include \"autoware_msgs\/NDTStat.h\"\r\n\r\n void PurePursuitNode::initForROS() {\r\n ...\r\n sub6_ = nh_.subscribe(\"ndt_stat\", 10, &PurePursuitNode::callbackFromNdtStat, this);\r\n ...\r\n }\r\n\r\n \/\/ \u8ba2\u9605ndt_stat\u72b6\u6001\uff0c\u7528\u4e8e\u6839\u636escore\u51b3\u5b9a\u662f\u5426\u505c\u4e0b\u8f66\u8f86 yan 24.12.25\r\n void PurePursuitNode::callbackFromNdtStat(const autoware_msgs::NDTStatConstPtr &msg) {\r\n if (msg->score != 0.0 && msg->score < 1.0) {\r\n good_location_quality_ = true;\r\n } else { \/\/ \u51fa\u73b01\u5e27\u5b9a\u4f4d\u8d28\u91cf\u98d8\uff0c\u5373\u7acb\u5373\u505c\u6b62\u79fb\u52a8\u8f66\u8f86\r\n good_location_quality_ = false;\r\n }\r\n }\r\n\r\n void PurePursuitNode::run() {\r\n ...\r\n bool can_get_curvature = pp_.canGetCurvature(&kappa);\r\n if (!good_location_quality_) \/\/ \u5f53\u5b9a\u4f4d\u8d28\u91cf\u4e0d\u597d\u65f6\uff0c\u4e0d\u5141\u8bb8\u63a7\u8f66\r\n can_get_curvature = false;\r\n ...\r\n }\r\n<\/code><\/pre>\n\u4fee\u6539\u540e\u7f16\u8bd1\u5e76\u8fd0\u884c\uff0c\u76d1\u63a7\u5728\u5b9a\u4f4d\u98d8\u65f6\uff0c\u63a7\u8f66\u901f\u5ea6\u4ee5\u53ca\u8f6c\u89d2topic\u662f\u5426\u53d8\u4e3a0\uff0c\u5e76\u5728\u5b9a\u4f4d\u6062\u590d\u540e\u7ee7\u7eed\u63a7\u8f66\uff1a<\/p>\n
# build\r\ncolcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select=pure_pursuit\r\n\r\n# check\r\nrostopic echo \/ctrl_raw<\/code><\/pre>\n2. \u5b9a\u9519\u4f4d\u7f6e\u573a\u666f<\/h4>\n
\u9664\u4e86\u4e0a\u8ff0\u573a\u666f\u65f6\uff0c\u8fd8\u5b58\u5728\u4e00\u79cd\u60c5\u51b5\uff0c\u5c31\u662f\u5b9a\u4f4d\u8d28\u91cf\u663e\u793a\u5f88\u597d\uff0c\u4f46\u5b9e\u9645\u8ba9\u5b83\u5b9a\u9519\u4f4d\u7f6e\u4e86\uff0c\u6b64\u65f6\u5b83\u4f1a\u57fa\u4e8e\u9519\u8bef\u7684\u4f4d\u7f6e\u89c4\u5212\u8def\u7ebf\uff0c\u800c\u969c\u788d\u7269\u57fa\u4e8emap\u5750\u6807\u7cfb\u8f6c\u6362\u540e\uff0c\u4e5f\u4f1a\u51fa\u73b0\u5728\u9519\u8bef\u7684\u4f4d\u7f6e\u4e0a\uff0c\u5bfc\u81f4\u6b64\u65f6\u8f66\u8f86\u78b0\u649e\u95ee\u9898\u3002\u8fd9\u91cc\u6709\u4e2a\u7b80\u5355\u7c97\u66b4\u7684\u65b9\u6cd5\uff0c\u5c31\u662f\u589e\u52a0lidar\u8fd1\u8ddd\u79bb\u70b9\u4e91\u68c0\u6d4b\uff0c\u5373lidar\u68c0\u6d4b\u5230\u81ea\u8eab\u5750\u6807\u7cfb\u9644\u8fd1\u8ddd\u79bb\u6709\u969c\u788d\u7269\uff0c\u90a3\u4e48\u5c31\u5fc5\u987b\u5b89\u5168\u505c\u8f66\u3002<\/p>\n
\u540c\u4e0a\uff0c\u6211\u4eec\u76f4\u63a5\u5728\u63a7\u5236\u7684\u6e90\u5934\u62e6\u622a\u539f\u59cb\u63a7\u5236\u6307\u4ee4\/twist_raw\u3001\/ctrl_raw\u7684\u53d1\u51fa\u3002<\/p>\n
vim Autoware\/src\/autoware\/core_planning\/pure_pursuit\/launch\/pure_pursuit.launch\r\n <arg name=\"safety_distance\" default=\"0.5\" \/> <!-- \u8f66\u8f86\u5b89\u5168\u8ddd\u79bb\uff08\u8f66\u8eab\u5750\u6807\u7cfb\u4e0b\uff0clidar\u5982\u679c\u63a2\u6d4b\u5230\u6b64\u8ddd\u79bb\u5185\u7684\u969c\u788d\u7269\uff0c\u5219\u8ba4\u4e3a\u8f66\u8f86\u4e0d\u5b89\u5168\uff0c\u6682\u65f6\u505c\u6b62\u63a7\u8f66\uff09 -->\r\n <arg name=\"points_topic\" default=\"\/points_no_ground\" \/> <!-- \uff0c\u8f66\u8f86\u5b89\u5168\u68c0\u67e5\u8f93\u5165\u70b9\u4e91topic\uff0c\u8fd9\u91cc\u4f7f\u7528\u8fc7\u6ee4\u5730\u9762\u540e\u7684\u96f7\u8fbe\u8bdd\u9898\/points_no_ground -->\r\nvim Autoware\/src\/autoware\/core_planning\/pure_pursuit\/include\/pure_pursuit\/pure_pursuit_core.h\r\n #include <sensor_msgs\/PointCloud2.h>\r\n #include <pcl_conversions\/pcl_conversions.h>\r\n #include <pcl\/PCLPointCloud2.h>\r\n #include <pcl\/conversions.h>\r\n #include <pcl_ros\/transforms.h>\r\n #include <pcl_ros\/point_cloud.h>\r\n ros::Subscriber sub_lidar_points;\r\n ...\r\n \/\/ \u8f66\u8eab\u5468\u56f4\u969c\u788d\u7269\u68c0\u67e5 yan 25.9.23\r\n bool safe_near_vehicle_ = false; \/\/ \u8f66\u8eab\u5750\u6807\u7cfb\u9644\u8fd1\u662f\u5426\u5b89\u5168\uff08\u65e0\u8fd1\u8ddd\u79bb\u969c\u788d\u7269\uff09\r\n double safety_distance_ = 0.5; \/\/ \u8f66\u8f86\u5b89\u5168\u8ddd\u79bb\uff08\u8f66\u8eab\u5750\u6807\u7cfb\u4e0b\uff0clidar\u5982\u679c\u63a2\u6d4b\u5230\u6b64\u8ddd\u79bb\u5185\u7684\u969c\u788d\u7269\uff0c\u5219\u8ba4\u4e3a\u8f66\u8f86\u4e0d\u5b89\u5168\uff0c\u6682\u65f6\u505c\u6b62\u63a7\u8f66\uff09\r\n std::string points_topic_ = \"\/points_no_ground\"; \/\/\u5b89\u5168\u68c0\u67e5\u8f93\u5165\u70b9\u4e91topic\uff0c\u8fd9\u91cc\u4f7f\u7528\u8fc7\u6ee4\u5730\u9762\u540e\u7684\u96f7\u8fbe\u8bdd\u9898\r\n void callbackLidarPoints(const sensor_msgs::PointCloud2ConstPtr& msg);\r\n\r\nvim Autoware\/src\/autoware\/core_planning\/pure_pursuit\/src\/pure_pursuit_core.cpp\r\n void PurePursuitNode::initForROS() {\r\n private_nh_.param(\"points_topic\", points_topic_);\r\n sub_lidar_points = nh_.subscribe(points_topic_, 10, &PurePursuitNode::callbackLidarPoints, this);\r\n }\r\n\r\n void PurePursuitNode::callbackLidarPoints(const sensor_msgs::PointCloud2ConstPtr& msg)\r\n {\r\n pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);\r\n pcl::fromROSMsg(*msg, *cloud_ptr);\r\n \/\/ \u68c0\u67e5\u8fc7\u8fd1\u7684\u70b9\u4e91\uff08\u9ed8\u8ba4\u4e0d\u79fb\u9664\uff09\r\n int near_points = 0;\r\n for (unsigned int i = 0; i < cloud_ptr->points.size(); i++) {\r\n double origin_distance = sqrt(pow(cloud_ptr->points[i].x, 2) + pow(cloud_ptr->points[i].y, 2));\r\n if (origin_distance < safety_distance_) { \/\/ \u5c0f\u4e8e\u8ddd\u79bb\u9608\u503c\u7684\u6280\u672f\r\n near_points++;\r\n }\r\n }\r\n if (near_points == 0) { \/\/ \u8f66\u8eab\u9644\u8fd1\u8fd1\u8ddd\u79bb\u6ca1\u6709\u969c\u788d\u7269\u70b9\u4e91\uff0c\u5141\u8bb8\u79fb\u52a8\u8f66\u8f86\r\n safe_near_vehicle_ = true;\r\n } else { \/\/ \u51fa\u73b0\u975e\u5e38\u8fd1\u8ddd\u79bb\u7684\u969c\u788d\u7269\u65f6\uff0c\u7acb\u5373\u505c\u6b62\u79fb\u52a8\u8f66\u8f86\r\n ROS_WARN(\"Unsafe! Lidar detect %d points neer vehicle!\", near_points);\r\n safe_near_vehicle_ = false;\r\n }\r\n }\r\n\r\n void PurePursuitNode::run(){\r\n if (!good_location_quality_ || !safe_near_vehicle_){ \/\/ \u5f53\u5b9a\u4f4d\u8d28\u91cf\u4e0d\u597d\u6216\u8f66\u8eab\u9644\u8fd1\u6709\u969c\u788d\u7269\u65f6\uff0c\u4e0d\u5141\u8bb8\u63a7\u8f66 yan 24.12.25\r\n can_get_curvature = false;\r\n ROS_WARN(\"Unsafe! Pause moving Vehicle! %d %d\", good_location_quality_, safe_near_vehicle_);\r\n }\r\n }\r\n }<\/code><\/pre>\n\u4fee\u6539\u540e\u7f16\u8bd1\u5e76\u8fd0\u884c\uff0c\u76d1\u63a7\u5728\u9760\u8fd1\u6fc0\u5149\u96f7\u8fbe1\u7c73\u5185\u65f6\uff0c\u63a7\u8f66\u901f\u5ea6\u4ee5\u53ca\u8f6c\u89d2topic\u662f\u5426\u53d8\u4e3a0\uff0c\u5e76\u5728\u8fdc\u79bb\u540e\u7ee7\u7eed\u63a7\u8f66\uff1a<\/p>\n
# build\r\ncd ~\/Autoware\r\n.\/build.sh pure_pursuit\r\n\r\n# check\r\nrostopic echo \/ctrl_raw\r\n cmd: \r\n linear_velocity: 0.0\r\n linear_acceleration: 0.0\r\n steering_angle: 0.0<\/code><\/pre>\n <\/p>\n
\u53c2\u8003\uff1a<\/p>\n
\u57fa\u4e8e\u5df2\u77e5\u70b9\u4e91\u5730\u56fe\u7684NDT\u7684\u6fc0\u5149SLAM\u5b9a\u4f4d<\/a><\/p>\nAutoware\u81ea\u52a8\u9a7e\u9a76\u4f7f\u7528\u6559\u7a0bv1.3<\/a><\/p>\n\u57fa\u4e8eSC-LeGO-LOAM\u7684\u5efa\u56fe\u548cndt_localizer\u7684\u5b9a\u4f4d<\/a><\/p>\nautoware\u4e0bndt_mapping\u8282\u70b9\u89e3\u8bfb<\/a><\/p>\n