├── CMakeLists.txt ├── README.md ├── config └── rvizConfig.rviz ├── gridmap.png ├── launch └── demo.launch ├── log ├── frenet_ilqr_test_demo.INFO ├── frenet_ilqr_test_demo.ljn-G7-7588.ljn.log.INFO.20230828-214549.26123 ├── frenet_ilqr_test_demo.ljn-G7-7588.ljn.log.INFO.20230830-004047.31464 ├── frenet_ilqr_test_demo.ljn-G7-7588.ljn.log.INFO.20230830-004101.31849 ├── frenet_ilqr_test_demo.ljn-G7-7588.ljn.log.INFO.20230901-000635.5689 ├── frenet_ilqr_test_demo.ljn-G7-7588.ljn.log.INFO.20230901-001130.9805 ├── frenet_ilqr_test_demo.ljn-G7-7588.ljn.log.INFO.20230901-001406.12923 ├── frenet_ilqr_test_demo.ljn-G7-7588.ljn.log.INFO.20230901-001812.17208 ├── frenet_ilqr_test_demo.ljn-G7-7588.ljn.log.INFO.20230901-002956.27961 └── frenet_ilqr_test_demo.ljn-G7-7588.ljn.log.INFO.20230901-003504.1195 ├── package.xml └── src ├── path ├── data_structure.cpp ├── data_structure.h ├── free_space.cpp ├── free_space.h ├── gflags.cpp ├── gflags.h ├── path_costs.h ├── path_problem_manager.cpp ├── path_problem_manager.h ├── reference_line.cpp ├── reference_line.h ├── spline.cpp ├── spline.h ├── tool.cpp └── tool.h ├── solver ├── problem_manager.cpp ├── problem_manager.h ├── solver.cpp ├── solver.h └── variable.h └── test ├── Map.cpp ├── Map.hpp ├── demo.cpp ├── eigen2cv.hpp ├── reference_line_processor.cpp └── reference_line_processor.h /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(frenet_ilqr_test) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_definitions(-std=c++17 -w) 6 | set(CMAKE_CXX_FLAGS " -Wall -Wextra ${CMAKE_CXX_FLAGS}") 7 | set(CMAKE_BUILD_TYPE "Release") 8 | 9 | set(catkin_deps 10 | roscpp 11 | grid_map_ros 12 | tinyspline_ros 13 | ros_viz_tools 14 | ) 15 | 16 | find_package(catkin REQUIRED COMPONENTS 17 | ${catkin_deps} 18 | ) 19 | 20 | find_package(Eigen3 REQUIRED) 21 | find_package(OpenCV REQUIRED) 22 | find_package(gflags REQUIRED) 23 | 24 | catkin_package( 25 | INCLUDE_DIRS src 26 | LIBRARIES ${PROJECT_NAME} 27 | CATKIN_DEPENDS ${catkin_deps} 28 | DEPENDS OpenCV 29 | ) 30 | 31 | include_directories( 32 | src 33 | ${OpenCV_INCLUDE_DIRS} 34 | ${catkin_INCLUDE_DIRS} 35 | ${EIGEN3_INCLUDE_DIR} 36 | ) 37 | 38 | add_library(${PROJECT_NAME} 39 | src/path/data_structure.cpp 40 | src/path/reference_line.cpp 41 | src/path/spline.cpp 42 | src/path/free_space.cpp 43 | src/path/tool.cpp 44 | src/path/free_space.cpp 45 | src/path/gflags.cpp 46 | src/solver/solver.cpp 47 | src/solver/problem_manager.cpp 48 | src/path/path_problem_manager.cpp 49 | ) 50 | target_link_libraries(${PROJECT_NAME} glog gflags ${catkin_LIBRARIES} 51 | ) 52 | 53 | add_executable(${PROJECT_NAME}_demo 54 | src/test/demo.cpp 55 | src/test/Map.cpp 56 | src/test/reference_line_processor.cpp) 57 | target_link_libraries(${PROJECT_NAME}_demo 58 | ${PROJECT_NAME} ${OpenCV_LIBRARIES} 59 | ) 60 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # path_optimizer_ilqr 2 | **Path planning for autonomous vehicles.** 3 | The problem formulation is basically the same as [path_optimizer](https://github.com/LiJiangnanBit/path_optimizer) and [path_optimizer_2](https://github.com/LiJiangnanBit/path_optimizer_2), but solved through constrained ilqr. 4 | 5 | ## Demo 6 | image 7 | 8 | Yellow line: initial path; 9 | Blue dots: free space boundaries; 10 | 11 | ## Features 12 | - Problem formulated under the frenet frame; 13 | - Kappa and kappa rate constraints; 14 | - Compared to the previous work, there is less accuracy loss on the safety constraints and the vehicle dynamics due to the ilqr solver. 15 | 16 | ## Method 17 | (...) 18 | 19 | ## Depencencies 20 | - Tested on ROS Kinetic (Ubuntu 16) and Noetic (Ubuntu 20); 21 | - Other dependencies: [glog](https://github.com/google/glog), [gflags](https://github.com/gflags/gflags), [grid_map](https://github.com/ANYbotics/grid_map); 22 | - Put these ROS packages in your ros workspace: [ros_viz_tools](https://github.com/Magic-wei/ros_viz_tools), [tinyspline_ros](https://github.com/qutas/tinyspline_ros). 23 | 24 | ## Usage 25 | A png image is loaded as the grid map. You can click to specify the global reference path and the start/goal state of the vehicle. 26 | ~~~ 27 | roslaunch frenet_ilqr_test demo.launch 28 | ~~~ 29 | #### (1) Pick reference points using "Publish Point" tool in RViz. 30 | - Pick at least six points. 31 | - There are no hard and fast rules about the spacing of the points. 32 | - If you want to abandon the chosen points, just double click anywhere when using the "Publish Point" tool. 33 | - You can replace `gridmap.png` with other black and white images. Note that the resolution in `demo.cpp` is set to 0.2m, whick means that the length of one pixel is 0.2m on the map. 34 | - In application, the reference path is given by a global path or by a search algorithm like A*. 35 | 36 | ![选点.gif](https://i.loli.net/2020/04/12/kRItwQTh5GJWHxV.gif) 37 | #### (2) Pick start state using "2D Pose Estimate" tool and pick goal state using "2D Nav Goal" tool. 38 | - Currently, it's not strictly required to reach the goal state. But this can be changed. 39 | - The start state must be ahead of the first reference point. 40 | 41 | ![规划.gif](https://i.loli.net/2020/04/12/XmxgwTGRI1MtoVK.gif) 42 | -------------------------------------------------------------------------------- /config/rvizConfig.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Map1 10 | - /MarkerArray1 11 | - /MarkerArray1/Namespaces1 12 | Splitter Ratio: 0.5 13 | Tree Height: 746 14 | - Class: rviz/Selection 15 | Name: Selection 16 | - Class: rviz/Tool Properties 17 | Expanded: 18 | - /2D Pose Estimate1 19 | - /2D Nav Goal1 20 | - /Publish Point1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.588679016 23 | - Class: rviz/Views 24 | Expanded: 25 | - /Current View1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | - Class: rviz/Time 29 | Experimental: false 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: "" 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.699999988 39 | Class: rviz/Map 40 | Color Scheme: map 41 | Draw Behind: false 42 | Enabled: true 43 | Name: Map 44 | Topic: /frenet_ilqr_test_demo/grid_map 45 | Unreliable: false 46 | Use Timestamp: false 47 | Value: true 48 | - Class: rviz/MarkerArray 49 | Enabled: true 50 | Marker Topic: /frenet_ilqr_test_demo/markers 51 | Name: MarkerArray 52 | Namespaces: 53 | end point: true 54 | reference point: true 55 | start point: true 56 | Queue Size: 100 57 | Value: true 58 | Enabled: true 59 | Global Options: 60 | Background Color: 48; 48; 48 61 | Default Light: true 62 | Fixed Frame: map 63 | Frame Rate: 30 64 | Name: root 65 | Tools: 66 | - Class: rviz/Interact 67 | Hide Inactive Objects: true 68 | - Class: rviz/MoveCamera 69 | - Class: rviz/Select 70 | - Class: rviz/FocusCamera 71 | - Class: rviz/Measure 72 | - Class: rviz/SetInitialPose 73 | Topic: /initialpose 74 | - Class: rviz/SetGoal 75 | Topic: /move_base_simple/goal 76 | - Class: rviz/PublishPoint 77 | Single click: false 78 | Topic: /clicked_point 79 | Value: true 80 | Views: 81 | Current: 82 | Angle: -101.884888 83 | Class: rviz/TopDownOrtho 84 | Enable Stereo Rendering: 85 | Stereo Eye Separation: 0.0599999987 86 | Stereo Focal Distance: 1 87 | Swap Stereo Eyes: false 88 | Value: false 89 | Invert Z Axis: false 90 | Name: Current View 91 | Near Clip Distance: 0.00999999978 92 | Scale: 19.4848251 93 | Target Frame: 94 | Value: TopDownOrtho (rviz) 95 | X: -29.0066814 96 | Y: -6.40178156 97 | Saved: ~ 98 | Window Geometry: 99 | Displays: 100 | collapsed: false 101 | Height: 1026 102 | Hide Left Dock: false 103 | Hide Right Dock: false 104 | QMainWindow State: 000000ff00000000fd00000004000000000000022b00000378fc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000378000000d600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730000000028000001ed0000006100fffffffc00000028000003960000000000fffffffa000000000100000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000000ffffffff0000000000000000fb0000000a0056006900650077007300000008b00000010f0000010f00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000054f0000037800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 105 | Selection: 106 | collapsed: false 107 | Time: 108 | collapsed: false 109 | Tool Properties: 110 | collapsed: false 111 | Views: 112 | collapsed: false 113 | Width: 1920 114 | X: 65 115 | Y: 24 116 | -------------------------------------------------------------------------------- /gridmap.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiJiangnanBit/path_optimizer_ilqr/08eeb84751b494161e8690f36dcbc8356b1b8dd9/gridmap.png -------------------------------------------------------------------------------- /launch/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /log/frenet_ilqr_test_demo.INFO: -------------------------------------------------------------------------------- 1 | frenet_ilqr_test_demo.ljn-G7-7588.ljn.log.INFO.20230901-003504.1195 -------------------------------------------------------------------------------- /log/frenet_ilqr_test_demo.ljn-G7-7588.ljn.log.INFO.20230828-214549.26123: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/08/28 21:45:49 2 | Running on machine: ljn-G7-7588 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0828 21:45:49.741853 26123 demo.cpp:482] map published. 5 | I0828 21:45:50.408890 26123 demo.cpp:482] map published. 6 | I0828 21:45:51.074870 26123 demo.cpp:482] map published. 7 | I0828 21:45:51.752756 26123 demo.cpp:482] map published. 8 | I0828 21:45:52.419888 26123 demo.cpp:482] map published. 9 | I0828 21:45:53.074896 26123 demo.cpp:482] map published. 10 | I0828 21:45:53.747835 26123 demo.cpp:482] map published. 11 | I0828 21:45:54.408960 26123 demo.cpp:482] map published. 12 | I0828 21:45:55.075708 26123 demo.cpp:482] map published. 13 | I0828 21:45:55.745596 26123 demo.cpp:482] map published. 14 | I0828 21:45:56.407851 26123 demo.cpp:482] map published. 15 | I0828 21:45:57.074689 26123 demo.cpp:482] map published. 16 | I0828 21:45:57.765318 26123 demo.cpp:482] map published. 17 | I0828 21:45:58.408092 26123 demo.cpp:482] map published. 18 | I0828 21:45:59.085906 26123 demo.cpp:482] map published. 19 | I0828 21:45:59.742363 26123 demo.cpp:482] map published. 20 | I0828 21:46:00.408016 26123 demo.cpp:482] map published. 21 | I0828 21:46:01.075731 26123 demo.cpp:482] map published. 22 | I0828 21:46:01.754235 26123 demo.cpp:482] map published. 23 | I0828 21:46:02.408382 26123 demo.cpp:482] map published. 24 | I0828 21:46:03.085583 26123 demo.cpp:482] map published. 25 | I0828 21:46:03.748277 26123 demo.cpp:482] map published. 26 | I0828 21:46:04.408207 26123 demo.cpp:482] map published. 27 | I0828 21:46:05.074441 26123 demo.cpp:482] map published. 28 | I0828 21:46:05.741200 26123 demo.cpp:482] map published. 29 | I0828 21:46:06.409734 26123 demo.cpp:482] map published. 30 | I0828 21:46:07.086216 26123 demo.cpp:482] map published. 31 | I0828 21:46:07.742610 26123 demo.cpp:482] map published. 32 | I0828 21:46:08.420325 26123 demo.cpp:482] map published. 33 | I0828 21:46:09.085858 26123 demo.cpp:482] map published. 34 | I0828 21:46:09.753310 26123 demo.cpp:482] map published. 35 | I0828 21:46:10.408457 26123 demo.cpp:482] map published. 36 | I0828 21:46:11.075304 26123 demo.cpp:482] map published. 37 | I0828 21:46:11.742064 26123 demo.cpp:482] map published. 38 | I0828 21:46:12.420931 26123 demo.cpp:482] map published. 39 | I0828 21:46:13.086616 26123 demo.cpp:482] map published. 40 | I0828 21:46:13.741204 26123 demo.cpp:482] map published. 41 | I0828 21:46:14.407837 26123 demo.cpp:482] map published. 42 | I0828 21:46:15.097235 26123 demo.cpp:482] map published. 43 | I0828 21:46:15.741724 26123 demo.cpp:482] map published. 44 | I0828 21:46:16.420792 26123 demo.cpp:482] map published. 45 | I0828 21:46:17.074332 26123 demo.cpp:482] map published. 46 | I0828 21:46:17.741605 26123 demo.cpp:482] map published. 47 | I0828 21:46:18.413095 26123 demo.cpp:482] map published. 48 | I0828 21:46:19.074836 26123 demo.cpp:482] map published. 49 | I0828 21:46:19.741420 26123 demo.cpp:482] map published. 50 | I0828 21:46:20.413309 26123 demo.cpp:482] map published. 51 | I0828 21:46:21.075577 26123 demo.cpp:482] map published. 52 | I0828 21:46:21.741230 26123 demo.cpp:482] map published. 53 | I0828 21:46:22.408910 26123 demo.cpp:482] map published. 54 | I0828 21:46:23.079252 26123 demo.cpp:482] map published. 55 | I0828 21:46:23.752223 26123 demo.cpp:482] map published. 56 | I0828 21:46:24.408080 26123 demo.cpp:482] map published. 57 | I0828 21:46:25.080685 26123 demo.cpp:482] map published. 58 | I0828 21:46:25.741266 26123 demo.cpp:482] map published. 59 | I0828 21:46:26.408286 26123 demo.cpp:482] map published. 60 | I0828 21:46:27.074672 26123 demo.cpp:482] map published. 61 | I0828 21:46:27.756808 26123 demo.cpp:482] map published. 62 | I0828 21:46:28.408185 26123 demo.cpp:482] map published. 63 | I0828 21:46:29.074258 26123 demo.cpp:482] map published. 64 | -------------------------------------------------------------------------------- /log/frenet_ilqr_test_demo.ljn-G7-7588.ljn.log.INFO.20230830-004047.31464: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/08/30 00:40:47 2 | Running on machine: ljn-G7-7588 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0830 00:40:47.076320 31464 demo.cpp:205] map published. 5 | I0830 00:40:47.716897 31464 demo.cpp:205] map published. 6 | I0830 00:40:48.378736 31464 demo.cpp:205] map published. 7 | I0830 00:40:49.045507 31464 demo.cpp:205] map published. 8 | I0830 00:40:49.711716 31464 demo.cpp:205] map published. 9 | I0830 00:40:50.379475 31464 demo.cpp:205] map published. 10 | I0830 00:40:51.045461 31464 demo.cpp:205] map published. 11 | I0830 00:40:51.711771 31464 demo.cpp:205] map published. 12 | I0830 00:40:52.378540 31464 demo.cpp:205] map published. 13 | I0830 00:40:53.045342 31464 demo.cpp:205] map published. 14 | I0830 00:40:53.734047 31464 demo.cpp:205] map published. 15 | I0830 00:40:54.382340 31464 demo.cpp:205] map published. 16 | I0830 00:40:55.045416 31464 demo.cpp:205] map published. 17 | I0830 00:40:55.711846 31464 demo.cpp:205] map published. 18 | I0830 00:40:56.378661 31464 demo.cpp:205] map published. 19 | I0830 00:40:57.045221 31464 demo.cpp:205] map published. 20 | I0830 00:40:57.711822 31464 demo.cpp:205] map published. 21 | I0830 00:40:58.378998 31464 demo.cpp:205] map published. 22 | I0830 00:40:59.065127 31464 demo.cpp:205] map published. 23 | -------------------------------------------------------------------------------- /log/frenet_ilqr_test_demo.ljn-G7-7588.ljn.log.INFO.20230830-004101.31849: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/08/30 00:41:01 2 | Running on machine: ljn-G7-7588 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0830 00:41:01.122856 31849 demo.cpp:205] map published. 5 | I0830 00:41:01.779891 31849 demo.cpp:205] map published. 6 | I0830 00:41:02.446074 31849 demo.cpp:205] map published. 7 | I0830 00:41:03.113075 31849 demo.cpp:205] map published. 8 | I0830 00:41:03.803463 31849 demo.cpp:205] map published. 9 | I0830 00:41:04.446318 31849 demo.cpp:205] map published. 10 | I0830 00:41:05.113217 31849 demo.cpp:205] map published. 11 | I0830 00:41:05.797130 31849 demo.cpp:205] map published. 12 | I0830 00:41:06.446288 31849 demo.cpp:205] map published. 13 | I0830 00:41:07.113214 31849 demo.cpp:205] map published. 14 | I0830 00:41:07.780781 31849 demo.cpp:205] map published. 15 | I0830 00:41:08.446278 31849 demo.cpp:205] map published. 16 | I0830 00:41:09.113271 31849 demo.cpp:205] map published. 17 | I0830 00:41:09.790131 31849 demo.cpp:205] map published. 18 | I0830 00:41:10.446295 31849 demo.cpp:205] map published. 19 | I0830 00:41:11.116056 31849 demo.cpp:205] map published. 20 | I0830 00:41:11.779902 31849 demo.cpp:205] map published. 21 | I0830 00:41:12.452960 31849 demo.cpp:205] map published. 22 | I0830 00:41:13.113301 31849 demo.cpp:205] map published. 23 | I0830 00:41:13.781143 31849 demo.cpp:205] map published. 24 | I0830 00:41:14.458742 31849 demo.cpp:205] map published. 25 | I0830 00:41:15.112916 31849 demo.cpp:205] map published. 26 | I0830 00:41:15.786070 31849 demo.cpp:205] map published. 27 | I0830 00:41:16.469375 31849 demo.cpp:205] map published. 28 | I0830 00:41:17.124693 31849 demo.cpp:205] map published. 29 | I0830 00:41:17.789224 31849 demo.cpp:205] map published. 30 | I0830 00:41:18.446816 31849 demo.cpp:205] map published. 31 | I0830 00:41:19.114128 31849 demo.cpp:205] map published. 32 | I0830 00:41:19.780229 31849 demo.cpp:205] map published. 33 | I0830 00:41:20.446805 31849 demo.cpp:205] map published. 34 | I0830 00:41:21.113612 31849 demo.cpp:205] map published. 35 | I0830 00:41:21.791887 31849 demo.cpp:205] map published. 36 | I0830 00:41:22.446715 31849 demo.cpp:205] map published. 37 | I0830 00:41:23.112923 31849 demo.cpp:205] map published. 38 | I0830 00:41:23.779906 31849 demo.cpp:205] map published. 39 | I0830 00:41:24.446853 31849 demo.cpp:205] map published. 40 | I0830 00:41:25.113025 31849 demo.cpp:205] map published. 41 | I0830 00:41:25.791894 31849 demo.cpp:205] map published. 42 | I0830 00:41:26.446573 31849 demo.cpp:205] map published. 43 | I0830 00:41:27.112958 31849 demo.cpp:205] map published. 44 | I0830 00:41:27.792201 31849 demo.cpp:205] map published. 45 | I0830 00:41:28.446347 31849 demo.cpp:205] map published. 46 | I0830 00:41:29.116099 31849 demo.cpp:205] map published. 47 | I0830 00:41:29.780102 31849 demo.cpp:205] map published. 48 | I0830 00:41:30.446125 31849 demo.cpp:205] map published. 49 | I0830 00:41:31.113306 31849 demo.cpp:205] map published. 50 | I0830 00:41:31.779754 31849 demo.cpp:205] map published. 51 | I0830 00:41:32.449975 31849 demo.cpp:205] map published. 52 | I0830 00:41:33.114591 31849 demo.cpp:205] map published. 53 | I0830 00:41:33.779920 31849 demo.cpp:205] map published. 54 | I0830 00:41:34.460888 31849 demo.cpp:205] map published. 55 | I0830 00:41:35.113656 31849 demo.cpp:205] map published. 56 | I0830 00:41:35.780082 31849 demo.cpp:205] map published. 57 | I0830 00:41:36.446805 31849 demo.cpp:205] map published. 58 | I0830 00:41:37.115082 31849 demo.cpp:205] map published. 59 | I0830 00:41:37.781958 31849 demo.cpp:205] map published. 60 | I0830 00:41:38.456580 31849 demo.cpp:205] map published. 61 | I0830 00:41:39.116894 31849 demo.cpp:205] map published. 62 | I0830 00:41:39.780901 31849 demo.cpp:205] map published. 63 | I0830 00:41:40.447557 31849 demo.cpp:205] map published. 64 | I0830 00:41:41.130365 31849 demo.cpp:205] map published. 65 | I0830 00:41:41.782853 31849 demo.cpp:205] map published. 66 | I0830 00:41:42.450201 31849 demo.cpp:205] map published. 67 | I0830 00:41:43.114286 31849 demo.cpp:205] map published. 68 | I0830 00:41:43.781455 31849 demo.cpp:205] map published. 69 | I0830 00:41:44.450301 31849 demo.cpp:205] map published. 70 | I0830 00:41:45.114573 31849 demo.cpp:205] map published. 71 | -------------------------------------------------------------------------------- /log/frenet_ilqr_test_demo.ljn-G7-7588.ljn.log.INFO.20230901-000635.5689: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/09/01 00:06:35 2 | Running on machine: ljn-G7-7588 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0901 00:06:35.079046 5689 demo.cpp:220] map published. 5 | I0901 00:06:35.754240 5689 demo.cpp:220] map published. 6 | I0901 00:06:36.433876 5689 demo.cpp:220] map published. 7 | I0901 00:06:37.076125 5689 demo.cpp:220] map published. 8 | I0901 00:06:37.742416 5689 demo.cpp:220] map published. 9 | I0901 00:06:38.408990 5689 demo.cpp:220] map published. 10 | I0901 00:06:39.075711 5689 demo.cpp:220] map published. 11 | I0901 00:06:39.742208 5689 demo.cpp:220] map published. 12 | I0901 00:06:40.409124 5689 demo.cpp:220] map published. 13 | I0901 00:06:41.089262 5689 demo.cpp:220] map published. 14 | I0901 00:06:41.761020 5689 demo.cpp:220] map published. 15 | I0901 00:06:42.409158 5689 demo.cpp:220] map published. 16 | I0901 00:06:43.082973 5689 demo.cpp:220] map published. 17 | I0901 00:06:43.742120 5689 demo.cpp:220] map published. 18 | I0901 00:06:44.409230 5689 demo.cpp:220] map published. 19 | I0901 00:06:45.076439 5689 demo.cpp:220] map published. 20 | I0901 00:06:45.742305 5689 demo.cpp:220] map published. 21 | I0901 00:06:46.409161 5689 demo.cpp:220] map published. 22 | I0901 00:06:47.083508 5689 demo.cpp:220] map published. 23 | I0901 00:06:47.758174 5689 demo.cpp:220] map published. 24 | I0901 00:06:48.408869 5689 demo.cpp:220] map published. 25 | I0901 00:06:49.080332 5689 demo.cpp:220] map published. 26 | I0901 00:06:49.742374 5689 demo.cpp:220] map published. 27 | I0901 00:06:50.409519 5689 demo.cpp:220] map published. 28 | I0901 00:06:51.079922 5689 demo.cpp:220] map published. 29 | I0901 00:06:51.753873 5689 demo.cpp:220] map published. 30 | I0901 00:06:52.418270 5689 demo.cpp:220] map published. 31 | I0901 00:06:53.095201 5689 demo.cpp:220] map published. 32 | I0901 00:06:53.742616 5689 demo.cpp:220] map published. 33 | I0901 00:06:54.409700 5689 demo.cpp:220] map published. 34 | I0901 00:06:55.077275 5689 demo.cpp:220] map published. 35 | I0901 00:06:55.742614 5689 demo.cpp:220] map published. 36 | I0901 00:06:56.409528 5689 demo.cpp:220] map published. 37 | I0901 00:06:57.090240 5689 demo.cpp:220] map published. 38 | I0901 00:06:57.742841 5689 demo.cpp:220] map published. 39 | I0901 00:06:58.409400 5689 demo.cpp:220] map published. 40 | I0901 00:06:59.076367 5689 demo.cpp:220] map published. 41 | I0901 00:06:59.742381 5689 demo.cpp:220] map published. 42 | I0901 00:07:00.409310 5689 demo.cpp:220] map published. 43 | I0901 00:07:01.083645 5689 demo.cpp:220] map published. 44 | I0901 00:07:01.742473 5689 demo.cpp:220] map published. 45 | I0901 00:07:02.409181 5689 demo.cpp:220] map published. 46 | I0901 00:07:03.076179 5689 demo.cpp:220] map published. 47 | I0901 00:07:03.743793 5689 demo.cpp:220] map published. 48 | I0901 00:07:04.409520 5689 demo.cpp:220] map published. 49 | I0901 00:07:05.076470 5689 demo.cpp:220] map published. 50 | I0901 00:07:05.743070 5689 demo.cpp:220] map published. 51 | I0901 00:07:06.410056 5689 demo.cpp:220] map published. 52 | I0901 00:07:07.086477 5689 demo.cpp:220] map published. 53 | I0901 00:07:07.754962 5689 demo.cpp:220] map published. 54 | I0901 00:07:08.417178 5689 demo.cpp:220] map published. 55 | I0901 00:07:09.081099 5689 demo.cpp:220] map published. 56 | I0901 00:07:09.743492 5689 demo.cpp:220] map published. 57 | I0901 00:07:10.416891 5689 demo.cpp:220] map published. 58 | I0901 00:07:11.087548 5689 demo.cpp:220] map published. 59 | I0901 00:07:11.743250 5689 demo.cpp:220] map published. 60 | I0901 00:07:12.430459 5689 demo.cpp:220] map published. 61 | I0901 00:07:13.077183 5689 demo.cpp:220] map published. 62 | I0901 00:07:13.743794 5689 demo.cpp:220] map published. 63 | I0901 00:07:14.430312 5689 demo.cpp:220] map published. 64 | I0901 00:07:15.076529 5689 demo.cpp:220] map published. 65 | I0901 00:07:15.743091 5689 demo.cpp:220] map published. 66 | I0901 00:07:16.420739 5689 demo.cpp:220] map published. 67 | I0901 00:07:17.085244 5689 demo.cpp:220] map published. 68 | I0901 00:07:17.742928 5689 demo.cpp:220] map published. 69 | I0901 00:07:18.410889 5689 demo.cpp:220] map published. 70 | I0901 00:07:19.076283 5689 demo.cpp:220] map published. 71 | I0901 00:07:19.742631 5689 demo.cpp:220] map published. 72 | I0901 00:07:20.427317 5689 demo.cpp:220] map published. 73 | I0901 00:07:21.079684 5689 demo.cpp:220] map published. 74 | I0901 00:07:21.742803 5689 demo.cpp:220] map published. 75 | I0901 00:07:22.409843 5689 demo.cpp:220] map published. 76 | I0901 00:07:23.076786 5689 demo.cpp:220] map published. 77 | I0901 00:07:23.742812 5689 demo.cpp:220] map published. 78 | I0901 00:07:24.409716 5689 demo.cpp:220] map published. 79 | I0901 00:07:25.086629 5689 demo.cpp:220] map published. 80 | I0901 00:07:25.742789 5689 demo.cpp:220] map published. 81 | I0901 00:07:26.409446 5689 demo.cpp:220] map published. 82 | I0901 00:07:27.086441 5689 demo.cpp:220] map published. 83 | I0901 00:07:27.742707 5689 demo.cpp:220] map published. 84 | I0901 00:07:28.411603 5689 demo.cpp:220] map published. 85 | I0901 00:07:29.076416 5689 demo.cpp:220] map published. 86 | I0901 00:07:29.743212 5689 demo.cpp:220] map published. 87 | I0901 00:07:30.411866 5689 demo.cpp:220] map published. 88 | I0901 00:07:31.092442 5689 demo.cpp:220] map published. 89 | I0901 00:07:31.744578 5689 demo.cpp:220] map published. 90 | I0901 00:07:32.416297 5689 demo.cpp:220] map published. 91 | I0901 00:07:33.081780 5689 demo.cpp:220] map published. 92 | I0901 00:07:33.744793 5689 demo.cpp:220] map published. 93 | I0901 00:07:34.411983 5689 demo.cpp:220] map published. 94 | I0901 00:07:35.079967 5689 demo.cpp:220] map published. 95 | I0901 00:07:35.756321 5689 demo.cpp:220] map published. 96 | I0901 00:07:36.428982 5689 demo.cpp:220] map published. 97 | I0901 00:07:37.076686 5689 demo.cpp:220] map published. 98 | I0901 00:07:37.764415 5689 demo.cpp:220] map published. 99 | I0901 00:07:38.409947 5689 demo.cpp:220] map published. 100 | I0901 00:07:39.076989 5689 demo.cpp:220] map published. 101 | I0901 00:07:39.750972 5689 demo.cpp:220] map published. 102 | I0901 00:07:40.411751 5689 demo.cpp:220] map published. 103 | I0901 00:07:41.086992 5689 demo.cpp:220] map published. 104 | I0901 00:07:41.766816 5689 demo.cpp:220] map published. 105 | I0901 00:07:42.410171 5689 demo.cpp:220] map published. 106 | I0901 00:07:43.089723 5689 demo.cpp:220] map published. 107 | I0901 00:07:43.744410 5689 demo.cpp:220] map published. 108 | I0901 00:07:44.419682 5689 demo.cpp:220] map published. 109 | I0901 00:07:45.076608 5689 demo.cpp:220] map published. 110 | I0901 00:07:45.763626 5689 demo.cpp:220] map published. 111 | I0901 00:07:46.410212 5689 demo.cpp:220] map published. 112 | I0901 00:07:47.085008 5689 demo.cpp:220] map published. 113 | I0901 00:07:47.743084 5689 demo.cpp:220] map published. 114 | I0901 00:07:48.409438 5689 demo.cpp:220] map published. 115 | I0901 00:07:49.080476 5689 demo.cpp:220] map published. 116 | I0901 00:07:49.743361 5689 demo.cpp:220] map published. 117 | I0901 00:07:50.410038 5689 demo.cpp:220] map published. 118 | I0901 00:07:51.077174 5689 demo.cpp:220] map published. 119 | -------------------------------------------------------------------------------- /log/frenet_ilqr_test_demo.ljn-G7-7588.ljn.log.INFO.20230901-001130.9805: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/09/01 00:11:30 2 | Running on machine: ljn-G7-7588 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0901 00:11:30.934177 9805 demo.cpp:220] map published. 5 | I0901 00:11:31.584965 9805 demo.cpp:220] map published. 6 | I0901 00:11:32.239312 9805 demo.cpp:220] map published. 7 | I0901 00:11:32.905861 9805 demo.cpp:220] map published. 8 | I0901 00:11:33.572571 9805 demo.cpp:220] map published. 9 | I0901 00:11:34.239349 9805 demo.cpp:220] map published. 10 | I0901 00:11:34.907152 9805 demo.cpp:220] map published. 11 | I0901 00:11:35.592191 9805 demo.cpp:220] map published. 12 | I0901 00:11:36.239280 9805 demo.cpp:220] map published. 13 | I0901 00:11:36.905947 9805 demo.cpp:220] map published. 14 | I0901 00:11:37.573004 9805 demo.cpp:220] map published. 15 | I0901 00:11:38.251240 9805 demo.cpp:220] map published. 16 | I0901 00:11:38.906088 9805 demo.cpp:220] map published. 17 | I0901 00:11:39.573048 9805 demo.cpp:220] map published. 18 | I0901 00:11:40.250959 9805 demo.cpp:220] map published. 19 | I0901 00:11:40.927206 9805 demo.cpp:220] map published. 20 | I0901 00:11:41.588774 9805 demo.cpp:220] map published. 21 | I0901 00:11:42.239933 9805 demo.cpp:220] map published. 22 | I0901 00:11:42.906615 9805 demo.cpp:220] map published. 23 | I0901 00:11:43.573199 9805 demo.cpp:220] map published. 24 | I0901 00:11:44.239941 9805 demo.cpp:220] map published. 25 | I0901 00:11:44.914801 9805 demo.cpp:220] map published. 26 | I0901 00:11:45.573244 9805 demo.cpp:220] map published. 27 | I0901 00:11:46.239694 9805 demo.cpp:220] map published. 28 | I0901 00:11:46.909523 9805 demo.cpp:220] map published. 29 | I0901 00:11:47.573801 9805 demo.cpp:220] map published. 30 | I0901 00:11:48.239794 9805 demo.cpp:220] map published. 31 | I0901 00:11:48.906958 9805 demo.cpp:220] map published. 32 | I0901 00:11:49.595510 9805 demo.cpp:220] map published. 33 | I0901 00:11:50.240605 9805 demo.cpp:220] map published. 34 | I0901 00:11:50.907428 9805 demo.cpp:220] map published. 35 | I0901 00:11:51.596295 9805 demo.cpp:220] map published. 36 | I0901 00:11:52.247766 9805 demo.cpp:220] map published. 37 | I0901 00:11:52.907792 9805 demo.cpp:220] map published. 38 | I0901 00:11:53.585870 9805 demo.cpp:220] map published. 39 | I0901 00:11:54.244967 9805 demo.cpp:220] map published. 40 | I0901 00:11:54.906996 9805 demo.cpp:220] map published. 41 | I0901 00:11:55.573633 9805 demo.cpp:220] map published. 42 | I0901 00:11:56.240728 9805 demo.cpp:220] map published. 43 | I0901 00:11:56.916672 9805 demo.cpp:220] map published. 44 | I0901 00:11:57.573943 9805 demo.cpp:220] map published. 45 | I0901 00:11:58.240088 9805 demo.cpp:220] map published. 46 | I0901 00:11:58.908095 9805 demo.cpp:220] map published. 47 | I0901 00:11:59.582633 9805 demo.cpp:220] map published. 48 | I0901 00:12:00.252008 9805 demo.cpp:220] map published. 49 | I0901 00:12:00.920367 9805 demo.cpp:220] map published. 50 | I0901 00:12:01.573640 9805 demo.cpp:220] map published. 51 | I0901 00:12:02.253787 9805 demo.cpp:220] map published. 52 | I0901 00:12:02.906939 9805 demo.cpp:220] map published. 53 | -------------------------------------------------------------------------------- /log/frenet_ilqr_test_demo.ljn-G7-7588.ljn.log.INFO.20230901-001406.12923: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/09/01 00:14:06 2 | Running on machine: ljn-G7-7588 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0901 00:14:06.125797 12923 demo.cpp:220] map published. 5 | I0901 00:14:06.791672 12923 demo.cpp:220] map published. 6 | I0901 00:14:07.458961 12923 demo.cpp:220] map published. 7 | I0901 00:14:08.126024 12923 demo.cpp:220] map published. 8 | I0901 00:14:08.792232 12923 demo.cpp:220] map published. 9 | I0901 00:14:09.473289 12923 demo.cpp:220] map published. 10 | I0901 00:14:10.125452 12923 demo.cpp:220] map published. 11 | I0901 00:14:10.810487 12923 demo.cpp:220] map published. 12 | I0901 00:14:11.466575 12923 demo.cpp:220] map published. 13 | I0901 00:14:12.125782 12923 demo.cpp:220] map published. 14 | I0901 00:14:12.792548 12923 demo.cpp:220] map published. 15 | I0901 00:14:13.464990 12923 demo.cpp:220] map published. 16 | I0901 00:14:14.125190 12923 demo.cpp:220] map published. 17 | I0901 00:14:14.792702 12923 demo.cpp:220] map published. 18 | I0901 00:14:15.477335 12923 demo.cpp:220] map published. 19 | I0901 00:14:16.126037 12923 demo.cpp:220] map published. 20 | I0901 00:14:16.792495 12923 demo.cpp:220] map published. 21 | I0901 00:14:17.467092 12923 demo.cpp:220] map published. 22 | I0901 00:14:18.137099 12923 demo.cpp:220] map published. 23 | I0901 00:14:18.808849 12923 demo.cpp:220] map published. 24 | I0901 00:14:19.459973 12923 demo.cpp:220] map published. 25 | I0901 00:14:20.125998 12923 demo.cpp:220] map published. 26 | I0901 00:14:20.793267 12923 demo.cpp:220] map published. 27 | I0901 00:14:21.459708 12923 demo.cpp:220] map published. 28 | I0901 00:14:22.146152 12923 demo.cpp:220] map published. 29 | I0901 00:14:22.792724 12923 demo.cpp:220] map published. 30 | I0901 00:14:23.478228 12923 demo.cpp:220] map published. 31 | I0901 00:14:24.126037 12923 demo.cpp:220] map published. 32 | I0901 00:14:24.792555 12923 demo.cpp:220] map published. 33 | I0901 00:14:25.459718 12923 demo.cpp:220] map published. 34 | I0901 00:14:26.125679 12923 demo.cpp:220] map published. 35 | I0901 00:14:26.792495 12923 demo.cpp:220] map published. 36 | I0901 00:14:27.469326 12923 demo.cpp:220] map published. 37 | I0901 00:14:28.128486 12923 demo.cpp:220] map published. 38 | I0901 00:14:28.793229 12923 demo.cpp:220] map published. 39 | I0901 00:14:29.477749 12923 demo.cpp:220] map published. 40 | I0901 00:14:30.125851 12923 demo.cpp:220] map published. 41 | I0901 00:14:30.792968 12923 demo.cpp:220] map published. 42 | I0901 00:14:31.459905 12923 demo.cpp:220] map published. 43 | I0901 00:14:32.137182 12923 demo.cpp:220] map published. 44 | I0901 00:14:32.793011 12923 demo.cpp:220] map published. 45 | I0901 00:14:33.466104 12923 demo.cpp:220] map published. 46 | I0901 00:14:34.143194 12923 demo.cpp:220] map published. 47 | I0901 00:14:34.792536 12923 demo.cpp:220] map published. 48 | I0901 00:14:35.463477 12923 demo.cpp:220] map published. 49 | I0901 00:14:36.131021 12923 demo.cpp:220] map published. 50 | I0901 00:14:36.792465 12923 demo.cpp:220] map published. 51 | I0901 00:14:37.459579 12923 demo.cpp:220] map published. 52 | I0901 00:14:38.130184 12923 demo.cpp:220] map published. 53 | I0901 00:14:38.793300 12923 demo.cpp:220] map published. 54 | I0901 00:14:39.459249 12923 demo.cpp:220] map published. 55 | I0901 00:14:40.126485 12923 demo.cpp:220] map published. 56 | I0901 00:14:40.792611 12923 demo.cpp:220] map published. 57 | I0901 00:14:41.459367 12923 demo.cpp:220] map published. 58 | I0901 00:14:42.129320 12923 demo.cpp:220] map published. 59 | I0901 00:14:42.805542 12923 demo.cpp:220] map published. 60 | I0901 00:14:43.460079 12923 demo.cpp:220] map published. 61 | I0901 00:14:44.126178 12923 demo.cpp:220] map published. 62 | I0901 00:14:44.792905 12923 demo.cpp:220] map published. 63 | I0901 00:14:45.467677 12923 demo.cpp:220] map published. 64 | I0901 00:14:46.126269 12923 demo.cpp:220] map published. 65 | I0901 00:14:46.822024 12923 demo.cpp:220] map published. 66 | I0901 00:14:47.459599 12923 demo.cpp:220] map published. 67 | I0901 00:14:48.125720 12923 demo.cpp:220] map published. 68 | I0901 00:14:48.792413 12923 demo.cpp:220] map published. 69 | I0901 00:14:49.471465 12923 demo.cpp:220] map published. 70 | I0901 00:14:50.141263 12923 demo.cpp:220] map published. 71 | I0901 00:14:50.823753 12923 demo.cpp:220] map published. 72 | I0901 00:14:51.459270 12923 demo.cpp:220] map published. 73 | I0901 00:14:52.125996 12923 demo.cpp:220] map published. 74 | I0901 00:14:52.800066 12923 demo.cpp:220] map published. 75 | I0901 00:14:53.465617 12923 demo.cpp:220] map published. 76 | I0901 00:14:54.131145 12923 demo.cpp:220] map published. 77 | I0901 00:14:54.792932 12923 demo.cpp:220] map published. 78 | I0901 00:14:55.459296 12923 demo.cpp:220] map published. 79 | I0901 00:14:56.126770 12923 demo.cpp:220] map published. 80 | I0901 00:14:56.793584 12923 demo.cpp:220] map published. 81 | I0901 00:14:57.459347 12923 demo.cpp:220] map published. 82 | I0901 00:14:58.125918 12923 demo.cpp:220] map published. 83 | I0901 00:14:58.792544 12923 demo.cpp:220] map published. 84 | I0901 00:14:59.459378 12923 demo.cpp:220] map published. 85 | I0901 00:15:00.127617 12923 demo.cpp:220] map published. 86 | I0901 00:15:00.792804 12923 demo.cpp:220] map published. 87 | I0901 00:15:01.459965 12923 demo.cpp:220] map published. 88 | I0901 00:15:02.125419 12923 demo.cpp:220] map published. 89 | I0901 00:15:02.793110 12923 demo.cpp:220] map published. 90 | I0901 00:15:03.459496 12923 demo.cpp:220] map published. 91 | I0901 00:15:04.157716 12923 demo.cpp:220] map published. 92 | I0901 00:15:04.795289 12923 demo.cpp:220] map published. 93 | I0901 00:15:05.460170 12923 demo.cpp:220] map published. 94 | I0901 00:15:06.125614 12923 demo.cpp:220] map published. 95 | I0901 00:15:06.792510 12923 demo.cpp:220] map published. 96 | I0901 00:15:07.459885 12923 demo.cpp:220] map published. 97 | I0901 00:15:08.126603 12923 demo.cpp:220] map published. 98 | I0901 00:15:08.792618 12923 demo.cpp:220] map published. 99 | I0901 00:15:09.459515 12923 demo.cpp:220] map published. 100 | I0901 00:15:10.134407 12923 demo.cpp:220] map published. 101 | I0901 00:15:10.792587 12923 demo.cpp:220] map published. 102 | I0901 00:15:11.489547 12923 demo.cpp:220] map published. 103 | I0901 00:15:12.126956 12923 demo.cpp:220] map published. 104 | I0901 00:15:12.793226 12923 demo.cpp:220] map published. 105 | I0901 00:15:13.463891 12923 demo.cpp:220] map published. 106 | I0901 00:15:14.141582 12923 demo.cpp:220] map published. 107 | I0901 00:15:14.800205 12923 demo.cpp:220] map published. 108 | I0901 00:15:15.459196 12923 demo.cpp:220] map published. 109 | I0901 00:15:16.144765 12923 demo.cpp:220] map published. 110 | I0901 00:15:16.792929 12923 demo.cpp:220] map published. 111 | I0901 00:15:17.459031 12923 demo.cpp:220] map published. 112 | I0901 00:15:18.134999 12923 demo.cpp:220] map published. 113 | I0901 00:15:18.793076 12923 demo.cpp:220] map published. 114 | I0901 00:15:19.459393 12923 demo.cpp:220] map published. 115 | I0901 00:15:20.127209 12923 demo.cpp:220] map published. 116 | I0901 00:15:20.803258 12923 demo.cpp:220] map published. 117 | I0901 00:15:21.460232 12923 demo.cpp:220] map published. 118 | I0901 00:15:22.138082 12923 demo.cpp:220] map published. 119 | I0901 00:15:22.792869 12923 demo.cpp:220] map published. 120 | I0901 00:15:23.459230 12923 demo.cpp:220] map published. 121 | I0901 00:15:24.126575 12923 demo.cpp:220] map published. 122 | I0901 00:15:24.793346 12923 demo.cpp:220] map published. 123 | I0901 00:15:25.475016 12923 demo.cpp:220] map published. 124 | I0901 00:15:26.127219 12923 demo.cpp:220] map published. 125 | I0901 00:15:26.792609 12923 demo.cpp:220] map published. 126 | I0901 00:15:27.465682 12923 demo.cpp:220] map published. 127 | I0901 00:15:28.125941 12923 demo.cpp:220] map published. 128 | I0901 00:15:28.793329 12923 demo.cpp:220] map published. 129 | I0901 00:15:29.468921 12923 demo.cpp:220] map published. 130 | I0901 00:15:30.127840 12923 demo.cpp:220] map published. 131 | I0901 00:15:30.792454 12923 demo.cpp:220] map published. 132 | I0901 00:15:31.459733 12923 demo.cpp:220] map published. 133 | I0901 00:15:32.126682 12923 demo.cpp:220] map published. 134 | I0901 00:15:32.792796 12923 demo.cpp:220] map published. 135 | I0901 00:15:33.459625 12923 demo.cpp:220] map published. 136 | I0901 00:15:34.126758 12923 demo.cpp:220] map published. 137 | I0901 00:15:34.792531 12923 demo.cpp:220] map published. 138 | I0901 00:15:35.459420 12923 demo.cpp:220] map published. 139 | I0901 00:15:36.133985 12923 demo.cpp:220] map published. 140 | I0901 00:15:36.792704 12923 demo.cpp:220] map published. 141 | I0901 00:15:37.462224 12923 demo.cpp:220] map published. 142 | I0901 00:15:38.126879 12923 demo.cpp:220] map published. 143 | I0901 00:15:38.793062 12923 demo.cpp:220] map published. 144 | I0901 00:15:39.468156 12923 demo.cpp:220] map published. 145 | -------------------------------------------------------------------------------- /log/frenet_ilqr_test_demo.ljn-G7-7588.ljn.log.INFO.20230901-001812.17208: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/09/01 00:18:12 2 | Running on machine: ljn-G7-7588 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0901 00:18:12.833156 17208 demo.cpp:220] map published. 5 | I0901 00:18:13.502799 17208 demo.cpp:220] map published. 6 | I0901 00:18:14.165905 17208 demo.cpp:220] map published. 7 | I0901 00:18:14.832239 17208 demo.cpp:220] map published. 8 | I0901 00:18:15.498867 17208 demo.cpp:220] map published. 9 | I0901 00:18:16.166173 17208 demo.cpp:220] map published. 10 | I0901 00:18:16.847754 17208 demo.cpp:220] map published. 11 | I0901 00:18:17.499220 17208 demo.cpp:220] map published. 12 | I0901 00:18:18.172890 17208 demo.cpp:220] map published. 13 | I0901 00:18:18.855253 17208 demo.cpp:220] map published. 14 | I0901 00:18:19.503530 17208 demo.cpp:220] map published. 15 | I0901 00:18:20.170868 17208 demo.cpp:220] map published. 16 | I0901 00:18:20.857192 17208 demo.cpp:220] map published. 17 | I0901 00:18:21.502671 17208 demo.cpp:220] map published. 18 | I0901 00:18:22.188902 17208 demo.cpp:220] map published. 19 | I0901 00:18:22.832397 17208 demo.cpp:220] map published. 20 | I0901 00:18:23.509713 17208 demo.cpp:220] map published. 21 | I0901 00:18:24.166133 17208 demo.cpp:220] map published. 22 | I0901 00:18:24.835162 17208 demo.cpp:220] map published. 23 | I0901 00:18:25.514906 17208 demo.cpp:220] map published. 24 | I0901 00:18:26.186419 17208 demo.cpp:220] map published. 25 | I0901 00:18:26.833654 17208 demo.cpp:220] map published. 26 | I0901 00:18:27.505311 17208 demo.cpp:220] map published. 27 | I0901 00:18:28.167472 17208 demo.cpp:220] map published. 28 | -------------------------------------------------------------------------------- /log/frenet_ilqr_test_demo.ljn-G7-7588.ljn.log.INFO.20230901-002956.27961: -------------------------------------------------------------------------------- 1 | Log file created at: 2023/09/01 00:29:56 2 | Running on machine: ljn-G7-7588 3 | Log line format: [IWEF]mmdd hh:mm:ss.uuuuuu threadid file:line] msg 4 | I0901 00:29:56.309742 27961 demo.cpp:220] map published. 5 | I0901 00:29:56.965260 27961 demo.cpp:220] map published. 6 | I0901 00:29:57.643285 27961 demo.cpp:220] map published. 7 | I0901 00:29:58.298234 27961 demo.cpp:220] map published. 8 | I0901 00:29:58.965135 27961 demo.cpp:220] map published. 9 | I0901 00:29:59.643630 27961 demo.cpp:220] map published. 10 | I0901 00:30:00.298431 27961 demo.cpp:220] map published. 11 | I0901 00:30:00.966037 27961 demo.cpp:220] map published. 12 | I0901 00:30:01.650770 27961 demo.cpp:220] map published. 13 | I0901 00:30:02.298732 27961 demo.cpp:220] map published. 14 | I0901 00:30:02.978760 27961 demo.cpp:220] map published. 15 | I0901 00:30:03.632011 27961 demo.cpp:220] map published. 16 | I0901 00:30:04.309527 27961 demo.cpp:220] map published. 17 | I0901 00:30:04.965348 27961 demo.cpp:220] map published. 18 | I0901 00:30:05.632366 27961 demo.cpp:220] map published. 19 | I0901 00:30:06.299149 27961 demo.cpp:220] map published. 20 | I0901 00:30:06.965734 27961 demo.cpp:220] map published. 21 | I0901 00:30:07.632375 27961 demo.cpp:220] map published. 22 | I0901 00:30:08.298874 27961 demo.cpp:220] map published. 23 | I0901 00:30:08.968946 27961 demo.cpp:220] map published. 24 | I0901 00:30:09.632501 27961 demo.cpp:220] map published. 25 | I0901 00:30:10.313977 27961 demo.cpp:220] map published. 26 | I0901 00:30:10.986595 27961 demo.cpp:220] map published. 27 | I0901 00:30:11.632457 27961 demo.cpp:220] map published. 28 | I0901 00:30:12.303498 27961 demo.cpp:220] map published. 29 | I0901 00:30:12.966682 27961 demo.cpp:220] map published. 30 | I0901 00:30:13.632267 27961 demo.cpp:220] map published. 31 | I0901 00:30:14.298650 27961 demo.cpp:220] map published. 32 | I0901 00:30:14.965873 27961 demo.cpp:220] map published. 33 | I0901 00:30:15.632398 27961 demo.cpp:220] map published. 34 | I0901 00:30:16.299012 27961 demo.cpp:220] map published. 35 | I0901 00:30:16.966394 27961 demo.cpp:220] map published. 36 | I0901 00:30:17.632025 27961 demo.cpp:220] map published. 37 | I0901 00:30:18.318203 27961 demo.cpp:220] map published. 38 | I0901 00:30:18.965826 27961 demo.cpp:220] map published. 39 | I0901 00:30:19.632592 27961 demo.cpp:220] map published. 40 | I0901 00:30:20.299249 27961 demo.cpp:220] map published. 41 | I0901 00:30:20.966149 27961 demo.cpp:220] map published. 42 | I0901 00:30:21.636528 27961 demo.cpp:220] map published. 43 | I0901 00:30:22.299257 27961 demo.cpp:220] map published. 44 | I0901 00:30:22.966107 27961 demo.cpp:220] map published. 45 | I0901 00:30:23.632886 27961 demo.cpp:220] map published. 46 | I0901 00:30:24.299453 27961 demo.cpp:220] map published. 47 | I0901 00:30:24.966552 27961 demo.cpp:220] map published. 48 | I0901 00:30:25.632928 27961 demo.cpp:220] map published. 49 | I0901 00:30:26.310048 27961 demo.cpp:220] map published. 50 | I0901 00:30:26.965962 27961 demo.cpp:220] map published. 51 | I0901 00:30:27.633786 27961 demo.cpp:220] map published. 52 | I0901 00:30:28.305792 27961 demo.cpp:220] map published. 53 | I0901 00:30:28.978597 27961 demo.cpp:220] map published. 54 | I0901 00:30:29.647610 27961 demo.cpp:220] map published. 55 | I0901 00:30:30.310012 27961 demo.cpp:220] map published. 56 | I0901 00:30:30.966185 27961 demo.cpp:220] map published. 57 | I0901 00:30:31.633476 27961 demo.cpp:220] map published. 58 | I0901 00:30:32.320127 27961 demo.cpp:220] map published. 59 | I0901 00:30:32.966691 27961 demo.cpp:220] map published. 60 | I0901 00:30:33.644711 27961 demo.cpp:220] map published. 61 | I0901 00:30:34.306493 27961 demo.cpp:220] map published. 62 | I0901 00:30:34.989486 27961 demo.cpp:220] map published. 63 | I0901 00:30:35.633543 27961 demo.cpp:220] map published. 64 | I0901 00:30:36.317800 27961 demo.cpp:220] map published. 65 | I0901 00:30:36.966707 27961 demo.cpp:220] map published. 66 | I0901 00:30:37.633052 27961 demo.cpp:220] map published. 67 | I0901 00:30:38.303629 27961 demo.cpp:220] map published. 68 | I0901 00:30:38.966919 27961 demo.cpp:220] map published. 69 | I0901 00:30:39.633213 27961 demo.cpp:220] map published. 70 | I0901 00:30:40.311086 27961 demo.cpp:220] map published. 71 | I0901 00:30:40.966580 27961 demo.cpp:220] map published. 72 | I0901 00:30:41.642294 27961 demo.cpp:220] map published. 73 | I0901 00:30:42.336447 27961 demo.cpp:220] map published. 74 | I0901 00:30:42.966082 27961 demo.cpp:220] map published. 75 | I0901 00:30:43.652617 27961 demo.cpp:220] map published. 76 | I0901 00:30:44.299618 27961 demo.cpp:220] map published. 77 | I0901 00:30:44.974107 27961 demo.cpp:220] map published. 78 | I0901 00:30:45.633073 27961 demo.cpp:220] map published. 79 | I0901 00:30:46.299172 27961 demo.cpp:220] map published. 80 | I0901 00:30:46.966800 27961 demo.cpp:220] map published. 81 | I0901 00:30:47.646400 27961 demo.cpp:220] map published. 82 | I0901 00:30:48.300333 27961 demo.cpp:220] map published. 83 | I0901 00:30:48.974993 27961 demo.cpp:220] map published. 84 | I0901 00:30:49.633157 27961 demo.cpp:220] map published. 85 | I0901 00:30:50.299088 27961 demo.cpp:220] map published. 86 | I0901 00:30:50.966034 27961 demo.cpp:220] map published. 87 | I0901 00:30:51.651983 27961 demo.cpp:220] map published. 88 | I0901 00:30:52.299307 27961 demo.cpp:220] map published. 89 | I0901 00:30:52.975582 27961 demo.cpp:220] map published. 90 | I0901 00:30:53.639472 27961 demo.cpp:220] map published. 91 | I0901 00:30:54.299031 27961 demo.cpp:220] map published. 92 | I0901 00:30:54.966681 27961 demo.cpp:220] map published. 93 | I0901 00:30:55.634006 27961 demo.cpp:220] map published. 94 | I0901 00:30:56.299505 27961 demo.cpp:220] map published. 95 | I0901 00:30:56.966626 27961 demo.cpp:220] map published. 96 | I0901 00:30:57.633486 27961 demo.cpp:220] map published. 97 | I0901 00:30:58.305281 27961 demo.cpp:220] map published. 98 | I0901 00:30:58.983773 27961 demo.cpp:220] map published. 99 | I0901 00:30:59.632607 27961 demo.cpp:220] map published. 100 | I0901 00:31:00.318125 27961 demo.cpp:220] map published. 101 | I0901 00:31:00.966120 27961 demo.cpp:220] map published. 102 | I0901 00:31:01.632908 27961 demo.cpp:220] map published. 103 | I0901 00:31:02.300215 27961 demo.cpp:220] map published. 104 | I0901 00:31:02.969595 27961 demo.cpp:220] map published. 105 | I0901 00:31:03.646376 27961 demo.cpp:220] map published. 106 | I0901 00:31:04.300665 27961 demo.cpp:220] map published. 107 | I0901 00:31:04.972046 27961 demo.cpp:220] map published. 108 | I0901 00:31:05.643385 27961 demo.cpp:220] map published. 109 | I0901 00:31:06.322620 27961 demo.cpp:220] map published. 110 | I0901 00:31:06.965875 27961 demo.cpp:220] map published. 111 | I0901 00:31:07.633308 27961 demo.cpp:220] map published. 112 | I0901 00:31:08.309594 27961 demo.cpp:220] map published. 113 | I0901 00:31:08.970993 27961 demo.cpp:220] map published. 114 | I0901 00:31:09.640645 27961 demo.cpp:220] map published. 115 | I0901 00:31:10.322463 27961 demo.cpp:220] map published. 116 | I0901 00:31:10.974926 27961 demo.cpp:220] map published. 117 | I0901 00:31:11.633643 27961 demo.cpp:220] map published. 118 | I0901 00:31:12.310458 27961 demo.cpp:220] map published. 119 | I0901 00:31:12.968681 27961 demo.cpp:220] map published. 120 | I0901 00:31:13.650996 27961 demo.cpp:220] map published. 121 | I0901 00:31:14.299005 27961 demo.cpp:220] map published. 122 | I0901 00:31:14.982439 27961 demo.cpp:220] map published. 123 | I0901 00:31:15.644176 27961 demo.cpp:220] map published. 124 | I0901 00:31:16.300596 27961 demo.cpp:220] map published. 125 | I0901 00:31:16.966149 27961 demo.cpp:220] map published. 126 | I0901 00:31:17.632576 27961 demo.cpp:220] map published. 127 | I0901 00:31:18.322696 27961 demo.cpp:220] map published. 128 | I0901 00:31:18.985534 27961 demo.cpp:220] map published. 129 | I0901 00:31:19.642920 27961 demo.cpp:220] map published. 130 | I0901 00:31:20.300014 27961 demo.cpp:220] map published. 131 | I0901 00:31:20.966637 27961 demo.cpp:220] map published. 132 | I0901 00:31:21.652952 27961 demo.cpp:220] map published. 133 | I0901 00:31:22.299712 27961 demo.cpp:220] map published. 134 | I0901 00:31:22.966414 27961 demo.cpp:220] map published. 135 | I0901 00:31:23.632936 27961 demo.cpp:220] map published. 136 | I0901 00:31:24.299824 27961 demo.cpp:220] map published. 137 | I0901 00:31:24.966722 27961 demo.cpp:220] map published. 138 | I0901 00:31:25.634627 27961 demo.cpp:220] map published. 139 | I0901 00:31:26.306320 27961 demo.cpp:220] map published. 140 | I0901 00:31:26.966459 27961 demo.cpp:220] map published. 141 | I0901 00:31:27.633569 27961 demo.cpp:220] map published. 142 | I0901 00:31:28.299474 27961 demo.cpp:220] map published. 143 | I0901 00:31:28.967027 27961 demo.cpp:220] map published. 144 | I0901 00:31:29.650491 27961 demo.cpp:220] map published. 145 | I0901 00:31:30.312742 27961 demo.cpp:220] map published. 146 | I0901 00:31:30.966722 27961 demo.cpp:220] map published. 147 | I0901 00:31:31.634001 27961 demo.cpp:220] map published. 148 | I0901 00:31:32.300202 27961 demo.cpp:220] map published. 149 | I0901 00:31:32.966569 27961 demo.cpp:220] map published. 150 | I0901 00:31:33.633646 27961 demo.cpp:220] map published. 151 | I0901 00:31:34.300420 27961 demo.cpp:220] map published. 152 | I0901 00:31:34.966708 27961 demo.cpp:220] map published. 153 | I0901 00:31:35.632695 27961 demo.cpp:220] map published. 154 | I0901 00:31:36.300156 27961 demo.cpp:220] map published. 155 | I0901 00:31:36.966730 27961 demo.cpp:220] map published. 156 | I0901 00:31:37.632578 27961 demo.cpp:220] map published. 157 | I0901 00:31:38.309468 27961 demo.cpp:220] map published. 158 | I0901 00:31:38.966909 27961 demo.cpp:220] map published. 159 | I0901 00:31:39.633121 27961 demo.cpp:220] map published. 160 | I0901 00:31:40.308041 27961 demo.cpp:220] map published. 161 | I0901 00:31:40.966317 27961 demo.cpp:220] map published. 162 | I0901 00:31:41.632781 27961 demo.cpp:220] map published. 163 | I0901 00:31:42.299707 27961 demo.cpp:220] map published. 164 | I0901 00:31:42.986014 27961 demo.cpp:220] map published. 165 | I0901 00:31:43.633001 27961 demo.cpp:220] map published. 166 | I0901 00:31:44.330663 27961 demo.cpp:220] map published. 167 | I0901 00:31:44.966167 27961 demo.cpp:220] map published. 168 | I0901 00:31:45.632637 27961 demo.cpp:220] map published. 169 | I0901 00:31:46.299656 27961 demo.cpp:220] map published. 170 | I0901 00:31:46.966115 27961 demo.cpp:220] map published. 171 | I0901 00:31:47.633381 27961 demo.cpp:220] map published. 172 | I0901 00:31:48.299757 27961 demo.cpp:220] map published. 173 | I0901 00:31:48.971596 27961 demo.cpp:220] map published. 174 | I0901 00:31:49.632958 27961 demo.cpp:220] map published. 175 | I0901 00:31:50.309876 27961 demo.cpp:220] map published. 176 | I0901 00:31:50.978307 27961 demo.cpp:220] map published. 177 | I0901 00:31:51.633314 27961 demo.cpp:220] map published. 178 | I0901 00:31:52.299875 27961 demo.cpp:220] map published. 179 | I0901 00:31:52.966203 27961 demo.cpp:220] map published. 180 | I0901 00:31:53.634218 27961 demo.cpp:220] map published. 181 | I0901 00:31:54.310513 27961 demo.cpp:220] map published. 182 | I0901 00:31:54.965984 27961 demo.cpp:220] map published. 183 | I0901 00:31:55.634379 27961 demo.cpp:220] map published. 184 | I0901 00:31:56.299489 27961 demo.cpp:220] map published. 185 | I0901 00:31:56.966298 27961 demo.cpp:220] map published. 186 | I0901 00:31:57.641638 27961 demo.cpp:220] map published. 187 | I0901 00:31:58.299230 27961 demo.cpp:220] map published. 188 | I0901 00:31:58.965623 27961 demo.cpp:220] map published. 189 | I0901 00:31:59.632917 27961 demo.cpp:220] map published. 190 | I0901 00:32:00.311422 27961 demo.cpp:220] map published. 191 | I0901 00:32:00.966015 27961 demo.cpp:220] map published. 192 | I0901 00:32:01.632556 27961 demo.cpp:220] map published. 193 | I0901 00:32:02.299398 27961 demo.cpp:220] map published. 194 | I0901 00:32:02.966256 27961 demo.cpp:220] map published. 195 | I0901 00:32:03.632685 27961 demo.cpp:220] map published. 196 | I0901 00:32:04.299849 27961 demo.cpp:220] map published. 197 | I0901 00:32:04.966495 27961 demo.cpp:220] map published. 198 | I0901 00:32:05.633950 27961 demo.cpp:220] map published. 199 | I0901 00:32:06.310868 27961 demo.cpp:220] map published. 200 | I0901 00:32:06.966006 27961 demo.cpp:220] map published. 201 | I0901 00:32:07.632587 27961 demo.cpp:220] map published. 202 | I0901 00:32:08.300182 27961 demo.cpp:220] map published. 203 | I0901 00:32:08.966413 27961 demo.cpp:220] map published. 204 | I0901 00:32:09.645931 27961 demo.cpp:220] map published. 205 | I0901 00:32:10.311766 27961 demo.cpp:220] map published. 206 | I0901 00:32:10.966387 27961 demo.cpp:220] map published. 207 | I0901 00:32:11.632858 27961 demo.cpp:220] map published. 208 | I0901 00:32:12.299835 27961 demo.cpp:220] map published. 209 | I0901 00:32:12.984365 27961 demo.cpp:220] map published. 210 | I0901 00:32:13.632853 27961 demo.cpp:220] map published. 211 | I0901 00:32:14.299006 27961 demo.cpp:220] map published. 212 | I0901 00:32:14.967314 27961 demo.cpp:220] map published. 213 | I0901 00:32:15.633236 27961 demo.cpp:220] map published. 214 | I0901 00:32:16.299736 27961 demo.cpp:220] map published. 215 | I0901 00:32:16.966130 27961 demo.cpp:220] map published. 216 | I0901 00:32:17.637109 27961 demo.cpp:220] map published. 217 | I0901 00:32:18.299103 27961 demo.cpp:220] map published. 218 | I0901 00:32:18.965858 27961 demo.cpp:220] map published. 219 | I0901 00:32:19.634142 27961 demo.cpp:220] map published. 220 | I0901 00:32:20.299752 27961 demo.cpp:220] map published. 221 | I0901 00:32:20.966140 27961 demo.cpp:220] map published. 222 | I0901 00:32:21.638882 27961 demo.cpp:220] map published. 223 | I0901 00:32:22.299228 27961 demo.cpp:220] map published. 224 | I0901 00:32:22.992424 27961 demo.cpp:220] map published. 225 | I0901 00:32:23.653982 27961 demo.cpp:220] map published. 226 | I0901 00:32:24.306985 27961 demo.cpp:220] map published. 227 | I0901 00:32:24.978113 27961 demo.cpp:220] map published. 228 | I0901 00:32:25.654402 27961 demo.cpp:220] map published. 229 | I0901 00:32:26.319722 27961 demo.cpp:220] map published. 230 | I0901 00:32:26.965749 27961 demo.cpp:220] map published. 231 | I0901 00:32:27.632786 27961 demo.cpp:220] map published. 232 | I0901 00:32:28.299327 27961 demo.cpp:220] map published. 233 | I0901 00:32:28.966063 27961 demo.cpp:220] map published. 234 | I0901 00:32:29.632656 27961 demo.cpp:220] map published. 235 | I0901 00:32:30.320566 27961 demo.cpp:220] map published. 236 | I0901 00:32:30.975658 27961 demo.cpp:220] map published. 237 | I0901 00:32:31.632846 27961 demo.cpp:220] map published. 238 | I0901 00:32:32.302055 27961 demo.cpp:220] map published. 239 | I0901 00:32:32.967072 27961 demo.cpp:220] map published. 240 | I0901 00:32:33.632736 27961 demo.cpp:220] map published. 241 | I0901 00:32:34.298841 27961 demo.cpp:220] map published. 242 | I0901 00:32:34.966171 27961 demo.cpp:220] map published. 243 | I0901 00:32:35.655846 27961 demo.cpp:220] map published. 244 | I0901 00:32:36.307163 27961 demo.cpp:220] map published. 245 | I0901 00:32:36.983107 27961 demo.cpp:220] map published. 246 | I0901 00:32:37.636724 27961 demo.cpp:220] map published. 247 | I0901 00:32:38.308161 27961 demo.cpp:220] map published. 248 | I0901 00:32:38.976693 27961 demo.cpp:220] map published. 249 | I0901 00:32:39.632827 27961 demo.cpp:220] map published. 250 | I0901 00:32:40.307771 27961 demo.cpp:220] map published. 251 | I0901 00:32:40.966053 27961 demo.cpp:220] map published. 252 | I0901 00:32:41.645540 27961 demo.cpp:220] map published. 253 | I0901 00:32:42.299332 27961 demo.cpp:220] map published. 254 | I0901 00:32:42.965900 27961 demo.cpp:220] map published. 255 | I0901 00:32:43.632803 27961 demo.cpp:220] map published. 256 | I0901 00:32:44.304222 27961 demo.cpp:220] map published. 257 | I0901 00:32:44.966627 27961 demo.cpp:220] map published. 258 | I0901 00:32:45.632920 27961 demo.cpp:220] map published. 259 | I0901 00:32:46.299682 27961 demo.cpp:220] map published. 260 | I0901 00:32:46.982228 27961 demo.cpp:220] map published. 261 | I0901 00:32:47.633232 27961 demo.cpp:220] map published. 262 | I0901 00:32:48.299808 27961 demo.cpp:220] map published. 263 | I0901 00:32:48.969683 27961 demo.cpp:220] map published. 264 | I0901 00:32:49.641455 27961 demo.cpp:220] map published. 265 | I0901 00:32:50.315245 27961 demo.cpp:220] map published. 266 | I0901 00:32:50.967172 27961 demo.cpp:220] map published. 267 | I0901 00:32:51.639041 27961 demo.cpp:220] map published. 268 | I0901 00:32:52.300256 27961 demo.cpp:220] map published. 269 | I0901 00:32:52.984248 27961 demo.cpp:220] map published. 270 | I0901 00:32:53.632927 27961 demo.cpp:220] map published. 271 | I0901 00:32:54.308754 27961 demo.cpp:220] map published. 272 | I0901 00:32:54.997364 27961 demo.cpp:220] map published. 273 | I0901 00:32:55.653721 27961 demo.cpp:220] map published. 274 | I0901 00:32:56.299886 27961 demo.cpp:220] map published. 275 | I0901 00:32:56.966373 27961 demo.cpp:220] map published. 276 | I0901 00:32:57.653648 27961 demo.cpp:220] map published. 277 | I0901 00:32:58.299319 27961 demo.cpp:220] map published. 278 | I0901 00:32:58.966327 27961 demo.cpp:220] map published. 279 | I0901 00:32:59.632746 27961 demo.cpp:220] map published. 280 | I0901 00:33:00.299409 27961 demo.cpp:220] map published. 281 | I0901 00:33:00.966498 27961 demo.cpp:220] map published. 282 | I0901 00:33:01.645337 27961 demo.cpp:220] map published. 283 | I0901 00:33:02.307989 27961 demo.cpp:220] map published. 284 | I0901 00:33:02.978488 27961 demo.cpp:220] map published. 285 | I0901 00:33:03.633026 27961 demo.cpp:220] map published. 286 | I0901 00:33:04.307694 27961 demo.cpp:220] map published. 287 | I0901 00:33:04.987552 27961 demo.cpp:220] map published. 288 | I0901 00:33:05.632488 27961 demo.cpp:220] map published. 289 | I0901 00:33:06.299158 27961 demo.cpp:220] map published. 290 | I0901 00:33:06.965987 27961 demo.cpp:220] map published. 291 | I0901 00:33:07.643311 27961 demo.cpp:220] map published. 292 | I0901 00:33:08.299487 27961 demo.cpp:220] map published. 293 | I0901 00:33:08.966624 27961 demo.cpp:220] map published. 294 | I0901 00:33:09.632786 27961 demo.cpp:220] map published. 295 | I0901 00:33:10.298929 27961 demo.cpp:220] map published. 296 | I0901 00:33:10.980965 27961 demo.cpp:220] map published. 297 | I0901 00:33:11.632748 27961 demo.cpp:220] map published. 298 | I0901 00:33:12.301033 27961 demo.cpp:220] map published. 299 | I0901 00:33:12.966310 27961 demo.cpp:220] map published. 300 | I0901 00:33:13.633273 27961 demo.cpp:220] map published. 301 | I0901 00:33:14.298750 27961 demo.cpp:220] map published. 302 | I0901 00:33:14.965837 27961 demo.cpp:220] map published. 303 | I0901 00:33:15.633261 27961 demo.cpp:220] map published. 304 | I0901 00:33:16.299384 27961 demo.cpp:220] map published. 305 | I0901 00:33:16.967547 27961 demo.cpp:220] map published. 306 | I0901 00:33:17.633116 27961 demo.cpp:220] map published. 307 | I0901 00:33:18.299248 27961 demo.cpp:220] map published. 308 | I0901 00:33:18.967144 27961 demo.cpp:220] map published. 309 | I0901 00:33:19.633419 27961 demo.cpp:220] map published. 310 | I0901 00:33:20.299819 27961 demo.cpp:220] map published. 311 | I0901 00:33:20.965929 27961 demo.cpp:220] map published. 312 | I0901 00:33:21.632958 27961 demo.cpp:220] map published. 313 | I0901 00:33:22.298897 27961 demo.cpp:220] map published. 314 | I0901 00:33:23.007810 27961 demo.cpp:220] map published. 315 | I0901 00:33:23.670468 27961 demo.cpp:220] map published. 316 | I0901 00:33:24.336941 27961 demo.cpp:220] map published. 317 | I0901 00:33:25.003358 27961 demo.cpp:220] map published. 318 | I0901 00:33:25.670646 27961 demo.cpp:220] map published. 319 | I0901 00:33:26.336977 27961 demo.cpp:220] map published. 320 | I0901 00:33:27.002977 27961 demo.cpp:220] map published. 321 | I0901 00:33:27.670239 27961 demo.cpp:220] map published. 322 | I0901 00:33:28.337234 27961 demo.cpp:220] map published. 323 | I0901 00:33:29.003309 27961 demo.cpp:220] map published. 324 | I0901 00:33:29.670491 27961 demo.cpp:220] map published. 325 | I0901 00:33:30.337316 27961 demo.cpp:220] map published. 326 | I0901 00:33:31.015508 27961 demo.cpp:220] map published. 327 | I0901 00:33:31.669665 27961 demo.cpp:220] map published. 328 | I0901 00:33:32.350884 27961 demo.cpp:220] map published. 329 | I0901 00:33:33.003307 27961 demo.cpp:220] map published. 330 | I0901 00:33:33.670243 27961 demo.cpp:220] map published. 331 | I0901 00:33:34.337057 27961 demo.cpp:220] map published. 332 | I0901 00:33:35.019065 27961 demo.cpp:220] map published. 333 | I0901 00:33:35.670733 27961 demo.cpp:220] map published. 334 | I0901 00:33:36.344578 27961 demo.cpp:220] map published. 335 | I0901 00:33:37.003561 27961 demo.cpp:220] map published. 336 | I0901 00:33:37.670719 27961 demo.cpp:220] map published. 337 | I0901 00:33:38.336735 27961 demo.cpp:220] map published. 338 | I0901 00:33:39.012130 27961 demo.cpp:220] map published. 339 | I0901 00:33:39.670114 27961 demo.cpp:220] map published. 340 | I0901 00:33:40.347743 27961 demo.cpp:220] map published. 341 | I0901 00:33:41.020964 27961 demo.cpp:220] map published. 342 | I0901 00:33:41.678530 27961 demo.cpp:220] map published. 343 | I0901 00:33:42.339381 27961 demo.cpp:220] map published. 344 | I0901 00:33:43.002955 27961 demo.cpp:220] map published. 345 | I0901 00:33:43.670024 27961 demo.cpp:220] map published. 346 | I0901 00:33:44.336577 27961 demo.cpp:220] map published. 347 | I0901 00:33:45.007071 27961 demo.cpp:220] map published. 348 | I0901 00:33:45.677110 27961 demo.cpp:220] map published. 349 | I0901 00:33:46.354965 27961 demo.cpp:220] map published. 350 | I0901 00:33:47.024134 27961 demo.cpp:220] map published. 351 | I0901 00:33:47.673031 27961 demo.cpp:220] map published. 352 | I0901 00:33:48.336643 27961 demo.cpp:220] map published. 353 | I0901 00:33:49.013851 27961 demo.cpp:220] map published. 354 | I0901 00:33:49.696002 27961 demo.cpp:220] map published. 355 | I0901 00:33:50.336349 27961 demo.cpp:220] map published. 356 | I0901 00:33:51.002629 27961 demo.cpp:220] map published. 357 | I0901 00:33:51.670987 27961 demo.cpp:220] map published. 358 | I0901 00:33:52.337098 27961 demo.cpp:220] map published. 359 | I0901 00:33:53.003571 27961 demo.cpp:220] map published. 360 | I0901 00:33:53.670486 27961 demo.cpp:220] map published. 361 | I0901 00:33:54.336477 27961 demo.cpp:220] map published. 362 | I0901 00:33:55.002663 27961 demo.cpp:220] map published. 363 | I0901 00:33:55.669797 27961 demo.cpp:220] map published. 364 | I0901 00:33:56.336977 27961 demo.cpp:220] map published. 365 | I0901 00:33:57.003273 27961 demo.cpp:220] map published. 366 | I0901 00:33:57.677873 27961 demo.cpp:220] map published. 367 | I0901 00:33:58.336477 27961 demo.cpp:220] map published. 368 | I0901 00:33:59.015504 27961 demo.cpp:220] map published. 369 | I0901 00:33:59.670090 27961 demo.cpp:220] map published. 370 | I0901 00:34:00.337854 27961 demo.cpp:220] map published. 371 | I0901 00:34:01.020032 27961 demo.cpp:220] map published. 372 | I0901 00:34:01.671200 27961 demo.cpp:220] map published. 373 | I0901 00:34:02.336948 27961 demo.cpp:220] map published. 374 | I0901 00:34:03.021368 27961 demo.cpp:220] map published. 375 | I0901 00:34:03.669873 27961 demo.cpp:220] map published. 376 | I0901 00:34:04.355551 27961 demo.cpp:220] map published. 377 | I0901 00:34:05.017581 27961 demo.cpp:220] map published. 378 | I0901 00:34:05.670851 27961 demo.cpp:220] map published. 379 | I0901 00:34:06.336555 27961 demo.cpp:220] map published. 380 | I0901 00:34:07.002393 27961 demo.cpp:220] map published. 381 | I0901 00:34:07.679008 27961 demo.cpp:220] map published. 382 | I0901 00:34:08.336747 27961 demo.cpp:220] map published. 383 | I0901 00:34:09.003388 27961 demo.cpp:220] map published. 384 | I0901 00:34:09.671520 27961 demo.cpp:220] map published. 385 | I0901 00:34:10.354318 27961 demo.cpp:220] map published. 386 | I0901 00:34:11.003981 27961 demo.cpp:220] map published. 387 | I0901 00:34:11.670795 27961 demo.cpp:220] map published. 388 | I0901 00:34:12.336686 27961 demo.cpp:220] map published. 389 | I0901 00:34:13.002959 27961 demo.cpp:220] map published. 390 | I0901 00:34:13.674599 27961 demo.cpp:220] map published. 391 | I0901 00:34:14.346482 27961 demo.cpp:220] map published. 392 | I0901 00:34:15.015612 27961 demo.cpp:220] map published. 393 | I0901 00:34:15.676816 27961 demo.cpp:220] map published. 394 | I0901 00:34:16.336910 27961 demo.cpp:220] map published. 395 | I0901 00:34:17.004040 27961 demo.cpp:220] map published. 396 | I0901 00:34:17.670528 27961 demo.cpp:220] map published. 397 | I0901 00:34:18.351713 27961 demo.cpp:220] map published. 398 | I0901 00:34:19.015702 27961 demo.cpp:220] map published. 399 | I0901 00:34:19.669906 27961 demo.cpp:220] map published. 400 | I0901 00:34:20.336441 27961 demo.cpp:220] map published. 401 | I0901 00:34:21.004002 27961 demo.cpp:220] map published. 402 | I0901 00:34:21.670184 27961 demo.cpp:220] map published. 403 | I0901 00:34:22.336177 27961 demo.cpp:220] map published. 404 | I0901 00:34:23.024817 27961 demo.cpp:220] map published. 405 | I0901 00:34:23.669765 27961 demo.cpp:220] map published. 406 | I0901 00:34:24.340267 27961 demo.cpp:220] map published. 407 | I0901 00:34:25.003317 27961 demo.cpp:220] map published. 408 | I0901 00:34:25.704550 27961 demo.cpp:220] map published. 409 | I0901 00:34:26.336670 27961 demo.cpp:220] map published. 410 | I0901 00:34:27.004694 27961 demo.cpp:220] map published. 411 | I0901 00:34:27.670359 27961 demo.cpp:220] map published. 412 | I0901 00:34:28.336678 27961 demo.cpp:220] map published. 413 | I0901 00:34:29.003423 27961 demo.cpp:220] map published. 414 | I0901 00:34:29.670724 27961 demo.cpp:220] map published. 415 | I0901 00:34:30.357407 27961 demo.cpp:220] map published. 416 | I0901 00:34:31.004613 27961 demo.cpp:220] map published. 417 | I0901 00:34:31.670212 27961 demo.cpp:220] map published. 418 | I0901 00:34:32.336886 27961 demo.cpp:220] map published. 419 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | frenet_ilqr_test 4 | 0.0.0 5 | The frenet_ilqr_test package 6 | 7 | 8 | 9 | 10 | ljn 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | roscpp 54 | roscpp 55 | grid_map_ros 56 | tinyspline_ros 57 | ros_viz_tools 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /src/path/data_structure.cpp: -------------------------------------------------------------------------------- 1 | #include "data_structure.h" -------------------------------------------------------------------------------- /src/path/data_structure.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include "solver/variable.h" 6 | 7 | namespace PathPlanning { 8 | 9 | using Solver::Variable; 10 | using Solver::Node; 11 | 12 | constexpr std::size_t N_PATH_STATE = 3; 13 | constexpr std::size_t N_PATH_CONTROL = 1; 14 | constexpr std::size_t L_INDEX = 0; 15 | constexpr std::size_t HD_INDEX = 1; // HD for heading diff. 16 | constexpr std::size_t K_INDEX = 2; 17 | constexpr std::size_t KR_INDEX = 0; // Control, kappa rate. 18 | 19 | struct XYPosition { 20 | double x = 0.0; 21 | double y = 0.0; 22 | }; 23 | using Vector = XYPosition; 24 | 25 | struct SLPosition { 26 | double s = 0.0; 27 | double l = 0.0; 28 | }; 29 | 30 | struct PathPoint : public XYPosition, SLPosition { 31 | double theta = 0.0; 32 | double kappa = 0.0; 33 | double dkappa = 0.0; 34 | double theta_diff = 0.0; 35 | double dl = 0.0; 36 | double ddl = 0.0; 37 | }; 38 | 39 | } // namespace PathPlanning 40 | -------------------------------------------------------------------------------- /src/path/free_space.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "free_space.h" 4 | 5 | namespace PathPlanning { 6 | bool FreeSpace::get_l_bound_for_circle(double s, double r, double* lower_bound, double* upper_bound) const { 7 | *lower_bound = -20.0; 8 | *upper_bound = 20.0; 9 | if (s < 0.0 || s > _p_reference_line->length()) { 10 | return false; 11 | } 12 | const auto ref_pt = _p_reference_line->get_reference_point(s); 13 | Vector norm_vec{cos(ref_pt.theta + M_PI_2), sin(ref_pt.theta + M_PI_2)}; 14 | auto comp = [](const BoundaryPoint& pt, double s) { return pt.s < s; }; 15 | const auto boundary_start_iter = std::lower_bound(_boundary_points.begin(), _boundary_points.end(), s - r, comp); 16 | const auto boundary_end_iter = std::lower_bound(_boundary_points.begin(), _boundary_points.end(), s + r, comp); 17 | 18 | for (auto iter = boundary_start_iter; iter < boundary_end_iter; ++iter) { 19 | // Upper bound. 20 | Vector ref_to_upper_bound{iter->ub_xy.x - ref_pt.x, iter->ub_xy.y - ref_pt.y}; 21 | const double ub_dist_to_norm = std::fabs(ref_to_upper_bound.x * norm_vec.y - ref_to_upper_bound.y * norm_vec.x); 22 | if (ub_dist_to_norm < r) { 23 | const double bound_l = ref_to_upper_bound.x * norm_vec.x + ref_to_upper_bound.y * norm_vec.y; 24 | *upper_bound = std::min(*upper_bound, bound_l - std::sqrt(r * r - ub_dist_to_norm * ub_dist_to_norm)); 25 | } 26 | // Lower bound. 27 | Vector ref_to_lower_bound{iter->lb_xy.x - ref_pt.x, iter->lb_xy.y - ref_pt.y}; 28 | const double lb_dist_to_norm = std::fabs(ref_to_lower_bound.x * norm_vec.y - ref_to_lower_bound.y * norm_vec.x); 29 | if (lb_dist_to_norm < r) { 30 | const double bound_l = ref_to_lower_bound.x * norm_vec.x + ref_to_lower_bound.y * norm_vec.y; 31 | *lower_bound = std::max(*lower_bound, bound_l + std::sqrt(r * r - lb_dist_to_norm * lb_dist_to_norm)); 32 | } 33 | } 34 | return true; 35 | } 36 | 37 | void FreeSpace::update_circle_bounds(double r) { 38 | _circle_bounds.clear(); 39 | for (const auto& bound : _boundary_points) { 40 | BoundaryPoint circle_bound; 41 | circle_bound.s = bound.s; 42 | if (get_l_bound_for_circle(bound.s, r, &circle_bound.lb_l, &circle_bound.ub_l)) { 43 | _circle_bounds.emplace_back(circle_bound); 44 | } 45 | } 46 | } 47 | 48 | BoundaryPoint FreeSpace::get_circle_bound(double s) const { 49 | BoundaryPoint ret; 50 | ret.s = s; 51 | if (_circle_bounds.empty() || s < _circle_bounds.front().s || s > _circle_bounds.back().s) { 52 | return ret; 53 | } 54 | const auto iter = std::lower_bound(_circle_bounds.begin(), _circle_bounds.end(), s, [](const BoundaryPoint& bound, double s) { 55 | return bound.s < s; 56 | }); 57 | if (iter == _circle_bounds.begin()) { 58 | ret = _circle_bounds.front(); 59 | } else if (iter != _circle_bounds.end()) { 60 | const auto prev_iter = iter - 1; 61 | const double iter_share = (s - prev_iter->s) / (iter->s - prev_iter->s); 62 | const double prev_iter_share = (iter->s - s) / (iter->s - prev_iter->s); 63 | ret.lb_l = iter_share * iter->lb_l + prev_iter_share * prev_iter->lb_l; 64 | ret.ub_l = iter_share * iter->ub_l + prev_iter_share * prev_iter->ub_l; 65 | } 66 | return ret; 67 | } 68 | 69 | } -------------------------------------------------------------------------------- /src/path/free_space.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include "data_structure.h" 6 | #include "reference_line.h" 7 | 8 | namespace PathPlanning 9 | { 10 | 11 | struct BoundaryPoint { 12 | double s = 0.0; 13 | double lb_l = DBL_MIN; 14 | double ub_l = DBL_MAX; 15 | XYPosition lb_xy; 16 | XYPosition ub_xy; 17 | }; 18 | 19 | class FreeSpace { 20 | public: 21 | FreeSpace() = default; 22 | void set_reference_line(std::shared_ptr ref_ptr) { _p_reference_line = ref_ptr; } 23 | std::shared_ptr reference_line_ptr() const { return _p_reference_line; } 24 | const std::vector& boundary_points() const { return _boundary_points; } 25 | std::vector* mutable_boundary_points() { return &_boundary_points; } 26 | bool get_l_bound_for_circle(double s, double r, double* lower_bound, double* upper_bound) const; 27 | void update_circle_bounds(double r); 28 | BoundaryPoint get_circle_bound(double s) const; 29 | bool is_initialized() const { return _is_initialized; } 30 | void set_is_initialized(bool val) { _is_initialized = val; } 31 | private: 32 | std::shared_ptr _p_reference_line; 33 | std::vector _boundary_points; 34 | std::vector _circle_bounds; 35 | bool _is_initialized = false; 36 | }; 37 | 38 | } // namespace PathPlanning 39 | -------------------------------------------------------------------------------- /src/path/gflags.cpp: -------------------------------------------------------------------------------- 1 | #include "gflags.h" 2 | 3 | DEFINE_double(vehicle_width, 1.9, "vehicle width"); 4 | DEFINE_double(vehicle_length, 4.8, "vehicle length"); 5 | DEFINE_double(rear_axle_to_center, 1.4, "distance from rear axle to center"); 6 | DEFINE_double(max_kappa, 0.2, "max kappa"); 7 | DEFINE_double(max_dkappa, 0.2, "max dkappa"); -------------------------------------------------------------------------------- /src/path/gflags.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | DECLARE_double(vehicle_width); 5 | DECLARE_double(vehicle_length); 6 | DECLARE_double(rear_axle_to_center); 7 | DECLARE_double(max_kappa); 8 | DECLARE_double(max_dkappa); -------------------------------------------------------------------------------- /src/path/path_costs.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include "data_structure.h" 5 | #include "solver/problem_manager.h" 6 | #include "gflags.h" 7 | 8 | namespace PathPlanning { 9 | 10 | using namespace Solver; 11 | using PathCost = Cost; 12 | 13 | class RefLCost : public PathCost { 14 | public: 15 | RefLCost(double weight, const std::string& name = "") : PathCost("ref_l_cost_" + name), _weight(weight) {} 16 | double cost_value(const Trajectory& trajectory, std::size_t step) override { 17 | CHECK(step < trajectory.size()); 18 | const double l = trajectory.at(step).state()(L_INDEX); 19 | return 0.5 * _weight * l * l; 20 | } 21 | 22 | void calculate_derivatives(const Trajectory& trajectory, std::size_t step, DerivativesInfo* derivatives) override { 23 | CHECK(step < trajectory.size()); 24 | const double l = trajectory.at(step).state()(L_INDEX); 25 | Eigen::Matrix dx{_weight * l, 0.0, 0.0}; 26 | Eigen::Matrix dxx; 27 | dxx << _weight, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; 28 | derivatives->lx += dx; 29 | derivatives->lxx += dxx; 30 | } 31 | private: 32 | double _weight = 0.0; 33 | }; 34 | 35 | class KappaCost : public PathCost { 36 | public: 37 | KappaCost(double weight, const std::string& name = "") : PathCost("kappa_cost_" + name), _weight(weight) {} 38 | double cost_value(const Trajectory& trajectory, std::size_t step) override { 39 | CHECK(step < trajectory.size()); 40 | const double kappa = trajectory.at(step).state()(K_INDEX); 41 | return 0.5 * _weight * kappa * kappa; 42 | } 43 | 44 | void calculate_derivatives(const Trajectory& trajectory, std::size_t step, DerivativesInfo* derivatives) override { 45 | CHECK(step < trajectory.size()); 46 | const double kappa = trajectory.at(step).state()(K_INDEX); 47 | Eigen::Matrix dx{0.0, 0.0, _weight * kappa}; 48 | Eigen::Matrix dxx; 49 | dxx << 0.0, 0.0, 0.0, 50 | 0.0, 0.0, 0.0, 51 | 0.0, 0.0, _weight; 52 | derivatives->lx += dx; 53 | derivatives->lxx += dxx; 54 | } 55 | private: 56 | double _weight = 0.0; 57 | }; 58 | 59 | class KappaRateCost : public PathCost { 60 | public: 61 | KappaRateCost(double weight, const std::string& name = "") : PathCost("kappa_rate_cost_" + name), _weight(weight) {} 62 | double cost_value(const Trajectory& trajectory, std::size_t step) override { 63 | CHECK(step < trajectory.size() - 1); 64 | const double kappa_rate = trajectory.at(step).control()(KR_INDEX); 65 | return 0.5 * _weight * kappa_rate * kappa_rate; 66 | } 67 | 68 | void calculate_derivatives(const Trajectory& trajectory, std::size_t step, DerivativesInfo* derivatives) override { 69 | CHECK(step < trajectory.size() - 1); 70 | const double kappa_rate = trajectory.at(step).control()(KR_INDEX); 71 | Eigen::Matrix du{_weight * kappa_rate}; 72 | Eigen::Matrix duu{_weight}; 73 | derivatives->lu += du; 74 | derivatives->luu += duu; 75 | } 76 | private: 77 | double _weight = 0.0; 78 | }; 79 | 80 | class TargetStateCost : public PathCost { 81 | public: 82 | TargetStateCost(double target_l, double target_heading_diff, double l_weight, double heading_diff_weight, const std::string& name = "") 83 | : PathCost("target_state_cost_" + name), _target_l(target_l), _target_heading_diff(target_heading_diff), _l_weight(l_weight), _heading_diff_weight(heading_diff_weight) {} 84 | double cost_value(const Trajectory& trajectory, std::size_t step) override { 85 | CHECK(step < trajectory.size()); 86 | const double l = trajectory.at(step).state()(L_INDEX); 87 | const double heading_diff = trajectory.at(step).state()(HD_INDEX); 88 | return 0.5 * _l_weight * (l - _target_l) * (l - _target_l) + 0.5 * _heading_diff_weight * (_target_heading_diff - heading_diff) * (_target_heading_diff - heading_diff); 89 | } 90 | void calculate_derivatives(const Trajectory& trajectory, std::size_t step, DerivativesInfo* derivatives) override { 91 | CHECK(step < trajectory.size()); 92 | const double l = trajectory.at(step).state()(L_INDEX); 93 | const double heading_diff = trajectory.at(step).state()(HD_INDEX); 94 | Eigen::Matrix dx{_l_weight * (l - _target_l), _heading_diff_weight * (heading_diff - _target_heading_diff), 0.0}; 95 | Eigen::Matrix dxx; 96 | dxx << _l_weight, 0.0, 0.0, 0.0, _heading_diff_weight, 0.0, 0.0, 0.0, 0.0; 97 | derivatives->lx += dx; 98 | derivatives->lxx += dxx; 99 | } 100 | 101 | private: 102 | double _l_weight = 0.0; 103 | double _heading_diff_weight = 0.0; 104 | double _target_l = 0.0; 105 | double _target_heading_diff = 0.0; 106 | }; 107 | 108 | // ----------Constraints---------------- 109 | 110 | class RearBoundaryConstraint : public PathCost { 111 | public: 112 | RearBoundaryConstraint(const FreeSpace& free_space, double q1, double q2, double buffer = 0.0, const std::string& name = "") 113 | : PathCost("rear_boundary_constraint" + name), _free_space(free_space), _barrier_function(q1, q2), _buffer(buffer) {} 114 | void update(const Trajectory& trajectory) override { 115 | if (not _bounds.empty()) { 116 | return; 117 | } 118 | for (const auto& pt : trajectory) { 119 | _bounds.emplace_back(_free_space.get_circle_bound(pt.sample())); 120 | } 121 | } 122 | double cost_value(const Trajectory& trajectory, std::size_t step) override { 123 | CHECK(step < _bounds.size()); 124 | const auto& bound = _bounds.at(step); 125 | const double l = trajectory.at(step).state()(L_INDEX); 126 | return _barrier_function.value(bound.lb_l + _buffer - l) + _barrier_function.value(l - bound.ub_l + _buffer); 127 | } 128 | 129 | void calculate_derivatives(const Trajectory& trajectory, std::size_t step, DerivativesInfo* derivatives) override { 130 | CHECK(step < _bounds.size()); 131 | const auto& bound = _bounds.at(step); 132 | const double l = trajectory.at(step).state()(L_INDEX); 133 | derivatives->lx += _barrier_function.dx(bound.lb_l + _buffer - l, {-1.0, 0.0, 0.0}) + _barrier_function.dx(l - bound.ub_l + _buffer, {1.0, 0.0, 0.0}); 134 | derivatives->lxx += _barrier_function.ddx(bound.lb_l + _buffer - l, {-1.0, 0.0, 0.0}, Eigen::MatrixXd::Zero(N_PATH_STATE, N_PATH_STATE)) 135 | + _barrier_function.ddx(l - bound.ub_l + _buffer, {1.0, 0.0, 0.0}, Eigen::MatrixXd::Zero(N_PATH_STATE, N_PATH_STATE)); 136 | } 137 | 138 | private: 139 | const FreeSpace& _free_space; 140 | double _buffer = 0.0; 141 | std::vector _bounds; 142 | ExpBarrierFunction _barrier_function; 143 | }; 144 | 145 | struct FrontBoundaryInfo { 146 | PathPoint front_ref_point; 147 | XYPosition front_xy; 148 | SLPosition front_sl; 149 | Vector norm_vec; 150 | double lb = 0.0; 151 | double ub = 0.0; 152 | }; 153 | 154 | class FrontBoundaryConstraint : public PathCost { 155 | public: 156 | FrontBoundaryConstraint(const FreeSpace& free_space, double q1, double q2, double buffer = 0.0, const std::string& name = "") 157 | : PathCost("front_boundary_constraint" + name), _free_space(free_space), _barrier_function(q1, q2), _buffer(buffer) {} 158 | void update(const Trajectory& trajectory) override { 159 | const auto& reference_line = *_free_space.reference_line_ptr(); 160 | if (_rear_ref_points.empty()) { 161 | for (const auto& pt : trajectory) { 162 | _rear_ref_points.emplace_back(reference_line.get_reference_point(pt.sample())); 163 | } 164 | } 165 | 166 | _info_vec.clear(); 167 | const double rear_axle_to_front = FLAGS_vehicle_length * 0.5 + FLAGS_rear_axle_to_center; 168 | for (std::size_t i = 0; i < trajectory.size(); ++i) { 169 | const auto& pt = trajectory.at(i); 170 | const auto& rear_ref_pt = _rear_ref_points.at(i); 171 | const XYPosition rear_xy{rear_ref_pt.x + pt.state()(L_INDEX) * cos(rear_ref_pt.theta + M_PI_2), rear_ref_pt.y + pt.state()(L_INDEX) * sin(rear_ref_pt.theta + M_PI_2)}; 172 | const double ego_heading = pt.state()(HD_INDEX) + rear_ref_pt.theta; 173 | const XYPosition front_xy{rear_xy.x + rear_axle_to_front * cos(ego_heading), rear_xy.y + rear_axle_to_front * sin(ego_heading)}; 174 | const double hint_s = pt.sample() + rear_axle_to_front * cos(pt.state()(HD_INDEX)); 175 | 176 | FrontBoundaryInfo info; 177 | info.front_xy = front_xy; 178 | info.front_sl = reference_line.get_projection_by_newton(front_xy, hint_s); 179 | info.front_ref_point = reference_line.get_reference_point(info.front_sl.s); 180 | info.norm_vec.x = cos(info.front_ref_point.theta + M_PI_2); 181 | info.norm_vec.y = sin(info.front_ref_point.theta + M_PI_2); 182 | _free_space.get_l_bound_for_circle(info.front_sl.s, FLAGS_vehicle_width / 2.0, &info.lb, &info.ub); 183 | 184 | _info_vec.emplace_back(std::move(info)); 185 | } 186 | } 187 | double cost_value(const Trajectory& trajectory, std::size_t step) override { 188 | CHECK(step < _rear_ref_points.size()); 189 | CHECK(step < _info_vec.size()); 190 | const double rear_axle_to_front = FLAGS_vehicle_length * 0.5 + FLAGS_rear_axle_to_center; 191 | const auto& pt = trajectory.at(step); 192 | const auto& rear_ref_pt = _rear_ref_points.at(step); 193 | const XYPosition rear_xy{rear_ref_pt.x + pt.state()(L_INDEX) * cos(rear_ref_pt.theta + M_PI_2), rear_ref_pt.y + pt.state()(L_INDEX) * sin(rear_ref_pt.theta + M_PI_2)}; 194 | const double ego_heading = pt.state()(HD_INDEX) + rear_ref_pt.theta; 195 | const XYPosition front_xy{rear_xy.x + rear_axle_to_front * cos(ego_heading), rear_xy.y + rear_axle_to_front * sin(ego_heading)}; 196 | const auto& info = _info_vec.at(step); 197 | double front_l = (front_xy.x - info.front_ref_point.x) * info.norm_vec.x + (front_xy.y - info.front_ref_point.y) * info.norm_vec.y; 198 | // if (step < 10) { 199 | // LOG(INFO) << step << "front l " << front_l; 200 | // } 201 | return _barrier_function.value(info.lb + _buffer - front_l) + _barrier_function.value(front_l - info.ub + _buffer); 202 | } 203 | 204 | void calculate_derivatives(const Trajectory& trajectory, std::size_t step, DerivativesInfo* derivatives) override { 205 | CHECK(step < _rear_ref_points.size()); 206 | CHECK(step < _info_vec.size()); 207 | const auto& info = _info_vec.at(step); 208 | const auto& norm_vec = info.norm_vec; 209 | const double front_l = info.front_sl.l; 210 | const auto& rear_ref_pt = _rear_ref_points.at(step); 211 | const auto& pt = trajectory.at(step); 212 | const double rear_axle_to_front = FLAGS_vehicle_length * 0.5 + FLAGS_rear_axle_to_center; 213 | const double ego_heading = pt.state()(HD_INDEX) + rear_ref_pt.theta; 214 | Eigen::Matrix dldx; 215 | dldx << norm_vec.x * cos(rear_ref_pt.theta + M_PI_2) + norm_vec.y * sin(rear_ref_pt.theta + M_PI_2), 216 | -norm_vec.x * rear_axle_to_front * sin(ego_heading) + norm_vec.y * rear_axle_to_front * cos(ego_heading), 217 | 0.0; 218 | Eigen::Matrix dlddx = Eigen::MatrixXd::Zero(N_PATH_STATE, N_PATH_STATE); 219 | dlddx(1, 1) = -norm_vec.x * rear_axle_to_front * cos(ego_heading) - norm_vec.y * rear_axle_to_front * sin(ego_heading); 220 | derivatives->lx += _barrier_function.dx(info.lb + _buffer - front_l, -dldx) + _barrier_function.dx(front_l - info.ub + _buffer, dldx); 221 | derivatives->lxx += _barrier_function.ddx(info.lb + _buffer - front_l, -dldx, -dlddx) 222 | + _barrier_function.ddx(front_l - info.ub + _buffer, dldx, dlddx); 223 | } 224 | 225 | private: 226 | const FreeSpace& _free_space; 227 | double _buffer = 0.0; 228 | ExpBarrierFunction _barrier_function; 229 | std::vector _rear_ref_points; 230 | std::vector _info_vec; 231 | }; 232 | 233 | class KappaConstraint : public PathCost { 234 | public: 235 | KappaConstraint(double q1, double q2, const std::string& name = "") 236 | : PathCost("kappa_constraint_" + name), _barrier_function(q1, q2) {} 237 | double cost_value(const Trajectory& trajectory, std::size_t step) override { 238 | CHECK(step < trajectory.size()); 239 | const double kappa = trajectory.at(step).state()(K_INDEX); 240 | return _barrier_function.value(-FLAGS_max_kappa - kappa) + _barrier_function.value(kappa - FLAGS_max_kappa); 241 | } 242 | void calculate_derivatives(const Trajectory& trajectory, std::size_t step, DerivativesInfo* derivatives) override { 243 | CHECK(step < trajectory.size()); 244 | const double kappa = trajectory.at(step).state()(K_INDEX); 245 | derivatives->lx += _barrier_function.dx(-FLAGS_max_kappa - kappa, {0.0, 0.0, -1.0}) + _barrier_function.dx(kappa - FLAGS_max_kappa, {0.0, 0.0, 1.0}); 246 | derivatives->lxx += _barrier_function.ddx(-FLAGS_max_kappa - kappa, {0.0, 0.0, -1.0}, Eigen::MatrixXd::Zero(N_PATH_STATE, N_PATH_STATE)) 247 | + _barrier_function.ddx(kappa - FLAGS_max_kappa, {0.0, 0.0, 1.0}, Eigen::MatrixXd::Zero(N_PATH_STATE, N_PATH_STATE)); 248 | } 249 | private: 250 | ExpBarrierFunction _barrier_function; 251 | }; 252 | 253 | } -------------------------------------------------------------------------------- /src/path/path_problem_manager.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "path_problem_manager.h" 3 | #include "path_costs.h" 4 | #include "tool.h" 5 | 6 | namespace PathPlanning { 7 | using namespace Solver; 8 | 9 | constexpr double WEIGHT_REF_L = 0.001; 10 | constexpr double WEIGHT_KAPPA = 10.0; 11 | constexpr double WEIGHT_KAPPA_RATE = 50.0; 12 | constexpr double WEIGHT_END_L = 0.5; 13 | constexpr double WEIGHT_END_HEADING_DIFF = 20.0; 14 | 15 | void PathProblemManager::formulate_path_problem(const FreeSpace& free_space, const ReferenceLine& reference_line, const PathPoint& init_state, const PathPoint& end_state) { 16 | // Initialize knots and costs. 17 | if (not free_space.is_initialized()) { 18 | LOG(INFO) << "[PathProblem] Quit for uninitialized free_space."; 19 | return; 20 | } 21 | // Set dynamics. 22 | _p_dynamics = std::make_shared(reference_line); 23 | // Init trajectory. 24 | calculate_init_trajectory(reference_line, init_state, end_state); 25 | // Add costs and constraints. 26 | add_costs(free_space, end_state); 27 | _problem_formulated = true; 28 | } 29 | 30 | void PathProblemManager::calculate_init_trajectory(const ReferenceLine& reference_line, const PathPoint& init_state, const PathPoint& end_state) { 31 | _init_trajectory.clear(); 32 | _knots.clear(); 33 | _costs.clear(); 34 | 35 | auto dynamics_by_kappa = [&reference_line](const Node& node, double move_dist)->Node { 36 | const double kappa_ref = reference_line.get_reference_point(node.sample()).kappa; 37 | const double l = node.state()(L_INDEX); 38 | const double hd = node.state()(HD_INDEX); 39 | const double kappa = node.state()(K_INDEX); 40 | const double dl = (1 - kappa_ref * l) * tan(hd); 41 | const double dhd = (1 - kappa_ref * l) * kappa / cos(hd) - kappa_ref; 42 | Node ret; 43 | (*ret.mutable_state())(L_INDEX) = l + dl * move_dist; 44 | (*ret.mutable_state())(HD_INDEX) = constrainAngle(hd + dhd * move_dist); 45 | return ret; 46 | }; 47 | 48 | const auto cart_state = init_state; 49 | Node node; 50 | const auto init_proj = reference_line.get_projection(init_state); 51 | node.set_sample(init_proj.s); 52 | (*node.mutable_state())(L_INDEX) = init_proj.l; 53 | (*node.mutable_state())(HD_INDEX) = constrainAngle(init_state.theta - reference_line.get_reference_point(init_proj.s).theta); 54 | (*node.mutable_state())(K_INDEX) = init_state.kappa; 55 | (*node.mutable_control())(KR_INDEX) = init_state.dkappa; 56 | _init_trajectory.emplace_back(node); 57 | _knots.emplace_back(init_proj.s); 58 | _costs.emplace_back(CostMap()); 59 | const double pursuit_dist = 10.0; 60 | const auto end_proj = reference_line.get_projection(end_state); 61 | 62 | for (std::size_t i = 0; node.sample() + _config.delta_s < end_proj.s; ++i) { 63 | auto new_node = dynamics_by_kappa(node, _config.delta_s); 64 | new_node.set_sample(node.sample() + _config.delta_s); 65 | if (i == 0) { 66 | (*new_node.mutable_state())(K_INDEX) = init_state.dkappa * _config.delta_s + init_state.kappa; 67 | } else { 68 | // Kappa by pure pursuit. 69 | const auto xy = reference_line.get_xy_by_sl(SLPosition{new_node.sample(), new_node.state()(L_INDEX)}); 70 | const double heading = constrainAngle(reference_line.get_reference_point(new_node.sample()).theta + new_node.state()(HD_INDEX)); 71 | const auto pursuit_point = reference_line.get_reference_point(new_node.sample() + pursuit_dist); 72 | const double dist = distance(xy, pursuit_point); 73 | double kappa = std::fabs(dist) > 1.0 ? 2 * sin(atan2(pursuit_point.y - xy.y, pursuit_point.x - xy.x) - heading) / dist : 0.0; 74 | kappa = std::min(kappa, FLAGS_max_kappa); 75 | kappa = std::max(kappa, -FLAGS_max_kappa); 76 | (*new_node.mutable_state())(K_INDEX) = kappa; 77 | } 78 | node = std::move(new_node); 79 | _init_trajectory.emplace_back(node); 80 | _knots.emplace_back(node.sample()); 81 | _costs.emplace_back(CostMap()); 82 | } 83 | 84 | for (std::size_t i = 0; i < _init_trajectory.size() - 1; ++i) { 85 | const double delta_s = _config.delta_s * (1 - reference_line.get_reference_point(_init_trajectory.at(i).sample()).kappa) / cos(_init_trajectory.at(i).state()(HD_INDEX)); 86 | (*_init_trajectory.at(i).mutable_control())(KR_INDEX) = 87 | (_init_trajectory.at(i + 1).state()(K_INDEX) - _init_trajectory.at(i).state()(K_INDEX)) / delta_s; 88 | } 89 | } 90 | 91 | std::vector PathProblemManager::transform_to_path_points( 92 | const ReferenceLine& reference_line, const Trajectory& trajectory) { 93 | std::vector ret; 94 | for (const auto& pt : trajectory) { 95 | SLPosition sl; 96 | sl.s = pt.sample(); 97 | sl.l = pt.state()(PathPlanning::L_INDEX); 98 | const auto xy = reference_line.get_xy_by_sl(sl); 99 | PathPoint path_point; 100 | path_point.s = sl.s; 101 | path_point.l = sl.l; 102 | path_point.x = xy.x; 103 | path_point.y = xy.y; 104 | path_point.theta = constrainAngle(reference_line.get_reference_point(sl.s).theta + pt.state()(HD_INDEX)); 105 | path_point.kappa = pt.state()(K_INDEX); 106 | ret.emplace_back(std::move(path_point)); 107 | } 108 | return ret; 109 | } 110 | 111 | void PathProblemManager::add_costs(const FreeSpace& free_space, const PathPoint& end_state) { 112 | CHECK(_costs.size() == num_steps()); 113 | std::shared_ptr ref_l_cost_ptr(new RefLCost(WEIGHT_REF_L)); 114 | std::shared_ptr kappa_cost_ptr(new KappaCost(WEIGHT_KAPPA)); 115 | std::shared_ptr kappa_rate_cost_ptr(new KappaRateCost(WEIGHT_KAPPA_RATE)); 116 | std::shared_ptr rear_boundary_constraint_ptr(new RearBoundaryConstraint(free_space, 0.5, 2.5)); 117 | std::shared_ptr front_boundary_constraint_ptr(new FrontBoundaryConstraint(free_space, 0.5, 2.5)); 118 | std::shared_ptr kappa_constraint_ptr(new KappaConstraint(0.5, 2.5)); 119 | const auto& reference_line = *(free_space.reference_line_ptr()); 120 | const double end_state_l = reference_line.get_projection(end_state).l; 121 | const double end_state_heading_diff = constrainAngle(end_state.theta - reference_line.get_reference_point(_knots.back()).theta); 122 | std::shared_ptr end_state_cost_ptr(new TargetStateCost(end_state_l, end_state_heading_diff, WEIGHT_END_L, WEIGHT_END_HEADING_DIFF, "end_state")); 123 | 124 | _dynamic_costs.emplace_back(rear_boundary_constraint_ptr); 125 | _dynamic_costs.emplace_back(front_boundary_constraint_ptr); 126 | 127 | for (std::size_t step = 0; step < num_steps(); ++step) { 128 | _costs.at(step)[ref_l_cost_ptr->name()] = ref_l_cost_ptr; 129 | _costs.at(step)[kappa_cost_ptr->name()] = kappa_cost_ptr; 130 | if (step < num_steps() - 1) { 131 | _costs.at(step)[kappa_rate_cost_ptr->name()] = kappa_rate_cost_ptr; 132 | } 133 | if (step > 0) { 134 | _costs.at(step)[rear_boundary_constraint_ptr->name()] = rear_boundary_constraint_ptr; 135 | _costs.at(step)[front_boundary_constraint_ptr->name()] = front_boundary_constraint_ptr; 136 | _costs.at(step)[kappa_constraint_ptr->name()] = kappa_constraint_ptr; 137 | } 138 | } 139 | _costs.back()[end_state_cost_ptr->name()] = end_state_cost_ptr; 140 | } 141 | 142 | Variable FrenetPathDynamics::move_forward(const Node& node, double move_dist) const { 143 | const double kappa_ref = _reference_line.get_reference_point(node.sample()).kappa; 144 | const double l = node.state()(L_INDEX); 145 | const double hd = node.state()(HD_INDEX); 146 | const double kappa = node.state()(K_INDEX); 147 | const double kappa_rate = node.control()(KR_INDEX); 148 | 149 | const double dl = (1 - kappa_ref * l) * tan(hd); 150 | const double dhd = (1 - kappa_ref * l) * kappa / cos(hd) - kappa_ref; 151 | const double dk = (1 - kappa_ref * l) / cos(hd) * kappa_rate; 152 | 153 | Variable ret; 154 | ret << l + dl * move_dist, constrainAngle(hd + dhd * move_dist), kappa + dk * move_dist; 155 | return ret; 156 | } 157 | 158 | Eigen::Matrix FrenetPathDynamics::dx( 159 | const Node& node, double move_dist) const { 160 | const double kappa_ref = _reference_line.get_reference_point(node.sample()).kappa; 161 | const double l = node.state()(L_INDEX); 162 | const double hd = node.state()(HD_INDEX); 163 | const double kappa = node.state()(K_INDEX); 164 | const double kappa_rate = node.control()(KR_INDEX); 165 | 166 | Eigen::Matrix ret; 167 | ret << 1 - move_dist * tan(hd) * kappa_ref, move_dist * (1 - kappa_ref * l) / cos(hd) / cos(hd), 0.0, 168 | -move_dist * kappa * kappa_ref / cos(hd), 1 + move_dist * (1 - kappa_ref * l) * kappa * tan(hd) / cos(hd), move_dist * (1 - kappa_ref * l) / cos(hd), 169 | -move_dist * kappa_rate * kappa_ref / cos(hd), move_dist * (1 - kappa_ref * l) * kappa_rate * tan(hd) / cos(hd), 1.0; 170 | return ret; 171 | } 172 | 173 | Eigen::Matrix FrenetPathDynamics::du( 174 | const Node& node, double move_dist) const { 175 | const double kappa_ref = _reference_line.get_reference_point(node.sample()).kappa; 176 | const double l = node.state()(L_INDEX); 177 | const double hd = node.state()(HD_INDEX); 178 | const double kappa = node.state()(K_INDEX); 179 | Eigen::Matrix ret; 180 | ret << 0.0, 0.0, move_dist * (1 - kappa_ref * l) / cos(hd); 181 | return ret; 182 | } 183 | 184 | } -------------------------------------------------------------------------------- /src/path/path_problem_manager.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "solver/problem_manager.h" 3 | #include "path/free_space.h" 4 | #include "path/reference_line.h" 5 | 6 | namespace PathPlanning { 7 | 8 | using namespace Solver; 9 | 10 | class FrenetPathDynamics : public Dynamics { 11 | public: 12 | FrenetPathDynamics(const ReferenceLine& reference_line) : _reference_line(reference_line) {} 13 | Variable move_forward(const Node& node, double move_dist) const override; 14 | Eigen::Matrix dx(const Node& node, double move_dist) const override; 15 | Eigen::Matrix du(const Node& node, double move_dist) const override; 16 | private: 17 | const ReferenceLine& _reference_line; 18 | }; 19 | 20 | struct Config { 21 | double delta_s = 0.3; 22 | // double planning_length = 0.0; 23 | }; 24 | 25 | class PathProblemManager : public ProblemManager { 26 | public: 27 | void formulate_path_problem(const FreeSpace& free_space, const ReferenceLine& reference_line, const PathPoint& init_state, const PathPoint& end_state); 28 | const Config& config() const { return _config; } 29 | static std::vector transform_to_path_points( 30 | const ReferenceLine& reference_line, const Trajectory& trajectory); 31 | 32 | private: 33 | void calculate_init_trajectory(const ReferenceLine& reference_line, const PathPoint& init_state, const PathPoint& end_state); 34 | void add_costs(const FreeSpace& free_space, const PathPoint& end_state); 35 | Config _config; 36 | }; 37 | 38 | } -------------------------------------------------------------------------------- /src/path/reference_line.cpp: -------------------------------------------------------------------------------- 1 | #include "reference_line.h" 2 | #include "tool.h" 3 | #include 4 | #include "glog/logging.h" 5 | 6 | namespace PathPlanning { 7 | 8 | void ReferenceLine::initialize(const tk::spline& x_s, const tk::spline& y_s, double length) { 9 | _x_s = x_s; 10 | _y_s = y_s; 11 | _length = length; 12 | _is_initialized = true; 13 | } 14 | 15 | PathPoint ReferenceLine::get_reference_point(double s) const { 16 | s = std::min(s, _length); 17 | s = std::max(s, 0.0); 18 | PathPoint ret; 19 | ret.x = _x_s(s); 20 | ret.y = _y_s(s); 21 | double x_d1 = _x_s.deriv(1, s); 22 | double y_d1 = _y_s.deriv(1, s); 23 | ret.theta = atan2(y_d1, x_d1); 24 | double x_d2 = _x_s.deriv(2, s); 25 | double y_d2 = _y_s.deriv(2, s); 26 | ret.kappa = (x_d1 * y_d2 - y_d1 * x_d2) / pow(pow(x_d1, 2) + pow(y_d1, 2), 1.5); 27 | return ret; 28 | } 29 | 30 | XYPosition ReferenceLine::get_xy_by_sl(const SLPosition& sl) const { 31 | const auto ref_pt = get_reference_point(sl.s); 32 | double norm = constrainAngle(ref_pt.theta + M_PI_2); 33 | return {ref_pt.x + sl.l * cos(norm), ref_pt.y + sl.l * sin(norm)}; 34 | } 35 | 36 | SLPosition ReferenceLine::get_projection(const XYPosition& xy) const { 37 | const double grid = 1.0; 38 | double tmp_s = 0.0, min_dis_s = 0.0; 39 | auto min_dis = DBL_MAX; 40 | while (tmp_s <= _length) { 41 | XYPosition ref_pt{_x_s(tmp_s), _y_s(tmp_s)}; 42 | double tmp_dis = distance(ref_pt, xy); 43 | if (tmp_dis < min_dis) { 44 | min_dis = tmp_dis; 45 | min_dis_s = tmp_s; 46 | } 47 | tmp_s += grid; 48 | } 49 | // Newton's method 50 | return get_projection_by_newton(xy, min_dis_s); 51 | } 52 | 53 | SLPosition ReferenceLine::get_projection_by_newton(const XYPosition& xy, double hint_s) const { 54 | hint_s = std::min(hint_s, _length); 55 | double cur_s = hint_s; 56 | double prev_s = hint_s; 57 | double x = 0.0, y = 0.0, dx = 0.0, dy = 0.0; 58 | for (int i = 0; i < 20; ++i) { 59 | x = _x_s(cur_s); 60 | y = _y_s(cur_s); 61 | dx = _x_s.deriv(1, cur_s); 62 | dy = _y_s.deriv(1, cur_s); 63 | double ddx = _x_s.deriv(2, cur_s); 64 | double ddy = _y_s.deriv(2, cur_s); 65 | // Ignore coeff 2 in J and H. 66 | double j = (x - xy.x) * dx + (y - xy.y) * dy; 67 | double h = dx * dx + (x - xy.x) * ddx + dy * dy + (y - xy.y) * ddy; 68 | cur_s -= j / h; 69 | if (fabs(cur_s - prev_s) < 1e-5) break; 70 | prev_s = cur_s; 71 | } 72 | 73 | cur_s = std::min(cur_s, _length); 74 | const double heading_vec_len = sqrt(dx*dx + dy*dy); 75 | return SLPosition{cur_s, -(xy.x - x) * dy / heading_vec_len + (xy.y - y) * dx / heading_vec_len}; 76 | } 77 | 78 | } -------------------------------------------------------------------------------- /src/path/reference_line.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include "spline.h" 4 | #include "data_structure.h" 5 | 6 | namespace PathPlanning { 7 | 8 | class ReferenceLine { 9 | public: 10 | ReferenceLine() : _length(0.0), _is_initialized(false) {}; 11 | void initialize(const tk::spline& x_s, const tk::spline& y_s, double length); 12 | bool is_initialized() const { return _is_initialized; } 13 | PathPoint get_reference_point(double s) const; 14 | XYPosition get_xy_by_sl(const SLPosition& sl) const; 15 | SLPosition get_projection(const XYPosition& xy) const; 16 | SLPosition get_projection_by_newton(const XYPosition& xy, double hint_s) const; 17 | const tk::spline& x_s() const { return _x_s; } 18 | const tk::spline& y_s() const { return _y_s; } 19 | double length() const { return _length; } 20 | private: 21 | tk::spline _x_s; 22 | tk::spline _y_s; 23 | double _length; 24 | bool _is_initialized; 25 | }; 26 | 27 | } -------------------------------------------------------------------------------- /src/path/spline.cpp: -------------------------------------------------------------------------------- 1 | 2 | // ------------------------- 3 | #include "spline.h" 4 | 5 | namespace tk { 6 | 7 | band_matrix::band_matrix(int dim, int n_u, int n_l) { 8 | resize(dim, n_u, n_l); 9 | } 10 | void band_matrix::resize(int dim, int n_u, int n_l) { 11 | assert(dim > 0); 12 | assert(n_u >= 0); 13 | assert(n_l >= 0); 14 | m_upper.resize(n_u + 1); 15 | m_lower.resize(n_l + 1); 16 | for (size_t i = 0; i < m_upper.size(); i++) { 17 | m_upper[i].resize(dim); 18 | } 19 | for (size_t i = 0; i < m_lower.size(); i++) { 20 | m_lower[i].resize(dim); 21 | } 22 | } 23 | int band_matrix::dim() const { 24 | if (m_upper.size() > 0) { 25 | return m_upper[0].size(); 26 | } else { 27 | return 0; 28 | } 29 | } 30 | 31 | // defines the new operator (), so that we can access the elements 32 | // by A(i,j), index going from i=0,...,dim()-1 33 | double &band_matrix::operator()(int i, int j) { 34 | int k = j - i; // what band is the entry 35 | assert((i >= 0) && (i < dim()) && (j >= 0) && (j < dim())); 36 | assert((-num_lower() <= k) && (k <= num_upper())); 37 | // k=0 -> diogonal, k<0 lower left part, k>0 upper right part 38 | if (k >= 0) return m_upper[k][i]; 39 | else return m_lower[-k][i]; 40 | } 41 | double band_matrix::operator()(int i, int j) const { 42 | int k = j - i; // what band is the entry 43 | assert((i >= 0) && (i < dim()) && (j >= 0) && (j < dim())); 44 | assert((-num_lower() <= k) && (k <= num_upper())); 45 | // k=0 -> diogonal, k<0 lower left part, k>0 upper right part 46 | if (k >= 0) return m_upper[k][i]; 47 | else return m_lower[-k][i]; 48 | } 49 | // second diag (used in LU decomposition), saved in m_lower 50 | double band_matrix::saved_diag(int i) const { 51 | assert((i >= 0) && (i < dim())); 52 | return m_lower[0][i]; 53 | } 54 | double &band_matrix::saved_diag(int i) { 55 | assert((i >= 0) && (i < dim())); 56 | return m_lower[0][i]; 57 | } 58 | 59 | // LR-Decomposition of a band matrix 60 | void band_matrix::lu_decompose() { 61 | int i_max, j_max; 62 | int j_min; 63 | double x; 64 | 65 | // preconditioning 66 | // normalize column i so that a_ii=1 67 | for (int i = 0; i < this->dim(); i++) { 68 | assert(this->operator()(i, i) != 0.0); 69 | this->saved_diag(i) = 1.0 / this->operator()(i, i); 70 | j_min = std::max(0, i - this->num_lower()); 71 | j_max = std::min(this->dim() - 1, i + this->num_upper()); 72 | for (int j = j_min; j <= j_max; j++) { 73 | this->operator()(i, j) *= this->saved_diag(i); 74 | } 75 | this->operator()(i, i) = 1.0; // prevents rounding errors 76 | } 77 | 78 | // Gauss LR-Decomposition 79 | for (int k = 0; k < this->dim(); k++) { 80 | i_max = std::min(this->dim() - 1, k + this->num_lower()); // num_lower not a mistake! 81 | for (int i = k + 1; i <= i_max; i++) { 82 | assert(this->operator()(k, k) != 0.0); 83 | x = -this->operator()(i, k) / this->operator()(k, k); 84 | this->operator()(i, k) = -x; // assembly part of L 85 | j_max = std::min(this->dim() - 1, k + this->num_upper()); 86 | for (int j = k + 1; j <= j_max; j++) { 87 | // assembly part of R 88 | this->operator()(i, j) = this->operator()(i, j) + x * this->operator()(k, j); 89 | } 90 | } 91 | } 92 | } 93 | // solves Ly=b 94 | std::vector band_matrix::l_solve(const std::vector &b) const { 95 | assert(this->dim() == (int) b.size()); 96 | std::vector x(this->dim()); 97 | int j_start; 98 | double sum; 99 | for (int i = 0; i < this->dim(); i++) { 100 | sum = 0; 101 | j_start = std::max(0, i - this->num_lower()); 102 | for (int j = j_start; j < i; j++) sum += this->operator()(i, j) * x[j]; 103 | x[i] = (b[i] * this->saved_diag(i)) - sum; 104 | } 105 | return x; 106 | } 107 | // solves Rx=y 108 | std::vector band_matrix::r_solve(const std::vector &b) const { 109 | assert(this->dim() == (int) b.size()); 110 | std::vector x(this->dim()); 111 | int j_stop; 112 | double sum; 113 | for (int i = this->dim() - 1; i >= 0; i--) { 114 | sum = 0; 115 | j_stop = std::min(this->dim() - 1, i + this->num_upper()); 116 | for (int j = i + 1; j <= j_stop; j++) sum += this->operator()(i, j) * x[j]; 117 | x[i] = (b[i] - sum) / this->operator()(i, i); 118 | } 119 | return x; 120 | } 121 | 122 | std::vector band_matrix::lu_solve(const std::vector &b, 123 | bool is_lu_decomposed) { 124 | assert(this->dim() == (int) b.size()); 125 | std::vector x, y; 126 | if (is_lu_decomposed == false) { 127 | this->lu_decompose(); 128 | } 129 | y = this->l_solve(b); 130 | x = this->r_solve(y); 131 | return x; 132 | } 133 | 134 | 135 | 136 | 137 | // spline implementation 138 | // ----------------------- 139 | 140 | void spline::set_boundary(spline::bd_type left, double left_value, 141 | spline::bd_type right, double right_value, 142 | bool force_linear_extrapolation) { 143 | assert(m_x.size() == 0); // set_points() must not have happened yet 144 | m_left = left; 145 | m_right = right; 146 | m_left_value = left_value; 147 | m_right_value = right_value; 148 | m_force_linear_extrapolation = force_linear_extrapolation; 149 | } 150 | 151 | void spline::set_points(const std::vector &x, 152 | const std::vector &y, bool cubic_spline) { 153 | assert(x.size() == y.size()); 154 | assert(x.size() > 2); 155 | m_x = x; 156 | m_y = y; 157 | int n = x.size(); 158 | // TODO: maybe sort x and y, rather than returning an error 159 | for (int i = 0; i < n - 1; i++) { 160 | assert(m_x[i] < m_x[i + 1]); 161 | } 162 | 163 | if (cubic_spline == true) { // cubic spline interpolation 164 | // setting up the matrix and right hand side of the equation system 165 | // for the parameters b[] 166 | band_matrix A(n, 1, 1); 167 | std::vector rhs(n); 168 | for (int i = 1; i < n - 1; i++) { 169 | A(i, i - 1) = 1.0 / 3.0 * (x[i] - x[i - 1]); 170 | A(i, i) = 2.0 / 3.0 * (x[i + 1] - x[i - 1]); 171 | A(i, i + 1) = 1.0 / 3.0 * (x[i + 1] - x[i]); 172 | rhs[i] = (y[i + 1] - y[i]) / (x[i + 1] - x[i]) - (y[i] - y[i - 1]) / (x[i] - x[i - 1]); 173 | } 174 | // boundary conditions 175 | if (m_left == spline::second_deriv) { 176 | // 2*b[0] = f'' 177 | A(0, 0) = 2.0; 178 | A(0, 1) = 0.0; 179 | rhs[0] = m_left_value; 180 | } else if (m_left == spline::first_deriv) { 181 | // c[0] = f', needs to be re-expressed in terms of b: 182 | // (2b[0]+b[1])(x[1]-x[0]) = 3 ((y[1]-y[0])/(x[1]-x[0]) - f') 183 | A(0, 0) = 2.0 * (x[1] - x[0]); 184 | A(0, 1) = 1.0 * (x[1] - x[0]); 185 | rhs[0] = 3.0 * ((y[1] - y[0]) / (x[1] - x[0]) - m_left_value); 186 | } else { 187 | assert(false); 188 | } 189 | if (m_right == spline::second_deriv) { 190 | // 2*b[n-1] = f'' 191 | A(n - 1, n - 1) = 2.0; 192 | A(n - 1, n - 2) = 0.0; 193 | rhs[n - 1] = m_right_value; 194 | } else if (m_right == spline::first_deriv) { 195 | // c[n-1] = f', needs to be re-expressed in terms of b: 196 | // (b[n-2]+2b[n-1])(x[n-1]-x[n-2]) 197 | // = 3 (f' - (y[n-1]-y[n-2])/(x[n-1]-x[n-2])) 198 | A(n - 1, n - 1) = 2.0 * (x[n - 1] - x[n - 2]); 199 | A(n - 1, n - 2) = 1.0 * (x[n - 1] - x[n - 2]); 200 | rhs[n - 1] = 3.0 * (m_right_value - (y[n - 1] - y[n - 2]) / (x[n - 1] - x[n - 2])); 201 | } else { 202 | assert(false); 203 | } 204 | 205 | // solve the equation system to obtain the parameters b[] 206 | m_b = A.lu_solve(rhs); 207 | 208 | // calculate parameters a[] and c[] based on b[] 209 | m_a.resize(n); 210 | m_c.resize(n); 211 | for (int i = 0; i < n - 1; i++) { 212 | m_a[i] = 1.0 / 3.0 * (m_b[i + 1] - m_b[i]) / (x[i + 1] - x[i]); 213 | m_c[i] = (y[i + 1] - y[i]) / (x[i + 1] - x[i]) 214 | - 1.0 / 3.0 * (2.0 * m_b[i] + m_b[i + 1]) * (x[i + 1] - x[i]); 215 | } 216 | } else { // linear interpolation 217 | m_a.resize(n); 218 | m_b.resize(n); 219 | m_c.resize(n); 220 | for (int i = 0; i < n - 1; i++) { 221 | m_a[i] = 0.0; 222 | m_b[i] = 0.0; 223 | m_c[i] = (m_y[i + 1] - m_y[i]) / (m_x[i + 1] - m_x[i]); 224 | } 225 | } 226 | 227 | // for left extrapolation coefficients 228 | m_b0 = (m_force_linear_extrapolation == false) ? m_b[0] : 0.0; 229 | m_c0 = m_c[0]; 230 | 231 | // for the right extrapolation coefficients 232 | // f_{n-1}(x) = b*(x-x_{n-1})^2 + c*(x-x_{n-1}) + y_{n-1} 233 | double h = x[n - 1] - x[n - 2]; 234 | // m_b[n-1] is determined by the boundary condition 235 | m_a[n - 1] = 0.0; 236 | m_c[n - 1] = 3.0 * m_a[n - 2] * h * h + 2.0 * m_b[n - 2] * h + m_c[n - 2]; // = f'_{n-2}(x_{n-1}) 237 | if (m_force_linear_extrapolation == true) 238 | m_b[n - 1] = 0.0; 239 | } 240 | 241 | double spline::operator()(double x) const { 242 | size_t n = m_x.size(); 243 | // find the closest point m_x[idx] < x, idx=0 even if x::const_iterator it; 245 | it = std::lower_bound(m_x.begin(), m_x.end(), x); 246 | int idx = std::max(int(it - m_x.begin()) - 1, 0); 247 | 248 | double h = x - m_x[idx]; 249 | double interpol; 250 | if (x < m_x[0]) { 251 | // extrapolation to the left 252 | interpol = (m_b0 * h + m_c0) * h + m_y[0]; 253 | } else if (x > m_x[n - 1]) { 254 | // extrapolation to the right 255 | interpol = (m_b[n - 1] * h + m_c[n - 1]) * h + m_y[n - 1]; 256 | } else { 257 | // interpolation 258 | interpol = ((m_a[idx] * h + m_b[idx]) * h + m_c[idx]) * h + m_y[idx]; 259 | } 260 | return interpol; 261 | } 262 | 263 | double spline::deriv(int order, double x) const { 264 | assert(order > 0); 265 | 266 | size_t n = m_x.size(); 267 | // find the closest point m_x[idx] < x, idx=0 even if x::const_iterator it; 269 | it = std::lower_bound(m_x.begin(), m_x.end(), x); 270 | int idx = std::max(int(it - m_x.begin()) - 1, 0); 271 | 272 | double h = x - m_x[idx]; 273 | double interpol; 274 | if (x < m_x[0]) { 275 | // extrapolation to the left 276 | switch (order) { 277 | case 1:interpol = 2.0 * m_b0 * h + m_c0; 278 | break; 279 | case 2:interpol = 2.0 * m_b0 * h; 280 | break; 281 | default:interpol = 0.0; 282 | break; 283 | } 284 | } else if (x > m_x[n - 1]) { 285 | // extrapolation to the right 286 | switch (order) { 287 | case 1:interpol = 2.0 * m_b[n - 1] * h + m_c[n - 1]; 288 | break; 289 | case 2:interpol = 2.0 * m_b[n - 1]; 290 | break; 291 | default:interpol = 0.0; 292 | break; 293 | } 294 | } else { 295 | // interpolation 296 | switch (order) { 297 | case 1:interpol = (3.0 * m_a[idx] * h + 2.0 * m_b[idx]) * h + m_c[idx]; 298 | break; 299 | case 2:interpol = 6.0 * m_a[idx] * h + 2.0 * m_b[idx]; 300 | break; 301 | case 3:interpol = 6.0 * m_a[idx]; 302 | break; 303 | default:interpol = 0.0; 304 | break; 305 | } 306 | } 307 | return interpol; 308 | } 309 | } 310 | -------------------------------------------------------------------------------- /src/path/spline.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by yao on 19-3-18. 3 | // 4 | 5 | /* 6 | * spline.h 7 | * 8 | * simple cubic spline interpolation library without external 9 | * dependencies 10 | * 11 | * --------------------------------------------------------------------- 12 | * Copyright (C) 2011, 2014 Tino Kluge (ttk448 at gmail.com) 13 | * 14 | * This program is free software; you can redistribute it and/or 15 | * modify it under the terms of the GNU General Public License 16 | * as published by the Free Software Foundation; either version 2 17 | * of the License, or (at your option) any later version. 18 | * 19 | * This program is distributed in the hope that it will be useful, 20 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 21 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 22 | * GNU General Public License for more details. 23 | * 24 | * You should have received a copy of the GNU General Public License 25 | * along with this program. If not, see . 26 | * --------------------------------------------------------------------- 27 | * 28 | */ 29 | 30 | 31 | #ifndef TK_SPLINE_H 32 | #define TK_SPLINE_H 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | 40 | // unnamed namespace only because the implementation is in this 41 | // header file and we don't want to export symbols to the obj files 42 | namespace tk { 43 | 44 | // band matrix solver 45 | class band_matrix { 46 | private: 47 | std::vector > m_upper; // upper band 48 | std::vector > m_lower; // lower band 49 | public: 50 | band_matrix() {}; // constructor 51 | band_matrix(int dim, int n_u, int n_l); // constructor 52 | ~band_matrix() {}; // destructor 53 | void resize(int dim, int n_u, int n_l); // init with dim,n_u,n_l 54 | int dim() const; // matrix dimension 55 | int num_upper() const { 56 | return m_upper.size() - 1; 57 | } 58 | int num_lower() const { 59 | return m_lower.size() - 1; 60 | } 61 | // access operator 62 | double &operator()(int i, int j); // write 63 | double operator()(int i, int j) const; // read 64 | // we can store an additional diogonal (in m_lower) 65 | double &saved_diag(int i); 66 | double saved_diag(int i) const; 67 | void lu_decompose(); 68 | std::vector r_solve(const std::vector &b) const; 69 | std::vector l_solve(const std::vector &b) const; 70 | std::vector lu_solve(const std::vector &b, 71 | bool is_lu_decomposed = false); 72 | 73 | }; 74 | 75 | // spline interpolation 76 | class spline { 77 | public: 78 | enum bd_type { 79 | first_deriv = 1, 80 | second_deriv = 2 81 | }; 82 | 83 | private: 84 | std::vector m_x, m_y; // x,y coordinates of points 85 | // interpolation parameters 86 | // f(x) = a*(x-x_i)^3 + b*(x-x_i)^2 + c*(x-x_i) + y_i 87 | std::vector m_a, m_b, m_c; // spline coefficients 88 | double m_b0, m_c0; // for left extrapol 89 | bd_type m_left, m_right; 90 | double m_left_value, m_right_value; 91 | bool m_force_linear_extrapolation; 92 | 93 | public: 94 | // set default boundary condition to be zero curvature at both ends 95 | spline() : m_left(second_deriv), m_right(second_deriv), 96 | m_left_value(0.0), m_right_value(0.0), 97 | m_force_linear_extrapolation(false) { 98 | ; 99 | } 100 | 101 | // optional, but if called it has to come be before set_points() 102 | void set_boundary(bd_type left, double left_value, 103 | bd_type right, double right_value, 104 | bool force_linear_extrapolation = false); 105 | void set_points(const std::vector &x, 106 | const std::vector &y, bool cubic_spline = true); 107 | double operator()(double x) const; 108 | double deriv(int order, double x) const; 109 | }; 110 | 111 | } // namespace tk 112 | 113 | #endif /* TK_SPLINE_H */ 114 | 115 | -------------------------------------------------------------------------------- /src/path/tool.cpp: -------------------------------------------------------------------------------- 1 | #include "tool.h" 2 | 3 | namespace PathPlanning { 4 | 5 | double distance(const XYPosition& p1, const XYPosition& p2) { 6 | return std::sqrt(std::pow(p1.x-p2.x, 2) + std::pow(p1.y-p2.y, 2)); 7 | } 8 | 9 | double constrainAngle(double angle) { 10 | if (angle > M_PI) { 11 | angle -= 2 * M_PI; 12 | return constrainAngle(angle); 13 | } else if (angle < -M_PI) { 14 | angle += 2 * M_PI; 15 | return constrainAngle(angle); 16 | } else { 17 | return angle; 18 | } 19 | } 20 | 21 | PathPoint local_to_global(const PathPoint& ref, const PathPoint& target) { 22 | PathPoint ret = target; 23 | ret.x = target.x * cos(ref.theta) - target.y * sin(ref.theta) + ref.x; 24 | ret.y = target.x * sin(ref.theta) + target.y * cos(ref.theta) + ref.y; 25 | ret.theta = ref.theta + target.theta; 26 | return ret; 27 | } 28 | 29 | PathPoint global_to_local(const PathPoint& ref, const PathPoint& target) { 30 | 31 | } 32 | 33 | } -------------------------------------------------------------------------------- /src/path/tool.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include "data_structure.h" 4 | 5 | namespace PathPlanning { 6 | 7 | double distance(const XYPosition& p1, const XYPosition& p2); 8 | 9 | double constrainAngle(double angle); 10 | 11 | PathPoint local_to_global(const PathPoint& ref, const PathPoint& target); 12 | 13 | PathPoint global_to_local(const PathPoint& ref, const PathPoint& target); 14 | 15 | } -------------------------------------------------------------------------------- /src/solver/problem_manager.cpp: -------------------------------------------------------------------------------- 1 | #include "problem_manager.h" 2 | 3 | namespace Solver { 4 | 5 | } -------------------------------------------------------------------------------- /src/solver/problem_manager.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "variable.h" 8 | 9 | namespace Solver { 10 | 11 | template 12 | class Cost { 13 | public: 14 | Cost(const std::string& name) : _name(name) {} 15 | virtual ~Cost() = default; 16 | virtual double cost_value(const Trajectory& trajectory, std::size_t step) { 17 | return 0.0; 18 | } 19 | virtual void calculate_derivatives(const Trajectory& trajectory, std::size_t step, DerivativesInfo* derivatives) = 0; 20 | const std::string name() const { return _name; } 21 | virtual void update(const Trajectory& trajectory) {} 22 | private: 23 | const std::string _name; 24 | }; 25 | 26 | template 27 | class ExpBarrierFunction { 28 | public: 29 | ExpBarrierFunction(double q1, double q2) : _q1(q1), _q2(q2) {} 30 | double value(double x) const { 31 | return _q1 * std::exp(_q2 * x); 32 | } 33 | Eigen::Matrix dx(double x, const Eigen::Matrix& dx) const { 34 | return _q1 * _q2 * std::exp(_q2 * x) * dx; 35 | } 36 | Eigen::Matrix ddx(double x, const Eigen::Matrix& dx, const Eigen::Matrix& ddx) const { 37 | return _q1 * _q2 * std::exp(_q2 * x) * ddx + _q1 * _q2 * _q2 * std::exp(_q2 * x) * dx * dx.transpose(); 38 | } 39 | 40 | private: 41 | double _q1 = 1.0; 42 | double _q2 = 1.0; 43 | }; 44 | 45 | template 46 | using CostMap = std::unordered_map>>; 47 | 48 | template 49 | class Dynamics { 50 | public: 51 | Dynamics() = default; 52 | virtual ~Dynamics() = default; 53 | virtual Variable move_forward(const Node& node, double move_dist) const = 0; 54 | virtual Eigen::Matrix dx(const Node& node, double move_dist) const = 0; 55 | virtual Eigen::Matrix du(const Node& node, double move_dist) const = 0; 56 | }; 57 | 58 | template 59 | class ProblemManager { 60 | public: 61 | ProblemManager() = default; 62 | virtual ~ProblemManager() = default; 63 | bool add_cost_item(std::shared_ptr> cost_item, std::size_t step); 64 | double calculate_total_cost(const Trajectory& trajectory) const; 65 | const std::vector& knots() const { return _knots; } 66 | const std::vector>& costs() const { return _costs; } 67 | std::size_t num_steps() const { return _knots.size(); } 68 | const Dynamics& dynamics() const { return *_p_dynamics; } 69 | const Trajectory& init_trajectory() const { return _init_trajectory; } 70 | void update_dynamic_costs(const Trajectory& trajectory); 71 | void calculate_derivatives(const Trajectory& trajectory, std::size_t step, DerivativesInfo* derivatives) const; 72 | bool problem_formulated() const { return _problem_formulated; } 73 | protected: 74 | std::vector _knots; 75 | std::vector> _costs; 76 | std::shared_ptr> _p_dynamics; 77 | Trajectory _init_trajectory; 78 | // Some costs may need to be refreshed. 79 | std::vector>> _dynamic_costs; 80 | bool _problem_formulated = false; 81 | }; 82 | 83 | template 84 | bool ProblemManager::add_cost_item(std::shared_ptr> cost_item, std::size_t step) { 85 | if (step >= _costs.size()) { 86 | LOG(ERROR) << "[Problem manager] Trying to add cost at wrong step! Total steps: " << num_steps() << ", input step; " << step; 87 | return false; 88 | } 89 | _costs.at(step)[cost_item->name()] = cost_item; 90 | } 91 | 92 | template 93 | void ProblemManager::calculate_derivatives(const Trajectory& trajectory, std::size_t step, DerivativesInfo* derivatives) const { 94 | CHECK(step < _costs.size()); 95 | DerivativesInfo tmp_derivatives; 96 | for (const auto& cost_pair : _costs.at(step)) { 97 | cost_pair.second->calculate_derivatives(trajectory, step, &tmp_derivatives); 98 | } 99 | *derivatives = std::move(tmp_derivatives); 100 | } 101 | 102 | template 103 | double ProblemManager::calculate_total_cost( 104 | const Trajectory& trajectory) const { 105 | // CHECK_EQ(Trajectory.size(), num_steps()); 106 | const std::size_t size = std::min(_costs.size(), trajectory.size()); 107 | double total_cost = 0.0; 108 | std::unordered_map cost_values; 109 | for (std::size_t i = 0; i < size; ++i) { 110 | for (const auto& cost_pair : _costs.at(i)) { 111 | total_cost += cost_pair.second->cost_value(trajectory, i); 112 | cost_values[cost_pair.second->name()] += cost_pair.second->cost_value(trajectory, i); 113 | } 114 | } 115 | // for (const auto& pair : cost_values) { 116 | // LOG(INFO) << "cost name " << pair.first << ", value " << pair.second; 117 | // } 118 | // LOG(INFO) << "[Test] total value " << total_cost; 119 | return total_cost; 120 | } 121 | 122 | template 123 | void ProblemManager::update_dynamic_costs(const Trajectory& trajectory) { 124 | for (auto cost : _dynamic_costs) { 125 | cost->update(trajectory); 126 | } 127 | } 128 | 129 | } -------------------------------------------------------------------------------- /src/solver/solver.cpp: -------------------------------------------------------------------------------- 1 | #include "solver.h" 2 | 3 | namespace Solver { 4 | 5 | 6 | 7 | } -------------------------------------------------------------------------------- /src/solver/solver.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include "variable.h" 4 | #include "problem_manager.h" 5 | 6 | namespace Solver { 7 | 8 | constexpr double EPS = 1e-5; 9 | 10 | enum class ILQRSolveStatus { 11 | SOLVED, 12 | REACHED_MAX_ITERATION, 13 | REACHED_MAX_MU, 14 | PROBLEM_NOT_FORMULATED, 15 | }; 16 | 17 | enum class LQRSolveStatus { 18 | RUNNING, 19 | CONVERGED, 20 | BACKWARD_PASS_FAIL, 21 | FORWARD_PASS_FAIL, 22 | FORWARD_PASS_SMALL_STEP, 23 | }; 24 | 25 | struct ILQRConfig { 26 | double min_alpha = 0.01; 27 | double accept_step_threshold = 0.5; 28 | std::size_t max_iter = 150; 29 | double delta_0 = 2.0; 30 | double min_mu = 1e-6; 31 | double max_mu = 1000.0; 32 | double convergence_threshold = 1e-2; 33 | }; 34 | 35 | template 36 | class ILQRSolver { 37 | 38 | public: 39 | ILQRSolver(ProblemManager& problem_manager) 40 | : _problem_manager(problem_manager), 41 | _current_trajectory(problem_manager.init_trajectory()), 42 | _num_steps(problem_manager.num_steps()), 43 | _k(problem_manager.num_steps(), Eigen::MatrixXd::Zero(N_CONTROL, N_CONTROL)), 44 | _K(problem_manager.num_steps(), Eigen::MatrixXd::Zero(N_CONTROL, N_STATE)), 45 | _vx(Eigen::MatrixXd::Zero(N_STATE, 1)), 46 | _vxx(Eigen::MatrixXd::Zero(N_STATE, N_STATE)), 47 | _derivatives(problem_manager.num_steps()), 48 | _approx_cost_decay_info({0.0, 0.0}) { 49 | CHECK_EQ(_current_trajectory.size(), _num_steps); 50 | _problem_manager.update_dynamic_costs(_current_trajectory); 51 | _current_cost = problem_manager.calculate_total_cost(problem_manager.init_trajectory()); 52 | }; 53 | ILQRSolveStatus solve(); 54 | Trajectory final_trajectory() const { return _current_trajectory; } 55 | 56 | private: 57 | void calculate_derivatives(); 58 | void backward_pass(); 59 | void forward_pass(); 60 | inline void increase_mu(); 61 | inline void decrease_mu(); 62 | ProblemManager& _problem_manager; 63 | Trajectory _current_trajectory; 64 | double _current_cost = 0.0; 65 | std::size_t _num_steps = 0; 66 | std::size_t _iter = 0; 67 | std::vector> _k; 68 | std::vector> _K; 69 | Eigen::Matrix _vx; 70 | Eigen::Matrix _vxx; 71 | std::vector> _derivatives; 72 | std::pair _approx_cost_decay_info; 73 | double _mu = 0.0; 74 | double _delta = 0.0; 75 | ILQRConfig _ilqr_config; 76 | LQRSolveStatus _current_solve_status = LQRSolveStatus::RUNNING; 77 | }; 78 | 79 | template 80 | ILQRSolveStatus ILQRSolver::solve() { 81 | if (not _problem_manager.problem_formulated() || _current_trajectory.empty()) { 82 | return ILQRSolveStatus::PROBLEM_NOT_FORMULATED; 83 | } 84 | _iter = 0; 85 | _current_solve_status = LQRSolveStatus::RUNNING; 86 | for (; _iter < _ilqr_config.max_iter; ++_iter) { 87 | // _current_solve_status is updated in the following functions. 88 | calculate_derivatives(); 89 | backward_pass(); 90 | forward_pass(); 91 | // Process mu. 92 | if (_current_solve_status == LQRSolveStatus::BACKWARD_PASS_FAIL 93 | || _current_solve_status == LQRSolveStatus::FORWARD_PASS_FAIL) { 94 | increase_mu(); 95 | } else if (_current_solve_status == LQRSolveStatus::RUNNING) { 96 | decrease_mu(); 97 | } 98 | // Process optimization result. 99 | if (_mu > _ilqr_config.max_mu) { 100 | return ILQRSolveStatus::REACHED_MAX_MU; 101 | } else if (_current_solve_status == LQRSolveStatus::CONVERGED) { 102 | return ILQRSolveStatus::SOLVED; 103 | } 104 | } 105 | return ILQRSolveStatus::REACHED_MAX_ITERATION; 106 | } 107 | 108 | template 109 | void ILQRSolver::increase_mu() { 110 | _delta = std::max(_ilqr_config.delta_0, _delta * _ilqr_config.delta_0); 111 | _mu = std::max(_ilqr_config.min_mu, _mu * _delta); 112 | LOG(INFO) << "Iter " << _iter << ", increase mu to " << _mu; 113 | } 114 | 115 | template 116 | void ILQRSolver::decrease_mu() { 117 | _delta = std::min(1.0 / _ilqr_config.delta_0, _delta / _ilqr_config.delta_0); 118 | _mu *= _delta; 119 | if (_mu < _ilqr_config.min_mu) { 120 | _mu = 0.0; 121 | } 122 | LOG(INFO) << "Iter " << _iter << ", decrease mu to " << _mu; 123 | } 124 | 125 | template 126 | void ILQRSolver::calculate_derivatives() { 127 | // Derivatives need to be calculated only if the trajectory is updated. 128 | if (_current_solve_status != LQRSolveStatus::RUNNING && _current_solve_status != LQRSolveStatus::FORWARD_PASS_SMALL_STEP) { 129 | _current_solve_status = LQRSolveStatus::RUNNING; 130 | return; 131 | } 132 | _current_solve_status = LQRSolveStatus::RUNNING; 133 | _problem_manager.update_dynamic_costs(_current_trajectory); 134 | for (std::size_t i = 0; i < _num_steps; ++i) { 135 | auto& derivative = _derivatives.at(i); 136 | _problem_manager.calculate_derivatives(_current_trajectory, i, &derivative); 137 | if (i < _num_steps - 1) { 138 | const double move_dist = _problem_manager.knots().at(i + 1) - _problem_manager.knots().at(i); 139 | derivative.fx = _problem_manager.dynamics().dx(_current_trajectory.at(i), move_dist); 140 | derivative.fu = _problem_manager.dynamics().du(_current_trajectory.at(i), move_dist); 141 | } 142 | // LOG_IF(INFO, _iter == 0 && i < 5) << "[Test] i " << i << ", lx " << derivative.lx << " lu " << derivative.lu << " lxx " << derivative.lxx; 143 | } 144 | } 145 | 146 | template 147 | void ILQRSolver::backward_pass() { 148 | _vx = _derivatives.back().lx; 149 | _vxx = _derivatives.back().lxx; 150 | _approx_cost_decay_info = {0.0, 0.0}; 151 | for (int i = _num_steps - 2; i >= 0; --i) { 152 | auto& derivative = _derivatives.at(i); 153 | const auto qx = derivative.lx + derivative.fx.transpose() * _vx; 154 | // LOG(INFO) << "[Test] Iter " << _iter << ", i " << i << ", qx " << qx; 155 | const auto qu = derivative.lu + derivative.fu.transpose() * _vx; 156 | const auto qxx = derivative.lxx + derivative.fx.transpose() * _vxx * derivative.fx; 157 | const auto quu = derivative.luu + derivative.fu.transpose() * _vxx * derivative.fu 158 | + _mu * Eigen::Matrix::Identity(); 159 | const auto qux = derivative.lux + derivative.fu.transpose() * _vxx * derivative.fx; 160 | 161 | Eigen::LLT llt_quu(quu); 162 | if (llt_quu.info() == Eigen::NumericalIssue) { 163 | LOG(INFO) << "[Backward pass] Non-PD quu at index " << i << ", mu " << _mu; 164 | _current_solve_status = LQRSolveStatus::BACKWARD_PASS_FAIL; 165 | return; 166 | } 167 | 168 | const auto quu_inverse = quu.inverse(); 169 | _k.at(i) = -quu_inverse * qu; 170 | _K.at(i) = -quu_inverse * qux; 171 | 172 | _vx = qx + _K.at(i).transpose() * quu * _k.at(i) + _K.at(i).transpose() * qu + qux.transpose() * _k.at(i); 173 | _vxx = qxx + _K.at(i).transpose() * quu * _K.at(i) + _K.at(i).transpose() * qux + qux.transpose() * _K.at(i); 174 | 175 | if (i < _num_steps - 1) { 176 | _approx_cost_decay_info.first += _k.at(i).transpose() * qu; 177 | _approx_cost_decay_info.second += (0.5 * _k.at(i).transpose() * quu * _k.at(i))(0); 178 | } 179 | } 180 | LOG(INFO) << "[Backward pass] Iter " << _iter << " OK"; 181 | } 182 | 183 | template 184 | void ILQRSolver::forward_pass() { 185 | if (_current_solve_status != LQRSolveStatus::RUNNING) { 186 | return; 187 | } 188 | double alpha = 1.0; 189 | double new_cost = 0.0; 190 | LOG(INFO) << "[Forward pass] Iter " << _iter << ", current cost " << _current_cost; 191 | while (alpha > _ilqr_config.min_alpha) { 192 | // Forward simulate. 193 | Trajectory new_trajectory = _current_trajectory; 194 | for (std::size_t i = 0; i < _num_steps - 1; ++i) { 195 | *(new_trajectory.at(i).mutable_control()) = 196 | _current_trajectory.at(i).control() + alpha * _k.at(i) + _K.at(i) * (new_trajectory.at(i).state() - _current_trajectory.at(i).state()); 197 | *(new_trajectory.at(i + 1).mutable_state()) = 198 | _problem_manager.dynamics().move_forward(new_trajectory.at(i), _problem_manager.knots().at(i + 1) - _problem_manager.knots().at(i)); 199 | } 200 | // Calculate actual cost decay. 201 | new_cost = _problem_manager.calculate_total_cost(new_trajectory); 202 | const double actual_cost_decay = _current_cost - new_cost; 203 | // if (std::fabs(alpha - 1.0) < EPS && actual_cost_decay < -100.0) { 204 | // for (std::size_t i = 0; i < new_trajectory.size(); ++i) { 205 | // LOG(INFO) << i << " old state " << _current_trajectory.at(i).state() << ", control " << _current_trajectory.at(i).control(); 206 | // LOG(INFO) << i << " new state " << new_trajectory.at(i).state() << ", control " << new_trajectory.at(i).control(); 207 | // } 208 | // } 209 | if (std::fabs(alpha - 1.0) < EPS && std::fabs(actual_cost_decay) < _ilqr_config.convergence_threshold) { 210 | LOG(INFO) << "[Forward pass] Iter " << _iter << ", optimization has converged."; 211 | _current_solve_status = LQRSolveStatus::CONVERGED; 212 | return; 213 | } 214 | const double approx_cost_decay = -(alpha * _approx_cost_decay_info.first + alpha * alpha * _approx_cost_decay_info.second); 215 | LOG(INFO) << "[Forward pass] Iter " << _iter << ", alpha " << alpha << ", actual cost decay " << actual_cost_decay << ", approx " << approx_cost_decay; 216 | if (actual_cost_decay > 0.0 217 | && (approx_cost_decay < 0.0 218 | || actual_cost_decay / approx_cost_decay > _ilqr_config.accept_step_threshold)) { 219 | // if (true) { 220 | LOG(INFO) << "[Forward pass] Iter " << _iter << ", accept alpha " << alpha << ", cost " << new_cost; 221 | _current_cost = new_cost; 222 | _current_trajectory = new_trajectory; 223 | if (std::fabs(alpha - 1.0) > EPS) { 224 | _current_solve_status = LQRSolveStatus::FORWARD_PASS_SMALL_STEP; 225 | } 226 | return; 227 | } 228 | alpha *= 0.5; 229 | } 230 | LOG(INFO) << "[Forward pass] Forward pass fail."; 231 | _current_solve_status = LQRSolveStatus::FORWARD_PASS_FAIL; 232 | } 233 | } -------------------------------------------------------------------------------- /src/solver/variable.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | 6 | namespace Solver { 7 | 8 | template 9 | using Variable = Eigen::Matrix; 10 | 11 | // template 12 | // class Variable { 13 | // public: 14 | // Variable() : _vector(Eigen::VectorXd::Zero(N)), _dim(N) {} 15 | // Variable(const Eigen::Matrix& vector) : _vector(vector), _dim(N) {} 16 | // virtual ~Variable() {} 17 | // const Eigen::Matrix& vector() const { return _vector; } 18 | // std::size_t dimension() const { return _dim; } 19 | // Eigen::Matrix* mutable_vector() { return &_vector; } 20 | 21 | // protected: 22 | // Eigen::Matrix _vector; 23 | // const std::size_t _dim; 24 | // }; 25 | 26 | template 27 | class Node { 28 | public: 29 | Node() = default; 30 | const Variable& state() const { return _state; } 31 | const Variable& control() const { return _control; } 32 | Variable* mutable_state() { return &_state; } 33 | Variable* mutable_control() { return &_control; } 34 | std::size_t step() const { return _step; } 35 | void set_step(std::size_t step) { _step = step; } 36 | double sample() const { return _sample; } 37 | void set_sample(double sample) { _sample = sample; } 38 | 39 | private: 40 | Variable _state; 41 | Variable _control; 42 | std::size_t _step = 0; 43 | double _sample = 0.0; 44 | }; 45 | 46 | template 47 | using Trajectory = std::vector>; 48 | 49 | template 50 | struct DerivativesInfo { 51 | Eigen::Matrix lx = Eigen::MatrixXd::Zero(N_STATE, 1); 52 | Eigen::Matrix lu = Eigen::MatrixXd::Zero(N_CONTROL, 1); 53 | Eigen::Matrix lxx = Eigen::MatrixXd::Zero(N_STATE, N_STATE); 54 | Eigen::Matrix luu = Eigen::MatrixXd::Zero(N_CONTROL, N_CONTROL); 55 | Eigen::Matrix lux = Eigen::MatrixXd::Zero(N_CONTROL, N_STATE); 56 | Eigen::Matrix fx = Eigen::MatrixXd::Zero(N_STATE, N_STATE); 57 | Eigen::Matrix fu = Eigen::MatrixXd::Zero(N_STATE, N_CONTROL); 58 | }; 59 | 60 | } -------------------------------------------------------------------------------- /src/test/Map.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by ljn on 20-2-12. 3 | // 4 | #include 5 | #include "Map.hpp" 6 | 7 | namespace Test { 8 | 9 | Map::Map(const grid_map::GridMap &grid_map) : 10 | maps(grid_map) { 11 | if (!grid_map.exists("distance")) { 12 | LOG(ERROR) << "grid map must contain 'distance' layer"; 13 | } 14 | } 15 | 16 | double Map::getObstacleDistance(const Eigen::Vector2d &pos) const { 17 | if (maps.isInside(pos)) { 18 | return this->maps.atPosition("distance", pos, grid_map::InterpolationMethods::INTER_LINEAR); 19 | } else { 20 | return 0.0; 21 | } 22 | } 23 | 24 | bool Map::isInside(const Eigen::Vector2d &pos) const { 25 | return maps.isInside(pos); 26 | } 27 | } -------------------------------------------------------------------------------- /src/test/Map.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by ljn on 20-2-12. 3 | // 4 | 5 | #ifndef PATH_OPTIMIZER_INCLUDE_TOOLS_MAP_HPP_ 6 | #define PATH_OPTIMIZER_INCLUDE_TOOLS_MAP_HPP_ 7 | 8 | #include 9 | #include 10 | #include 11 | #include "Eigen/Core" 12 | #include 13 | 14 | namespace Test { 15 | 16 | class Map { 17 | public: 18 | Map() = delete; 19 | explicit Map(const grid_map::GridMap &grid_map); 20 | double getObstacleDistance(const Eigen::Vector2d &pos) const; 21 | bool isInside(const Eigen::Vector2d &pos) const; 22 | 23 | private: 24 | const grid_map::GridMap &maps; 25 | }; 26 | } 27 | 28 | #endif //PATH_OPTIMIZER_INCLUDE_TOOLS_MAP_HPP_ 29 | -------------------------------------------------------------------------------- /src/test/demo.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by ljn on 20-2-4. 3 | // 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include "glog/logging.h" 23 | #include "eigen3/Eigen/Dense" 24 | #include "opencv2/core/core.hpp" 25 | #include "opencv2/core/eigen.hpp" 26 | #include "opencv2/opencv.hpp" 27 | #include "eigen2cv.hpp" 28 | #include "reference_line_processor.h" 29 | #include "path/data_structure.h" 30 | #include "path/path_problem_manager.h" 31 | #include "solver/solver.h" 32 | #include "path/gflags.h" 33 | #include "path/tool.h" 34 | 35 | // TODO: this file is a mess. 36 | 37 | using namespace PathPlanning; 38 | 39 | 40 | PathPlanning::PathPoint start_state, end_state; 41 | bool start_state_rcv = false, end_state_rcv = false, reference_rcv = false; 42 | std::vector reference_points; 43 | 44 | void referenceCb(const geometry_msgs::PointStampedConstPtr &p) { 45 | if (start_state_rcv && end_state_rcv) { 46 | reference_points.clear(); 47 | } 48 | PathPlanning::PathPoint point; 49 | point.x = p->point.x; 50 | point.y = p->point.y; 51 | reference_points.emplace_back(point); 52 | start_state_rcv = end_state_rcv = false; 53 | reference_rcv = reference_points.size() >= 6; 54 | std::cout << "received a reference point" << std::endl; 55 | } 56 | 57 | void startCb(const geometry_msgs::PoseWithCovarianceStampedConstPtr &start) { 58 | start_state.x = start->pose.pose.position.x; 59 | start_state.y = start->pose.pose.position.y; 60 | start_state.theta = tf::getYaw(start->pose.pose.orientation); 61 | if (reference_rcv) { 62 | start_state_rcv = true; 63 | } 64 | std::cout << "get initial state." << std::endl; 65 | } 66 | 67 | void goalCb(const geometry_msgs::PoseStampedConstPtr &goal) { 68 | end_state.x = goal->pose.position.x; 69 | end_state.y = goal->pose.position.y; 70 | end_state.theta = tf::getYaw(goal->pose.orientation); 71 | if (reference_rcv) { 72 | end_state_rcv = true; 73 | } 74 | std::cout << "get the goal." << std::endl; 75 | } 76 | 77 | int main(int argc, char **argv) { 78 | ros::init(argc, argv, "path_optimization"); 79 | ros::NodeHandle nh("~"); 80 | 81 | std::string base_dir = ros::package::getPath("frenet_ilqr_test"); 82 | auto log_dir = base_dir + "/log"; 83 | if (0 != access(log_dir.c_str(), 0)) { 84 | // if this folder not exist, create a new one. 85 | mkdir(log_dir.c_str(), 0777); 86 | } 87 | 88 | google::InitGoogleLogging(argv[0]); 89 | FLAGS_colorlogtostderr = true; 90 | FLAGS_stderrthreshold = google::INFO; 91 | FLAGS_log_dir = log_dir; 92 | FLAGS_logbufsecs = 0; 93 | FLAGS_max_log_size = 100; 94 | FLAGS_stop_logging_if_full_disk = true; 95 | 96 | // Initialize grid map from image. 97 | std::string image_dir = ros::package::getPath("frenet_ilqr_test"); 98 | std::string image_file = "gridmap.png"; 99 | image_dir.append("/" + image_file); 100 | cv::Mat img_src = cv::imread(image_dir, CV_8UC1); 101 | double resolution = 0.2; // in meter 102 | grid_map::GridMap grid_map(std::vector{"obstacle", "distance"}); 103 | grid_map::GridMapCvConverter::initializeFromImage( 104 | img_src, resolution, grid_map, grid_map::Position::Zero()); 105 | // Add obstacle layer. 106 | unsigned char OCCUPY = 0; 107 | unsigned char FREE = 255; 108 | grid_map::GridMapCvConverter::addLayerFromImage( 109 | img_src, "obstacle", grid_map, OCCUPY, FREE, 0.5); 110 | // Update distance layer. 111 | Eigen::Matrix binary = 112 | grid_map.get("obstacle").cast(); 113 | cv::distanceTransform(eigen2cv(binary), eigen2cv(grid_map.get("distance")), 114 | CV_DIST_L2, CV_DIST_MASK_PRECISE); 115 | grid_map.get("distance") *= resolution; 116 | grid_map.setFrameId("map"); 117 | 118 | // Set publishers. 119 | ros::Publisher map_publisher = 120 | nh.advertise("grid_map", 1, true); 121 | // Set subscribers. 122 | ros::Subscriber reference_sub = 123 | nh.subscribe("/clicked_point", 1, referenceCb); 124 | ros::Subscriber start_sub = 125 | nh.subscribe("/initialpose", 1, startCb); 126 | ros::Subscriber end_sub = 127 | nh.subscribe("/move_base_simple/goal", 1, goalCb); 128 | 129 | // Markers initialization. 130 | ros_viz_tools::RosVizTools markers(nh, "markers"); 131 | std::string marker_frame_id = "map"; 132 | 133 | // Loop. 134 | ros::Rate rate(30.0); 135 | while (nh.ok()) { 136 | ros::Time time = ros::Time::now(); 137 | markers.clear(); 138 | int id = 0; 139 | 140 | // Cancel at double click. 141 | if (reference_points.size() >= 2) { 142 | const auto &p1 = reference_points[reference_points.size() - 2]; 143 | const auto &p2 = reference_points.back(); 144 | if (std::sqrt(std::pow(p1.x-p2.x, 2) + std::pow(p1.y-p2.y, 2)) <= 0.001) { 145 | reference_points.clear(); 146 | reference_rcv = false; 147 | } 148 | } 149 | 150 | // Visualize reference path selected by mouse. 151 | visualization_msgs::Marker reference_marker = 152 | markers.newSphereList(0.5, "reference point", id++, ros_viz_tools::RED, marker_frame_id); 153 | for (size_t i = 0; i != reference_points.size(); ++i) { 154 | geometry_msgs::Point p; 155 | p.x = reference_points[i].x; 156 | p.y = reference_points[i].y; 157 | p.z = 1.0; 158 | reference_marker.points.push_back(p); 159 | } 160 | markers.append(reference_marker); 161 | // Visualize start and end point selected by mouse. 162 | geometry_msgs::Vector3 scale; 163 | scale.x = 2.0; 164 | scale.y = 0.3; 165 | scale.z = 0.3; 166 | geometry_msgs::Pose start_pose; 167 | start_pose.position.x = start_state.x; 168 | start_pose.position.y = start_state.y; 169 | start_pose.position.z = 1.0; 170 | auto start_quat = tf::createQuaternionFromYaw(start_state.theta); 171 | start_pose.orientation.x = start_quat.x(); 172 | start_pose.orientation.y = start_quat.y(); 173 | start_pose.orientation.z = start_quat.z(); 174 | start_pose.orientation.w = start_quat.w(); 175 | visualization_msgs::Marker start_marker = 176 | markers.newArrow(scale, start_pose, "start point", id++, ros_viz_tools::CYAN, marker_frame_id); 177 | markers.append(start_marker); 178 | geometry_msgs::Pose end_pose; 179 | end_pose.position.x = end_state.x; 180 | end_pose.position.y = end_state.y; 181 | end_pose.position.z = 1.0; 182 | auto end_quat = tf::createQuaternionFromYaw(end_state.theta); 183 | end_pose.orientation.x = end_quat.x(); 184 | end_pose.orientation.y = end_quat.y(); 185 | end_pose.orientation.z = end_quat.z(); 186 | end_pose.orientation.w = end_quat.w(); 187 | visualization_msgs::Marker end_marker = 188 | markers.newArrow(scale, end_pose, "end point", id++, ros_viz_tools::CYAN, marker_frame_id); 189 | markers.append(end_marker); 190 | 191 | if (reference_rcv && start_state_rcv && end_state_rcv) { 192 | Test::Map map(grid_map); 193 | Test::ReferenceLineProcessor reference_line_processor(reference_points, map, start_state); 194 | std::shared_ptr ref_line_ptr = std::make_shared(); 195 | std::shared_ptr free_space_ptr = std::make_shared(); 196 | reference_line_processor.solve(ref_line_ptr, free_space_ptr); 197 | 198 | visualization_msgs::Marker boundary_points_marker = 199 | markers.newSphereList(0.3, "boundary points", id++, ros_viz_tools::BLUE, marker_frame_id); 200 | for (const auto& pt : free_space_ptr->boundary_points()) { 201 | geometry_msgs::Point p; 202 | p.x = pt.lb_xy.x; 203 | p.y = pt.lb_xy.y; 204 | p.z = 0.5; 205 | boundary_points_marker.points.push_back(p); 206 | p.x = pt.ub_xy.x; 207 | p.y = pt.ub_xy.y; 208 | p.z = 0.5; 209 | boundary_points_marker.points.push_back(p); 210 | } 211 | markers.append(boundary_points_marker); 212 | 213 | // solve 214 | PathPlanning::PathProblemManager path_problem_manager; 215 | path_problem_manager.formulate_path_problem(*free_space_ptr, *ref_line_ptr, start_state, end_state); 216 | Solver::ILQRSolver ilqr_solver(path_problem_manager); 217 | const auto solve_status = ilqr_solver.solve(); 218 | // LOG(INFO) << "Solve status " << solve_status; 219 | 220 | visualization_msgs::Marker init_path_marker = 221 | markers.newLineStrip(0.3, "init path", id++, ros_viz_tools::YELLOW, marker_frame_id); 222 | const auto& init_path_raw = path_problem_manager.init_trajectory(); 223 | for (size_t i = 0; i != init_path_raw.size(); ++i) { 224 | PathPlanning::SLPosition sl; 225 | sl.s = init_path_raw[i].sample(); 226 | sl.l = init_path_raw[i].state()(PathPlanning::L_INDEX); 227 | const auto xy = ref_line_ptr->get_xy_by_sl(sl); 228 | geometry_msgs::Point p; 229 | p.x = xy.x; 230 | p.y = xy.y; 231 | p.z = 1.0; 232 | init_path_marker.points.push_back(p);; 233 | } 234 | markers.append(init_path_marker); 235 | ros_viz_tools::ColorRGBA path_color; 236 | path_color.r = 0.063; 237 | path_color.g = 0.305; 238 | path_color.b = 0.545; 239 | if (solve_status != ILQRSolveStatus::SOLVED) { 240 | path_color.r = 1.0; 241 | path_color.g = 0.0; 242 | path_color.b = 0.0; 243 | } 244 | visualization_msgs::Marker result_marker = 245 | markers.newLineStrip(FLAGS_vehicle_width, "optimized path", id++, path_color, marker_frame_id); 246 | visualization_msgs::Marker vehicle_geometry_marker = 247 | markers.newLineList(0.02, "vehicle", id++, ros_viz_tools::GRAY, marker_frame_id); 248 | // Visualize vehicle geometry. 249 | static const double length{FLAGS_vehicle_length}; 250 | static const double width{FLAGS_vehicle_width}; 251 | static const double rtc{FLAGS_rear_axle_to_center}; 252 | static const double rear_d{length / 2 - rtc}; 253 | static const double front_d{length - rear_d}; 254 | const auto& opt_path_raw = ilqr_solver.final_trajectory(); 255 | const auto result = PathProblemManager::transform_to_path_points(*ref_line_ptr, opt_path_raw); 256 | for (size_t i = 0; i != result.size(); ++i) { 257 | const auto path_point = result.at(i); 258 | geometry_msgs::Point p; 259 | p.x = path_point.x; 260 | p.y = path_point.y; 261 | p.z = 1.0; 262 | result_marker.points.push_back(p); 263 | const auto k = path_point.kappa; 264 | path_color.a = std::min(fabs(k) / 0.15, 1.0); 265 | path_color.a = std::max((float)0.1, path_color.a); 266 | result_marker.colors.emplace_back(path_color); 267 | // 268 | const double heading = path_point.theta; 269 | PathPoint p1, p2, p3, p4; 270 | p1.x = front_d; 271 | p1.y = width / 2; 272 | p2.x = front_d; 273 | p2.y = -width / 2; 274 | p3.x = -rear_d; 275 | p3.y = -width / 2; 276 | p4.x = -rear_d; 277 | p4.y = width / 2; 278 | p1 = local_to_global(path_point, p1); 279 | p2 = local_to_global(path_point, p2); 280 | p3 = local_to_global(path_point, p3); 281 | p4 = local_to_global(path_point, p4); 282 | geometry_msgs::Point pp1, pp2, pp3, pp4; 283 | pp1.x = p1.x; 284 | pp1.y = p1.y; 285 | pp1.z = 0.1; 286 | pp2.x = p2.x; 287 | pp2.y = p2.y; 288 | pp2.z = 0.1; 289 | pp3.x = p3.x; 290 | pp3.y = p3.y; 291 | pp3.z = 0.1; 292 | pp4.x = p4.x; 293 | pp4.y = p4.y; 294 | pp4.z = 0.1; 295 | vehicle_geometry_marker.points.push_back(pp1); 296 | vehicle_geometry_marker.points.push_back(pp2); 297 | vehicle_geometry_marker.points.push_back(pp2); 298 | vehicle_geometry_marker.points.push_back(pp3); 299 | vehicle_geometry_marker.points.push_back(pp3); 300 | vehicle_geometry_marker.points.push_back(pp4); 301 | vehicle_geometry_marker.points.push_back(pp4); 302 | vehicle_geometry_marker.points.push_back(pp1); 303 | } 304 | markers.append(result_marker); 305 | markers.append(vehicle_geometry_marker); 306 | } 307 | 308 | // Publish the grid_map. 309 | grid_map.setTimestamp(time.toNSec()); 310 | nav_msgs::OccupancyGrid message; 311 | grid_map::GridMapRosConverter::toOccupancyGrid( 312 | grid_map, "obstacle", FREE, OCCUPY, message); 313 | map_publisher.publish(message); 314 | 315 | // Publish markers. 316 | markers.publish(); 317 | LOG_EVERY_N(INFO, 20) << "map published."; 318 | 319 | // Wait for next cycle. 320 | ros::spinOnce(); 321 | rate.sleep(); 322 | } 323 | 324 | google::ShutdownGoogleLogging(); 325 | return 0; 326 | } 327 | -------------------------------------------------------------------------------- /src/test/eigen2cv.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief Mapping functions from Eigen data types to OpenCV 3 | * @author Eugene Khvedchenya 4 | * @details This header file contains code snippet for easy mapping Eigen types 5 | * to OpenCV and back with minimal overhead. 6 | * @more computer-vision.talks.com/articles/mapping-eigen-to-opencv/ 7 | * Features: 8 | * - Mapping plain data types with no overhead (read/write access) 9 | * - Mapping expressions via evaluation (read only acess) 10 | * 11 | * Known issues: 12 | * - Does not support .transpose() 13 | */ 14 | #ifndef INTERNAL_GRID_MAP_EIGEN2CV_H 15 | #define INTERNAL_GRID_MAP_EIGEN2CV_H 16 | 17 | #include 18 | #include 19 | 20 | namespace details { 21 | struct Const {}; 22 | struct Mutable {}; 23 | } 24 | 25 | template 26 | struct opencv_matrix; 27 | 28 | template <> 29 | struct opencv_matrix { 30 | static const int type = CV_64FC1; 31 | }; 32 | template <> 33 | struct opencv_matrix { 34 | static const int type = CV_32FC1; 35 | }; 36 | template <> 37 | struct opencv_matrix { 38 | static const int type = CV_32SC1; 39 | }; 40 | template <> 41 | struct opencv_matrix { 42 | static const int type = CV_8UC1; 43 | }; 44 | template <> 45 | struct opencv_matrix { 46 | static_assert(sizeof(bool) == 1, "Requires bool to be 1 byte"); 47 | static const int type = CV_8UC1; 48 | }; 49 | 50 | /** 51 | * PlainObjectBase - map directly (read/write) 52 | * ArrayWrapper/MatrixWrapper on PlainObjectBase - map directly (read/write) 53 | * Block - recursively instantiate. Can be read-only (derived is expression) 54 | * or read/write (derived is planar) 55 | * Expression - evaluate (read-only) 56 | */ 57 | template ::StorageKind> 60 | class Eigen2CV; 61 | 62 | class Eigen2CVBase { 63 | public: 64 | operator cv::Mat() const { return mBody; } 65 | 66 | operator cv::_InputArray() const { return cv::_InputArray(mBody); } 67 | 68 | protected: 69 | template 70 | void mapPlaneMemory(const Derived& src) { 71 | const bool isRowMajor = int(Derived::Flags) & Eigen::RowMajorBit; 72 | const int stride = src.outerStride() * sizeof(typename Derived::Scalar); 73 | 74 | if (isRowMajor) 75 | this->mapPlaneMemoryRowMajor(src.data(), src.rows(), src.cols(), 76 | stride); 77 | else 78 | this->mapPlaneMemoryColMajor(src.data(), src.rows(), src.cols(), 79 | stride); 80 | } 81 | 82 | template 83 | void mapPlaneMemoryRowMajor(const Scalar* planeData, int rows, int cols, 84 | int stride) { 85 | this->mBody = cv::Mat(rows, cols, opencv_matrix::type, 86 | const_cast(planeData), stride); 87 | } 88 | 89 | template 90 | void mapPlaneMemoryColMajor(const Scalar* planeData, int rows, int cols, 91 | int stride) { 92 | this->mBody = cv::Mat(cols, rows, opencv_matrix::type, 93 | const_cast(planeData), stride); 94 | } 95 | 96 | template 97 | void assignMatrix(Eigen::DenseBase& dst, const cv::Mat_& src) { 98 | typedef Eigen::Matrix 100 | PlainMatrixType; 101 | 102 | dst = Eigen::Map((T*)src.data, src.rows, src.cols) 103 | .template cast(); 104 | } 105 | 106 | cv::Mat mBody; 107 | }; 108 | 109 | template 110 | class Eigen2CV, details::Const> 111 | : public Eigen2CVBase { 112 | public: 113 | typedef typename Derived::Scalar Scalar; 114 | typedef Eigen2CV, details::Mutable> 115 | Self; 116 | 117 | Eigen2CV(const Eigen::PlainObjectBase& src) : mMappedView(src) { 118 | this->mapPlaneMemory(mMappedView); 119 | } 120 | 121 | private: 122 | const Eigen::PlainObjectBase& mMappedView; 123 | }; 124 | 125 | template 126 | class Eigen2CV, details::Mutable> 127 | : public Eigen2CVBase { 128 | public: 129 | typedef typename Derived::Scalar Scalar; 130 | typedef Eigen2CV, details::Mutable> 131 | Self; 132 | 133 | Eigen2CV(Eigen::PlainObjectBase& src) : mMappedView(src) { 134 | this->mapPlaneMemory(mMappedView); 135 | } 136 | 137 | template 138 | Self& operator=(const cv::Mat_& src) { 139 | assignMatrix(mMappedView, src); 140 | return *this; 141 | } 142 | 143 | /** 144 | * @brief Assignment operator to copy OpenCV Mat data to mapped Eigen 145 | * object. 146 | */ 147 | Self& operator=(const cv::Mat& m) { 148 | switch (m.type()) { 149 | case CV_8U: 150 | return *this = (cv::Mat_)m; 151 | case CV_16U: 152 | return *this = (cv::Mat_)m; 153 | case CV_16S: 154 | return *this = (cv::Mat_)m; 155 | case CV_32S: 156 | return *this = (cv::Mat_)m; 157 | case CV_32F: 158 | return *this = (cv::Mat_)m; 159 | case CV_64F: 160 | return *this = (cv::Mat_)m; 161 | default: 162 | throw std::runtime_error("Unsupported OpenCV matrix type"); 163 | }; 164 | } 165 | 166 | operator cv::_InputOutputArray() { 167 | return cv::_InputOutputArray(this->mBody); 168 | } 169 | 170 | protected: 171 | private: 172 | Eigen::PlainObjectBase& mMappedView; 173 | }; 174 | 175 | template 176 | class Eigen2CV, details::Mutable, Eigen::Dense> 177 | : public Eigen2CVBase { 178 | public: 179 | typedef typename Derived::Scalar Scalar; 180 | typedef Eigen2CV, details::Mutable> Self; 181 | 182 | Eigen2CV(const Eigen::Block& src) : mMappedView(src) { 183 | this->mapPlaneMemory(mMappedView); 184 | } 185 | 186 | operator cv::_OutputArray() { return cv::_OutputArray(this->mBody); } 187 | 188 | template 189 | Self& operator=(const cv::Mat_& src) { 190 | assignMatrix(mMappedView, src); 191 | return *this; 192 | } 193 | 194 | /** 195 | * @brief Assignment operator to copy OpenCV Mat data to mapped Eigen 196 | * object. 197 | */ 198 | Self& operator=(const cv::Mat& m) throw() { 199 | switch (m.type()) { 200 | case CV_8U: 201 | return *this = (cv::Mat_)m; 202 | case CV_16U: 203 | return *this = (cv::Mat_)m; 204 | case CV_16S: 205 | return *this = (cv::Mat_)m; 206 | case CV_32S: 207 | return *this = (cv::Mat_)m; 208 | case CV_32F: 209 | return *this = (cv::Mat_)m; 210 | case CV_64F: 211 | return *this = (cv::Mat_)m; 212 | default: 213 | throw std::runtime_error("Unsupported OpenCV matrix type"); 214 | }; 215 | } 216 | 217 | private: 218 | const Eigen::Block& mMappedView; 219 | }; 220 | 221 | /** 222 | * @brief Most generic and most inefficient mapper - it evaluates input object 223 | * into a local copy. 224 | */ 225 | template 226 | class Eigen2CV, details::Const> 227 | : public Eigen2CVBase { 228 | public: 229 | typedef typename Derived::Scalar Scalar; 230 | typedef typename Eigen::internal::plain_matrix_type::type 231 | StorageType; 232 | 233 | Eigen2CV(const Eigen::EigenBase& src) { 234 | mStorage = src; 235 | this->mapPlaneMemory(mStorage); 236 | } 237 | 238 | private: 239 | StorageType mStorage; 240 | }; 241 | 242 | template 243 | Eigen2CV, details::Mutable> eigen2cv( 244 | Eigen::PlainObjectBase& src) { 245 | return Eigen2CV, details::Mutable>(src); 246 | } 247 | 248 | template 249 | Eigen2CV, details::Const> eigen2cv( 250 | const Eigen::PlainObjectBase& src) { 251 | return Eigen2CV, details::Const>(src); 252 | } 253 | 254 | template 255 | Eigen2CV, details::Mutable> eigen2cv( 256 | const Eigen::Block& src) { 257 | return Eigen2CV, details::Mutable>(src); 258 | } 259 | 260 | template 261 | auto eigen2cv(const Eigen::ArrayWrapper& src) 262 | -> decltype(eigen2cv(src.nestedExpression().eval())) { 263 | return eigen2cv(src.nestedExpression().eval()); 264 | } 265 | 266 | template 267 | auto eigen2cv(const Eigen::MatrixWrapper& src) 268 | -> decltype(eigen2cv(src.nestedExpression().eval())) { 269 | return eigen2cv(src.nestedExpression().eval()); 270 | } 271 | 272 | template 273 | Eigen2CV, details::Const> eigen2cv( 274 | const Eigen::EigenBase& src) { 275 | return Eigen2CV, details::Const>(src); 276 | } // namespace hmpl 277 | #endif // INTERNAL_GRID_MAP_EIGEN2CV_H 278 | -------------------------------------------------------------------------------- /src/test/reference_line_processor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "reference_line_processor.h" 5 | #include "path/tool.h" 6 | #include "path/gflags.h" 7 | 8 | using namespace PathPlanning; 9 | namespace Test { 10 | 11 | const double max_lateral_range = 10.0; 12 | const double lateral_spacing = 0.4; 13 | 14 | bool ReferenceLineProcessor::solve(std::shared_ptr reference_line_out, std::shared_ptr free_space_out) { 15 | if (not b_spline(reference_line_out)) { 16 | return false; 17 | } 18 | return search(reference_line_out, free_space_out); 19 | } 20 | 21 | bool ReferenceLineProcessor::b_spline(std::shared_ptr reference_line_out) const { 22 | // B spline smoothing. 23 | double length = 0; 24 | for (size_t i = 0; i != _reference_line_points.size() - 1; ++i) { 25 | length += distance(_reference_line_points[i], _reference_line_points[i + 1]); 26 | } 27 | int degree = 3; 28 | double average_length = length / (_reference_line_points.size() - 1); 29 | if (average_length > 10) degree = 3; 30 | else if (average_length > 5) degree = 4; 31 | else degree = 5; 32 | 33 | tinyspline::BSpline b_spline_raw(_reference_line_points.size(), 2, degree); 34 | std::vector ctrlp_raw = b_spline_raw.controlPoints(); 35 | for (size_t i = 0; i != _reference_line_points.size(); ++i) { 36 | ctrlp_raw[2 * (i)] = _reference_line_points[i].x; 37 | ctrlp_raw[2 * (i) + 1] = _reference_line_points[i].y; 38 | } 39 | b_spline_raw.setControlPoints(ctrlp_raw); 40 | double delta_t = 1.0 / length; 41 | double tmp_t = 0; 42 | std::vector x_list, y_list, s_list; 43 | while (tmp_t < 1) { 44 | auto result = b_spline_raw.eval(tmp_t).result(); 45 | x_list.emplace_back(result[0]); 46 | y_list.emplace_back(result[1]); 47 | tmp_t += delta_t; 48 | } 49 | auto result = b_spline_raw.eval(1).result(); 50 | x_list.emplace_back(result[0]); 51 | y_list.emplace_back(result[1]); 52 | s_list.emplace_back(0); 53 | for (size_t i = 1; i != x_list.size(); ++i) { 54 | double dis = sqrt(pow(x_list[i] - x_list[i - 1], 2) + pow(y_list[i] - y_list[i - 1], 2)); 55 | s_list.emplace_back(s_list.back() + dis); 56 | } 57 | 58 | tk::spline x_s, y_s; 59 | x_s.set_points(s_list, x_list); 60 | y_s.set_points(s_list, y_list); 61 | reference_line_out->initialize(x_s, y_s, s_list.back()); 62 | return true; 63 | } 64 | 65 | bool ReferenceLineProcessor::smooth(std::shared_ptr reference_line_out) const { 66 | 67 | } 68 | 69 | void ReferenceLineProcessor::calculate_cost(std::vector> &samples, 70 | int layer_index, 71 | int lateral_index) { 72 | if (layer_index == 0) return; 73 | if (!samples[layer_index][lateral_index].is_feasible) return; 74 | 75 | static const double weight_ref_offset = 1.0; 76 | static const double weight_obstacle = 0.5; 77 | static const double weight_angle_change = 16.0; 78 | static const double weight_ref_angle_diff = 0.5; 79 | static const double safe_distance = 3.0; 80 | 81 | auto &point = samples[layer_index][lateral_index]; 82 | double self_cost = 0; 83 | if (point.dis_to_obs < safe_distance) self_cost += (safe_distance - point.dis_to_obs) / safe_distance * weight_obstacle; 84 | self_cost += fabs(point.l) / max_lateral_range * weight_ref_offset; 85 | 86 | auto min_cost = DBL_MAX; 87 | for (const auto &pre_point : samples[layer_index - 1]) { 88 | if (!pre_point.is_feasible) continue; 89 | if (fabs(pre_point.l - point.l) > (point.s - pre_point.s)) continue; 90 | double direction = atan2(point.y - pre_point.y, point.x - pre_point.x); 91 | double edge_cost = fabs(constrainAngle(direction - pre_point.dir)) / M_PI_2 * weight_angle_change 92 | + fabs(constrainAngle(direction - point.heading)) / M_PI_2 * weight_ref_angle_diff; 93 | double total_cost = self_cost + edge_cost + pre_point.cost; 94 | if (total_cost < min_cost) { 95 | min_cost = total_cost; 96 | point.parent = &pre_point; 97 | point.dir = direction; 98 | } 99 | } 100 | 101 | if (point.parent) point.cost = min_cost; 102 | } 103 | 104 | bool ReferenceLineProcessor::search(std::shared_ptr reference_line_out, std::shared_ptr free_space_out) { 105 | if (not reference_line_out->is_initialized()) { 106 | return false; 107 | } 108 | 109 | // const tk::spline &x_s = reference_line_out->x_s(); 110 | // const tk::spline &y_s = reference_line_out->y_s(); 111 | // Sampling interval. 112 | const auto init_sl = reference_line_out->get_projection(XYPosition{_init_point.x, _init_point.y}); 113 | double tmp_s = init_sl.s; 114 | layers_s_list_.clear(); 115 | layers_bounds_.clear(); 116 | double search_ds = 0.6; 117 | while (tmp_s < reference_line_out->length()) { 118 | layers_s_list_.emplace_back(tmp_s); 119 | tmp_s += search_ds; 120 | } 121 | layers_s_list_.emplace_back(reference_line_out->length()); 122 | if (layers_s_list_.empty()) return false; 123 | 124 | double vehicle_s = layers_s_list_.front(); 125 | 126 | if (fabs(init_sl.l) > 5.0) { 127 | LOG(ERROR) << "Vehicle far from ref, quit graph search. Init l " << init_sl.l; 128 | return false; 129 | } 130 | LOG(INFO) << "[FreeSpace] init s " << init_sl.s << ", init l " << init_sl.l; 131 | int start_lateral_index = 132 | static_cast((max_lateral_range + init_sl.l) / lateral_spacing); 133 | 134 | std::vector> samples; 135 | samples.reserve(layers_s_list_.size()); 136 | const double search_threshold = 0.7; 137 | // Sample nodes. 138 | for (int i = 0; i < layers_s_list_.size(); ++i) { 139 | samples.emplace_back(std::vector()); 140 | double cur_s = layers_s_list_[i]; 141 | const auto ref_pt = reference_line_out->get_reference_point(cur_s); 142 | double ref_r = 1 / ref_pt.kappa; 143 | double cur_l = -max_lateral_range; 144 | int lateral_index = 0; 145 | while (cur_l <= max_lateral_range) { 146 | DpPoint dp_point; 147 | dp_point.x = ref_pt.x + cur_l * cos(ref_pt.theta + M_PI_2); 148 | dp_point.y = ref_pt.y + cur_l * sin(ref_pt.theta + M_PI_2); 149 | dp_point.heading = ref_pt.theta; 150 | dp_point.s = cur_s; 151 | dp_point.l = cur_l; 152 | dp_point.layer_index = i; 153 | dp_point.lateral_index = lateral_index; 154 | grid_map::Position node_pose(dp_point.x, dp_point.y); 155 | dp_point.dis_to_obs = _map.isInside(node_pose) ? _map.getObstacleDistance(node_pose) : -1; 156 | if ((ref_pt.kappa < 0 && cur_l < ref_r) || (ref_pt.kappa > 0 && cur_l > ref_r) 157 | || dp_point.dis_to_obs < search_threshold) { 158 | dp_point.is_feasible = false; 159 | } 160 | if (i == 0 && dp_point.lateral_index != start_lateral_index) dp_point.is_feasible = false; 161 | if (i == 0 && dp_point.lateral_index == start_lateral_index) { 162 | dp_point.is_feasible = true; 163 | dp_point.dir = _init_point.theta; 164 | dp_point.cost = 0.0; 165 | } 166 | samples.back().emplace_back(dp_point); 167 | cur_l += lateral_spacing; 168 | ++lateral_index; 169 | } 170 | // Get rough bounds. 171 | auto &point_set = samples.back(); 172 | for (int j = 0; j < point_set.size(); ++j) { 173 | if (j == 0 || !point_set[j - 1].is_feasible || !point_set[j].is_feasible) { 174 | point_set[j].rough_lower_bound = point_set[j].l; 175 | } else { 176 | point_set[j].rough_lower_bound = point_set[j - 1].rough_lower_bound; 177 | } 178 | } 179 | for (int j = point_set.size() - 1; j >= 0; --j) { 180 | if (j == point_set.size() - 1 || !point_set[j + 1].is_feasible || !point_set[j].is_feasible) { 181 | point_set[j].rough_upper_bound = point_set[j].l; 182 | } else { 183 | point_set[j].rough_upper_bound = point_set[j + 1].rough_upper_bound; 184 | } 185 | } 186 | } 187 | 188 | // Calculate cost. 189 | int max_layer_reached = 0; 190 | for (const auto &layer : samples) { 191 | bool is_layer_feasible = false; 192 | for (const auto &point : layer) { 193 | calculate_cost(samples, point.layer_index, point.lateral_index); 194 | if (point.parent) is_layer_feasible = true; 195 | } 196 | if (layer.front().layer_index != 0 && !is_layer_feasible) break; 197 | max_layer_reached = layer.front().layer_index; 198 | } 199 | 200 | // Retrieve path. 201 | const DpPoint *ptr = nullptr; 202 | auto min_cost = DBL_MAX; 203 | for (const auto &point : samples[max_layer_reached]) { 204 | if (point.cost < min_cost) { 205 | ptr = &point; 206 | min_cost = point.cost; 207 | } 208 | } 209 | 210 | while (ptr) { 211 | if (ptr->layer_index == 0) { 212 | layers_bounds_.emplace_back(-10, 10); 213 | } else { 214 | static const double check_s = 0.2; 215 | double upper_bound = check_s + ptr->rough_upper_bound; 216 | double lower_bound = -check_s + ptr->rough_lower_bound; 217 | static const double check_limit = 6.0; 218 | const auto ref_pt = reference_line_out->get_reference_point(ptr->s); 219 | while (upper_bound < check_limit) { 220 | grid_map::Position pos; 221 | pos(0) = ref_pt.x + upper_bound * cos(ptr->heading + M_PI_2); 222 | pos(1) = ref_pt.y + upper_bound * sin(ptr->heading + M_PI_2); 223 | if (_map.isInside(pos) 224 | && _map.getObstacleDistance(pos) > check_s) { 225 | upper_bound += check_s; 226 | } else { 227 | upper_bound -= check_s; 228 | break; 229 | } 230 | } 231 | while (lower_bound > -check_limit) { 232 | grid_map::Position pos; 233 | pos(0) = ref_pt.x + lower_bound * cos(ptr->heading + M_PI_2); 234 | pos(1) = ref_pt.y + lower_bound * sin(ptr->heading + M_PI_2); 235 | if (_map.isInside(pos) 236 | && _map.getObstacleDistance(pos) > check_s) { 237 | lower_bound -= check_s; 238 | } else { 239 | lower_bound += check_s; 240 | break; 241 | } 242 | } 243 | if (ref_pt.kappa > 0.0001) { 244 | upper_bound = std::min(upper_bound, 1.0 / ref_pt.kappa); 245 | } else if (ref_pt.kappa < -0.0001) { 246 | lower_bound = std::max(lower_bound, 1.0 / ref_pt.kappa); 247 | } 248 | layers_bounds_.emplace_back(lower_bound, upper_bound); 249 | } 250 | ptr = ptr->parent; 251 | } 252 | 253 | std::reverse(layers_bounds_.begin(), layers_bounds_.end()); 254 | layers_s_list_.resize(layers_bounds_.size()); 255 | LOG(INFO) << "[FreeSpace] Search ok."; 256 | free_space_out->set_is_initialized(true); 257 | free_space_out->set_reference_line(reference_line_out); 258 | free_space_out->mutable_boundary_points()->clear(); 259 | for (std::size_t i = 0; i < layers_s_list_.size(); ++i) { 260 | const double s = layers_s_list_.at(i); 261 | const auto& bound = layers_bounds_.at(i); 262 | BoundaryPoint bd_pt; 263 | bd_pt.s = s; 264 | bd_pt.lb_l = bound.first; 265 | bd_pt.ub_l = bound.second; 266 | bd_pt.lb_xy = reference_line_out->get_xy_by_sl({s, bound.first}); 267 | bd_pt.ub_xy = reference_line_out->get_xy_by_sl({s, bound.second}); 268 | free_space_out->mutable_boundary_points()->emplace_back(bd_pt); 269 | } 270 | free_space_out->update_circle_bounds(FLAGS_vehicle_width * 0.5); 271 | return true; 272 | } 273 | 274 | } -------------------------------------------------------------------------------- /src/test/reference_line_processor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include "path/reference_line.h" 6 | #include "path/free_space.h" 7 | #include "path/data_structure.h" 8 | #include "Map.hpp" 9 | 10 | namespace Test { 11 | using PathPlanning::PathPoint; 12 | using PathPlanning::ReferenceLine; 13 | using PathPlanning::FreeSpace; 14 | 15 | // Point for A* search. 16 | struct APoint { 17 | double x{}; 18 | double y{}; 19 | double s{}; 20 | double l{}; 21 | double g{}; 22 | double h{}; 23 | double dir{}; 24 | // Layer denotes the index of the longitudinal layer that the point lies on. 25 | int layer{-1}; 26 | int offset_idx{}; 27 | double rough_upper_bound, rough_lower_bound; 28 | bool is_in_open_set{false}; 29 | APoint *parent{nullptr}; 30 | inline double f() { 31 | return g + h; 32 | } 33 | }; 34 | 35 | // Point for DP. 36 | struct DpPoint { 37 | double x, y, heading, s, l, cost = DBL_MAX, dir, dis_to_obs; 38 | int layer_index, lateral_index; 39 | double rough_upper_bound, rough_lower_bound; 40 | const DpPoint *parent = nullptr; 41 | bool is_feasible = true; 42 | }; 43 | 44 | class PointComparator { 45 | public: 46 | bool operator()(APoint *a, APoint *b) { 47 | return a->f() > b->f(); 48 | } 49 | }; 50 | 51 | class ReferenceLineProcessor { 52 | public: 53 | ReferenceLineProcessor( 54 | const std::vector& reference_line_points, 55 | const Map& map, 56 | const PathPoint& init_point) : 57 | _reference_line_points(reference_line_points), 58 | _map(map), 59 | _init_point(init_point) {}; 60 | bool solve(std::shared_ptr reference_line_out, std::shared_ptr free_space_out); 61 | bool b_spline(std::shared_ptr reference_line_out) const; 62 | bool smooth(std::shared_ptr reference_line_out) const; 63 | bool search(std::shared_ptr reference_line_out, std::shared_ptr free_space_out); 64 | private: 65 | void calculate_cost(std::vector> &samples, 66 | int layer_index, 67 | int lateral_index); 68 | 69 | std::vector _reference_line_points; 70 | const Map& _map; 71 | const PathPoint& _init_point; 72 | 73 | std::priority_queue, PointComparator> open_set_; 74 | std::unordered_set closed_set_; 75 | std::vector layers_s_list_; 76 | std::vector> layers_bounds_; 77 | }; 78 | 79 | } --------------------------------------------------------------------------------