├── .gitignore
├── CMakeLists.txt
├── LICENSE
├── README.md
├── csv
    ├── README.md
    ├── csv_display.ipynb
    ├── csv_eval_measurement.py
    ├── csv_manipulate.py
    ├── csv_vizu.py
    ├── sim_waypoints1.csv
    ├── sim_waypoints2.csv
    ├── sim_waypoints3.csv
    ├── sim_waypoints4.csv
    ├── tmp01_metrics.csv
    ├── tmp01_xypose.csv
    ├── tmp02_metrics.csv
    ├── tmp02_xypose.csv
    ├── tmp03_metrics.csv
    └── tmp03_xypose.csv
├── img
    ├── csv_eval01.svg
    ├── gz_rviz01.gif
    ├── sim_waypoints1.svg
    ├── sim_waypoints2.svg
    ├── sim_waypoints3.svg
    ├── sim_waypoints4.svg
    └── sim_waypoints4_zalazone_uni_track.png
├── launch
    ├── all_in_once.launch.py
    ├── all_in_once_save_metrics.launch.py
    ├── csv_saver.launch.py
    ├── gazebo_bridge.launch.py
    ├── rviz1.launch.py
    ├── single_goal_pursuit.launch.py
    ├── stanley.launch.py
    ├── waypoint_loader.launch.py
    ├── waypoint_saver.launch.py
    └── waypoint_to_target.launch.py
├── package.xml
├── rviz
    └── rviz1.rviz
└── src
    ├── csv_saver.cpp
    ├── pose_arr_to_tf.cpp
    └── visuals.cpp


/.gitignore:
--------------------------------------------------------------------------------
 1 | devel/
 2 | logs/
 3 | build/
 4 | bin/
 5 | lib/
 6 | msg_gen/
 7 | srv_gen/
 8 | msg/*Action.msg
 9 | msg/*ActionFeedback.msg
10 | msg/*ActionGoal.msg
11 | msg/*ActionResult.msg
12 | msg/*Feedback.msg
13 | msg/*Goal.msg
14 | msg/*Result.msg
15 | msg/_*.py
16 | build_isolated/
17 | devel_isolated/
18 | 
19 | # Generated by dynamic reconfigure
20 | *.cfgc
21 | /cfg/cpp/
22 | /cfg/*.py
23 | 
24 | # Ignore generated docs
25 | *.dox
26 | *.wikidoc
27 | 
28 | # eclipse stuff
29 | .project
30 | .cproject
31 | 
32 | # qcreator stuff
33 | CMakeLists.txt.user
34 | 
35 | srv/_*.py
36 | *.pcd
37 | *.pyc
38 | qtcreator-*
39 | *.user
40 | 
41 | /planning/cfg
42 | /planning/docs
43 | /planning/src
44 | 
45 | *~
46 | 
47 | # Emacs
48 | .#*
49 | 
50 | # Catkin custom files
51 | CATKIN_IGNORE
52 | 
53 | # vscode
54 | .vscode
55 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.8)
 2 | project(sim_wayp_plan_tools)
 3 | 
 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 5 |   add_compile_options(-Wall -Wextra -Wpedantic)
 6 | endif()
 7 | 
 8 | # find dependencies
 9 | find_package(ament_cmake REQUIRED)
10 | find_package(rclcpp REQUIRED)
11 | find_package(geometry_msgs REQUIRED)
12 | find_package(nav_msgs REQUIRED)
13 | find_package(visualization_msgs REQUIRED)
14 | find_package(tf2 REQUIRED)
15 | find_package(tf2_msgs REQUIRED)
16 | find_package(tf2_ros REQUIRED)
17 | find_package(tf2_geometry_msgs REQUIRED)
18 | find_package(wayp_plan_tools REQUIRED)
19 | 
20 | # uncomment the following section in order to fill in
21 | # further dependencies manually.
22 | # find_package(<dependency> REQUIRED)
23 | 
24 | set(ament_dependencies
25 |   rclcpp
26 |   geometry_msgs
27 |   nav_msgs
28 |   visualization_msgs
29 |   tf2
30 |   tf2_msgs
31 |   tf2_ros
32 |   tf2_geometry_msgs  
33 |   wayp_plan_tools
34 | )
35 | 
36 | include_directories(
37 |   # include
38 |   # share/${wayp_plan_tools}
39 |   # ${wayp_plan_tools_INCLUDE_DIRS}
40 |   ../wayp_plan_tools/include # TODO: this solution only works for the current project structure
41 | )
42 | # ament_export_include_directories(include) 
43 | # ament_export_dependencies(${ament_dependencies})
44 | 
45 | add_executable(pose_arr_to_tf src/pose_arr_to_tf.cpp)
46 | ament_target_dependencies(pose_arr_to_tf  ${ament_dependencies} )
47 | add_executable(visuals src/visuals.cpp)
48 | ament_target_dependencies(visuals  ${ament_dependencies} )
49 | add_executable(csv_saver src/csv_saver.cpp)
50 | ament_target_dependencies(csv_saver  ${ament_dependencies} )
51 | 
52 | 
53 | # uncomment the following section in order to fill in
54 | # further dependencies manually.
55 | # find_package(<dependency> REQUIRED)
56 | 
57 | if(BUILD_TESTING)
58 |   find_package(ament_lint_auto REQUIRED)
59 |   # the following line skips the linter which checks for copyrights
60 |   # comment the line when a copyright and license is added to all source files
61 |   set(ament_cmake_copyright_FOUND TRUE)
62 |   # the following line skips cpplint (only works in a git repo)
63 |   # comment the line when this package is in a git repo and when
64 |   # a copyright and license is added to all source files
65 |   set(ament_cmake_cpplint_FOUND TRUE)
66 |   ament_lint_auto_find_test_dependencies()
67 | endif()
68 | 
69 | install(DIRECTORY 
70 |   launch
71 |   rviz
72 |   csv
73 |   DESTINATION share/${PROJECT_NAME})
74 | install(TARGETS
75 |   pose_arr_to_tf
76 |   visuals
77 |   csv_saver
78 |   DESTINATION lib/${PROJECT_NAME}
79 | )
80 | ament_package()


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | BSD 3-Clause License
 2 | 
 3 | Copyright (c) 2023, JKK - Vehicle Industry Research Center
 4 | 
 5 | Redistribution and use in source and binary forms, with or without
 6 | modification, are permitted provided that the following conditions are met:
 7 | 
 8 | 1. Redistributions of source code must retain the above copyright notice, this
 9 |    list of conditions and the following disclaimer.
10 | 
11 | 2. Redistributions in binary form must reproduce the above copyright notice,
12 |    this list of conditions and the following disclaimer in the documentation
13 |    and/or other materials provided with the distribution.
14 | 
15 | 3. Neither the name of the copyright holder nor the names of its
16 |    contributors may be used to endorse or promote products derived from
17 |    this software without specific prior written permission.
18 | 
19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
29 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
  1 | # ROS 2 `sim_wayp_plan_tools` package
  2 | Gazebo Fortress ROS 2 simulation for the waypoint and planner tools.
  3 | 
  4 | 
  5 | ![](img/gz_rviz01.gif)
  6 | 
  7 | 
  8 | # Requirements
  9 | - ROS 2 Humble: [docs.ros.org/en/humble/Installation.html](https://docs.ros.org/en/humble/Installation.html)
 10 | - Gazebo Fortress: [gazebosim.org/docs/fortress/install_ubuntu](https://gazebosim.org/docs/fortress/install_ubuntu), read more about integration: [gazebosim.org/docs/fortress/ros2_integration](https://gazebosim.org/docs/fortress/ros2_integration)
 11 | - `ros-gz-bridge` install with a single command: `sudo apt install ros-humble-ros-gz-bridge`
 12 |  
 13 | ## Packages and build
 14 | 
 15 | It is assumed that the workspace is `~/ros2_ws/`.
 16 | 
 17 | ### Clone the packages
 18 | ```
 19 | cd ~/ros2_ws/src && \
 20 | git clone https://github.com/jkk-research/wayp_plan_tools && \
 21 | git clone https://github.com/jkk-research/sim_wayp_plan_tools
 22 | ```
 23 | 
 24 | ### Build ROS 2 packages
 25 | ```
 26 | cd ~/ros2_ws
 27 | ```
 28 | ```
 29 | colcon build --packages-select wayp_plan_tools sim_wayp_plan_tools --symlink-install
 30 | ```
 31 | 
 32 | 
 33 | # Usage of `wayp_plan_tools` as a simulation
 34 | 
 35 | ## 1. Start the simulation
 36 | ```
 37 | ign gazebo -v 4 -r ackermann_steering.sdf
 38 | ```
 39 | 
 40 | ## 2. Start the Gazebo bridge
 41 | 
 42 | Don't forget to `source` before ROS commands.
 43 | 
 44 | ``` r
 45 | source ~/ros2_ws/install/local_setup.bash
 46 | ```
 47 | 
 48 | ``` r
 49 | ros2 launch sim_wayp_plan_tools gazebo_bridge.launch.py
 50 | ```
 51 | 
 52 | In the background this `launch` file starts nodes similar to:
 53 | 
 54 | ``` r
 55 | ros2 run ros_gz_bridge parameter_bridge /world/ackermann_steering/pose/info@geometry_msgs/msg/PoseArray[ignition.msgs.Pose_V
 56 | ros2 run ros_gz_bridge parameter_bridge /model/vehicle_blue/cmd_vel@geometry_msgs/msg/Twist]ignition.msgs.Twist
 57 | ros2 run ros_gz_bridge parameter_bridge /model/vehicle_blue/odometry@nav_msgs/msg/Odometry[ignition.msgs.Odometry --ros-args -r /model/vehicle_blue/odometry:=/odom
 58 | ```
 59 | More about the bridge here: [github.com/gazebosim/ros_gz/blob/ros2/ros_gz_bridge/README.md](https://github.com/gazebosim/ros_gz/blob/ros2/ros_gz_bridge/README.md)
 60 | 
 61 | Also this `launch` creates `/tf` from the `PoseArray` with `pose_arr_to_tf`.
 62 | 
 63 | ## *Optional*: teleoperation via keyboard
 64 | 
 65 | ``` r
 66 | ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r /cmd_vel:=/model/vehicle_blue/cmd_vel
 67 | ```
 68 | 
 69 | ## 3. Load waypoints 
 70 | 
 71 | Use you ROS 2 workspace as `file_dir`:
 72 | ``` r
 73 | ros2 run wayp_plan_tools waypoint_loader --ros-args -p file_name:=sim_waypoints1.csv -p file_dir:=$HOME/ros2_ws/src/sim_wayp_plan_tools/csv -r __ns:=/sim1
 74 | ```
 75 | Or simply with default parameters:
 76 | 
 77 | ``` r
 78 | ros2 launch sim_wayp_plan_tools waypoint_loader.launch.py
 79 | ```
 80 | 
 81 | ## 4. Waypoint to target
 82 | 
 83 | ``` r
 84 | ros2 run wayp_plan_tools waypoint_to_target --ros-args -p lookahead_min:=2.5 -p lookahead_max:=4.5 -p mps_alpha:=1.5 -p mps_beta:=3.5 -p waypoint_topic:=waypointarray -p tf_frame_id:=base_link -p tf_child_frame_id:=map -r __ns:=/sim1
 85 | ```
 86 | Or simply with default parameters:
 87 | 
 88 | ``` r
 89 | ros2 launch sim_wayp_plan_tools waypoint_to_target.launch.py
 90 | ```
 91 | 
 92 | ## 5. Start a control
 93 | 
 94 | There are some options:
 95 | - `single_goal_pursuit`: Pure pursuit (for vehicles / robots), a simple cross-track error method
 96 | - `multiple_goal_pursuit`: Multiple goal pursuit for vehicles / robots an implementation of our [paper](https://hjic.mk.uni-pannon.hu/index.php/hjic/article/view/914)
 97 | - `stanley_control`: Stanley controller, a heading error + cross-track error method
 98 | - `follow_the_carrot`: Follow-the-carrot, the simplest controller
 99 | 
100 | This is a pure pursuit example:
101 | 
102 | ``` r
103 | ros2 run wayp_plan_tools single_goal_pursuit --ros-args -p cmd_topic:=/model/vehicle_blue/cmd_vel -p wheelbase:=1.0 -p waypoint_topic:=targetpoints -r __ns:=/sim1
104 | ```
105 | Or simply with default parameters:
106 | 
107 | ``` r
108 | ros2 launch sim_wayp_plan_tools single_goal_pursuit.launch.py
109 | ```
110 | 
111 | ## 6. Visualize results in `RViz2`
112 | ``` r
113 | ros2 launch sim_wayp_plan_tools rviz1.launch.py
114 | ```
115 | 
116 | # Or run everything with a single command
117 | 
118 | After `ign gazebo -v 4 -r ackermann_steering.sdf` (terminal 1) and `source ~/ros2_ws/install/local_setup.bash` (terminal 2), run this command (also in terminal 2): 
119 | ``` r
120 | ros2 launch sim_wayp_plan_tools all_in_once.launch.py
121 | ```
122 | 
123 | # Troubleshoot 
124 | 
125 | Kill `ign gazebo server` if stuck:
126 | 
127 | ``` r
128 | ps aux | grep ign
129 | ```
130 | 
131 | ``` r
132 | ab  12345 49.9  1.2 2412624 101608 ?      Sl   08:26  27:20 ign gazebo server
133 | ab  12346  518  6.6 10583664 528352 ?     Sl   08:26 283:45 ign gazebo gui
134 | ab  12347  0.0  0.0   9396  2400 pts/2    S+   09:21   0:00 grep --color=auto ign
135 | ```
136 | 
137 | Once you have identified the PID, use the kill command followed by the PID to terminate the process. For example:
138 | 
139 | ``` r
140 | kill 12345
141 | ```
142 | 
143 | 
144 | 


--------------------------------------------------------------------------------
/csv/README.md:
--------------------------------------------------------------------------------
 1 | 
 2 | # `sim_waypoints1.csv`
 3 | 
 4 | ![](../img/sim_waypoints1.svg)
 5 | 
 6 | # `sim_waypoints2.csv`
 7 | 
 8 | A [Hungaroring](https://en.wikipedia.org/wiki/Hungaroring) inspired track.
 9 | 
10 | ![](../img/sim_waypoints2.svg)
11 | 
12 | # `sim_waypoints3.csv`
13 | 
14 | ![](../img/sim_waypoints3.svg)
15 | 
16 | # `sim_waypoints4.csv`
17 | 
18 | A real-word measurement at the [ZalaZone](https://zalazone.hu/en/) Automotive Proving Ground, University track.
19 | 
20 | ![](../img/sim_waypoints4.svg)
21 | ![](../img/sim_waypoints4_zalazone_uni_track.png)
22 | 
23 | 
24 | # Evaluation example
25 | 
26 | ![](../img/csv_eval01.svg)


--------------------------------------------------------------------------------
/csv/csv_eval_measurement.py:
--------------------------------------------------------------------------------
 1 | # load csv and  plot it
 2 | import numpy as np
 3 | import matplotlib.pyplot as plt
 4 | import matplotlib.gridspec as gridspec
 5 | 
 6 | # waypoint csv header: x,y,z,yaw,mps,change_flag
 7 | W_X = 0
 8 | W_Y = 1
 9 | W_Z = 2
10 | W_YAW = 3
11 | W_MPS = 4
12 | # measure csv header: x,y,time,steering_angle,speed_mps,cur_lat_dist_abs,trg_way_lon_dist
13 | M_X = 0
14 | M_Y = 1
15 | M_TIME = 2
16 | M_STEER = 3
17 | M_SPEED = 4
18 | M_LAT_DIST = 5
19 | M_LON_DIST = 6
20 | 
21 | # create plot layout
22 | # f, (ax_xy, ax_st, ax_dist) = plt.subplots(nrows=3, ncols=1, sharex=False, gridspec_kw={"height_ratios": [3, 1, 1]})
23 | 
24 | f = plt.figure(constrained_layout=True)
25 | gs = f.add_gridspec(3, 3, width_ratios=[3, 1, 1], height_ratios=[3, 1, 1])
26 | ax_xy = f.add_subplot(gs[0, :])
27 | ax_st = f.add_subplot(gs[1, :-1])
28 | ax_txt = f.add_subplot(gs[1:, -1])
29 | ax_dist = f.add_subplot(gs[-1, :-1])
30 | 
31 | # ax_xy.set_xlabel("x") # , fontsize=6
32 | # ax_xy.set_ylabel("y")
33 | ax_st.set_xlabel("time")
34 | ax_st.set_ylabel("steer ang")
35 | ax_dist.set_xlabel("time")
36 | ax_dist.set_ylabel("lat dist")
37 | # ax_st.set_title("steering angle")
38 | # ax_dist.set_title("lat distance (abs)")
39 | ax_xy.axis("equal")
40 | ax_xy.grid()
41 | ax_st.grid()
42 | ax_dist.grid()
43 | ax_xy.legend(['first stock name', 'second stock name'])
44 | ax_txt.axis("off")
45 | 
46 | 
47 | 
48 | # read the data
49 | def read_data(filename):
50 |     points = np.genfromtxt(filename, delimiter=",", skip_header=1)
51 |     return points
52 | 
53 | def read_data_arr(filenames):
54 |     points = []
55 |     for filename in filenames:
56 |         points.append(np.genfromtxt(filename + "_xypose.csv", delimiter=",", skip_header=1))
57 |     return points
58 | 
59 | # plot x y data
60 | def plot_xy(points, fmt="", labl=""):
61 |     ax_xy.plot(points[:, W_X], points[:, W_Y], fmt, label=labl, alpha=0.8)
62 |     ax_xy.legend()
63 | 
64 | def plot_all(points, fmt="", labl=""):
65 |     ax_xy.plot(points[:, W_X], points[:, W_Y], label=labl, alpha=0.8)
66 |     ax_st.plot(points[:, M_TIME], points[:, M_STEER], fmt, label=labl, alpha=0.8)
67 |     ax_dist.plot(points[:, M_TIME], points[:, M_LAT_DIST], fmt, label=labl, alpha=0.8)
68 |     ax_xy.legend()
69 | 
70 | def read_text(filenames):
71 |     text = ""
72 |     for filename in filenames:
73 |         text += "\n\n" + filename + "\n"
74 |         with open(filename + "_metrics.csv", "r") as file:
75 |             text += file.read()
76 |     return text
77 |             
78 | 
79 | # main function
80 | if __name__ == "__main__":
81 |     meas_arr = ["csv/tmp01", "csv/tmp02", "csv/tmp03"]
82 |     wp_file = "csv/sim_waypoints3.csv"
83 |     points_wp = read_data(wp_file)
84 |     points_meas = read_data_arr(meas_arr)
85 |     # ax_xy.plot(points_meas[0][:, M_X], points_meas[0][:, M_Y], "r.-", label="xy", alpha=0.1, linewidth=4.2)
86 |     plot_xy(points_wp, fmt="k--", labl="waypoints")
87 |     plot_all(points_meas[0], labl="p pursuit A")
88 |     plot_all(points_meas[1], labl="p pursuit B")
89 |     plot_all(points_meas[2], labl="p pursuit C")
90 |     ax_txt.text(0.5, 0.5, read_text(meas_arr), ha="center", va="center", fontsize=8)
91 |     # f.tight_layout()
92 |     # f.savefig("img/csv_eval01.svg")
93 |     plt.show()


--------------------------------------------------------------------------------
/csv/csv_manipulate.py:
--------------------------------------------------------------------------------
 1 | # load csv and  plot it
 2 | import numpy as np
 3 | import matplotlib.pyplot as plt
 4 | 
 5 | # csv header: x,y,z,yaw,mps,change_flag
 6 | # x = [:, 0]
 7 | # y = [:, 1]
 8 | # z = [:, 2]
 9 | # yaw = [:, 3]
10 | 
11 | 
12 | # read the data
13 | def read_data(filename = 'csv/sim_waypoints3.csv'):
14 |     points = np.genfromtxt(filename, delimiter=',', skip_header=1)
15 |     return points
16 | 
17 | def rotate_points(points, theta):
18 |     t = np.array([[np.cos(theta), np.sin(theta)], [-np.sin(theta), np.cos(theta)]])
19 |     rotatedxy = np.dot(points[:, 0:2], t)
20 |     points[:, 0] = rotatedxy[:, 0]  
21 |     points[:, 1] = rotatedxy[:, 1]
22 |     return points
23 | 
24 | def scale_points(points,scale=60.0):
25 |     # scale xy points
26 |     points[:, 0] =  points[:, 0] / scale
27 |     points[:, 1] =  points[:, 1] / scale
28 |     return points
29 | 
30 | def plot_points(points):
31 |     plt.plot(points[:, 0], points[:, 1], 'r.-', label='xy', alpha=0.1, linewidth=4.2)
32 |     plt.quiver(points[:, 0], points[:, 1], np.cos(points[:, 3]), np.sin(points[:, 3]), angles='xy', scale_units='xy', scale=2.2, alpha=0.4)
33 |     plt.xlabel('x', fontsize=6)
34 |     plt.ylabel('y', fontsize=6)
35 |     plt.title('CSV plot', fontsize=6)
36 |     plt.axis('equal')
37 |     plt.grid()
38 |     plt.show()
39 | 
40 | def recalc_yaw(points):
41 |     angle = 0.0
42 |     prev_angle = 0.0
43 |     # get next point orientation
44 |     for i in range(len(points)-1):
45 |         dx = points[i + 1, 0] - points[i, 0]
46 |         dy = points[i + 1, 1] - points[i, 1]
47 |         angle = np.arctan2(dy, dx) 
48 |         points[i, 3] = angle
49 |         points[i, 2] = 0.0
50 |         speed = 4.8 - np.abs(prev_angle - angle) * 2
51 |         if speed < 0.5:
52 |             speed = 0.5
53 |         points[i, 4] = speed
54 |         points[i, 5] = 0
55 |         prev_angle = angle
56 |         # print(f"Index: {i}, Point: {point}")
57 |     return points
58 | 
59 | 
60 | # main function
61 | if __name__ == '__main__':
62 |     points = read_data()
63 |     # extend [:, 4:5]
64 |     points = np.append(points, np.zeros((len(points), 2)), axis=1)
65 |     # resample points
66 |     points = points[::4, :]
67 |     points = scale_points(points, 10.0)
68 |     points = rotate_points(points, -0.85 + np.pi/2*3)
69 |     # points[:, 0] = points[:, 0] * -1.0
70 |     # points[:, 1] = points[:, 1] + 2.0
71 |     points = recalc_yaw(points)
72 |     plt.plot(points[:, 0], points[:, 1], 'r.-', label='xy', alpha=0.1, linewidth=4.2)
73 |     plt.quiver(points[:, 0], points[:, 1], np.cos(points[:, 3]), np.sin(points[:, 3]), angles='xy', scale_units='xy', scale=2.2, alpha=0.4)
74 |     # print(points.shape)
75 |     plt.xlabel('x', fontsize=6)
76 |     plt.ylabel('y', fontsize=6)
77 |     plt.title('CSV plot', fontsize=6)
78 |     plt.axis('equal')
79 |     plt.grid()
80 |     plt.show()
81 |     # save points
82 |     # file_saved = 'csv/sim_waypoints3.csv'
83 |     # f=open(file_saved,'a')
84 |     # np.savetxt(f, np.array(['x', 'y', 'z', 'yaw', 'mps', 'change_flag']), newline=",", fmt="%s")
85 |     # f.write("\n")
86 |     # np.savetxt(f, points, delimiter=',', fmt='%.4f')


--------------------------------------------------------------------------------
/csv/csv_vizu.py:
--------------------------------------------------------------------------------
 1 | # load csv and  plot it
 2 | import numpy as np
 3 | import matplotlib.pyplot as plt
 4 | 
 5 | # csv header: x,y,z,yaw,mps,change_flag
 6 | # x = [:, 0]
 7 | # y = [:, 1]
 8 | # z = [:, 2]
 9 | # yaw = [:, 3]
10 | 
11 | 
12 | # read the data
13 | def read_data(filename = 'csv/sim_waypoints1.csv'):
14 |     points = np.genfromtxt(filename, delimiter=',', skip_header=1)
15 |     return points
16 | 
17 | 
18 | # main function
19 | if __name__ == '__main__':
20 |     my_file = 'csv/sim_waypoints2.csv'
21 |     points = read_data(my_file)
22 |     plt.plot(points[:, 0], points[:, 1], 'r.-', label=my_file, alpha=0.1, linewidth=4.2)
23 |     # plt.quiver(points[:, 0], points[:, 1], np.cos(points[:, 3]), np.sin(points[:, 3]), angles='xy', scale_units='xy', scale=2.2, alpha=0.4)
24 |     plt.xlabel('x', fontsize=6)
25 |     plt.ylabel('y', fontsize=6)
26 |     plt.title(my_file, fontsize=6)
27 |     plt.axis('equal')
28 |     plt.grid()
29 |     plt.legend()
30 |     plt.show()


--------------------------------------------------------------------------------
/csv/sim_waypoints1.csv:
--------------------------------------------------------------------------------
  1 | x,y,z,yaw,mps,change_flag
  2 | 0,2,0.325,0,0.1,0
  3 | 0.4006,2,0.325,0,0.17,0
  4 | 0.8011,2,0.325,0,0.24,0
  5 | 1.2051,2,0.325,0,0.31,0
  6 | 1.6091,2,0.325,0,0.38,0
  7 | 2.0059,1.9483,0.325,-0.0751,0.45,0
  8 | 2.3915,1.8382,0.325,-0.1525,0.52,0
  9 | 2.7834,1.7424,0.325,-0.1763,0.59,0
 10 | 3.1757,1.6572,0.325,-0.1866,0.66,0
 11 | 3.5631,1.5379,0.325,-0.2475,0.73,0
 12 | 3.9446,1.4043,0.325,-0.2719,0.8,0
 13 | 4.3264,1.2812,0.325,-0.2816,0.87,0
 14 | 4.717,1.1588,0.325,-0.2853,0.94,0
 15 | 5.1041,1.0393,0.325,-0.2872,1.01,0
 16 | 5.4959,0.9195,0.325,-0.2884,1.08,0
 17 | 5.8835,0.802,0.325,-0.2892,1.15,0
 18 | 6.2787,0.6826,0.325,-0.2897,1.22,0
 19 | 6.6601,0.5489,0.325,-0.3519,1.29,0
 20 | 7.0185,0.3643,0.325,-0.3691,1.36,0
 21 | 7.3819,0.188,0.325,-0.3791,1.4,0
 22 | 7.7488,0.018,0.325,-0.3857,1.4,0
 23 | 8.1204,-0.1487,0.325,-0.3902,1.4,0
 24 | 8.4972,-0.3067,0.325,-0.4382,1.4,0
 25 | 8.8613,-0.4806,0.325,-0.5965,1.4,0
 26 | 9.1724,-0.7453,0.325,-0.7602,1.4,0
 27 | 9.4116,-1.0668,0.325,-0.7877,1.4,0
 28 | 9.6607,-1.3817,0.325,-0.8018,1.4,0
 29 | 9.9243,-1.6987,0.325,-0.811,1.4,0
 30 | 10.1861,-2.0032,0.325,-0.817,1.4,0
 31 | 10.4517,-2.3056,0.325,-0.821,1.4,0
 32 | 10.7205,-2.6072,0.325,-0.8237,1.4,0
 33 | 10.9968,-2.9145,0.325,-0.8256,1.4,0
 34 | 11.2716,-3.2182,0.325,-0.8268,1.4,0
 35 | 11.5411,-3.5147,0.325,-0.8276,1.4,0
 36 | 11.8117,-3.8117,0.325,-0.8281,1.4,0
 37 | 12.0762,-4.1244,0.325,-0.7274,1.4,0
 38 | 12.3792,-4.3898,0.325,-0.5434,1.4,0
 39 | 12.7517,-4.5404,0.325,-0.4652,1.4,0
 40 | 13.1311,-4.6741,0.325,-0.4496,1.4,0
 41 | 13.5108,-4.8277,0.325,-0.2785,1.4,0
 42 | 13.9171,-4.8887,0.325,-0.0889,1.4,0
 43 | 14.3276,-4.8453,0.325,0.0994,1.4,0
 44 | 14.7151,-4.7101,0.325,0.284,1.4,0
 45 | 15.0587,-4.4994,0.325,0.4631,1.4,0
 46 | 15.3568,-4.221,0.325,0.6425,1.4,0
 47 | 15.5924,-3.8963,0.325,0.8174,1.4,0
 48 | 15.772,-3.5273,0.325,0.8754,1.4,0
 49 | 15.9646,-3.1711,0.325,0.8927,1.4,0
 50 | 16.1739,-2.8266,0.325,0.9045,1.4,0
 51 | 16.3945,-2.4887,0.325,0.9125,1.4,0
 52 | 16.6243,-2.1526,0.325,0.9179,1.4,0
 53 | 16.8563,-1.8235,0.325,0.9215,1.4,0
 54 | 17.0995,-1.4853,0.325,0.9241,1.4,0
 55 | 17.3388,-1.1569,0.325,0.9257,1.4,0
 56 | 17.577,-0.8328,0.325,0.9268,1.4,0
 57 | 17.8243,-0.4981,0.325,0.9275,1.4,0
 58 | 18.0633,-0.176,0.325,0.928,1.4,0
 59 | 18.3056,0.1498,0.325,0.9283,1.4,0
 60 | 18.5476,0.4747,0.325,0.9285,1.4,0
 61 | 18.7897,0.7993,0.325,0.9287,1.4,0
 62 | 19.0332,1.1255,0.325,0.9288,1.4,0
 63 | 19.2743,1.4485,0.325,0.9288,1.4,0
 64 | 19.5291,1.7633,0.325,1.0436,1.4,0
 65 | 19.7275,2.1238,0.325,1.2326,1.4,0
 66 | 19.815,2.5271,0.325,1.4232,1.4,0
 67 | 19.799,2.9283,0.325,1.6065,1.4,0
 68 | 19.6906,3.322,0.325,1.7904,1.4,0
 69 | 19.5047,3.6764,0.325,1.9683,1.4,0
 70 | 19.2422,3.9971,0.325,2.1506,1.4,0
 71 | 18.9277,4.2564,0.325,2.3286,1.4,0
 72 | 18.5714,4.4477,0.325,2.4706,1.4,0
 73 | 18.1957,4.6142,0.325,2.4931,1.4,0
 74 | 17.8362,4.8019,0.325,2.5078,1.397,0
 75 | 17.4803,5.0073,0.325,2.5179,1.394,0
 76 | 17.1324,5.221,0.325,2.5247,1.391,0
 77 | 16.7886,5.4406,0.325,2.5292,1.388,0
 78 | 16.4411,5.6685,0.325,2.5323,1.385,0
 79 | 16.0988,5.8968,0.325,2.5343,1.382,0
 80 | 15.7603,6.125,0.325,2.5357,1.379,0
 81 | 15.4201,6.356,0.325,2.5366,1.376,0
 82 | 15.0838,6.5855,0.325,2.5372,1.373,0
 83 | 14.743,6.8189,0.325,2.5376,1.37,0
 84 | 14.4106,7.0469,0.325,2.5378,1.367,0
 85 | 14.0685,7.2819,0.325,2.538,1.364,0
 86 | 13.7364,7.5103,0.325,2.5381,1.361,0
 87 | 13.4036,7.7393,0.325,2.5382,1.358,0
 88 | 13.0609,7.9752,0.325,2.5383,1.355,0
 89 | 12.724,8.2071,0.325,2.5383,1.352,0
 90 | 12.3938,8.4345,0.325,2.5383,1.349,0
 91 | 12.0643,8.6614,0.325,2.5383,1.4,0
 92 | 11.7316,8.8906,0.325,2.5383,1.561,0
 93 | 11.3948,9.1226,0.325,2.5384,1.722,0
 94 | 11.0629,9.3512,0.325,2.5384,1.883,0
 95 | 10.7211,9.5866,0.325,2.5384,2.044,0
 96 | 10.386,9.8175,0.325,2.5384,2.205,0
 97 | 10.0508,10.0484,0.325,2.5384,2.366,0
 98 | 9.7198,10.2765,0.325,2.5384,2.527,0
 99 | 9.3805,10.5102,0.325,2.5384,2.688,0
100 | 9.0436,10.7422,0.325,2.5384,2.849,0
101 | 8.711,10.9714,0.325,2.5384,3.01,0
102 | 8.3684,11.2074,0.325,2.5384,3.171,0
103 | 8.0324,11.4389,0.325,2.5384,3.332,0
104 | 7.698,11.6692,0.325,2.5384,3.493,0
105 | 7.3555,11.9052,0.325,2.5384,3.654,0
106 | 7.0227,12.1391,0.325,2.5671,3.815,0
107 | 6.6847,12.3701,0.325,2.7484,3.976,0
108 | 6.298,12.4818,0.325,2.8876,4.137,0
109 | 5.9004,12.5357,0.325,2.9963,4.298,0
110 | 5.4904,12.5383,0.325,-3.0979,4.459,0
111 | 5.0967,12.4411,0.325,-2.9133,4.62,0
112 | 4.7358,12.2597,0.325,-2.7317,4.2,0
113 | 4.4201,11.999,0.325,-2.6639,4.28,0
114 | 4.0999,11.7466,0.325,-2.6472,4.28,0
115 | 3.7611,11.505,0.325,-2.6357,4.38,0
116 | 3.422,11.279,0.325,-2.6282,4.46,0
117 | 3.0755,11.0579,0.325,-2.623,4.48,0
118 | 2.7307,10.8442,0.325,-2.6196,4.62,0
119 | 2.3807,10.6316,0.325,-2.6173,4.62,0
120 | 2.0245,10.418,0.325,-2.6158,4.62,0
121 | 1.6668,10.2068,0.325,-2.6054,4.62,0
122 | 1.3097,10.0053,0.325,-2.4339,4.62,0
123 | 1.0323,9.7008,0.325,-2.3561,4.62,0
124 | 0.7702,9.3767,0.325,-2.3463,4.62,0
125 | 0.5032,9.0638,0.325,-2.3399,4.62,0
126 | 0.228,8.7521,0.325,-2.3355,4.62,0
127 | -0.0412,8.454,0.325,-2.3327,4.62,0
128 | -0.3164,8.1536,0.325,-2.3308,4.62,0
129 | -0.5916,7.8562,0.325,-2.3295,4.62,0
130 | -0.8671,7.5604,0.325,-2.3286,4.62,0
131 | -1.1403,7.2681,0.325,-2.3281,4.62,0
132 | -1.4147,6.9755,0.325,-2.3277,4.62,0
133 | -1.6914,6.6811,0.325,-2.3274,4.62,0
134 | -1.9751,6.3795,0.325,-2.3273,4.62,0
135 | -2.2554,6.0817,0.325,-2.3272,4.62,0
136 | -2.5284,5.7875,0.325,-2.349,4.62,0
137 | -2.8095,5.4896,0.325,-2.4997,4.62,0
138 | -3.1607,5.2784,0.325,-2.6298,4.52,0
139 | -3.5463,5.1389,0.325,-2.6036,4.32,0
140 | -3.9127,4.947,0.325,-2.4566,4.12,0
141 | -4.2096,4.6659,0.325,-2.5574,3.9,0
142 | -4.5615,4.4425,0.325,-2.7225,3.9,0
143 | -4.9563,4.3141,0.325,-2.9029,3.8,0
144 | -5.3645,4.2972,0.325,-2.9903,3.7,0
145 | -5.7692,4.2958,0.325,-2.8412,3.7,0
146 | -6.1571,4.1794,0.325,-2.6613,3.7,0
147 | -6.4962,3.959,0.325,-2.4766,3.8,0
148 | -6.7693,3.6555,0.325,-2.2914,3.9,0
149 | -6.9647,3.2929,0.325,-2.1066,4.1,0
150 | -7.0749,2.904,0.325,-1.9274,4.3,0
151 | -7.1045,2.4969,0.325,-1.7481,4.4,0
152 | -7.0521,2.0985,0.325,-1.6174,4.62,0
153 | -6.9747,1.6986,0.325,-1.597,4.62,0
154 | -6.9209,1.2876,0.325,-1.583,4.62,0
155 | -6.884,0.8815,0.325,-1.5736,4.62,0
156 | -6.857,0.4681,0.325,-1.5672,4.62,0
157 | -6.8373,0.0627,0.325,-1.563,4.62,0
158 | -6.8221,-0.3405,0.325,-1.5602,4.62,0
159 | -6.8095,-0.7557,0.325,-1.5582,4.62,0
160 | -6.799,-1.1629,0.325,-1.5569,4.62,0
161 | -6.7897,-1.5739,0.325,-1.5561,4.62,0
162 | -6.7815,-1.975,0.325,-1.5555,4.62,0
163 | -6.7738,-2.376,0.325,-1.5552,4.62,0
164 | -6.7663,-2.7879,0.325,-1.5549,4.62,0
165 | -6.759,-3.2049,0.325,-1.5547,4.62,0
166 | -6.7521,-3.6059,0.325,-1.5546,4.62,0
167 | -6.7451,-4.0178,0.325,-1.5546,4.62,0
168 | -6.7382,-4.4298,0.325,-1.5545,4.62,0
169 | -6.7314,-4.8397,0.325,-1.5545,4.62,0
170 | -6.7247,-5.2427,0.325,-1.5545,4.62,0
171 | -6.718,-5.6526,0.325,-1.5544,4.62,0
172 | -6.7113,-6.0596,0.325,-1.5544,4.62,0
173 | -6.7047,-6.4595,0.325,-1.5544,4.62,0
174 | -6.6981,-6.8595,0.325,-1.5544,4.62,0
175 | -6.6913,-7.2754,0.325,-1.5544,4.62,0
176 | -6.6847,-7.6774,0.325,-1.5544,4.62,0
177 | -6.6782,-8.0783,0.325,-1.5544,4.82,0
178 | -6.6715,-8.4873,0.325,-1.5544,4.82,0
179 | -6.6647,-8.8972,0.325,-1.5544,4.92,0
180 | -6.658,-9.3051,0.325,-1.5544,4.92,0
181 | -6.6513,-9.7181,0.325,-1.5544,5.22,0
182 | -6.6445,-10.132,0.325,-1.5544,5.32,0
183 | -6.6377,-10.546,0.325,-1.5544,5.12,0
184 | -6.6311,-10.9479,0.325,-1.5544,4.92,0
185 | -6.6246,-11.3479,0.325,-1.5544,4.82,0
186 | -6.618,-11.7508,0.325,-1.5544,5.1,0
187 | -6.6114,-12.1518,0.325,-1.5544,5.2,0
188 | -6.6048,-12.5547,0.325,-1.5544,5.22,0
189 | -6.598,-12.9697,0.325,-1.5544,5.43,0
190 | -6.5914,-13.3706,0.325,-1.5544,5.18,0
191 | -6.5848,-13.7725,0.325,-1.5544,5.08,0
192 | -6.5781,-14.1865,0.325,-1.5544,4.28,0
193 | -6.5812,-14.5897,0.325,-1.4881,4.18,0
194 | -6.545,-14.9984,0.325,-1.3175,4.08,0
195 | -6.3918,-15.3845,0.325,-1.3024,3.88,0
196 | -6.2508,-15.7697,0.325,-1.2891,3.78,0
197 | -6.1219,-16.1618,0.325,-1.1181,3.78,0
198 | -5.8973,-16.4939,0.325,-1.0901,3.78,0
199 | -5.6767,-16.8308,0.325,-1.0811,3.78,0
200 | -5.4724,-17.175,0.325,-1.0092,3.78,0
201 | -5.2342,-17.5128,0.325,-0.8251,3.78,0
202 | -4.9127,-17.7646,0.325,-0.8015,3.78,0
203 | -4.595,-18.0284,0.325,-0.7917,3.78,0
204 | -4.2749,-18.2916,0.325,-0.903,3.78,0
205 | -4.0218,-18.6124,0.325,-1.0614,3.78,0
206 | -3.8394,-18.9774,0.325,-1.0613,3.78,0
207 | -3.6311,-19.3307,0.325,-1.0582,3.78,0
208 | -3.4244,-19.6872,0.325,-1.0566,3.78,0
209 | -3.221,-20.0415,0.325,-1.0303,3.78,0
210 | -3.0185,-20.3876,0.325,-0.8544,3.78,0
211 | -2.7091,-20.6649,0.325,-0.8251,3.78,0
212 | -2.413,-20.9346,0.325,-0.8173,3.78,0
213 | -2.1165,-21.2164,0.325,-0.8119,3.78,0
214 | -1.8343,-21.5105,0.325,-0.6987,3.78,0
215 | -1.4963,-21.7418,0.325,-0.5756,3.78,0
216 | -1.1348,-21.9176,0.325,-0.5646,3.78,0
217 | -0.7801,-22.1035,0.325,-0.5572,3.78,0
218 | -0.4252,-22.3121,0.325,-0.4596,3.78,0
219 | -0.0554,-22.4721,0.325,-0.2731,3.78,0
220 | 0.3417,-22.5256,0.325,-0.0885,3.78,0
221 | 0.7411,-22.4815,0.325,0.0943,3.78,0
222 | 1.1182,-22.3472,0.325,0.2715,3.78,0
223 | 1.4613,-22.1215,0.325,0.4475,3.78,0
224 | 1.7554,-21.8409,0.325,0.6237,3.78,0
225 | 1.9943,-21.5116,0.325,0.7995,3.78,0
226 | 2.1722,-21.1414,0.325,0.9767,3.78,0
227 | 2.2819,-20.7414,0.325,1.1554,3.78,0
228 | 2.3182,-20.3301,0.325,1.3332,3.78,0
229 | 2.2806,-19.9179,0.325,1.5113,3.78,0
230 | 2.1711,-19.5207,0.325,1.6885,3.78,0
231 | 1.9935,-19.1499,0.325,1.8653,3.78,0
232 | 1.7503,-18.8175,0.325,1.9875,3.78,0
233 | 1.5025,-18.4995,0.325,2.0084,3.78,0
234 | 1.2737,-18.1647,0.325,2.0227,3.78,0
235 | 1.0583,-17.8207,0.325,2.0324,3.78,0
236 | 0.8469,-17.4621,0.325,2.0391,3.78,0
237 | 0.644,-17.1036,0.325,2.0435,3.78,0
238 | 0.4435,-16.7396,0.325,2.0465,3.78,0
239 | 0.2529,-16.3875,0.325,2.0484,3.78,0
240 | 0.064,-16.0346,0.325,2.0497,3.78,0
241 | -0.1307,-15.6679,0.325,2.0506,3.78,0
242 | -0.3175,-15.314,0.325,2.0512,3.78,0
243 | -0.5043,-14.9591,0.325,2.0516,3.78,0
244 | -0.6977,-14.5908,0.325,2.0519,3.78,0
245 | -0.8909,-14.2223,0.325,2.052,3.78,0
246 | -1.0931,-13.8635,0.325,1.9735,3.78,0
247 | -1.2478,-13.4891,0.325,1.8139,3.78,0
248 | -1.3065,-13.0855,0.325,1.6371,3.78,0
249 | -1.261,-12.6864,0.325,1.4773,3.78,0
250 | -1.1436,-12.2988,0.325,1.4455,3.78,0
251 | -1.0399,-11.9045,0.325,1.4284,3.78,0
252 | -0.9484,-11.5124,0.325,1.4179,3.78,0
253 | -0.8619,-11.1074,0.325,1.4107,3.78,0
254 | -0.7821,-10.7091,0.325,1.4061,3.78,0
255 | -0.7048,-10.3067,0.325,1.4029,3.78,0
256 | -0.6301,-9.9064,0.325,1.4008,3.78,0
257 | -0.5555,-9.4987,0.325,1.3994,3.78,0
258 | -0.4836,-9.1004,0.325,1.3985,3.78,0
259 | -0.4117,-8.699,0.325,1.3979,3.78,0
260 | -0.3394,-8.2926,0.325,1.3975,3.78,0
261 | -0.2674,-7.886,0.325,1.3973,3.78,0
262 | -0.1951,-7.4774,0.325,1.3971,3.78,0
263 | -0.1234,-7.0707,0.325,1.397,3.78,0
264 | -0.0522,-6.667,0.325,1.3969,3.78,0
265 | 0.0199,-6.2573,0.325,1.3968,3.78,0
266 | 0.0912,-5.8526,0.325,1.3968,3.78,0
267 | 0.1481,-5.4502,0.325,1.291,3.78,0
268 | 0.2738,-5.059,0.325,1.1256,3.78,0
269 | 0.4902,-4.715,0.325,0.95,3.78,0
270 | 0.7951,-4.4443,0.325,0.9797,3.78,0
271 | 1.0379,-4.1215,0.325,1.1274,3.78,0
272 | 1.1794,-3.743,0.325,1.288,3.7,0
273 | 1.2334,-3.3351,0.325,1.4765,3.62,0
274 | 1.1879,-2.9361,0.325,1.6582,3.54,0
275 | 1.0547,-2.5547,0.325,1.8386,3.46,0
276 | 0.8411,-2.2026,0.325,2.0205,3.38,0
277 | 0.5604,-1.8987,0.325,2.2016,3.3,0
278 | 0.2284,-1.6553,0.325,2.3808,3.22,0
279 | -0.1447,-1.4773,0.325,2.4013,3.14,0
280 | -0.4805,-1.2581,0.325,2.2656,3.06,0
281 | -0.7463,-0.9529,0.325,2.1106,2.98,0
282 | -0.9289,-0.5893,0.325,1.9381,2.9,0
283 | -1.0246,-0.1955,0.325,1.8857,2.82,0
284 | -1.1142,0.1949,0.325,1.871,2.74,0
285 | -1.2124,0.5885,0.325,1.47371,2.66,0
286 | -1.2349,0.9973,0.325,1.3583,2.58,0
287 | -1.1542,1.4025,0.325,1.2813,2.5,0
288 | -0.9896,1.783,0.325,1.1326,2,0
289 | -0.6385,2.1589,0.325,0.9134,1.2,0
290 | 


--------------------------------------------------------------------------------
/csv/sim_waypoints2.csv:
--------------------------------------------------------------------------------
  1 | x,y,z,yaw,mps,change_flag
  2 | 0.1922,0.1521,0.0000,0.0312,4.7377,0
  3 | 2.1912,0.2144,0.0000,0.0315,4.7994,0
  4 | 4.1901,0.2774,0.0000,0.0315,4.7999,0
  5 | 6.1890,0.3404,0.0000,0.0313,4.7996,0
  6 | 8.1879,0.4030,0.0000,0.0308,4.7991,0
  7 | 10.1869,0.4647,0.0000,0.0303,4.7989,0
  8 | 12.1859,0.5252,0.0000,0.0299,4.7993,0
  9 | 14.1849,0.5851,0.0000,0.0298,4.7998,0
 10 | 16.1839,0.6447,0.0000,0.0299,4.7998,0
 11 | 18.1829,0.7046,0.0000,0.0303,4.7993,0
 12 | 20.1820,0.7651,0.0000,0.0308,4.7989,0
 13 | 22.1809,0.8267,0.0000,0.0316,4.7984,0
 14 | 24.1799,0.8900,0.0000,0.0325,4.7983,0
 15 | 26.1787,0.9549,0.0000,0.0332,4.7986,0
 16 | 28.1776,1.0212,0.0000,0.0337,4.7990,0
 17 | 30.1764,1.0885,0.0000,0.0340,4.7994,0
 18 | 32.1751,1.1565,0.0000,0.0341,4.7997,0
 19 | 34.1739,1.2247,0.0000,0.0341,4.7999,0
 20 | 36.1726,1.2929,0.0000,0.0339,4.7996,0
 21 | 38.1714,1.3606,0.0000,0.0334,4.7991,0
 22 | 40.1701,1.4275,0.0000,0.0328,4.7987,0
 23 | 42.1689,1.4930,0.0000,0.0319,4.7982,0
 24 | 44.1678,1.5568,0.0000,0.0307,4.7977,0
 25 | 46.1668,1.6182,0.0000,0.0294,4.7972,0
 26 | 48.1659,1.6769,0.0000,0.0277,4.7968,0
 27 | 50.1651,1.7324,0.0000,0.0261,4.7968,0
 28 | 52.1644,1.7847,0.0000,0.0270,4.7982,0
 29 | 54.1637,1.8387,0.0000,0.0318,4.7904,0
 30 | 56.1627,1.9024,0.0000,0.0400,4.7838,0
 31 | 58.1612,1.9823,0.0000,0.0026,4.7252,0
 32 | 60.1558,1.9874,0.0000,-0.1827,4.4294,0
 33 | 62.1077,1.6267,0.0000,-0.8067,3.5521,0
 34 | 63.4536,0.2222,0.0000,-1.8175,2.7784,0
 35 | 62.9850,-1.6389,0.0000,-2.2830,3.8690,0
 36 | 61.6845,-3.1452,0.0000,-2.4123,4.5415,0
 37 | 60.1952,-4.4763,0.0000,-2.4789,4.6667,0
 38 | 58.6157,-5.7089,0.0000,-2.5325,4.6928,0
 39 | 56.9769,-6.8519,0.0000,-2.5876,4.6898,0
 40 | 55.2797,-7.9018,0.0000,-2.6449,4.6854,0
 41 | 53.5229,-8.8539,0.0000,-2.7091,4.6718,0
 42 | 51.7051,-9.6931,0.0000,-2.7989,4.6204,0
 43 | 49.8224,-10.3648,0.0000,-2.9032,4.5914,0
 44 | 47.8822,-10.8363,0.0000,-2.9950,4.6164,0
 45 | 45.9095,-11.1277,0.0000,-3.0647,4.6605,0
 46 | 43.9198,-11.2809,0.0000,-3.1066,4.7163,0
 47 | 41.9201,-11.3509,0.0000,-3.1207,4.7718,0
 48 | 39.9173,-11.3927,0.0000,-3.1160,4.7907,0
 49 | 37.9161,-11.4439,0.0000,-3.1112,4.7904,0
 50 | 35.9167,-11.5046,0.0000,-3.1087,4.7949,0
 51 | 33.9183,-11.5703,0.0000,-3.1084,4.7995,0
 52 | 31.9206,-11.6366,0.0000,-3.1104,4.7960,0
 53 | 29.9229,-11.6989,0.0000,-3.1147,4.7915,0
 54 | 27.9248,-11.7526,0.0000,-3.1209,4.7877,0
 55 | 25.9256,-11.7941,0.0000,-3.1052,4.7687,0
 56 | 23.9252,-11.8670,0.0000,-3.0100,4.6096,0
 57 | 21.9437,-12.1293,0.0000,-2.5905,3.9611,0
 58 | 20.2719,-13.1568,0.0000,-2.1210,3.8609,0
 59 | 19.2320,-14.8521,0.0000,-1.7534,4.0648,0
 60 | 18.8759,-16.7809,0.0000,-1.2655,3.8243,0
 61 | 19.4775,-18.6899,0.0000,-0.9709,4.2107,0
 62 | 20.6007,-20.3318,0.0000,-0.5869,4.0321,0
 63 | 22.2514,-21.4297,0.0000,-0.3797,4.3856,0
 64 | 24.1091,-22.1710,0.0000,-0.3567,4.7540,0
 65 | 25.9835,-22.8695,0.0000,-0.3642,4.7849,0
 66 | 27.8512,-23.5816,0.0000,-0.3647,4.7991,0
 67 | 29.7215,-24.2956,0.0000,-0.3769,4.7756,0
 68 | 31.5759,-25.0297,0.0000,-0.5576,4.4387,0
 69 | 33.2734,-26.0882,0.0000,-1.0628,3.7895,0
 70 | 34.2276,-27.8023,0.0000,-1.2958,4.3341,0
 71 | 34.7727,-29.7340,0.0000,-1.3298,4.7319,0
 72 | 35.2494,-31.6735,0.0000,-1.3290,4.7984,0
 73 | 35.7279,-33.6138,0.0000,-1.3265,4.7949,0
 74 | 36.2118,-35.5544,0.0000,-1.3263,4.7996,0
 75 | 36.6961,-37.4954,0.0000,-1.3281,4.7962,0
 76 | 37.1768,-39.4373,0.0000,-1.3304,4.7956,0
 77 | 37.6531,-41.3799,0.0000,-1.3321,4.7964,0
 78 | 38.1258,-43.3231,0.0000,-1.3335,4.7972,0
 79 | 38.5958,-45.2668,0.0000,-1.3345,4.7981,0
 80 | 39.0640,-47.2109,0.0000,-1.3351,4.7989,0
 81 | 39.5310,-49.1553,0.0000,-1.3352,4.7997,0
 82 | 39.9978,-51.0999,0.0000,-1.3347,4.7991,0
 83 | 40.4655,-53.0444,0.0000,-1.3327,4.7960,0
 84 | 40.9372,-54.9881,0.0000,-1.3289,4.7923,0
 85 | 41.4164,-56.9301,0.0000,-1.3245,4.7913,0
 86 | 41.9041,-58.8700,0.0000,-1.3460,4.7570,0
 87 | 42.3495,-60.8181,0.0000,-1.4096,4.6729,0
 88 | 42.6703,-62.7903,0.0000,-1.4619,4.6953,0
 89 | 42.8877,-64.7797,0.0000,-1.4986,4.7267,0
 90 | 43.0320,-66.7738,0.0000,-1.5665,4.6642,0
 91 | 43.0407,-68.7715,0.0000,-1.6516,4.6298,0
 92 | 42.8794,-70.7639,0.0000,-1.3703,4.2375,0
 93 | 43.2733,-72.7024,0.0000,-0.8548,3.7691,0
 94 | 44.5731,-74.1965,0.0000,-0.6136,4.3175,0
 95 | 46.2080,-75.3479,0.0000,-0.6092,4.7912,0
 96 | 47.8482,-76.4924,0.0000,-0.6049,4.7914,0
 97 | 49.4932,-77.6295,0.0000,-0.6028,4.7958,0
 98 | 51.1406,-78.7633,0.0000,-0.6065,4.7924,0
 99 | 52.7838,-79.9033,0.0000,-0.6118,4.7896,0
100 | 54.4214,-81.0521,0.0000,-0.5972,4.7708,0
101 | 56.0755,-82.1769,0.0000,-0.6145,4.7654,0
102 | 57.7067,-83.3279,0.0000,-0.7842,4.4606,0
103 | 59.1216,-84.7394,0.0000,-1.1744,4.0196,0
104 | 59.8815,-86.5547,0.0000,-1.5571,4.0345,0
105 | 59.9087,-88.5410,0.0000,-1.8399,4.2343,0
106 | 59.3780,-90.4650,0.0000,-2.1066,4.2666,0
107 | 58.3586,-92.1817,0.0000,-2.4408,4.1317,0
108 | 56.8511,-93.4535,0.0000,-2.7941,4.0934,0
109 | 54.9818,-94.1305,0.0000,-2.9737,4.4407,0
110 | 53.0106,-94.4646,0.0000,-3.0329,4.6817,0
111 | 51.0227,-94.6815,0.0000,-3.0938,4.6782,0
112 | 49.0271,-94.7770,0.0000,3.1240,-7.6356,0
113 | 47.0309,-94.7419,0.0000,3.0629,4.6778,0
114 | 45.0389,-94.5849,0.0000,3.0239,4.7220,0
115 | 43.0508,-94.3498,0.0000,3.0088,4.7699,0
116 | 41.0653,-94.0846,0.0000,3.0147,4.7883,0
117 | 39.0815,-93.8316,0.0000,3.0230,4.7835,0
118 | 37.0978,-93.5951,0.0000,3.0262,4.7934,0
119 | 35.1125,-93.3651,0.0000,3.0237,4.7949,0
120 | 33.1243,-93.1296,0.0000,2.9545,4.6615,0
121 | 31.1656,-92.7588,0.0000,2.2966,3.4843,0
122 | 29.8956,-91.3277,0.0000,1.6147,3.4362,0
123 | 29.8083,-89.3422,0.0000,1.9584,4.1126,0
124 | 29.0993,-87.6055,0.0000,2.5324,3.6521,0
125 | 27.4535,-86.4572,0.0000,2.5314,4.7980,0
126 | 25.8137,-85.3105,0.0000,2.5269,4.7910,0
127 | 24.1821,-84.1586,0.0000,2.5360,4.7818,0
128 | 22.5382,-83.0203,0.0000,2.5348,4.7977,0
129 | 20.8943,-81.8792,0.0000,2.5228,4.7760,0
130 | 19.2637,-80.7181,0.0000,2.5572,4.7312,0
131 | 17.5992,-79.6172,0.0000,2.8071,4.3002,0
132 | 15.7291,-78.9673,0.0000,-3.1052,-7.0246,0
133 | 13.7353,-79.0400,0.0000,-2.6504,3.8905,0
134 | 12.0021,-79.9671,0.0000,-2.5464,4.5919,0
135 | 10.3328,-81.0974,0.0000,-2.5359,4.7791,0
136 | 8.7020,-82.2268,0.0000,-2.5612,4.7494,0
137 | 7.0240,-83.3270,0.0000,-2.7405,4.4414,0
138 | 5.1829,-84.1077,0.0000,3.1298,-6.9407,0
139 | 3.2371,-84.0848,0.0000,2.6764,3.8931,0
140 | 1.4436,-83.1846,0.0000,2.3001,4.0474,0
141 | 0.1255,-81.7096,0.0000,2.1208,4.4413,0
142 | -0.9227,-79.9999,0.0000,2.1307,4.7802,0
143 | -1.9864,-78.3028,0.0000,2.1341,4.7931,0
144 | -3.0531,-76.6138,0.0000,2.1234,4.7787,0
145 | -4.1015,-74.9140,0.0000,2.1230,4.7992,0
146 | -5.1502,-73.2120,0.0000,2.1341,4.7779,0
147 | -6.2203,-71.5176,0.0000,2.1753,4.7175,0
148 | -7.3586,-69.8697,0.0000,2.3332,4.4842,0
149 | -8.7281,-68.4357,0.0000,2.5655,4.3353,0
150 | -10.4065,-67.3455,0.0000,2.7511,4.4289,0
151 | -12.2551,-66.5845,0.0000,2.8521,4.5980,0
152 | -14.1657,-66.0154,0.0000,2.8514,4.7986,0
153 | -16.0838,-65.4426,0.0000,2.8484,4.7940,0
154 | -18.0001,-64.8641,0.0000,2.8616,4.7737,0
155 | -19.9190,-64.3123,0.0000,2.8416,4.7601,0
156 | -21.8339,-63.7201,0.0000,2.6985,4.5137,0
157 | -23.6223,-62.8712,0.0000,2.3735,4.1501,0
158 | -25.0626,-61.4799,0.0000,2.1262,4.3055,0
159 | -26.1099,-59.7924,0.0000,1.8575,4.2625,0
160 | -26.6746,-57.8769,0.0000,1.6285,4.3419,0
161 | -26.7896,-55.8861,0.0000,1.5528,4.6486,0
162 | -26.7535,-53.8858,0.0000,1.5360,4.7665,0
163 | -26.6840,-51.8861,0.0000,1.5378,4.7965,0
164 | -26.6179,-49.8888,0.0000,1.5366,4.7978,0
165 | -26.5497,-47.8910,0.0000,1.5351,4.7970,0
166 | -26.4784,-45.8923,0.0000,1.5344,4.7985,0
167 | -26.4055,-43.8929,0.0000,1.5343,4.7999,0
168 | -26.3325,-41.8932,0.0000,1.5350,4.7986,0
169 | -26.2610,-39.8937,0.0000,1.5365,4.7971,0
170 | -26.1924,-37.8945,0.0000,1.5387,4.7956,0
171 | -26.1282,-35.8961,0.0000,1.5412,4.7950,0
172 | -26.0690,-33.8985,0.0000,1.5427,4.7970,0
173 | -26.0129,-31.9012,0.0000,1.5429,4.7997,0
174 | -25.9571,-29.9034,0.0000,1.5417,4.7977,0
175 | -25.8989,-27.9043,0.0000,1.5393,4.7951,0
176 | -25.8358,-25.9032,0.0000,1.5355,4.7925,0
177 | -25.7651,-23.8992,0.0000,1.5295,4.7880,0
178 | -25.6822,-21.8924,0.0000,1.2063,4.1536,0
179 | -25.0015,-20.1081,0.0000,0.3796,3.1464,0
180 | -23.1639,-19.3750,0.0000,-0.0845,3.8719,0
181 | -21.1783,-19.5432,0.0000,-0.1408,4.6874,0
182 | -19.1971,-19.8240,0.0000,-0.1429,4.7958,0
183 | -17.2176,-20.1088,0.0000,-0.1385,4.7913,0
184 | -15.2372,-20.3849,0.0000,-0.1275,4.7779,0
185 | -13.2532,-20.6392,0.0000,-0.0882,4.7214,0
186 | -11.2635,-20.8151,0.0000,0.0031,4.6175,0
187 | -9.2662,-20.8089,0.0000,0.0431,4.7200,0
188 | -7.2648,-20.7227,0.0000,0.0094,4.7327,0
189 | -5.2652,-20.7038,0.0000,0.2862,4.2464,0
190 | -3.3849,-20.1504,0.0000,0.7848,3.8029,0
191 | -1.9632,-18.7303,0.0000,1.1304,4.1088,0
192 | -1.1281,-16.9584,0.0000,1.6439,3.7729,0
193 | -1.2739,-14.9672,0.0000,2.2261,3.6357,0
194 | -2.4582,-13.4263,0.0000,2.7317,3.7888,0
195 | -4.2845,-12.6326,0.0000,3.0974,4.0684,0
196 | -6.2666,-12.5451,0.0000,-3.0788,-7.5525,0
197 | -8.2714,-12.6711,0.0000,-3.1167,4.7242,0
198 | -10.2673,-12.7207,0.0000,-3.1103,4.7872,0
199 | -12.2657,-12.7832,0.0000,-3.0980,4.7754,0
200 | -14.2662,-12.8704,0.0000,-3.1064,4.7832,0
201 | -16.2622,-12.9406,0.0000,-3.1091,4.7946,0
202 | -18.2651,-13.0056,0.0000,3.0158,-7.4499,0
203 | -20.2174,-12.7588,0.0000,2.6136,3.9955,0
204 | -21.9397,-11.7543,0.0000,2.3228,4.2184,0
205 | -23.3020,-10.2978,0.0000,2.0237,4.2018,0
206 | -24.1708,-8.5122,0.0000,1.7143,4.1813,0
207 | -24.4550,-6.5459,0.0000,1.3918,4.1550,0
208 | -24.1000,-4.5841,0.0000,1.0453,4.1070,0
209 | -23.1056,-2.8693,0.0000,0.7010,4.1115,0
210 | -21.5805,-1.5820,0.0000,0.4077,4.2134,0
211 | -19.7563,-0.7941,0.0000,0.1852,4.3550,0
212 | -17.7964,-0.4268,0.0000,0.0561,4.5417,0
213 | -15.8012,-0.3147,0.0000,0.0150,4.7178,0
214 | -13.8002,-0.2846,0.0000,0.0310,4.7681,0
215 | -11.8012,-0.2227,0.0000,0.0362,4.7895,0
216 | -9.8029,-0.1504,0.0000,0.0322,4.7920,0
217 | -7.8041,-0.0860,0.0000,0.0296,4.7949,0
218 | -5.8051,-0.0267,0.0000,0.0290,4.7987,0
219 | -3.8059,0.0313,0.0000,0.0298,4.7984,0
220 | -1.8068,0.0909,0.0000,0.0298,0.0000,0
221 | 


--------------------------------------------------------------------------------
/csv/sim_waypoints3.csv:
--------------------------------------------------------------------------------
  1 | x,y,z,yaw,mps,change_flag
  2 | -0.0000,2.0000,0.0000,0.0043,2.2322,0.0000
  3 | 0.9414,2.0041,0.0000,0.0045,2.8455,0.0000
  4 | 1.8829,2.0083,0.0000,0.0046,3.2652,0.0000
  5 | 2.8243,2.0126,0.0000,0.0047,3.6995,0.0000
  6 | 3.7659,2.0171,0.0000,0.0049,4.0112,0.0000
  7 | 4.7074,2.0216,0.0000,0.0050,4.4884,0.0000
  8 | 5.6491,2.0263,0.0000,0.0051,4.7998,0.0000
  9 | 6.5907,2.0311,0.0000,0.0052,4.7998,0.0000
 10 | 7.5325,2.0361,0.0000,0.0053,4.7998,0.0000
 11 | 8.4743,2.0411,0.0000,0.0054,4.7998,0.0000
 12 | 9.4163,2.0462,0.0000,0.0055,4.7998,0.0000
 13 | 10.3583,2.0513,0.0000,0.0056,4.7998,0.0000
 14 | 11.3002,2.0566,0.0000,0.0058,4.7997,0.0000
 15 | 12.2417,2.0620,0.0000,0.0060,4.7996,0.0000
 16 | 13.1825,2.0677,0.0000,0.0063,4.7994,0.0000
 17 | 14.1222,2.0736,0.0000,0.0066,4.7993,0.0000
 18 | 15.0603,2.0798,0.0000,0.0066,4.8000,0.0000
 19 | 15.9976,2.0860,0.0000,-0.0004,4.7859,0.0000
 20 | 16.9508,2.0856,0.0000,-0.1021,4.5966,0.0000
 21 | 17.8942,1.9889,0.0000,-0.5877,3.8288,0.0000
 22 | 18.6388,1.4928,0.0000,-0.9407,4.0942,0.0000
 23 | 19.2053,0.7160,0.0000,-0.9076,4.7339,0.0000
 24 | 19.7760,-0.0144,0.0000,-0.6599,4.3046,0.0000
 25 | 20.5186,-0.5907,0.0000,-0.3016,4.0835,0.0000
 26 | 21.4122,-0.8687,0.0000,-0.2641,4.7249,0.0000
 27 | 22.3167,-1.1133,0.0000,-0.2503,4.7724,0.0000
 28 | 23.2235,-1.3451,0.0000,-0.5063,4.2880,0.0000
 29 | 24.8602,-2.2526,0.0000,-0.6334,4.5457,0.0000
 30 | 26.3736,-3.3641,0.0000,-0.5740,4.6811,0.0000
 31 | 27.9192,-4.3635,0.0000,-0.4221,4.4961,0.0000
 32 | 28.8027,-4.7602,0.0000,-0.1018,4.1596,0.0000
 33 | 29.7307,-4.8550,0.0000,0.2949,4.0064,0.0000
 34 | 30.6257,-4.5831,0.0000,0.6940,4.0020,0.0000
 35 | 31.3461,-3.9837,0.0000,1.1041,3.9798,0.0000
 36 | 31.7640,-3.1543,0.0000,1.2938,4.4205,0.0000
 37 | 32.0221,-2.2464,0.0000,1.3050,4.7777,0.0000
 38 | 32.2696,-1.3374,0.0000,1.3096,4.7907,0.0000
 39 | 32.5125,-0.4285,0.0000,1.3093,4.7994,0.0000
 40 | 32.7557,0.4802,0.0000,1.3041,4.7895,0.0000
 41 | 33.0038,1.3882,0.0000,1.2939,4.7797,0.0000
 42 | 33.2615,2.2953,0.0000,1.3016,4.7846,0.0000
 43 | 33.5123,3.2043,0.0000,1.4491,4.5051,0.0000
 44 | 33.6253,4.1279,0.0000,2.0510,3.5962,0.0000
 45 | 33.1999,4.9446,0.0000,2.1167,4.6685,0.0000
 46 | 32.7129,5.7463,0.0000,1.5126,3.5918,0.0000
 47 | 32.7658,6.6556,0.0000,1.2797,4.3340,0.0000
 48 | 33.0393,7.5684,0.0000,1.5433,4.2726,0.0000
 49 | 33.0650,8.5030,0.0000,1.8349,4.2168,0.0000
 50 | 32.8213,9.4039,0.0000,2.1117,4.2465,0.0000
 51 | 32.3392,10.2067,0.0000,2.3768,4.2698,0.0000
 52 | 31.6600,10.8585,0.0000,2.6249,4.3037,0.0000
 53 | 30.8409,11.3239,0.0000,2.8729,4.3041,0.0000
 54 | 29.9397,11.5721,0.0000,3.0427,4.4602,0.0000
 55 | 29.0043,11.6648,0.0000,3.0600,4.7655,0.0000
 56 | 28.0654,11.7416,0.0000,3.0556,4.7914,0.0000
 57 | 27.1276,11.8225,0.0000,3.0582,4.7948,0.0000
 58 | 26.1894,11.9008,0.0000,3.0586,4.7993,0.0000
 59 | 25.2511,11.9789,0.0000,3.0566,4.7960,0.0000
 60 | 24.3130,12.0588,0.0000,3.0539,4.7946,0.0000
 61 | 23.3751,12.1412,0.0000,3.0517,4.7956,0.0000
 62 | 22.4374,12.2258,0.0000,3.0500,4.7966,0.0000
 63 | 21.4999,12.3119,0.0000,3.0488,4.7975,0.0000
 64 | 20.5624,12.3992,0.0000,3.0480,4.7985,0.0000
 65 | 19.6251,12.4871,0.0000,3.0478,4.7995,0.0000
 66 | 18.6877,12.5753,0.0000,3.0480,4.7995,0.0000
 67 | 17.7504,12.6633,0.0000,3.0487,4.7985,0.0000
 68 | 16.8129,12.7506,0.0000,3.0500,4.7975,0.0000
 69 | 15.8754,12.8367,0.0000,3.0517,4.7966,0.0000
 70 | 14.9377,12.9213,0.0000,3.0539,4.7956,0.0000
 71 | 13.9999,13.0037,0.0000,3.0563,4.7952,0.0000
 72 | 13.0618,13.0839,0.0000,3.0583,4.7961,0.0000
 73 | 12.1236,13.1623,0.0000,3.0596,4.7973,0.0000
 74 | 11.1853,13.2394,0.0000,3.0603,4.7985,0.0000
 75 | 10.2469,13.3159,0.0000,3.0604,4.7997,0.0000
 76 | 9.3085,13.3922,0.0000,3.0600,4.7990,0.0000
 77 | 8.3700,13.4690,0.0000,3.0587,4.7975,0.0000
 78 | 7.4318,13.5469,0.0000,3.0564,4.5954,0.0000
 79 | 6.4938,13.6270,0.0000,3.0535,4.1942,0.0000
 80 | 5.5563,13.7098,0.0000,3.0648,3.7773,0.0000
 81 | 4.6161,13.7821,0.0000,-3.0065,1.5000,0.0000
 82 | 3.7099,13.6589,0.0000,-2.4285,3.6441,0.0000
 83 | 2.9979,13.0431,0.0000,-2.2177,4.3783,0.0000
 84 | 2.4306,12.2920,0.0000,-2.2112,4.7871,0.0000
 85 | 1.8684,11.5375,0.0000,-2.2234,4.7756,0.0000
 86 | 1.2968,10.7897,0.0000,-2.4067,4.4335,0.0000
 87 | 0.6009,10.1607,0.0000,-2.8947,3.8239,0.0000
 88 | -0.2949,9.9350,0.0000,3.0428,1.5000,0.0000
 89 | -1.2301,10.0277,0.0000,2.9955,3.7055,0.0000
 90 | -2.1614,10.1647,0.0000,2.9783,4.7656,0.0000
 91 | -3.0910,10.3179,0.0000,2.9850,4.7867,0.0000
 92 | -4.0211,10.4648,0.0000,2.9925,4.7849,0.0000
 93 | -4.9519,10.6046,0.0000,2.9922,4.7994,0.0000
 94 | -5.8827,10.7447,0.0000,2.9868,4.7891,0.0000
 95 | -6.8131,10.8899,0.0000,2.9851,4.7966,0.0000
 96 | -7.7433,11.0367,0.0000,2.9890,4.0921,0.0000
 97 | -8.6738,11.1797,0.0000,2.9919,3.7943,0.0000
 98 | -9.6045,11.3201,0.0000,2.9905,2.7973,0.0000
 99 | -10.5352,11.4618,0.0000,-3.1358,1.5000,0.0000
100 | -11.4762,11.4563,0.0000,-2.6480,3.8245,0.0000
101 | -12.2820,11.0228,0.0000,-2.2473,3.9986,0.0000
102 | -12.8718,10.2883,0.0000,-2.1815,4.0684,0.0000
103 | -13.4111,9.5178,0.0000,-2.1879,4.3872,0.0000
104 | -13.9567,8.7490,0.0000,-2.1945,4.7869,0.0000
105 | -14.5073,7.9837,0.0000,-2.1957,4.7975,0.0000
106 | -15.0574,7.2212,0.0000,-2.1905,4.7895,0.0000
107 | -15.6025,6.4570,0.0000,-2.1775,4.7740,0.0000
108 | -16.1399,5.6827,0.0000,-2.1479,4.7409,0.0000
109 | -16.6575,4.8877,0.0000,-1.9357,4.3755,0.0000
110 | -16.9861,4.0274,0.0000,-1.4731,3.8748,0.0000
111 | -16.8946,3.0944,0.0000,-0.9944,3.8427,0.0000
112 | -16.3846,2.3097,0.0000,-0.3685,3.5482,0.0000
113 | -15.5315,1.9803,0.0000,-0.0446,4.1521,0.0000
114 | -14.5956,1.9386,0.0000,0.0164,4.6781,0.0000
115 | -13.6517,1.9540,0.0000,0.0073,4.7819,0.0000
116 | -12.7092,1.9609,0.0000,0.0026,4.7905,0.0000
117 | -11.7675,1.9634,0.0000,0.0019,4.7987,0.0000
118 | -10.8259,1.9652,0.0000,0.0022,4.6995,0.0000
119 | -9.8845,1.9672,0.0000,0.0024,4.5996,0.0000
120 | -8.9430,1.9695,0.0000,0.0026,4.4996,0.0000
121 | -8.0016,1.9719,0.0000,0.0028,4.3996,0.0000
122 | -7.0602,1.9746,0.0000,0.0030,4.2996,0.0000
123 | -6.1188,1.9774,0.0000,0.0032,4.1996,0.0000
124 | -5.1775,1.9804,0.0000,0.0034,4.0996,0.0000
125 | -4.2361,1.9836,0.0000,0.0036,3.9996,0.0000
126 | -3.2948,1.9870,0.0000,0.0038,3.8996,0.0000
127 | -2.3534,1.9905,0.0000,0.0039,3.7997,0.0000
128 | -1.4121,1.9942,0.0000,0.0041,3.6997,0.0000
129 | 


--------------------------------------------------------------------------------
/csv/sim_waypoints4.csv:
--------------------------------------------------------------------------------
  1 | x,y,z,yaw,mps,change_flag,
  2 | 0.4824,1.3072,0.0000,-0.0165,1.9500,0.0000,0.0000,0.0000
  3 | 2.2034,1.2789,0.0000,0.0266,2.2800,0.0000,0.0000,0.0000
  4 | 4.1298,1.3302,0.0000,0.0603,2.4900,0.0000,0.0000,0.0000
  5 | 6.1751,1.4536,0.0000,0.1282,2.6300,0.0000,0.0000,0.0000
  6 | 8.3858,1.7386,0.0000,0.1987,2.9200,0.0000,0.0000,0.0000
  7 | 10.4983,2.1640,0.0000,0.2873,3.1900,0.0000,0.0000,0.0000
  8 | 13.0401,2.9151,0.0000,0.3655,3.4300,0.0000,0.0000,0.0000
  9 | 15.8849,4.0038,0.0000,0.4720,3.6100,0.0000,0.0000,0.0000
 10 | 18.3964,5.2860,0.0000,0.5827,3.7600,0.0000,0.0000,0.0000
 11 | 20.6516,6.7723,0.0000,0.6781,3.8500,0.0000,0.0000,0.0000
 12 | 22.5150,8.2734,0.0000,0.7589,4.0400,0.0000,0.0000,0.0000
 13 | 23.7212,9.4173,0.0000,0.7670,4.2700,0.0000,0.0000,0.0000
 14 | 25.0125,10.6621,0.0000,0.7856,4.5700,0.0000,0.0000,0.0000
 15 | 26.7016,12.3519,0.0000,0.8207,4.8800,0.0000,0.0000,0.0000
 16 | 28.0614,13.8113,0.0000,0.8068,5.1200,0.0000,0.0000,0.0000
 17 | 29.5225,15.3364,0.0000,0.8412,5.3300,0.0000,0.0000,0.0000
 18 | 30.9758,16.9617,0.0000,0.8378,5.5700,0.0000,0.0000,0.0000
 19 | 32.4960,18.6502,0.0000,0.8434,5.7600,0.0000,0.0000,0.0000
 20 | 34.0661,20.4137,0.0000,0.8602,5.9100,0.0000,0.0000,0.0000
 21 | 35.6305,22.2317,0.0000,0.8527,5.9100,0.0000,0.0000,0.0000
 22 | 37.2105,24.0401,0.0000,0.8432,5.8400,0.0000,0.0000,0.0000
 23 | 38.7844,25.8076,0.0000,0.8356,5.7700,0.0000,0.0000,0.0000
 24 | 40.3614,27.5514,0.0000,0.8363,5.7100,0.0000,0.0000,0.0000
 25 | 41.9169,29.2737,0.0000,0.8126,5.5800,0.0000,0.0000,0.0000
 26 | 43.4639,30.9073,0.0000,0.7993,5.4100,0.0000,0.0000,0.0000
 27 | 44.9721,32.4582,0.0000,0.7625,5.2100,0.0000,0.0000,0.0000
 28 | 46.4889,33.9070,0.0000,0.7220,5.0100,0.0000,0.0000,0.0000
 29 | 48.0231,35.2580,0.0000,0.6776,4.9400,0.0000,0.0000,0.0000
 30 | 49.9340,36.7959,0.0000,0.6014,4.8000,0.0000,0.0000,0.0000
 31 | 51.5279,37.8896,0.0000,0.5425,4.6700,0.0000,0.0000,0.0000
 32 | 53.4755,39.0637,0.0000,0.4709,4.5600,0.0000,0.0000,0.0000
 33 | 55.1251,39.9036,0.0000,0.4237,4.5000,0.0000,0.0000,0.0000
 34 | 57.1528,40.8181,0.0000,0.3982,4.4300,0.0000,0.0000,0.0000
 35 | 58.8019,41.5117,0.0000,0.3917,4.3600,0.0000,0.0000,0.0000
 36 | 60.8041,42.3386,0.0000,0.3993,4.3000,0.0000,0.0000,0.0000
 37 | 62.7974,43.1797,0.0000,0.4263,4.2200,0.0000,0.0000,0.0000
 38 | 64.3528,43.8860,0.0000,0.4757,4.1800,0.0000,0.0000,0.0000
 39 | 66.1951,44.8351,0.0000,0.5210,4.1100,0.0000,0.0000,0.0000
 40 | 67.6258,45.6562,0.0000,0.5882,4.0800,0.0000,0.0000,0.0000
 41 | 69.6825,47.0279,0.0000,0.6503,4.0400,0.0000,0.0000,0.0000
 42 | 70.9766,48.0122,0.0000,0.7109,4.0100,0.0000,0.0000,0.0000
 43 | 72.8103,49.5913,0.0000,0.7942,3.9700,0.0000,0.0000,0.0000
 44 | 74.7615,51.5772,0.0000,0.9206,3.9100,0.0000,0.0000,0.0000
 45 | 76.6463,54.0555,0.0000,1.0510,3.8400,0.0000,0.0000,0.0000
 46 | 78.1670,56.7131,0.0000,1.1939,3.7600,0.0000,0.0000,0.0000
 47 | 79.2785,59.5214,0.0000,1.3438,3.7100,0.0000,0.0000,0.0000
 48 | 79.9418,62.3934,0.0000,1.5076,3.6700,0.0000,0.0000,0.0000
 49 | 80.1379,65.4935,0.0000,1.6589,3.6700,0.0000,0.0000,0.0000
 50 | 79.8956,68.2358,0.0000,1.7962,3.6700,0.0000,0.0000,0.0000
 51 | 79.3161,70.7633,0.0000,1.9328,3.6800,0.0000,0.0000,0.0000
 52 | 78.2701,73.5256,0.0000,2.0754,3.7000,0.0000,0.0000,0.0000
 53 | 76.8411,76.1131,0.0000,2.2112,3.7100,0.0000,0.0000,0.0000
 54 | 75.0629,78.4993,0.0000,2.3463,3.7000,0.0000,0.0000,0.0000
 55 | 72.9801,80.6237,0.0000,2.4890,3.7000,0.0000,0.0000,0.0000
 56 | 70.6159,82.4307,0.0000,2.6332,3.6800,0.0000,0.0000,0.0000
 57 | 68.0161,83.8796,0.0000,2.7743,3.6600,0.0000,0.0000,0.0000
 58 | 65.2550,84.9419,0.0000,2.9138,3.6500,0.0000,0.0000,0.0000
 59 | 62.3882,85.6064,0.0000,3.0395,3.6400,0.0000,0.0000,0.0000
 60 | 59.4842,85.9041,0.0000,-3.1266,3.6100,0.0000,0.0000,0.0000
 61 | 56.5835,85.8607,0.0000,-3.0078,3.5900,0.0000,0.0000,0.0000
 62 | 53.7353,85.4773,0.0000,-2.8799,3.5700,0.0000,0.0000,0.0000
 63 | 50.9866,84.7411,0.0000,-2.7406,3.5200,0.0000,0.0000,0.0000
 64 | 48.3900,83.6403,0.0000,-2.5987,3.4900,0.0000,0.0000,0.0000
 65 | 46.0057,82.2015,0.0000,-2.4624,3.4400,0.0000,0.0000,0.0000
 66 | 43.8680,80.4757,0.0000,-2.3219,3.4200,0.0000,0.0000,0.0000
 67 | 42.0138,78.4897,0.0000,-2.1869,3.3700,0.0000,0.0000,0.0000
 68 | 40.4605,76.2958,0.0000,-2.0772,3.3300,0.0000,0.0000,0.0000
 69 | 39.1657,73.9615,0.0000,-2.0113,3.2800,0.0000,0.0000,0.0000
 70 | 38.0446,71.5833,0.0000,-1.9545,3.2500,0.0000,0.0000,0.0000
 71 | 37.0660,69.1590,0.0000,-1.9190,3.2000,0.0000,0.0000,0.0000
 72 | 36.1760,66.7073,0.0000,-1.8895,3.1900,0.0000,0.0000,0.0000
 73 | 35.3631,64.2438,0.0000,-1.8660,3.1800,0.0000,0.0000,0.0000
 74 | 34.6156,61.7854,0.0000,-1.8534,3.1700,0.0000,0.0000,0.0000
 75 | 33.9019,59.3278,0.0000,-1.8573,3.1500,0.0000,0.0000,0.0000
 76 | 33.1814,56.8822,0.0000,-1.8935,3.1200,0.0000,0.0000,0.0000
 77 | 32.3827,54.4939,0.0000,-1.9628,3.1500,0.0000,0.0000,0.0000
 78 | 31.4111,52.1437,0.0000,-2.0579,3.1800,0.0000,0.0000,0.0000
 79 | 30.2061,49.8685,0.0000,-2.1775,3.2100,0.0000,0.0000,0.0000
 80 | 28.7382,47.7532,0.0000,-2.3351,3.2500,0.0000,0.0000,0.0000
 81 | 26.9332,45.8703,0.0000,-2.5210,3.3400,0.0000,0.0000,0.0000
 82 | 24.7209,44.2889,0.0000,-2.7280,3.5300,0.0000,0.0000,0.0000
 83 | 22.0404,43.1125,0.0000,-2.9104,3.8800,0.0000,0.0000,0.0000
 84 | 19.7183,42.5658,0.0000,-3.0167,4.1000,0.0000,0.0000,0.0000
 85 | 18.0540,42.3568,0.0000,-3.0891,4.2900,0.0000,0.0000,0.0000
 86 | 16.2978,42.2645,0.0000,-3.1404,4.4600,0.0000,0.0000,0.0000
 87 | 14.4743,42.2623,0.0000,3.1119,4.6400,0.0000,0.0000,0.0000
 88 | 12.5653,42.3190,0.0000,3.1022,4.8300,0.0000,0.0000,0.0000
 89 | 10.5759,42.3973,0.0000,3.0958,4.9900,0.0000,0.0000,0.0000
 90 | 8.5350,42.4908,0.0000,3.0996,5.1600,0.0000,0.0000,0.0000
 91 | 6.4180,42.5797,0.0000,3.0996,5.3800,0.0000,0.0000,0.0000
 92 | 4.2077,42.6727,0.0000,3.1018,5.5700,0.0000,0.0000,0.0000
 93 | 1.9245,42.7635,0.0000,3.1014,5.7200,0.0000,0.0000,0.0000
 94 | -0.4197,42.8577,0.0000,3.0934,5.8800,0.0000,0.0000,0.0000
 95 | -2.8046,42.9727,0.0000,3.1028,5.9600,0.0000,0.0000,0.0000
 96 | -5.2230,43.0665,0.0000,3.0978,6.0000,0.0000,0.0000,0.0000
 97 | -7.6507,43.1729,0.0000,3.1054,6.0200,0.0000,0.0000,0.0000
 98 | -10.0825,43.2610,0.0000,3.1059,6.0500,0.0000,0.0000,0.0000
 99 | -12.5212,43.3481,0.0000,3.0978,6.0800,0.0000,0.0000,0.0000
100 | -14.9899,43.4562,0.0000,3.1040,6.1000,0.0000,0.0000,0.0000
101 | -17.4611,43.5490,0.0000,3.1021,6.1400,0.0000,0.0000,0.0000
102 | -19.9611,43.6477,0.0000,3.1086,6.1700,0.0000,0.0000,0.0000
103 | -22.4554,43.7301,0.0000,3.1107,6.1800,0.0000,0.0000,0.0000
104 | -24.9529,43.8073,0.0000,3.1123,6.2200,0.0000,0.0000,0.0000
105 | -27.4855,43.8814,0.0000,3.1123,6.2300,0.0000,0.0000,0.0000
106 | -30.0034,43.9553,0.0000,3.1139,6.2600,0.0000,0.0000,0.0000
107 | -32.5360,44.0255,0.0000,3.1158,6.2700,0.0000,0.0000,0.0000
108 | -35.0848,44.0912,0.0000,3.1150,6.3000,0.0000,0.0000,0.0000
109 | -37.6290,44.1589,0.0000,3.1123,6.3200,0.0000,0.0000,0.0000
110 | -40.1944,44.2340,0.0000,3.1104,6.3100,0.0000,0.0000,0.0000
111 | -42.7541,44.3139,0.0000,3.1084,6.3400,0.0000,0.0000,0.0000
112 | -45.3191,44.3990,0.0000,3.1043,6.3600,0.0000,0.0000,0.0000
113 | -47.8958,44.4952,0.0000,3.1060,6.3800,0.0000,0.0000,0.0000
114 | -50.4780,44.5872,0.0000,3.1060,6.3800,0.0000,0.0000,0.0000
115 | -53.0689,44.6794,0.0000,3.1059,6.4000,0.0000,0.0000,0.0000
116 | -55.6638,44.7722,0.0000,3.1032,6.4200,0.0000,0.0000,0.0000
117 | -58.2779,44.8725,0.0000,3.1038,6.4500,0.0000,0.0000,0.0000
118 | -60.8809,44.9708,0.0000,3.1005,6.4700,0.0000,0.0000,0.0000
119 | -63.5042,45.0787,0.0000,3.1033,6.4800,0.0000,0.0000,0.0000
120 | -66.1303,45.1794,0.0000,3.0991,6.5000,0.0000,0.0000,0.0000
121 | -68.7666,45.2915,0.0000,3.0975,6.5200,0.0000,0.0000,0.0000
122 | -71.4039,45.4078,0.0000,3.1025,6.5300,0.0000,0.0000,0.0000
123 | -74.0417,45.5110,0.0000,3.0978,6.5300,0.0000,0.0000,0.0000
124 | -76.6938,45.6271,0.0000,3.0972,6.5600,0.0000,0.0000,0.0000
125 | -79.3486,45.7449,0.0000,3.1028,6.5500,0.0000,0.0000,0.0000
126 | -82.0036,45.8480,0.0000,3.0991,6.5600,0.0000,0.0000,0.0000
127 | -84.6658,45.9612,0.0000,3.1038,6.5900,0.0000,0.0000,0.0000
128 | -87.3297,46.0620,0.0000,3.1001,6.5900,0.0000,0.0000,0.0000
129 | -89.9973,46.1728,0.0000,3.1012,6.6000,0.0000,0.0000,0.0000
130 | -92.6829,46.2815,0.0000,3.1031,6.6100,0.0000,0.0000,0.0000
131 | -95.3618,46.3846,0.0000,3.1026,6.6200,0.0000,0.0000,0.0000
132 | -98.0443,46.4891,0.0000,3.0984,6.6500,0.0000,0.0000,0.0000
133 | -100.7417,46.6058,0.0000,3.0968,6.6500,0.0000,0.0000,0.0000
134 | -103.4369,46.7265,0.0000,3.0996,6.6700,0.0000,0.0000,0.0000
135 | -106.1338,46.8399,0.0000,3.0894,6.6900,0.0000,0.0000,0.0000
136 | -108.8336,46.9809,0.0000,3.0941,6.6800,0.0000,0.0000,0.0000
137 | -111.5360,47.1095,0.0000,3.0850,6.6900,0.0000,0.0000,0.0000
138 | -114.2426,47.2628,0.0000,3.0814,6.7100,0.0000,0.0000,0.0000
139 | -116.9567,47.4265,0.0000,3.0811,6.7000,0.0000,0.0000,0.0000
140 | -119.6691,47.5907,0.0000,3.0786,6.7000,0.0000,0.0000,0.0000
141 | -122.3716,47.7610,0.0000,3.0830,6.6500,0.0000,0.0000,0.0000
142 | -125.0465,47.9180,0.0000,3.0801,6.5500,0.0000,0.0000,0.0000
143 | -127.6758,48.0798,0.0000,3.0836,6.4100,0.0000,0.0000,0.0000
144 | -130.2583,48.2296,0.0000,3.0986,6.2400,0.0000,0.0000,0.0000
145 | -132.7682,48.3377,0.0000,3.1048,6.0900,0.0000,0.0000,0.0000
146 | -135.2231,48.4280,0.0000,3.1232,5.9200,0.0000,0.0000,0.0000
147 | -137.5796,48.4713,0.0000,-3.1398,5.7700,0.0000,0.0000,0.0000
148 | -139.9149,48.4671,0.0000,-3.1095,5.6000,0.0000,0.0000,0.0000
149 | -142.1715,48.3945,0.0000,-3.0708,5.4400,0.0000,0.0000,0.0000
150 | -144.3008,48.2435,0.0000,-3.0376,5.2800,0.0000,0.0000,0.0000
151 | -146.4047,48.0240,0.0000,-2.9776,5.1100,0.0000,0.0000,0.0000
152 | -148.3896,47.6954,0.0000,-2.9276,4.9500,0.0000,0.0000,0.0000
153 | -150.3247,47.2750,0.0000,-2.8624,4.7800,0.0000,0.0000,0.0000
154 | -152.3781,46.6864,0.0000,-2.7864,4.6000,0.0000,0.0000,0.0000
155 | -154.2756,45.9825,0.0000,-2.7240,4.3900,0.0000,0.0000,0.0000
156 | -155.8913,45.2657,0.0000,-2.6506,4.2800,0.0000,0.0000,0.0000
157 | -157.7373,44.2787,0.0000,-2.5813,4.1500,0.0000,0.0000,0.0000
158 | -159.1362,43.4011,0.0000,-2.5151,4.0800,0.0000,0.0000,0.0000
159 | -160.4559,42.4458,0.0000,-2.4304,4.0300,0.0000,0.0000,0.0000
160 | -162.2689,40.8837,0.0000,-2.3202,3.9700,0.0000,0.0000,0.0000
161 | -164.1532,38.8586,0.0000,-2.1726,3.9500,0.0000,0.0000,0.0000
162 | -165.9311,36.2699,0.0000,-2.0375,3.9700,0.0000,0.0000,0.0000
163 | -167.2000,33.7510,0.0000,-1.9398,4.0700,0.0000,0.0000,0.0000
164 | -167.7942,32.2147,0.0000,-1.8282,4.0800,0.0000,0.0000,0.0000
165 | -168.2117,30.6286,0.0000,-1.7497,4.1000,0.0000,0.0000,0.0000
166 | -168.5030,29.0182,0.0000,-1.7145,4.0300,0.0000,0.0000,0.0000
167 | -168.7393,27.3854,0.0000,-1.5846,4.0100,0.0000,0.0000,0.0000
168 | -168.7671,25.3759,0.0000,-1.5171,3.9900,0.0000,0.0000,0.0000
169 | -168.6802,23.7591,0.0000,-1.4196,4.0300,0.0000,0.0000,0.0000
170 | -168.4363,22.1580,0.0000,-1.3424,4.0600,0.0000,0.0000,0.0000
171 | -168.0598,20.5389,0.0000,-1.2804,4.0800,0.0000,0.0000,0.0000
172 | -167.5793,18.9307,0.0000,-1.1978,4.2000,0.0000,0.0000,0.0000
173 | -166.9647,17.3602,0.0000,-1.1184,4.2500,0.0000,0.0000,0.0000
174 | -166.2150,15.8174,0.0000,-1.0514,4.3000,0.0000,0.0000,0.0000
175 | -165.3433,14.2928,0.0000,-0.9765,4.3600,0.0000,0.0000,0.0000
176 | -164.3546,12.8297,0.0000,-0.8803,4.3900,0.0000,0.0000,0.0000
177 | -163.2361,11.4760,0.0000,-0.7961,4.4000,0.0000,0.0000,0.0000
178 | -161.9863,10.1990,0.0000,-0.7183,4.4100,0.0000,0.0000,0.0000
179 | -160.6345,9.0175,0.0000,-0.6315,4.4500,0.0000,0.0000,0.0000
180 | -159.1799,7.9537,0.0000,-0.5303,4.4800,0.0000,0.0000,0.0000
181 | -157.6131,7.0351,0.0000,-0.4595,4.5300,0.0000,0.0000,0.0000
182 | -155.9576,6.2158,0.0000,-0.3791,4.5800,0.0000,0.0000,0.0000
183 | -154.2282,5.5270,0.0000,-0.3147,4.7000,0.0000,0.0000,0.0000
184 | -152.3934,4.9298,0.0000,-0.2454,4.9000,0.0000,0.0000,0.0000
185 | -150.4449,4.4417,0.0000,-0.1634,5.1300,0.0000,0.0000,0.0000
186 | -147.8505,4.0140,0.0000,-0.1028,5.4000,0.0000,0.0000,0.0000
187 | -145.6495,3.7870,0.0000,-0.0530,5.6300,0.0000,0.0000,0.0000
188 | -143.3415,3.6646,0.0000,-0.0193,5.8600,0.0000,0.0000,0.0000
189 | -140.9471,3.6183,0.0000,0.0067,6.0700,0.0000,0.0000,0.0000
190 | -138.4554,3.6350,0.0000,0.0165,6.2900,0.0000,0.0000,0.0000
191 | -135.8652,3.6777,0.0000,0.0310,6.5200,0.0000,0.0000,0.0000
192 | -133.2024,3.7602,0.0000,0.0322,6.7100,0.0000,0.0000,0.0000
193 | -130.4308,3.8496,0.0000,0.0454,6.9300,0.0000,0.0000,0.0000
194 | -126.8806,4.0110,0.0000,0.0507,7.1700,0.0000,0.0000,0.0000
195 | -123.9463,4.1600,0.0000,0.0479,7.3600,0.0000,0.0000,0.0000
196 | -120.1671,4.3410,0.0000,0.0467,7.5700,0.0000,0.0000,0.0000
197 | -117.0558,4.4863,0.0000,0.0383,7.7600,0.0000,0.0000,0.0000
198 | -113.8780,4.6081,0.0000,0.0322,7.9700,0.0000,0.0000,0.0000
199 | -110.6351,4.7124,0.0000,0.0221,8.1100,0.0000,0.0000,0.0000
200 | -107.3246,4.7854,0.0000,0.0172,8.2000,0.0000,0.0000,0.0000
201 | -103.9807,4.8430,0.0000,0.0074,8.2500,0.0000,0.0000,0.0000
202 | -100.6235,4.8679,0.0000,-0.0073,8.2700,0.0000,0.0000,0.0000
203 | -97.2558,4.8432,0.0000,-0.0121,8.3300,0.0000,0.0000,0.0000
204 | -93.8592,4.8022,0.0000,-0.0215,8.3600,0.0000,0.0000,0.0000
205 | -90.4309,4.7284,0.0000,-0.0350,8.4100,0.0000,0.0000,0.0000
206 | -87.0228,4.6093,0.0000,-0.0371,8.4500,0.0000,0.0000,0.0000
207 | -83.5867,4.4817,0.0000,-0.0434,8.4800,0.0000,0.0000,0.0000
208 | -80.1762,4.3337,0.0000,-0.0458,8.4900,0.0000,0.0000,0.0000
209 | -76.7230,4.1755,0.0000,-0.0477,8.4800,0.0000,0.0000,0.0000
210 | -73.2686,4.0106,0.0000,-0.0477,8.4500,0.0000,0.0000,0.0000
211 | -69.8755,3.8485,0.0000,-0.0504,8.4200,0.0000,0.0000,0.0000
212 | -66.4599,3.6763,0.0000,-0.0500,8.3700,0.0000,0.0000,0.0000
213 | -62.6782,3.4873,0.0000,-0.0490,8.3300,0.0000,0.0000,0.0000
214 | -59.7110,3.3417,0.0000,-0.0480,8.2900,0.0000,0.0000,0.0000
215 | -56.3427,3.1800,0.0000,-0.0464,8.2600,0.0000,0.0000,0.0000
216 | -53.0276,3.0261,0.0000,-0.0398,8.2200,0.0000,0.0000,0.0000
217 | -49.6858,2.8930,0.0000,-0.0347,8.2000,0.0000,0.0000,0.0000
218 | -45.9823,2.7646,0.0000,-0.0292,8.1600,0.0000,0.0000,0.0000
219 | -43.0839,2.6799,0.0000,-0.0250,8.1100,0.0000,0.0000,0.0000
220 | -39.7900,2.5976,0.0000,-0.0240,8.0800,0.0000,0.0000,0.0000
221 | -36.5426,2.5198,0.0000,-0.0223,8.0400,0.0000,0.0000,0.0000
222 | -33.2875,2.4472,0.0000,-0.0249,7.9700,0.0000,0.0000,0.0000
223 | -30.0622,2.3670,0.0000,-0.0283,7.8400,0.0000,0.0000,0.0000
224 | -26.9259,2.2781,0.0000,-0.0352,7.6300,0.0000,0.0000,0.0000
225 | -23.1130,2.1439,0.0000,-0.0349,7.3200,0.0000,0.0000,0.0000
226 | -20.1994,2.0422,0.0000,-0.0444,7.0500,0.0000,0.0000,0.0000
227 | -17.3860,1.9172,0.0000,-0.0474,6.7300,0.0000,0.0000,0.0000
228 | -14.3959,1.7752,0.0000,-0.0455,6.3600,0.0000,0.0000,0.0000
229 | -11.0237,1.6217,0.0000,-0.0453,5.6600,0.0000,0.0000,0.0000
230 | -8.5453,1.5094,0.0000,-0.0427,5.1500,0.0000,0.0000,0.0000
231 | -6.3324,1.4147,0.0000,-0.0332,4.5200,0.0000,0.0000,0.0000
232 | -4.5899,1.3568,0.0000,-0.0318,3.9700,0.0000,0.0000,0.0000
233 | -2.1209,1.2783,0.0000,-0.0288,2.8300,0.0000,0.0000,0.0000
234 | 


--------------------------------------------------------------------------------
/csv/tmp01_metrics.csv:
--------------------------------------------------------------------------------
1 | MAX_LAT_DISTANCE,9.8051
2 | AVG_LAT_DISTANCE,0.7500
3 | lookahead_min,2.5
4 | lookahead_max,6.5


--------------------------------------------------------------------------------
/csv/tmp01_xypose.csv:
--------------------------------------------------------------------------------
  1 | x,y,time,steering_angle,speed_mps,cur_lat_dist_abs,trg_way_lon_dist
  2 | 0.0000,2.0000,6.0000,0.0000,0.0000,0.0000,3.7659
  3 | 0.0761,2.0001,6.5000,0.0024,2.2322,0.0409,3.7250
  4 | 0.4836,2.0012,7.0000,0.0022,2.2322,0.4396,3.3263
  5 | 0.8885,2.0026,7.5000,0.0015,2.8455,0.1099,3.8759
  6 | 1.2745,2.0042,8.0000,0.0013,2.8455,0.2801,3.4859
  7 | 1.5965,2.0056,8.5000,0.0009,3.2652,0.3344,4.1007
  8 | 2.0305,2.0077,9.0000,0.0007,3.2652,0.0996,4.6083
  9 | 2.4415,2.0096,9.5002,0.0008,3.6995,0.4488,4.2152
 10 | 2.8735,2.0114,10.0000,0.0007,3.6995,0.0158,4.7240
 11 | 3.3036,2.0131,10.5004,0.0008,3.6995,0.4043,4.3040
 12 | 3.6906,2.0146,11.0000,0.0008,4.0112,0.1354,4.8438
 13 | 4.0846,2.0161,11.5000,0.0009,4.0112,0.2697,4.4388
 14 | 4.5076,2.0177,12.0000,0.0008,4.4884,0.2669,4.9758
 15 | 4.9446,2.0194,12.5004,0.0010,4.4884,0.1872,4.5218
 16 | 5.3365,2.0208,13.0000,0.0008,4.7998,0.3696,5.0789
 17 | 5.7165,2.0224,13.5006,0.0009,4.7998,0.0179,4.6919
 18 | 6.1415,2.0243,14.0000,0.0008,4.7998,0.4274,5.2238
 19 | 6.5455,2.0259,14.5000,0.0010,4.7998,0.1003,4.8098
 20 | 6.9615,2.0277,15.0000,0.0009,4.7998,0.3038,5.3473
 21 | 7.3065,2.0291,15.5000,0.0010,4.7998,0.2941,5.0033
 22 | 7.6665,2.0305,16.0000,0.0011,4.7998,0.0842,4.6253
 23 | 8.0704,2.0324,16.5000,0.0008,4.7998,0.4640,5.1722
 24 | 8.4294,2.0343,17.0002,0.0009,4.7998,0.0832,4.7912
 25 | 8.8204,2.0363,17.5002,0.0008,4.7998,0.2822,5.3659
 26 | 9.2234,2.0383,18.0000,0.0009,4.7998,0.2560,4.9619
 27 | 9.6264,2.0403,18.5000,0.0011,4.7998,0.1612,4.5449
 28 | 10.0044,2.0421,19.0000,0.0009,4.7998,0.4120,5.1141
 29 | 10.4054,2.0444,19.5000,0.0010,4.7998,0.0115,4.7111
 30 | 10.8134,2.0466,20.0000,0.0009,4.7998,0.4011,5.2384
 31 | 11.2464,2.0489,20.5000,0.0010,4.7997,0.1281,4.8254
 32 | 11.6583,2.0511,21.0015,0.0003,4.7997,0.2992,5.3516
 33 | 12.0933,2.0537,21.5001,0.0003,4.7996,0.2256,4.9346
 34 | 12.5003,2.0561,22.0000,0.0004,4.7996,0.1997,4.5096
 35 | 12.9473,2.0587,22.5002,-0.0078,4.7994,0.3003,5.0124
 36 | 13.3207,2.0613,23.0000,-0.0083,4.7994,0.0904,4.6221
 37 | 13.7370,2.0623,23.5000,-0.0460,4.7993,0.4413,4.9904
 38 | 14.1594,2.0629,24.0002,-0.0486,4.7993,0.0309,4.5815
 39 | 14.5582,2.0532,24.5025,-0.0997,4.7993,0.3872,4.8835
 40 | 14.8884,2.0402,25.0000,-0.1029,4.8000,0.2292,4.5684
 41 | 15.2705,2.0076,25.5000,-0.1277,4.8000,0.1581,5.0017
 42 | 15.6779,1.9530,26.0000,-0.1303,4.7859,0.3981,4.6030
 43 | 16.0852,1.8719,26.5000,-0.1205,4.7859,0.2061,5.1208
 44 | 16.4647,1.7717,27.0000,-0.1202,4.7859,0.5163,4.7398
 45 | 16.8777,1.6387,27.5000,-0.0831,4.5966,0.4463,5.2432
 46 | 17.2628,1.4889,28.0005,-0.0808,4.5966,0.6201,4.8441
 47 | 17.6423,1.3304,28.5000,-0.0489,3.8288,0.7053,5.3378
 48 | 18.0229,1.1602,29.0000,-0.0450,4.0942,0.7313,4.9085
 49 | 18.3847,0.9927,29.5000,-0.0426,4.0942,0.5650,4.5126
 50 | 18.7648,0.8110,30.0000,-0.0152,4.7339,0.5152,5.0188
 51 | 19.1033,0.6444,30.5000,-0.0119,4.7339,0.1600,4.6323
 52 | 19.4854,0.4571,31.0000,-0.0121,4.7339,0.3339,6.0692
 53 | 19.8607,0.2735,31.5000,-0.0110,4.3046,0.3144,5.6574
 54 | 20.2288,0.0930,32.0000,-0.0100,4.3046,0.4172,5.2565
 55 | 20.5921,-0.0861,32.5000,-0.0092,4.0835,0.5312,4.8455
 56 | 20.9828,-0.2798,33.0000,-0.0085,4.0835,0.5210,4.4375
 57 | 21.3400,-0.4581,33.5000,-0.0229,4.7249,0.4573,5.8752
 58 | 21.6762,-0.6258,34.0000,-0.0234,4.7249,0.3424,5.4962
 59 | 22.0632,-0.8242,34.5000,-0.0235,4.7724,0.4515,5.0741
 60 | 22.4213,-1.0129,35.0000,-0.0234,4.7724,0.1389,4.6618
 61 | 22.7668,-1.1996,35.5000,-0.0196,4.7724,0.4156,6.0913
 62 | 23.1390,-1.4059,36.0000,-0.0193,4.2880,0.1361,5.6752
 63 | 23.4966,-1.6077,36.5014,-0.0190,4.2880,0.3267,5.2650
 64 | 23.8639,-1.8186,37.0000,-0.0187,4.2880,0.7288,4.8557
 65 | 24.1977,-2.0136,37.5000,-0.0183,4.5457,0.7506,4.4484
 66 | 24.5429,-2.2186,38.0001,-0.0034,4.5457,0.3652,5.0104
 67 | 24.9020,-2.4360,38.5001,-0.0021,4.5457,0.1546,4.5966
 68 | 25.2537,-2.6483,39.0000,0.0296,4.5457,0.4857,5.0660
 69 | 25.6132,-2.8660,39.5000,0.0323,4.5457,0.9092,4.6353
 70 | 25.9726,-3.0743,40.0004,0.0797,4.6811,0.5522,4.9482
 71 | 26.3469,-3.2839,40.5000,0.0833,4.6811,0.1421,4.5388
 72 | 26.7223,-3.4722,41.0000,0.1414,4.6811,0.3064,4.7082
 73 | 27.0982,-3.6429,41.5002,0.2006,4.6811,0.7057,4.7535
 74 | 27.4458,-3.7744,42.0012,0.2448,4.4961,0.8132,4.8788
 75 | 27.8142,-3.8755,42.5000,0.2483,4.4961,0.5226,4.5553
 76 | 28.1585,-3.9291,43.0014,0.2648,4.4961,0.4775,4.9033
 77 | 28.5836,-3.9468,43.5000,0.2675,4.4961,0.7367,4.5627
 78 | 28.9618,-3.9170,44.0012,0.2625,4.1596,0.8449,5.0148
 79 | 29.2509,-3.8652,44.5000,0.2639,4.1596,0.9622,4.7911
 80 | 29.6572,-3.7509,45.0010,0.2660,4.0064,1.0911,4.4402
 81 | 30.0448,-3.5937,45.5000,0.2454,4.0020,1.1580,4.9615
 82 | 30.3893,-3.4068,46.0001,0.2463,3.9798,1.1468,4.6033
 83 | 30.7233,-3.1857,46.5000,0.2474,3.9798,1.0168,4.2491
 84 | 31.0285,-2.9420,47.0001,0.2153,4.4205,0.7943,4.8057
 85 | 31.2602,-2.7194,47.5000,0.2147,4.4205,0.6699,4.5025
 86 | 31.5074,-2.4517,48.0000,0.1819,4.7777,0.5982,5.1067
 87 | 31.7513,-2.1436,48.5000,0.1804,4.7777,0.3072,4.7356
 88 | 31.9808,-1.8203,49.0001,0.1500,4.7777,0.3851,5.3084
 89 | 32.1943,-1.4747,49.5000,0.1477,4.7907,0.2170,4.9195
 90 | 32.3876,-1.1304,50.0000,0.1458,4.7907,0.1790,4.5359
 91 | 32.5734,-0.7610,50.5000,0.1252,4.7994,0.3851,5.0557
 92 | 32.7274,-0.4105,51.0000,0.1228,4.7994,0.1949,4.6866
 93 | 32.8604,-0.0772,51.5000,0.1439,4.7994,0.4549,5.0749
 94 | 33.0017,0.3157,52.0000,0.1434,4.7895,0.3173,4.6911
 95 | 33.1237,0.7295,52.5001,0.1555,4.7895,0.4022,5.0885
 96 | 33.2115,1.0994,53.0001,0.1554,4.7797,0.3877,4.7167
 97 | 33.2796,1.4907,53.5000,0.1171,4.7797,0.2724,5.2430
 98 | 33.3219,1.9031,54.0000,0.1143,4.7846,0.4442,4.8326
 99 | 33.3484,2.3168,54.5000,0.0667,4.7846,0.0890,5.3099
100 | 33.3582,2.7368,55.0000,0.0616,4.7846,0.3929,4.9024
101 | 33.3596,3.1595,55.5001,0.0414,4.5051,0.1848,5.4113
102 | 33.3554,3.5723,56.0001,0.0377,4.5051,0.3419,5.0030
103 | 33.3468,4.0111,56.5001,0.0345,3.5962,0.3311,4.5655
104 | 33.3347,4.4128,57.0000,0.0319,3.5962,0.3672,4.1569
105 | 33.3202,4.7816,57.5001,0.0403,4.6685,0.2429,4.6960
106 | 33.3007,5.2109,58.0000,0.0619,4.6685,0.2312,5.1467
107 | 33.2759,5.6365,58.5000,0.0624,3.5918,0.5896,4.7174
108 | 33.2417,6.0297,59.0000,0.0621,3.5918,0.5806,4.3297
109 | 33.1928,6.4534,59.5000,0.0614,4.3340,0.5047,3.9046
110 | 33.1327,6.8719,60.0001,0.1325,4.3340,0.4063,5.0696
111 | 33.0678,7.2702,60.5000,0.1361,4.2726,0.3477,4.6711
112 | 32.9976,7.5834,61.0000,0.1369,4.2726,0.0428,4.3623
113 | 32.8933,7.9481,61.5007,0.1696,4.2726,0.3566,4.7219
114 | 32.7694,8.3059,62.0000,0.1713,4.2168,0.3735,4.3787
115 | 32.6106,8.6715,62.5009,0.1949,4.2168,0.4405,4.7456
116 | 32.4212,9.0318,63.0001,0.1965,4.2465,0.5619,4.3662
117 | 32.2017,9.3727,63.5001,0.2001,4.2465,0.5913,4.8198
118 | 31.9422,9.7081,64.0000,0.2007,4.2698,0.6511,4.4303
119 | 31.6450,10.0286,64.5000,0.1892,4.2698,0.6851,4.9219
120 | 31.3305,10.3120,65.0000,0.1887,4.3037,0.6515,4.5269
121 | 31.0048,10.5639,65.5001,0.1696,4.3037,0.6884,5.0584
122 | 30.6266,10.8106,66.0001,0.1682,4.3041,0.5702,4.6322
123 | 30.2888,11.0021,66.5000,0.1459,4.3041,0.6115,5.1852
124 | 29.9302,11.1762,67.0000,0.1440,4.4602,0.4186,4.7962
125 | 29.5336,11.3452,67.5000,0.1213,4.4602,0.4309,5.3257
126 | 29.1633,11.4797,68.0001,0.1192,4.7655,0.2943,4.9400
127 | 28.7754,11.6047,68.5000,0.1171,4.7655,0.1977,4.5322
128 | 28.3524,11.7223,69.0000,0.0933,4.7914,0.3443,5.0515
129 | 27.9490,11.8144,69.5000,0.0903,4.7914,0.0787,4.6505
130 | 27.5482,11.8941,70.0008,0.0698,4.7948,0.4792,5.1768
131 | 27.1097,11.9665,70.5000,0.0669,4.7948,0.1435,4.7529
132 | 26.7311,12.0217,71.0001,0.0502,4.7948,0.3932,5.2928
133 | 26.3546,12.0687,71.5001,0.0477,4.7993,0.2777,4.9225
134 | 25.9503,12.1140,72.0004,0.0458,4.7993,0.2732,4.5155
135 | 25.5285,12.1560,72.5000,0.0318,4.7960,0.3925,5.0488
136 | 25.1061,12.1918,73.0000,0.0292,4.7960,0.2254,4.6072
137 | 24.7051,12.2223,73.5000,0.0188,4.7946,0.4708,5.1378
138 | 24.2785,12.2517,74.0001,0.0170,4.7946,0.1929,4.7392
139 | 23.8786,12.2779,74.5001,0.0096,4.7946,0.4383,5.2524
140 | 23.4664,12.3031,75.0000,0.0082,4.7956,0.2125,4.8364
141 | 23.0562,12.3279,75.5000,0.0035,4.7956,0.3104,5.3843
142 | 22.6577,12.3514,76.0001,0.0026,4.7966,0.3039,4.9752
143 | 22.3092,12.3724,76.5000,0.0020,4.7966,0.1556,4.6341
144 | 21.9189,12.3962,77.0005,-0.0009,4.7975,0.4662,5.1583
145 | 21.5085,12.4214,77.5000,-0.0015,4.7975,0.1206,4.7551
146 | 21.0802,12.4482,78.0006,-0.0030,4.7975,0.3787,5.2843
147 | 20.6908,12.4732,78.5026,-0.0033,4.7985,0.2046,4.8931
148 | 20.2725,12.5010,79.0001,-0.0038,4.7985,0.2424,5.4194
149 | 19.8463,12.5302,79.5003,-0.0039,4.7995,0.2877,4.9882
150 | 19.4551,12.5579,80.0002,-0.0040,4.7995,0.1313,4.5891
151 | 19.0361,12.5884,80.5001,-0.0038,4.7995,0.4123,5.1173
152 | 18.5972,12.6212,81.0000,-0.0037,4.7995,0.0535,4.6702
153 | 18.1833,12.6529,81.5007,-0.0032,4.7995,0.4585,5.1916
154 | 17.7465,12.6872,82.0001,-0.0031,4.7985,0.0722,4.7755
155 | 17.3327,12.7201,82.5005,-0.0024,4.7985,0.3656,5.2839
156 | 16.9439,12.7516,83.0000,-0.0023,4.7975,0.1879,4.8948
157 | 16.5342,12.7851,83.5001,-0.0016,4.7975,0.2209,5.4282
158 | 16.1256,12.8188,84.0001,-0.0016,4.7966,0.3088,5.0161
159 | 15.7229,12.8522,84.5001,-0.0015,4.7966,0.0813,4.6261
160 | 15.3073,12.8869,85.0001,-0.0009,4.7956,0.4232,5.1305
161 | 14.9157,12.9198,85.5000,-0.0009,4.7956,0.0393,4.7466
162 | 14.5181,12.9530,86.0000,-0.0006,4.7956,0.3628,5.2861
163 | 14.1036,12.9876,86.5001,-0.0006,4.7952,0.1728,4.8801
164 | 13.6980,13.0214,87.0002,-0.0004,4.7952,0.2405,5.4088
165 | 13.2814,13.0560,87.5003,-0.0005,4.7961,0.2824,4.9898
166 | 12.8709,13.0901,88.0001,-0.0006,4.7961,0.1361,4.5718
167 | 12.4583,13.1244,88.5000,-0.0004,4.7973,0.3878,5.0952
168 | 12.0218,13.1606,89.0000,-0.0005,4.7973,0.0406,4.6682
169 | 11.6451,13.1919,89.5004,-0.0006,4.7973,0.4274,5.2216
170 | 11.2365,13.2257,90.0000,-0.0007,4.7985,0.1235,4.8306
171 | 10.8508,13.2577,90.5000,-0.0009,4.7985,0.2730,5.3758
172 | 10.4013,13.2950,91.0006,-0.0011,4.7997,0.2228,4.9298
173 | 10.0057,13.3278,91.5000,-0.0011,4.7997,0.1706,4.5367
174 | 9.6060,13.3611,92.0000,-0.0003,4.7990,0.3501,5.0586
175 | 9.2831,13.3883,92.5001,-0.0003,4.7990,0.0307,4.7386
176 | 8.8985,13.4207,93.0001,0.0142,4.7990,0.3530,5.2520
177 | 8.4748,13.4569,93.5000,0.0151,4.7975,0.1634,4.8270
178 | 8.0577,13.4889,94.0001,0.0618,4.7975,0.2480,5.1435
179 | 7.6547,13.5187,94.5001,0.0651,4.5954,0.2832,4.7389
180 | 7.2590,13.5347,95.0000,0.1096,4.5954,0.1220,5.0354
181 | 6.8690,13.5397,95.5001,0.1126,4.1942,0.4391,4.6638
182 | 6.4272,13.5193,96.0000,0.1377,4.1942,0.1030,5.0325
183 | 6.0248,13.4781,96.5000,0.1394,4.1942,0.4311,4.6463
184 | 5.6315,13.4112,97.0000,0.1460,3.7773,0.3158,5.1186
185 | 5.2452,13.3203,97.5000,0.1464,3.7773,0.4531,4.7452
186 | 4.8662,13.2052,98.0000,0.1462,1.5000,0.6353,4.3593
187 | 4.4856,13.0634,98.5000,0.1306,1.5000,0.7012,3.0871
188 | 4.1191,12.8995,99.0001,0.1250,3.6441,0.8676,2.6878
189 | 3.7392,12.7056,99.5000,0.1224,4.3783,0.8563,4.1056
190 | 3.4141,12.5204,100.0001,0.0606,4.3783,0.6783,4.5784
191 | 3.0679,12.3014,100.5000,-0.0056,4.7871,0.6879,4.9208
192 | 2.7296,12.0789,101.0000,-0.0136,4.7871,0.3960,4.5244
193 | 2.3861,11.8626,101.5000,-0.0609,4.7871,0.3943,4.9202
194 | 2.0286,11.6456,102.0000,-0.0676,4.7756,0.2608,4.5105
195 | 1.6512,11.4380,102.5001,-0.0953,4.7756,0.1896,4.9205
196 | 1.2449,11.2362,103.0000,-0.1053,4.4335,0.4756,5.3880
197 | 0.8312,11.0589,103.5001,-0.1075,4.4335,0.4994,4.9530
198 | 0.4106,10.9068,104.0000,-0.1085,3.8239,0.7777,4.5186
199 | -0.0017,10.7826,104.5000,-0.1084,3.8239,0.8353,4.0991
200 | -0.4287,10.6781,105.0001,-0.1040,1.5000,0.7591,4.5771
201 | -0.8555,10.5965,105.5001,-0.1063,3.7055,0.7273,3.2332
202 | -1.2646,10.5366,106.0001,-0.0922,3.7055,0.5186,4.6902
203 | -1.6859,10.4950,106.5001,-0.0904,3.7055,0.6162,4.2639
204 | -2.0421,10.4713,107.0000,-0.0784,4.7656,0.3538,4.8411
205 | -2.4460,10.4578,107.5000,-0.0672,4.7656,0.3672,5.3936
206 | -2.8419,10.4553,108.0001,-0.0658,4.7867,0.3330,4.9897
207 | -3.2537,10.4609,108.5001,-0.0645,4.7867,0.1684,4.5973
208 | -3.6371,10.4741,109.0000,-0.0528,4.7849,0.4332,5.1348
209 | -4.0380,10.4971,109.5001,-0.0513,4.7849,0.0407,4.7308
210 | -4.4655,10.5284,110.0000,-0.0409,4.7849,0.3778,5.2707
211 | -4.9010,10.5685,110.5001,-0.0394,4.7994,0.1223,4.8272
212 | -5.2881,10.6085,111.0001,-0.0308,4.7994,0.2744,5.3781
213 | -5.6944,10.6556,111.5001,-0.0296,4.7891,0.2690,4.9706
214 | -6.1191,10.7083,112.0001,-0.0286,4.7891,0.1868,4.5350
215 | -6.5304,10.7630,112.5006,-0.0095,4.7966,0.3660,5.0523
216 | -6.9508,10.8233,113.0001,-0.0072,4.7966,0.1183,4.6144
217 | -7.3070,10.8742,113.5001,0.0375,4.7966,0.4273,5.0448
218 | -7.7499,10.9388,114.0001,0.0419,4.0921,0.1199,4.5952
219 | -8.2063,10.9927,114.5001,0.1021,4.0921,0.4048,4.7781
220 | -8.6361,11.0341,115.0000,0.1072,3.7943,0.1748,4.3527
221 | -9.0496,11.0498,115.5000,0.1489,3.7943,0.3425,4.6784
222 | -9.4257,11.0453,116.0020,0.1518,2.7973,0.3664,4.3315
223 | -9.8252,11.0110,116.5000,0.1530,2.7973,0.3418,3.9431
224 | -10.2108,10.9496,117.0000,0.1530,1.5000,0.6291,3.5606
225 | -10.6174,10.8556,117.5000,0.1518,1.5000,0.5920,3.1546
226 | -11.0011,10.7392,118.0001,0.1488,1.5000,0.8160,2.7584
227 | -11.3615,10.6049,118.5002,0.1827,3.8245,0.8515,3.2231
228 | -11.7601,10.4296,119.0001,0.1591,3.9986,0.8176,4.6826
229 | -12.1291,10.2274,119.5000,0.1573,3.9986,0.7920,4.2547
230 | -12.4941,9.9950,120.0000,0.1354,4.0684,0.4957,4.7572
231 | -12.8312,9.7468,120.5001,0.1331,4.0684,0.5120,4.3614
232 | -13.1468,9.4903,121.0001,0.1116,4.3872,0.3060,4.8966
233 | -13.4708,9.1975,121.5001,0.1086,4.3872,0.2904,4.4584
234 | -13.7791,8.8968,122.0001,0.0903,4.7869,0.2933,4.9971
235 | -14.0735,8.5851,122.5001,0.0873,4.7869,0.1432,4.5690
236 | -14.3549,8.2698,123.0000,0.0861,4.7975,0.3972,5.0657
237 | -14.6180,7.9573,123.5002,0.0843,4.7975,0.0786,4.6404
238 | -14.8752,7.6324,124.0001,0.1118,4.7975,0.4489,5.0259
239 | -15.1122,7.3168,124.5019,0.1121,4.7895,0.1437,4.6391
240 | -15.3304,6.9962,125.0001,0.1586,4.7895,0.2949,4.8620
241 | -15.5363,6.6661,125.5001,0.2220,4.7740,0.2810,4.7402
242 | -15.7210,6.3223,126.0001,0.2851,4.7740,0.1179,4.5760
243 | -15.8773,5.9422,126.5001,0.3239,4.7409,0.4263,4.6094
244 | -15.9789,5.5421,127.0001,0.3298,4.7409,0.1869,4.8950
245 | -16.0159,5.1402,127.5001,0.3337,4.7409,0.4999,4.6269
246 | -15.9956,4.7539,128.0000,0.3172,4.3755,0.6627,5.0951
247 | -15.9340,4.4252,128.5000,0.3204,4.3755,0.8300,4.8677
248 | -15.8193,4.0529,129.0001,0.3243,4.3755,1.1297,4.6025
249 | -15.6574,3.6913,129.5001,0.2939,3.8748,1.3390,5.1661
250 | -15.4572,3.3666,130.0000,0.2968,3.5482,1.4278,4.8984
251 | -15.2201,3.0548,130.5006,0.3005,4.1521,1.1525,4.5744
252 | -14.9349,2.7526,131.0001,0.2616,4.6781,0.9383,5.1614
253 | -14.6025,2.4790,131.5001,0.2634,4.6781,0.5791,4.7969
254 | -14.2592,2.2438,132.0001,0.2243,4.6781,0.4421,5.3745
255 | -13.8664,2.0314,132.5001,0.2245,4.7819,0.3173,5.0044
256 | -13.4772,1.8557,133.0001,0.2251,4.7819,0.1353,4.5958
257 | -13.1022,1.7190,133.5001,0.1822,4.7905,0.5126,5.1764
258 | -12.6909,1.6093,134.0001,0.1799,4.7905,0.3393,4.7643
259 | -12.2736,1.5224,134.5001,0.1412,4.7905,0.5698,5.2910
260 | -11.8422,1.4622,135.0000,0.1380,4.7987,0.5140,4.8819
261 | -11.4408,1.4218,135.5000,0.1054,4.7987,0.5880,5.4317
262 | -11.0580,1.4002,136.0000,0.1014,4.6995,0.6306,5.0249
263 | -10.6434,1.3876,136.5000,0.0987,4.6995,0.5884,4.6258
264 | -10.2071,1.3869,137.0000,0.0687,4.5996,0.6999,5.1316
265 | -9.7988,1.4001,137.5000,0.0180,2.2322,9.8669,9.8669
266 | -9.3809,1.4203,138.0001,0.0157,2.2322,9.4648,9.4648
267 | -8.9470,1.4383,138.5000,0.0142,2.2322,9.0286,9.0286
268 | -8.5272,1.4540,139.0000,0.0131,2.2322,8.6067,8.6067
269 | -8.1096,1.4693,139.5000,0.0123,2.2322,8.1809,8.1809
270 | -7.6780,1.4853,140.0000,0.0117,2.2322,7.7702,7.7702
271 | -7.2785,1.5009,140.5000,0.0112,2.2322,7.3555,7.3555
272 | -6.9110,1.5164,141.0001,0.0109,2.2322,6.9879,6.9879
273 | -6.4659,1.5364,141.5000,0.0106,2.2322,6.5584,6.5584
274 | -6.0527,1.5563,142.0000,0.0104,2.2322,6.1258,6.1258
275 | -5.6116,1.5789,142.5000,0.0102,2.2322,5.6983,5.6983
276 | -5.2194,1.6004,143.0000,0.0100,2.2322,5.2787,5.2787
277 | -4.7825,1.6259,143.5002,0.0098,2.2322,4.8511,4.8511
278 | -4.3856,1.6506,144.0000,0.0096,2.2322,4.4544,4.4544
279 | -3.9628,1.6784,144.5001,0.0093,2.2322,4.0358,4.0358
280 | -3.5630,1.7062,145.0001,0.0090,2.2322,3.6421,3.6421
281 | -3.1135,1.7392,145.5001,0.0085,2.2322,3.1924,3.1924
282 | -2.6960,1.7713,146.0001,-0.0050,2.2322,2.7717,3.7108
283 | -2.2650,1.8065,146.5001,-0.0068,2.2322,2.3303,3.2695
284 | -1.8707,1.8369,147.0001,-0.0132,2.2322,1.9139,3.7936
285 | -1.4212,1.8695,147.5001,-0.0143,2.2322,1.5153,3.3951
286 | -1.0154,1.8956,148.0001,-0.0157,2.2322,1.0768,3.8975
287 | -0.5654,1.9209,148.5000,-0.0161,2.2322,0.6298,3.4500
288 | -0.1383,1.9414,149.0001,-0.0161,2.2322,0.2095,3.0255
289 | 0.2569,1.9573,149.5001,-0.0141,2.2322,0.2165,3.5546
290 | 0.6831,1.9713,150.0000,-0.0136,2.8455,0.3282,3.1512
291 | 1.1294,1.9831,150.5001,-0.0107,2.8455,0.1201,3.6483
292 | 1.5827,1.9921,151.0001,-0.0082,3.2652,0.3807,4.1466
293 | 2.0039,1.9988,151.5001,-0.0077,3.2652,0.0589,3.7084
294 | 2.4191,2.0044,152.0000,-0.0057,3.6995,0.4703,4.2367


--------------------------------------------------------------------------------
/csv/tmp02_metrics.csv:
--------------------------------------------------------------------------------
1 | MAX_LAT_DISTANCE,9.9071
2 | AVG_LAT_DISTANCE,0.6500
3 | lookahead_min,1.5
4 | lookahead_max,2.5
5 | 


--------------------------------------------------------------------------------
/csv/tmp02_xypose.csv:
--------------------------------------------------------------------------------
  1 | x,y,time,steering_angle,speed_mps,cur_lat_dist_abs,trg_way_lon_dist
  2 | 0.0000,2.0000,6.0000,0.0000,0.0000,0.0000,1.8829
  3 | 0.0000,2.0000,6.5013,0.0000,0.0000,0.0000,1.8829
  4 | 0.2105,2.0007,7.0000,0.0043,2.2322,0.1152,1.7677
  5 | 0.6273,2.0031,7.5000,0.0015,2.8455,0.3910,2.2740
  6 | 1.0673,2.0061,8.0000,0.0003,2.8455,0.0500,2.7746
  7 | 1.4934,2.0090,8.4999,-0.0005,3.2652,0.4665,2.3495
  8 | 1.8094,2.0110,9.0012,-0.0004,3.2652,0.1295,2.9540
  9 | 2.1645,2.0131,9.5000,-0.0008,3.2652,0.1896,2.6350
 10 | 2.5835,2.0154,10.0000,-0.0014,3.6995,0.3368,2.2199
 11 | 3.0136,2.0177,10.5000,-0.0007,3.6995,0.1004,2.7246
 12 | 3.4286,2.0194,11.0000,-0.0009,4.0112,0.4143,2.2975
 13 | 3.8346,2.0211,11.5000,-0.0004,4.0112,0.0073,2.8312
 14 | 4.2716,2.0229,12.0000,-0.0005,4.0112,0.4287,2.3962
 15 | 4.6796,2.0247,12.5000,-0.0001,4.4884,0.0939,2.9190
 16 | 5.0966,2.0264,13.0000,-0.0001,4.4884,0.2962,2.5290
 17 | 5.5266,2.0282,13.5000,0.0001,4.7998,0.2086,3.0338
 18 | 5.9255,2.0299,14.0000,0.0002,4.7998,0.1915,2.6338
 19 | 6.3125,2.0315,14.5000,0.0004,4.7998,0.3522,3.1778
 20 | 6.7405,2.0333,15.0001,0.0005,4.7998,0.0769,2.7488
 21 | 7.1425,2.0349,15.5001,0.0005,4.7998,0.4709,3.2968
 22 | 7.5775,2.0367,16.0000,0.0007,4.7998,0.0420,2.8678
 23 | 7.9985,2.0385,16.5000,0.0008,4.7998,0.3770,3.3907
 24 | 8.3965,2.0402,17.0000,0.0010,4.7998,0.1338,2.9597
 25 | 8.7875,2.0418,17.5000,0.0013,4.7998,0.2432,2.5827
 26 | 9.1805,2.0434,18.0000,0.0010,4.7998,0.3288,3.1543
 27 | 9.6095,2.0455,18.5000,0.0012,4.7998,0.1242,2.7013
 28 | 10.0304,2.0475,19.0000,0.0009,4.7998,0.4119,3.2362
 29 | 10.4604,2.0498,19.5000,0.0010,4.7998,0.0202,2.8042
 30 | 10.8784,2.0520,20.0000,0.0007,4.7998,0.4401,3.3239
 31 | 11.2944,2.0545,20.5000,0.0008,4.7997,0.0709,2.8929
 32 | 11.7084,2.0569,21.0001,0.0010,4.7997,0.3172,2.5049
 33 | 12.0703,2.0590,21.5000,0.0007,4.7996,0.2654,3.0841
 34 | 12.4863,2.0616,22.0004,0.0009,4.7996,0.1676,2.6511
 35 | 12.9003,2.0641,22.5001,0.0007,4.7994,0.3672,3.1824
 36 | 13.2673,2.0664,23.0003,0.0009,4.7994,0.0026,2.8134
 37 | 13.6873,2.0689,23.5002,-0.0005,4.7994,0.4248,3.3435
 38 | 14.0853,2.0713,24.0001,-0.0007,4.7993,0.1199,2.9485
 39 | 14.4673,2.0736,24.5000,-0.0009,4.7993,0.2741,2.5545
 40 | 14.8473,2.0759,25.0000,-0.0215,4.8000,0.2940,3.1291
 41 | 15.2754,2.0794,25.5000,-0.0247,4.8000,0.1219,2.7135
 42 | 15.6915,2.0776,26.0000,-0.1165,4.7859,0.3964,3.0935
 43 | 16.0990,2.0740,26.5003,-0.1314,4.7859,0.0090,2.7049
 44 | 16.5232,2.0442,27.0000,-0.2278,4.7859,0.4535,3.0622
 45 | 16.9678,1.9832,27.5000,-0.2456,4.5966,0.1081,2.6495
 46 | 17.3865,1.8729,28.0000,-0.2432,4.5966,0.3940,3.1313
 47 | 17.7347,1.7349,28.5000,-0.2451,3.8288,0.3203,2.7629
 48 | 18.1333,1.5213,29.0001,-0.2446,3.8288,0.4665,2.3180
 49 | 18.4957,1.2679,29.5000,-0.1337,4.0942,0.2711,2.8256
 50 | 18.8154,0.9869,30.0000,-0.1098,4.0942,0.4506,2.4214
 51 | 19.1291,0.6901,30.5000,0.0413,4.7339,0.1495,2.8577
 52 | 19.4225,0.3938,31.0000,0.1130,4.7339,0.3154,3.3351
 53 | 19.7102,0.1241,31.5000,0.1353,4.3046,0.2202,2.9536
 54 | 20.0635,-0.1565,32.0000,0.1527,4.3046,0.2197,2.5486
 55 | 20.3859,-0.3714,32.5000,0.1401,4.0835,0.3245,3.0728
 56 | 20.7215,-0.5567,33.0000,0.1417,4.0835,0.1421,2.7164
 57 | 21.0835,-0.7257,33.5000,0.0318,4.7249,0.4343,4.1495
 58 | 21.4656,-0.8714,34.0000,0.0201,4.7249,0.0460,3.7587
 59 | 21.8717,-1.0246,34.5000,0.0120,4.7249,0.3953,3.3209
 60 | 22.2580,-1.1714,35.0000,0.0044,4.7724,0.1252,2.8839
 61 | 22.6208,-1.3104,35.5007,-0.0020,4.7724,0.2854,2.5086
 62 | 23.0231,-1.4668,36.0000,-0.0607,4.2880,0.2901,3.9297
 63 | 23.4195,-1.6220,36.5000,-0.0690,4.2880,0.2716,3.5133
 64 | 23.8405,-1.8065,37.0000,-0.0744,4.2880,0.6733,3.0727
 65 | 24.2045,-1.9839,37.5007,-0.0777,4.5457,0.8070,2.6692
 66 | 24.5603,-2.1741,38.0000,-0.0426,4.5457,0.3825,4.0837
 67 | 24.9452,-2.4022,38.5000,-0.0395,4.5457,0.1001,3.6587
 68 | 25.3094,-2.6255,39.0000,-0.0370,4.5457,0.5124,3.2077
 69 | 25.6858,-2.8634,39.5000,-0.0343,4.5457,0.9309,2.7867
 70 | 26.0337,-3.0898,40.0000,0.0086,4.6811,0.5097,3.3070
 71 | 26.3763,-3.3199,40.5000,0.0166,4.6811,0.1015,2.8934
 72 | 26.7306,-3.5522,41.0000,0.0850,4.6811,0.3142,3.3593
 73 | 27.1071,-3.7938,41.5000,0.0981,4.6811,0.7739,2.9053
 74 | 27.4414,-3.9859,42.0000,0.1044,4.4961,0.7008,2.5408
 75 | 27.7643,-4.1508,42.5000,0.2028,4.4961,0.3148,2.9487
 76 | 28.1583,-4.3307,43.0000,0.2161,4.4961,0.1793,2.5535
 77 | 28.5560,-4.4613,43.5000,0.3016,4.1596,0.4579,2.9058
 78 | 28.9461,-4.5467,44.0000,0.3132,4.1596,0.2354,2.5350
 79 | 29.3704,-4.5724,44.5000,0.3773,4.1596,0.5190,2.8547
 80 | 29.7777,-4.5361,45.0014,0.3872,4.0064,0.3073,2.5035
 81 | 30.1613,-4.4315,45.5001,0.3891,4.0064,0.5383,2.9339
 82 | 30.5321,-4.2574,46.0000,0.3945,4.0020,0.3306,2.5858
 83 | 30.8707,-4.0229,46.5008,0.3374,4.0020,0.5376,3.1060
 84 | 31.1695,-3.7207,47.0000,0.3344,3.9798,0.3068,2.6974
 85 | 31.4186,-3.3976,47.5000,0.2601,4.4205,0.4942,3.2374
 86 | 31.6339,-3.0213,48.0000,0.2493,4.4205,0.1801,2.8250
 87 | 31.8101,-2.6500,48.5007,0.1795,4.4205,0.4255,3.3557
 88 | 31.9707,-2.2506,49.0000,0.1795,4.4205,0.1043,2.9125
 89 | 32.1098,-1.8441,49.5000,0.1077,4.7777,0.3328,3.4330
 90 | 32.2359,-1.4286,50.0000,0.0889,4.7907,0.1891,3.0116
 91 | 32.3385,-1.0538,50.5001,0.0701,4.7907,0.2138,2.6091
 92 | 32.4425,-0.6409,51.0000,0.0701,4.7907,0.2929,3.1177
 93 | 32.5384,-0.2283,51.5000,0.0152,4.7994,0.1079,2.7200
 94 | 32.6286,0.1577,52.0000,0.0024,4.7895,0.4271,3.2532
 95 | 32.7154,0.5203,52.5000,-0.0059,4.7895,0.0655,2.8729
 96 | 32.8149,0.9201,53.0000,0.0174,4.7895,0.3445,3.4088
 97 | 32.9076,1.2748,53.5000,0.0163,4.7797,0.1827,2.9790
 98 | 32.9869,1.5833,54.0000,0.0149,4.7797,0.1267,2.6993
 99 | 33.0915,1.9968,54.5000,0.1416,4.7846,0.4191,3.0260
100 | 33.1926,2.3812,55.0001,0.1585,4.7846,0.0890,2.6477
101 | 33.2588,2.7171,55.5000,0.1585,4.7846,0.3544,3.1427
102 | 33.3028,3.0512,56.0002,0.2185,4.5051,0.3081,2.8230
103 | 33.3141,3.4678,56.5000,0.2185,4.5051,0.2611,3.3252
104 | 33.2711,3.8818,57.0013,0.1027,3.5962,0.4821,2.9125
105 | 33.2140,4.2774,57.5002,0.0912,3.5962,0.4084,2.4906
106 | 33.1423,4.6861,58.0000,-0.0461,4.6685,0.3423,2.9658
107 | 33.0546,5.1158,58.5000,-0.0793,4.6685,0.1565,2.5362
108 | 32.9857,5.5401,58.9999,-0.0751,3.5918,0.4081,3.0499
109 | 32.9444,5.9370,59.5000,-0.0842,3.5918,0.2655,2.6409
110 | 32.9191,6.3833,60.0000,-0.0921,4.3340,0.4038,2.2241
111 | 32.9151,6.7956,60.5000,0.0222,4.3340,0.1578,2.6966
112 | 32.9333,7.1851,61.0000,0.1208,4.2726,0.4590,3.1405
113 | 32.9481,7.5748,61.5000,0.1377,4.2726,0.1209,2.7838
114 | 32.9347,7.9847,62.0000,0.2012,4.2726,0.3454,3.2243
115 | 32.8948,8.3768,62.5000,0.2130,4.2168,0.2521,2.8378
116 | 32.8134,8.7647,63.0000,0.2182,4.2168,0.3022,2.4605
117 | 32.6973,9.1245,63.5000,0.2554,4.2465,0.3690,2.9553
118 | 32.5458,9.4764,64.0000,0.2597,4.2465,0.2410,2.5894
119 | 32.3545,9.8097,64.5000,0.2774,4.2698,0.4794,3.0756
120 | 32.1067,10.1457,65.0007,0.2792,4.2698,0.2196,2.6686
121 | 31.8285,10.4390,65.5000,0.2742,4.2698,0.4805,3.1598
122 | 31.5111,10.7017,66.0008,0.2728,4.3037,0.2215,2.7613
123 | 31.1506,10.9357,66.5000,0.2343,4.3037,0.4307,3.2778
124 | 30.7746,11.1192,67.0000,0.2266,4.3041,0.2335,2.8491
125 | 30.4018,11.2646,67.5000,0.2178,4.3041,0.3817,2.4545
126 | 29.9766,11.3916,68.0014,0.1580,4.4602,0.2266,2.9546
127 | 29.5729,11.4743,68.5002,0.1402,4.4602,0.3149,2.5433
128 | 29.1730,11.5379,69.0002,0.0944,4.7655,0.2861,3.0879
129 | 28.7299,11.5938,69.5000,0.0735,4.7655,0.2002,2.6509
130 | 28.3214,11.6362,70.0000,0.0434,4.7914,0.3632,3.1786
131 | 27.8951,11.6727,70.5000,0.0278,4.7914,0.1140,2.7474
132 | 27.4948,11.7049,71.0000,0.0145,4.7948,0.4314,3.2483
133 | 27.1247,11.7344,71.5002,0.0075,4.7948,0.1192,2.9063
134 | 26.7037,11.7696,72.0000,-0.0009,4.7948,0.3368,2.5011
135 | 26.3265,11.8028,72.5000,0.0007,4.7993,0.2420,3.0518
136 | 25.8959,11.8437,73.0001,-0.0031,4.7993,0.2123,2.6304
137 | 25.4835,11.8848,73.5003,0.0007,4.7960,0.3235,3.1402
138 | 25.0855,11.9267,74.0006,-0.0004,4.7960,0.1030,2.7480
139 | 24.6598,11.9722,74.5000,0.0026,4.7946,0.4597,3.2812
140 | 24.2701,12.0144,75.0000,0.0023,4.7946,0.0631,2.8642
141 | 23.8357,12.0611,75.5000,0.0037,4.7946,0.3829,3.3857
142 | 23.4292,12.1047,76.0000,0.0036,4.7956,0.1537,2.9748
143 | 23.0305,12.1468,76.5000,0.0034,4.7956,0.2591,2.5670
144 | 22.6238,12.1892,77.0001,0.0038,4.7966,0.2617,3.0855
145 | 22.1833,12.2344,77.5006,0.0036,4.7966,0.1696,2.6556
146 | 21.7615,12.2770,78.0000,0.0032,4.7975,0.3519,3.1762
147 | 21.3696,12.3160,78.5000,0.0029,4.7975,0.0538,2.7713
148 | 20.9735,12.3549,79.0000,0.0023,4.7975,0.4521,3.3139
149 | 20.5387,12.3971,79.5000,0.0021,4.7985,0.0607,2.8849
150 | 20.1087,12.4385,80.0000,0.0017,4.7985,0.3674,3.3986
151 | 19.7046,12.4771,80.5002,0.0015,4.7995,0.1561,2.9806
152 | 19.3005,12.5156,81.0000,0.0013,4.7995,0.2529,2.5717
153 | 18.9790,12.5460,81.5000,0.0013,4.7995,0.3537,3.1781
154 | 18.5698,12.5846,82.0000,0.0012,4.7995,0.0303,2.7942
155 | 18.2194,12.6175,82.5000,0.0013,4.7995,0.4302,3.3357
156 | 17.8371,12.6534,83.0000,0.0012,4.7985,0.1653,2.9897
157 | 17.4976,12.6850,83.5000,0.0012,4.7985,0.1807,2.6438
158 | 17.1103,12.7210,84.0004,0.0015,4.7975,0.3759,3.2002
159 | 16.7121,12.7578,84.5001,0.0015,4.7975,0.0093,2.8153
160 | 16.2899,12.7966,85.0000,0.0019,4.7975,0.4510,3.3148
161 | 15.9075,12.8315,85.5000,0.0019,4.7966,0.1165,2.9409
162 | 15.4873,12.8694,86.0000,0.0018,4.7966,0.3095,2.5150
163 | 15.0750,12.9063,86.5001,0.0021,4.7956,0.2381,3.0625
164 | 14.6687,12.9424,87.0001,0.0021,4.7956,0.1759,2.6486
165 | 14.2882,12.9759,87.5061,0.0021,4.7956,0.3527,3.1771
166 | 13.9784,13.0028,88.0000,0.0021,4.7952,0.0298,2.8541
167 | 13.7065,13.0264,88.5000,0.0020,4.7952,0.2323,2.5922
168 | 13.3130,13.0601,89.0003,0.0018,4.7961,0.3263,3.1508
169 | 12.9544,13.0906,89.5000,0.0017,4.7961,0.0347,2.7898
170 | 12.4980,13.1290,90.0000,0.0014,4.7973,0.4699,3.2944
171 | 12.0805,13.1638,90.5000,0.0013,4.7973,0.0550,2.8794
172 | 11.6580,13.1989,91.0002,0.0009,4.7973,0.3860,3.3801
173 | 11.2464,13.2328,91.5000,0.0009,4.7985,0.1424,2.9671
174 | 10.8328,13.2670,92.0000,0.0011,4.7985,0.2896,2.5350
175 | 10.3943,13.3034,92.5000,0.0005,4.7997,0.2289,3.0535
176 | 9.9937,13.3364,93.0000,0.0005,4.7997,0.1971,2.6275
177 | 9.5910,13.3696,93.5000,-0.0001,4.7990,0.3714,3.1959
178 | 9.2552,13.3973,94.0000,-0.0002,4.7990,0.0284,2.8529
179 | 8.8715,13.4290,94.5000,-0.0011,4.7990,0.3556,3.4100
180 | 8.4907,13.4604,95.0009,-0.0012,4.7975,0.1970,3.0210
181 | 8.0840,13.4943,95.5000,-0.0013,4.7975,0.1901,2.6339
182 | 7.6405,13.5315,96.0000,0.0007,4.5954,0.3033,3.1288
183 | 7.2270,13.5665,96.5000,0.0011,4.5954,0.1397,2.6858
184 | 6.8166,13.6013,97.0003,0.0399,4.1942,0.3998,3.1830
185 | 6.3942,13.6384,97.5000,0.0457,4.1942,0.0307,2.7539
186 | 5.9733,13.6657,98.0000,0.1600,4.1942,0.4284,3.1303
187 | 5.5499,13.6864,98.5000,0.1778,3.7773,0.0789,2.7106
188 | 5.1061,13.6679,99.0000,0.1859,3.7773,0.3591,2.2900
189 | 4.6965,13.6097,99.5000,0.2668,1.5000,0.2421,2.7235
190 | 4.2932,13.5165,100.0013,0.2796,1.5000,0.3356,2.3171
191 | 3.9065,13.3697,100.5000,0.2821,3.6441,0.3755,1.9137
192 | 3.5607,13.1831,101.0000,0.2505,3.6441,0.4454,2.4333
193 | 3.2383,12.9505,101.5003,0.1891,4.3783,0.2912,2.9616
194 | 2.9494,12.6879,102.0000,0.1768,4.3783,0.2964,2.6023
195 | 2.6452,12.3762,102.5001,0.0862,4.7871,0.3018,3.0915
196 | 2.3832,12.0811,103.0000,0.0629,4.7871,0.1431,2.7110
197 | 2.1107,11.7636,103.5000,-0.0776,4.7756,0.4036,3.0926
198 | 1.8240,11.4204,104.0000,-0.1125,4.7756,0.0574,2.6657
199 | 1.5372,11.1156,104.5000,-0.2237,4.7756,0.4588,3.0454
200 | 1.2111,10.8138,105.0000,-0.2514,4.4335,0.0839,2.6495
201 | 0.8458,10.5552,105.5001,-0.2645,4.4335,0.4018,3.1299
202 | 0.4948,10.3707,106.0000,-0.2732,3.8239,0.2446,2.7354
203 | 0.0745,10.2175,106.5000,-0.2772,3.8239,0.4435,2.3283
204 | -0.3562,10.1262,107.0000,-0.2203,1.5000,0.2115,2.8431
205 | -0.7884,10.0978,107.5000,-0.2086,1.5000,0.4437,2.3943
206 | -1.2379,10.1070,108.0000,-0.1938,3.7055,0.1262,1.9750
207 | -1.6591,10.1484,108.5000,-0.1118,3.7055,0.3560,2.4740
208 | -2.0563,10.2099,109.0000,-0.0572,4.7656,0.1896,3.0054
209 | -2.4674,10.2848,109.5000,-0.0324,4.7656,0.2317,2.6045
210 | -2.8932,10.3665,110.0000,-0.0084,4.7867,0.2876,3.1027
211 | -3.2966,10.4438,110.5000,0.0068,4.7867,0.1556,2.6989
212 | -3.6970,10.5164,111.0000,0.0070,4.7849,0.4015,3.2150
213 | -4.1061,10.5839,111.5000,0.0135,4.7849,0.1069,2.8030
214 | -4.5258,10.6484,112.0000,0.0059,4.7849,0.4514,3.3286
215 | -4.9293,10.7049,112.5000,0.0077,4.7994,0.1375,2.9164
216 | -5.2766,10.7518,113.0000,0.0093,4.7994,0.2717,2.5729
217 | -5.6737,10.8035,113.5002,-0.0001,4.7891,0.2923,3.1035
218 | -6.0687,10.8524,114.0000,-0.0007,4.7891,0.1399,2.7116
219 | -6.4955,10.9054,114.5000,-0.0053,4.7966,0.3941,3.2136
220 | -6.9255,10.9585,115.0000,-0.0061,4.7966,0.0605,2.7964
221 | -7.3374,11.0108,115.5000,-0.0074,4.7966,0.4685,3.2994
222 | -7.7681,11.0669,116.0000,-0.0077,4.0921,0.0735,2.8922
223 | -8.1996,11.1249,116.5011,-0.0078,4.0921,0.3848,2.4399
224 | -8.6010,11.1805,117.0000,0.0269,3.7943,0.1444,2.9604
225 | -9.0389,11.2440,117.5000,0.0326,3.7943,0.2801,2.5371
226 | -9.4875,11.3012,118.0000,0.1419,2.7973,0.2260,2.9134
227 | -9.9095,11.3507,118.5000,0.1629,2.7973,0.2230,2.4766
228 | -10.3376,11.3667,119.0000,0.1732,1.5000,0.3077,2.0683
229 | -10.7602,11.3441,119.5002,0.1773,1.5000,0.1813,1.6362
230 | -11.1436,11.2902,120.0001,0.3558,3.8245,0.4231,2.0558
231 | -11.5524,11.2050,120.5000,0.3460,3.8245,0.2283,2.5866
232 | -11.9123,11.0589,121.0000,0.2790,3.9986,0.4533,3.1645
233 | -12.2447,10.8497,121.5000,0.2739,3.9986,0.1633,2.7708
234 | -12.5713,10.5870,122.0001,0.2687,3.9986,0.4548,2.3713
235 | -12.8731,10.2829,122.5000,0.1741,4.0684,0.0741,2.8991
236 | -13.1341,9.9508,123.0003,0.1512,4.0684,0.3571,2.4693
237 | -13.3843,9.5979,123.5000,0.0822,4.3872,0.1594,2.9829
238 | -13.6236,9.2375,124.0000,0.0563,4.3872,0.2599,2.5662
239 | -13.8513,8.8824,124.5000,0.0203,4.7869,0.2553,3.0770
240 | -14.0673,8.5394,125.0000,0.0029,4.7869,0.1494,2.6750
241 | -14.3055,8.1680,125.5000,-0.0039,4.7975,0.3633,3.1810
242 | -14.5395,7.8142,126.0002,-0.0130,4.7975,0.0956,2.7565
243 | -14.8006,7.4301,126.5000,-0.0026,4.7895,0.4272,3.2475
244 | -15.0281,7.1066,127.0001,-0.0047,4.7895,0.0907,2.8410
245 | -15.2780,6.7549,127.5004,0.0398,4.7895,0.4333,3.3022
246 | -15.5188,6.4218,128.0001,0.0445,4.7740,0.1384,2.8946
247 | -15.7519,6.0845,128.5000,0.1377,4.7740,0.3201,3.2825
248 | -15.9797,5.7455,129.0002,0.1505,4.7409,0.2437,2.8850
249 | -16.1950,5.3664,129.5000,0.2575,4.7409,0.2339,3.1480
250 | -16.3759,4.9842,130.0000,0.2719,4.3755,0.3648,2.7601
251 | -16.4980,4.5951,130.5000,0.3981,4.3755,0.2847,2.8471
252 | -16.5715,4.1829,131.0000,0.4145,3.8748,0.4903,2.5154
253 | -16.5608,3.7804,131.5000,0.4807,3.8748,0.4504,2.7523
254 | -16.4705,3.3801,132.0001,0.4963,3.8427,0.5337,2.4214
255 | -16.3005,3.0243,132.5000,0.4614,3.8427,0.5509,2.9252
256 | -16.0807,2.7321,133.0004,0.4709,3.5482,0.5418,2.6134
257 | -15.7725,2.4489,133.5000,0.4825,3.5482,0.5836,2.2453
258 | -15.4069,2.2265,134.0000,0.3737,4.1521,0.2860,2.7828
259 | -14.9899,2.0912,134.5000,0.2765,4.1521,0.4742,3.3125
260 | -14.5884,2.0038,135.0000,0.2642,4.6781,0.1006,2.8923
261 | -14.1560,1.9489,135.5000,0.1847,4.6781,0.3429,3.4273
262 | -13.7244,1.9155,136.0000,0.1643,4.7819,0.1718,2.9947
263 | -13.3057,1.9028,136.5001,0.1453,4.7819,0.2694,2.5619
264 | -12.9225,1.9072,137.0000,0.0822,4.7905,0.3010,3.1210
265 | -12.5051,1.9253,137.5001,0.0569,4.7905,0.1450,2.6853
266 | -12.0871,1.9499,138.0000,0.0217,4.7987,0.3965,3.2206
267 | -11.6848,1.9769,138.5000,0.0057,4.7987,0.0099,2.8196
268 | -11.3396,1.9979,139.0000,-0.0032,4.7987,0.3402,3.4271
269 | -10.9298,2.0181,139.5000,-0.0121,4.6995,0.1820,2.9997
270 | -10.5659,2.0318,140.0000,-0.0194,4.6995,0.2043,2.6311
271 | -10.2067,2.0408,140.5002,-0.0124,4.5996,0.4285,3.2474
272 | -9.8259,2.0444,141.0003,-0.0002,2.2322,9.9071,9.9071
273 | -9.3838,2.0438,141.5000,-0.0001,2.2322,9.4699,9.4699
274 | -8.9638,2.0429,142.0000,-0.0002,2.2322,9.0619,9.0619
275 | -8.5258,2.0418,142.4999,-0.0003,2.2322,8.6179,8.6179
276 | -8.0948,2.0406,143.0000,-0.0004,2.2322,8.1959,8.1959
277 | -7.6588,2.0392,143.5000,-0.0004,2.2322,7.7599,7.7599
278 | -7.2437,2.0379,144.0014,-0.0005,2.2322,7.3128,7.3128
279 | -6.7957,2.0365,144.5000,-0.0006,2.2322,6.8998,6.8998
280 | -6.3777,2.0351,145.0000,-0.0006,2.2322,6.4518,6.4518
281 | -5.9757,2.0338,145.5000,-0.0007,2.2322,6.0528,6.0528
282 | -5.5807,2.0325,146.0000,-0.0008,2.2322,5.6468,5.6468
283 | -5.1498,2.0310,146.5000,-0.0010,2.2322,5.2228,5.2228
284 | -4.7238,2.0296,147.0000,-0.0012,2.2322,4.7908,4.7908
285 | -4.3117,2.0282,147.5000,-0.0013,2.2322,4.3898,4.3898
286 | -3.8736,2.0265,148.0000,-0.0013,2.2322,3.9577,3.9577
287 | -3.4516,2.0245,148.5000,-0.0013,2.2322,3.5417,3.5417
288 | -3.0055,2.0221,149.0013,-0.0013,2.2322,3.1006,3.1006
289 | -2.5845,2.0196,149.5003,-0.0013,2.2322,2.6806,2.6806
290 | -2.1485,2.0167,150.0000,-0.0012,2.2322,2.2345,2.2345
291 | -1.7694,2.0140,150.5000,-0.0011,2.2322,1.8475,1.8475
292 | -1.3344,2.0106,151.0000,0.0036,2.2322,1.4235,2.3648
293 | -0.9216,2.0070,151.5001,0.0049,2.2322,0.9866,1.9280
294 | -0.4869,2.0042,152.0016,0.0052,2.2322,0.5609,2.4438
295 | -0.0651,2.0027,152.5000,0.0055,2.2322,0.1491,2.0320
296 | 0.3446,2.0023,153.0001,0.0038,2.2322,0.2617,2.5626
297 | 0.7695,2.0033,153.5000,0.0033,2.8455,0.2549,2.1378
298 | 1.2134,2.0050,154.0000,0.0017,2.8455,0.1960,2.6285
299 | 1.6384,2.0073,154.5000,0.0011,3.2652,0.3095,2.1925
300 | 2.0544,2.0097,155.0072,0.0011,3.2652,0.1155,2.7090
301 | 2.4214,2.0119,155.5000,-0.0000,3.6995,0.4659,2.3490
302 | 


--------------------------------------------------------------------------------
/csv/tmp03_metrics.csv:
--------------------------------------------------------------------------------
1 | MAX_LAT_DISTANCE,9.9566
2 | AVG_LAT_DISTANCE,0.9162
3 | lookahead_min,1.5
4 | lookahead_max,6.5


--------------------------------------------------------------------------------
/csv/tmp03_xypose.csv:
--------------------------------------------------------------------------------
  1 | x,y,time,steering_angle,speed_mps,cur_lat_dist_abs,trg_way_lon_dist
  2 | 0.0000,2.0000,7.5000,0.0000,0.0000,0.0000,3.7659
  3 | 0.0195,2.0000,8.0000,0.0024,2.2322,0.0005,3.7655
  4 | 0.3746,2.0009,8.4999,0.0018,2.2322,0.2986,4.4088
  5 | 0.7936,2.0022,9.0012,0.0016,2.8455,0.2348,4.0009
  6 | 1.2415,2.0039,9.5000,0.0012,2.8455,0.2191,4.4886
  7 | 1.6635,2.0057,10.0000,0.0008,3.2652,0.3084,5.0163
  8 | 2.1135,2.0077,10.4999,0.0007,3.2652,0.1306,5.5190
  9 | 2.4396,2.0091,11.0000,0.0008,3.6995,0.4388,5.1470
 10 | 2.8166,2.0106,11.5000,0.0007,3.6995,0.0818,5.7318
 11 | 3.2226,2.0121,11.9999,0.0007,3.6995,0.3123,6.2798
 12 | 3.6066,2.0136,12.5000,0.0007,4.0112,0.2234,5.8738
 13 | 3.9816,2.0149,12.9999,0.0007,4.0112,0.1177,6.4748
 14 | 4.4206,2.0165,13.5000,0.0008,4.4884,0.3689,6.0198
 15 | 4.8006,2.0179,14.0000,0.0008,4.4884,0.0463,6.5467
 16 | 5.1616,2.0191,14.5000,0.0007,4.4884,0.3872,7.1472
 17 | 5.5596,2.0205,14.9999,0.0008,4.7998,0.1856,6.7782
 18 | 5.9196,2.0218,15.4999,0.0008,4.7998,0.2055,7.3281
 19 | 6.3526,2.0233,15.9999,0.0009,4.7998,0.3172,6.9091
 20 | 6.7846,2.0248,16.4999,0.0008,4.7998,0.1071,7.4248
 21 | 7.2086,2.0263,16.9999,0.0009,4.7998,0.4111,7.0008
 22 | 7.6296,2.0278,17.4999,0.0011,4.7998,0.0125,6.5988
 23 | 8.0095,2.0291,18.0000,0.0009,4.7998,0.4211,7.1070
 24 | 8.4705,2.0309,18.5000,0.0011,4.7998,0.0854,6.6710
 25 | 8.8745,2.0325,19.0000,0.0009,4.7998,0.3293,7.1943
 26 | 9.2635,2.0343,19.5000,0.0010,4.7998,0.2401,6.8213
 27 | 9.6945,2.0363,20.0000,0.0007,4.7998,0.2004,7.3345
 28 | 10.0885,2.0381,20.5000,0.0007,4.7998,0.3361,6.9285
 29 | 10.4875,2.0398,21.0001,0.0008,4.7998,0.0486,6.5455
 30 | 10.9215,2.0418,21.5000,-0.0033,4.7997,0.4550,7.0489
 31 | 11.3396,2.0437,22.0001,-0.0035,4.7997,0.0475,6.6398
 32 | 11.7658,2.0448,22.5000,-0.0234,4.7997,0.4048,6.9559
 33 | 12.1800,2.0460,22.9999,-0.0242,4.7996,0.1408,6.5604
 34 | 12.5920,2.0415,23.5000,-0.0530,4.7996,0.2728,6.8218
 35 | 13.0003,2.0331,24.0000,-0.0717,4.7994,0.2566,7.1483
 36 | 13.3716,2.0159,24.4999,-0.0729,4.7994,0.1336,6.7810
 37 | 13.8257,1.9776,25.0000,-0.0760,4.7993,0.4010,7.2610
 38 | 14.2398,1.9270,25.5000,-0.0764,4.7993,0.1397,6.8457
 39 | 14.6724,1.8578,26.0000,-0.0637,4.7993,0.5115,7.3500
 40 | 15.0782,1.7774,26.5001,-0.0632,4.8000,0.2924,6.9434
 41 | 15.4916,1.6845,26.9999,-0.0629,4.8000,0.5106,6.5337
 42 | 15.9112,1.5788,27.5006,-0.0486,4.7859,0.5137,7.0297
 43 | 16.3054,1.4680,28.0000,-0.0479,4.7859,0.6494,6.6048
 44 | 16.6939,1.3523,28.5000,-0.0354,4.5966,0.7828,7.1337
 45 | 17.0963,1.2248,29.0000,-0.0347,4.5966,0.8380,6.7272
 46 | 17.4908,1.0956,29.4999,-0.0322,3.8288,0.9924,8.1781
 47 | 17.8915,0.9598,30.0000,-0.0339,4.0942,0.9668,5.8888
 48 | 18.2983,0.8172,30.5000,-0.0314,4.0942,0.7720,7.3303
 49 | 18.7031,0.6694,31.0000,-0.0310,4.7339,0.5703,6.8871
 50 | 19.0763,0.5285,31.4999,-0.0338,4.7339,0.2511,8.3352
 51 | 19.4731,0.3735,32.0000,-0.0337,4.7339,0.3758,7.9174
 52 | 19.8316,0.2280,32.5000,-0.0336,4.3046,0.2709,7.5370
 53 | 20.2499,0.0513,33.0000,-0.0334,4.3046,0.3991,7.1075
 54 | 20.6151,-0.1090,33.5000,-0.0332,4.0835,0.5152,6.6956
 55 | 20.9856,-0.2775,34.0000,-0.0329,4.0835,0.5296,6.2784
 56 | 21.3564,-0.4522,34.5006,-0.0279,4.7249,0.4723,7.7248
 57 | 21.7607,-0.6496,35.0005,-0.0276,4.7249,0.3682,7.2945
 58 | 22.1208,-0.8301,35.5000,-0.0273,4.7724,0.4166,6.8701
 59 | 22.4746,-1.0119,36.0000,-0.0191,4.7724,0.1643,7.4259
 60 | 22.8333,-1.2013,36.5000,-0.0187,4.7724,0.4496,7.0288
 61 | 23.2095,-1.4025,37.0000,-0.0184,4.2880,0.0838,6.5997
 62 | 23.5695,-1.5978,37.5000,0.0001,4.2880,0.3403,7.0583
 63 | 23.9342,-1.7990,38.0000,0.0012,4.2880,0.7526,6.6437
 64 | 24.2839,-1.9903,38.5000,0.0018,4.5457,0.7149,6.2361
 65 | 24.6422,-2.1848,39.0000,0.0330,4.5457,0.2972,6.5157
 66 | 25.0054,-2.3818,39.5010,0.0691,4.5457,0.1315,6.6037
 67 | 25.3943,-2.5836,40.0000,0.1423,4.5457,0.5397,6.7141
 68 | 25.7610,-2.7580,40.5003,0.1705,4.6811,0.9365,6.7265
 69 | 26.1729,-2.9144,41.0000,0.1870,4.6811,0.5604,6.8819
 70 | 26.5678,-3.0232,41.5002,0.1885,4.6811,0.3782,6.5437
 71 | 26.9708,-3.0946,42.0000,0.1948,4.6811,0.5841,6.8668
 72 | 27.3902,-3.1310,42.5001,0.1959,4.6811,0.9491,6.5462
 73 | 27.7840,-3.1309,43.0000,0.1939,4.4961,1.2501,6.9745
 74 | 28.1828,-3.0981,43.5046,0.1948,4.4961,1.2668,6.6599
 75 | 28.5943,-3.0307,44.0000,0.1867,4.4961,1.4560,7.1316
 76 | 28.9958,-2.9309,44.5000,0.1874,4.4961,1.7303,6.8105
 77 | 29.4018,-2.7971,45.0000,0.1759,4.1596,2.0106,7.3319
 78 | 29.8121,-2.6250,45.4999,0.1900,3.9798,2.0901,6.0912
 79 | 30.1775,-2.4425,46.0000,0.1760,4.4205,1.7922,6.6313
 80 | 30.5277,-2.2318,46.5000,0.1660,4.7777,1.5721,7.1522
 81 | 30.8768,-1.9877,47.0000,0.1662,4.7777,1.2286,6.7754
 82 | 31.2025,-1.7290,47.5000,0.1747,4.7777,0.9993,7.0386
 83 | 31.5132,-1.4493,48.0000,0.1754,4.7907,0.8293,6.6789
 84 | 31.8043,-1.1478,48.5001,0.1795,4.7907,0.5274,7.0055
 85 | 32.0553,-0.8502,49.0000,0.1801,4.7907,0.5025,6.6904
 86 | 32.2965,-0.5198,49.5000,0.1588,4.7994,0.3028,7.2578
 87 | 32.5211,-0.1574,50.0000,0.1588,4.7994,0.2054,6.8870
 88 | 32.7261,0.2218,50.5000,0.1590,4.7895,0.3314,6.5010
 89 | 32.8986,0.5929,51.0001,0.1287,4.7895,0.1179,7.0509
 90 | 33.0542,0.9951,51.5000,0.1277,4.7797,0.4809,6.6607
 91 | 33.1924,1.4043,52.0000,0.1092,4.7797,0.1762,7.1882
 92 | 33.3060,1.8015,52.5000,0.1081,4.7797,0.4396,6.7808
 93 | 33.4087,2.2146,53.0027,0.1008,4.7846,0.2018,7.2851
 94 | 33.4943,2.6258,53.5000,0.1001,4.7846,0.3421,6.8781
 95 | 33.5645,3.0336,54.0000,0.0996,4.5051,0.2712,6.5091
 96 | 33.6191,3.4358,54.5000,0.1011,4.5051,0.1810,6.9665
 97 | 33.6536,3.7742,55.0000,0.1008,3.5962,0.4293,6.6377
 98 | 33.6800,4.1670,55.5000,0.0970,3.5962,0.0629,5.3815
 99 | 33.6919,4.5944,56.0000,0.1002,3.5962,0.3721,5.8701
100 | 33.6874,5.0207,56.4999,0.0996,4.6685,0.4903,5.4492
101 | 33.6670,5.4247,57.0000,0.1245,4.6685,0.6172,6.6185
102 | 33.6277,5.8643,57.5000,0.1393,3.5918,0.9248,6.8700
103 | 33.5652,6.2865,58.0000,0.1238,4.3340,0.9278,5.8018
104 | 33.4861,6.6598,58.4999,0.1544,4.3340,0.7384,6.7789
105 | 33.3833,7.0530,59.0000,0.1552,4.2726,0.6952,6.4297
106 | 33.2434,7.4670,59.5000,0.1610,4.2726,0.2953,6.7877
107 | 33.0877,7.8414,60.0000,0.1616,4.2726,0.2244,6.4189
108 | 32.8982,8.2210,60.4999,0.1602,4.2168,0.3812,6.8792
109 | 32.6833,8.5860,61.0000,0.1605,4.2168,0.3375,6.5039
110 | 32.4502,8.9278,61.5000,0.1607,4.2465,0.6323,6.1344
111 | 32.1915,9.2583,62.0000,0.1537,4.2465,0.6155,6.6278
112 | 31.8982,9.5848,62.4999,0.1537,4.2698,0.7859,6.2451
113 | 31.5955,9.8820,63.0000,0.1431,4.2698,0.7807,6.7619
114 | 31.2797,10.1558,63.5000,0.1428,4.3037,0.8173,6.3684
115 | 30.9632,10.4022,64.0000,0.1302,4.3037,0.8099,6.9237
116 | 30.6075,10.6482,64.5001,0.1297,4.3041,0.7401,6.5283
117 | 30.2522,10.8694,65.0005,0.1162,4.3041,0.7181,7.0690
118 | 29.8802,11.0756,65.5001,0.1155,4.4602,0.5334,6.6660
119 | 29.4865,11.2722,65.9999,0.1019,4.4602,0.4985,7.2106
120 | 29.1201,11.4354,66.5000,0.1010,4.7655,0.3210,6.8045
121 | 28.7475,11.5864,67.0000,0.0880,4.7655,0.2207,7.3470
122 | 28.3521,11.7296,67.5000,0.0871,4.7914,0.3551,6.9453
123 | 27.9452,11.8634,68.0000,0.0864,4.7914,0.1088,6.5363
124 | 27.5219,11.9873,68.5001,0.0730,4.7948,0.4957,7.0533
125 | 27.1284,12.0880,69.0000,0.0720,4.7948,0.2582,6.6447
126 | 26.7182,12.1823,69.4999,0.0598,4.7948,0.4836,7.1703
127 | 26.2962,12.2672,70.0000,0.0588,4.7993,0.3930,6.7387
128 | 25.8913,12.3405,70.5000,0.0482,4.7993,0.4819,7.2825
129 | 25.4748,12.4067,71.0000,0.0474,4.7960,0.5128,6.8653
130 | 25.0672,12.4653,71.5000,0.0382,4.7960,0.4899,7.3908
131 | 24.6664,12.5163,72.0000,0.0376,4.7946,0.6208,6.9939
132 | 24.2611,12.5635,72.5001,0.0370,4.7946,0.4966,6.5909
133 | 23.8364,12.6077,73.0009,0.0286,4.7946,0.6613,7.1195
134 | 23.4070,12.6463,73.5000,0.0280,4.7956,0.5135,6.6945
135 | 22.9941,12.6799,74.0000,0.0211,4.7956,0.6114,7.2012
136 | 22.5798,12.7097,74.5000,0.0205,4.7966,0.5341,6.8034
137 | 22.1633,12.7373,75.0000,0.0149,4.7966,0.5424,7.3078
138 | 21.7465,12.7623,75.5001,0.0144,4.7975,0.5596,6.9035
139 | 21.3076,12.7869,76.0000,0.0100,4.7975,0.4868,7.3809
140 | 20.8804,12.8091,76.5000,0.0096,4.7985,0.5818,6.9832
141 | 20.4772,12.8290,77.0000,0.0093,4.7985,0.4253,6.5745
142 | 20.0619,12.8485,77.5000,0.0057,4.7985,0.6113,7.0860
143 | 19.6534,12.8666,78.0000,0.0053,4.7995,0.3922,6.6791
144 | 19.2248,12.8851,78.5000,0.0025,4.7995,0.5069,7.1886
145 | 18.8112,12.9023,79.0000,0.0023,4.7995,0.3798,6.7676
146 | 18.4005,12.9195,79.5000,0.0003,4.7995,0.3956,7.3083
147 | 17.9788,12.9370,80.0000,0.0001,4.7985,0.4115,6.8822
148 | 17.5731,12.9542,80.5000,-0.0012,4.7985,0.3057,7.4091
149 | 17.1673,12.9716,81.0000,-0.0014,4.7975,0.4797,7.0020
150 | 16.7227,12.9912,81.4999,-0.0014,4.7975,0.2364,6.5809
151 | 16.3170,13.0095,82.0000,-0.0024,4.7975,0.4880,7.0990
152 | 15.8734,13.0299,82.5000,-0.0025,4.7966,0.2096,6.6679
153 | 15.4667,13.0492,83.0000,-0.0032,4.7966,0.4025,7.1741
154 | 15.0131,13.0713,83.5007,-0.0032,4.7956,0.2077,6.7270
155 | 14.5875,13.0928,84.0000,-0.0036,4.7956,0.3092,7.2601
156 | 14.1680,13.1146,84.5000,-0.0036,4.7952,0.2898,6.8520
157 | 13.7744,13.1357,85.0025,-0.0039,4.7952,0.1992,7.3702
158 | 13.3559,13.1588,85.5000,-0.0039,4.7961,0.3718,6.9491
159 | 12.9355,13.1828,86.0001,-0.0039,4.7961,0.1061,6.5350
160 | 12.5011,13.2083,86.4999,-0.0040,4.7973,0.4652,7.0489
161 | 12.0867,13.2334,87.0000,-0.0040,4.7973,0.0749,6.6188
162 | 11.6484,13.2607,87.5000,-0.0036,4.7973,0.4003,7.1376
163 | 11.2262,13.2878,88.0000,-0.0035,4.7985,0.1310,6.7115
164 | 10.7930,13.3162,88.5000,0.0044,4.7985,0.3176,7.1744
165 | 10.3882,13.3436,89.0041,0.0047,4.7997,0.2410,6.7847
166 | 9.9774,13.3702,89.5000,0.0303,4.7997,0.1951,7.0676
167 | 9.5688,13.3962,90.0000,0.0315,4.7990,0.3558,6.6754
168 | 9.1467,13.4155,90.5000,0.0590,4.7990,0.0944,6.8777
169 | 8.7397,13.4282,91.0000,0.0783,4.7975,0.4501,7.2019
170 | 8.3184,13.4283,91.5000,0.0795,4.7975,0.0445,6.7913
171 | 7.8946,13.4106,92.0003,0.0914,4.7975,0.3802,7.1934
172 | 7.4860,13.3775,92.5000,0.0920,4.5954,0.2093,6.7842
173 | 7.0859,13.3274,92.9999,0.0915,4.5954,0.3424,7.2885
174 | 6.6597,13.2555,93.5000,0.0915,4.1942,0.4347,6.8852
175 | 6.2425,13.1672,93.9999,0.0914,4.1942,0.4807,6.4598
176 | 5.8570,13.0694,94.5000,0.0702,3.7773,0.7240,6.9772
177 | 5.4628,12.9529,94.9999,0.0928,3.7773,0.7336,5.6852
178 | 5.0747,12.8289,95.5000,0.0662,3.7773,0.9457,6.1799
179 | 4.6532,12.6736,95.9999,0.0646,1.5000,1.0799,5.7589
180 | 4.2791,12.5258,96.5000,0.0338,1.5000,1.2609,2.6731
181 | 3.8737,12.3548,97.0006,0.1148,4.3783,1.1587,3.1017
182 | 3.4850,12.1896,97.5000,-0.0358,4.3783,0.9924,6.9124
183 | 3.0846,11.9871,98.0000,-0.0428,4.7871,0.7783,6.4814
184 | 2.6972,11.8070,98.4999,-0.0555,4.7871,0.5684,6.9415
185 | 2.3218,11.6470,99.0000,-0.0577,4.7756,0.5472,6.5321
186 | 1.9417,11.5001,99.5000,-0.0634,4.7756,0.1363,7.0162
187 | 1.5278,11.3552,99.9999,-0.0643,4.7756,0.3123,6.5968
188 | 1.1027,11.2228,100.5000,-0.0655,4.4335,0.4707,7.0792
189 | 0.7087,11.1137,101.0009,-0.0657,4.4335,0.6265,6.6647
190 | 0.2795,11.0090,101.4999,-0.0642,3.8239,0.8995,7.1780
191 | -0.1350,10.9212,102.0000,-0.0655,3.8239,1.0104,5.8435
192 | -0.5311,10.8486,102.5000,-0.0636,1.5000,0.9405,6.3514
193 | -0.8484,10.7987,102.9999,-0.0451,3.7055,0.9059,3.2664
194 | -1.2842,10.7414,103.5000,-0.0651,3.7055,0.7256,5.6182
195 | -1.6949,10.6921,104.0000,-0.0612,4.7656,0.7544,6.1236
196 | -2.1218,10.6537,104.5000,-0.0558,4.7656,0.5077,6.6464
197 | -2.5415,10.6271,105.0006,-0.0503,4.7656,0.5490,7.1882
198 | -2.9566,10.6100,105.5000,-0.0498,4.7867,0.3640,6.7642
199 | -3.4081,10.6000,106.0007,-0.0443,4.7867,0.3736,7.2517
200 | -3.8615,10.5994,106.5000,-0.0438,4.7849,0.2818,6.8168
201 | -4.2416,10.6049,107.0000,-0.0332,4.7849,0.2022,7.3574
202 | -4.6655,10.6180,107.5000,-0.0326,4.7994,0.3857,6.9610
203 | -5.0872,10.6352,108.0000,-0.0322,4.7994,0.0660,6.5165
204 | -5.4805,10.6554,108.5000,-0.0057,4.7994,0.4560,6.8865
205 | -5.9330,10.6841,109.0000,0.0273,4.7891,0.0724,7.0304
206 | -6.3513,10.7101,109.5000,0.0295,4.7891,0.3870,6.6172
207 | -6.8010,10.7284,110.0000,0.0591,4.7966,0.1956,6.8115
208 | -7.2120,10.7379,110.5000,0.0800,4.7966,0.3666,7.0946
209 | -7.6430,10.7335,111.0000,0.0814,4.0921,0.3564,6.7060
210 | -8.0378,10.7121,111.5000,0.0821,4.0921,0.3819,6.3177
211 | -8.4444,10.6737,112.0000,0.0958,3.7943,0.5752,6.6858
212 | -8.8682,10.6171,112.5000,0.0962,3.7943,0.5641,6.2959
213 | -9.2659,10.5452,113.0000,0.0963,3.7943,0.8087,5.9057
214 | -9.6728,10.4533,113.4999,0.1036,2.7973,0.8476,6.3578
215 | -10.0561,10.3500,113.9999,0.0687,2.7973,1.0172,4.2991
216 | -10.4324,10.2289,114.5000,0.0969,1.5000,1.2224,4.7238
217 | -10.8346,10.0906,114.9999,-0.0600,1.5000,1.3552,2.7330
218 | -11.2346,9.9319,115.5000,0.0697,3.8245,1.5293,3.0507
219 | -11.6345,9.7984,116.0000,0.1297,4.0684,1.3949,6.1789
220 | -12.0176,9.6592,116.5000,0.1232,4.0684,1.0983,6.7176
221 | -12.3888,9.4942,117.0000,0.1238,4.0684,0.9416,6.3572
222 | -12.7585,9.3023,117.5000,0.1242,4.3872,0.7339,5.9522
223 | -13.1075,9.0949,118.0001,0.1211,4.3872,0.5324,6.4593
224 | -13.4499,8.8655,118.5000,0.1326,4.7869,0.5922,6.7913
225 | -13.7808,8.6191,119.0000,0.1539,4.7869,0.2533,6.8969
226 | -14.0957,8.3561,119.5000,0.1548,4.7869,0.3500,6.5349
227 | -14.3987,8.0650,120.0000,0.2758,4.7975,0.2137,6.6746
228 | -14.6790,7.7615,120.5000,0.2780,4.7975,0.2113,6.5167
229 | -14.9015,7.4426,120.9999,0.2716,4.7895,0.3482,6.8560
230 | -15.0920,7.0771,121.4999,0.2734,4.7895,0.0648,6.6961
231 | -15.2356,6.6976,122.0000,0.2756,4.7895,0.4695,6.5093
232 | -15.3312,6.3221,122.5004,0.2614,4.7740,0.2930,7.0071
233 | -15.3864,5.9176,123.0000,0.2636,4.7740,0.5139,6.8123
234 | -15.4008,5.5106,123.5000,0.2661,4.7409,0.7448,6.5986
235 | -15.3730,5.0967,124.0000,0.2469,4.7409,0.9161,7.1894
236 | -15.3092,4.7287,124.5000,0.2488,4.7409,1.1983,6.9831
237 | -15.2047,4.3299,124.9999,0.2511,4.3755,1.5124,6.7367
238 | -15.0793,3.9797,125.5000,0.2532,4.3755,1.7684,6.5005
239 | -14.8966,3.5890,126.0000,0.2296,4.6781,1.7599,7.1385
240 | -14.6803,3.2295,126.5000,0.2314,4.6781,1.3584,6.8470
241 | -14.4585,2.9186,127.0001,0.2332,4.6781,1.0404,6.5766
242 | -14.2001,2.6118,127.5001,0.2080,4.6781,0.8002,7.2116
243 | -13.8918,2.3085,128.0000,0.2091,4.7819,0.5195,6.9114
244 | -13.5746,2.0388,128.4999,0.2105,4.7819,0.1364,6.5790
245 | -13.2560,1.8063,129.0000,0.1842,4.7819,0.3429,7.2064
246 | -12.9012,1.5893,129.5000,0.1846,4.7905,0.4222,6.8611
247 | -12.5543,1.4047,130.0020,0.1853,4.7905,0.5289,6.5264
248 | -12.1796,1.2351,130.5000,0.1585,4.7905,0.8346,7.1082
249 | -11.7769,1.0874,131.0000,0.1582,4.7987,0.8539,6.7329
250 | -11.3862,0.9670,131.5000,0.1338,4.7987,1.0140,7.3070
251 | -10.9702,0.8654,132.0008,0.1330,4.6995,1.1048,6.8979
252 | -10.5409,0.7803,132.5000,0.1106,4.6995,1.1886,7.4225
253 | -10.1203,0.7186,133.0032,0.1096,4.5996,1.2760,7.0056
254 | -9.7811,0.6796,133.5001,0.0617,2.2322,9.9566,9.9566
255 | -9.3411,0.6431,133.9999,0.0595,2.2322,9.5168,9.5168
256 | -8.9112,0.6157,134.5001,0.0584,2.2322,9.1029,9.1029
257 | -8.4830,0.5953,135.0000,0.0578,2.2322,8.6913,8.6913
258 | -8.0766,0.5824,135.5000,0.0573,2.2322,8.2880,8.2880
259 | -7.6582,0.5766,136.0002,0.0568,2.2322,7.8628,7.8628
260 | -7.2339,0.5794,136.5000,0.0563,2.2322,7.4486,7.4486
261 | -6.8069,0.5913,137.0000,0.0559,2.2322,7.0366,7.0366
262 | -6.3892,0.6120,137.5000,0.0554,2.2322,6.6081,6.6081
263 | -5.9651,0.6424,138.0000,0.0549,2.2322,6.1965,6.1965
264 | -5.5418,0.6823,138.4999,0.0544,2.2322,5.7852,5.7852
265 | -5.1017,0.7338,139.0000,0.0537,2.2322,5.3564,5.3564
266 | -4.7131,0.7880,139.5000,0.0530,2.2322,4.9338,4.9338
267 | -4.3257,0.8500,140.0000,0.0521,2.2322,4.5444,4.5444
268 | -3.9073,0.9260,140.4999,0.0511,2.2322,4.1435,4.1435
269 | -3.4964,1.0097,141.0005,0.0497,2.2322,3.7164,3.7164
270 | -3.0959,1.0999,141.5000,0.0086,2.2322,3.3037,4.2171
271 | -2.6840,1.2022,142.0000,0.0035,2.2322,2.8730,3.7861
272 | -2.2890,1.2987,142.5001,-0.0210,2.2322,2.4851,4.3228
273 | -1.8644,1.4008,142.9999,-0.0250,2.2322,2.0614,3.8994
274 | -1.4833,1.4857,143.5000,-0.0364,2.2322,1.6328,4.4027
275 | -1.0870,1.5677,144.0000,-0.0383,2.2322,1.2488,4.0166
276 | -0.6981,1.6399,144.5000,-0.0396,2.2322,0.8470,3.6061
277 | -0.3664,1.6953,145.0000,-0.0413,2.2322,0.5337,4.2099
278 | 0.0375,1.7554,145.5000,-0.0416,2.2322,0.2604,3.8217
279 | 0.4484,1.8083,146.0000,-0.0383,2.2322,0.4136,4.3524
280 | 0.8650,1.8538,146.5000,-0.0376,2.8455,0.2241,3.9282
281 | 1.2942,1.8933,147.0007,-0.0321,2.8455,0.2928,4.4420
282 | 1.7170,1.9249,147.5001,-0.0267,3.2652,0.2518,4.9449
283 | 2.1521,1.9518,148.0000,-0.0223,3.2652,0.1932,5.4672
284 | 2.5562,1.9730,148.5000,-0.0216,3.6995,0.3538,5.0597
285 | 


--------------------------------------------------------------------------------
/img/gz_rviz01.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/jkk-research/sim_wayp_plan_tools/15dd2b4977d004a2f74a936df6805c3681bbac82/img/gz_rviz01.gif


--------------------------------------------------------------------------------
/img/sim_waypoints3.svg:
--------------------------------------------------------------------------------
   1 | <?xml version="1.0" encoding="utf-8" standalone="no"?>
   2 | <!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN"
   3 |   "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
   4 | <svg xmlns:xlink="http://www.w3.org/1999/xlink" width="460.8pt" height="345.6pt" viewBox="0 0 460.8 345.6" xmlns="http://www.w3.org/2000/svg" version="1.1">
   5 |  <metadata>
   6 |   <rdf:RDF xmlns:dc="http://purl.org/dc/elements/1.1/" xmlns:cc="http://creativecommons.org/ns#" xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#">
   7 |    <cc:Work>
   8 |     <dc:type rdf:resource="http://purl.org/dc/dcmitype/StillImage"/>
   9 |     <dc:date>2024-02-21T09:31:33.791614</dc:date>
  10 |     <dc:format>image/svg+xml</dc:format>
  11 |     <dc:creator>
  12 |      <cc:Agent>
  13 |       <dc:title>Matplotlib v3.5.1, https://matplotlib.org/</dc:title>
  14 |      </cc:Agent>
  15 |     </dc:creator>
  16 |    </cc:Work>
  17 |   </rdf:RDF>
  18 |  </metadata>
  19 |  <defs>
  20 |   <style type="text/css">*{stroke-linejoin: round; stroke-linecap: butt}</style>
  21 |  </defs>
  22 |  <g id="figure_1">
  23 |   <g id="patch_1">
  24 |    <path d="M 0 345.6 
  25 | L 460.8 345.6 
  26 | L 460.8 0 
  27 | L 0 0 
  28 | z
  29 | " style="fill: #ffffff"/>
  30 |   </g>
  31 |   <g id="axes_1">
  32 |    <g id="patch_2">
  33 |     <path d="M 57.6 307.584 
  34 | L 414.72 307.584 
  35 | L 414.72 41.472 
  36 | L 57.6 41.472 
  37 | z
  38 | " style="fill: #ffffff"/>
  39 |    </g>
  40 |    <g id="matplotlib.axis_1">
  41 |     <g id="xtick_1">
  42 |      <g id="line2d_1">
  43 |       <path d="M 118.646131 307.584 
  44 | L 118.646131 41.472 
  45 | " clip-path="url(#pdc4efacc78)" style="fill: none; stroke: #b0b0b0; stroke-width: 0.8; stroke-linecap: square"/>
  46 |      </g>
  47 |      <g id="line2d_2">
  48 |       <defs>
  49 |        <path id="m879b34f4ca" d="M 0 0 
  50 | L 0 3.5 
  51 | " style="stroke: #000000; stroke-width: 0.8"/>
  52 |       </defs>
  53 |       <g>
  54 |        <use xlink:href="#m879b34f4ca" x="118.646131" y="307.584" style="stroke: #000000; stroke-width: 0.8"/>
  55 |       </g>
  56 |      </g>
  57 |      <g id="text_1">
  58 |       <!-- −10 -->
  59 |       <g transform="translate(108.093788 322.182437)scale(0.1 -0.1)">
  60 |        <defs>
  61 |         <path id="DejaVuSans-2212" d="M 678 2272 
  62 | L 4684 2272 
  63 | L 4684 1741 
  64 | L 678 1741 
  65 | L 678 2272 
  66 | z
  67 | " transform="scale(0.015625)"/>
  68 |         <path id="DejaVuSans-31" d="M 794 531 
  69 | L 1825 531 
  70 | L 1825 4091 
  71 | L 703 3866 
  72 | L 703 4441 
  73 | L 1819 4666 
  74 | L 2450 4666 
  75 | L 2450 531 
  76 | L 3481 531 
  77 | L 3481 0 
  78 | L 794 0 
  79 | L 794 531 
  80 | z
  81 | " transform="scale(0.015625)"/>
  82 |         <path id="DejaVuSans-30" d="M 2034 4250 
  83 | Q 1547 4250 1301 3770 
  84 | Q 1056 3291 1056 2328 
  85 | Q 1056 1369 1301 889 
  86 | Q 1547 409 2034 409 
  87 | Q 2525 409 2770 889 
  88 | Q 3016 1369 3016 2328 
  89 | Q 3016 3291 2770 3770 
  90 | Q 2525 4250 2034 4250 
  91 | z
  92 | M 2034 4750 
  93 | Q 2819 4750 3233 4129 
  94 | Q 3647 3509 3647 2328 
  95 | Q 3647 1150 3233 529 
  96 | Q 2819 -91 2034 -91 
  97 | Q 1250 -91 836 529 
  98 | Q 422 1150 422 2328 
  99 | Q 422 3509 836 4129 
 100 | Q 1250 4750 2034 4750 
 101 | z
 102 | " transform="scale(0.015625)"/>
 103 |        </defs>
 104 |        <use xlink:href="#DejaVuSans-2212"/>
 105 |        <use xlink:href="#DejaVuSans-31" x="83.789062"/>
 106 |        <use xlink:href="#DejaVuSans-30" x="147.412109"/>
 107 |       </g>
 108 |      </g>
 109 |     </g>
 110 |     <g id="xtick_2">
 111 |      <g id="line2d_3">
 112 |       <path d="M 182.792657 307.584 
 113 | L 182.792657 41.472 
 114 | " clip-path="url(#pdc4efacc78)" style="fill: none; stroke: #b0b0b0; stroke-width: 0.8; stroke-linecap: square"/>
 115 |      </g>
 116 |      <g id="line2d_4">
 117 |       <g>
 118 |        <use xlink:href="#m879b34f4ca" x="182.792657" y="307.584" style="stroke: #000000; stroke-width: 0.8"/>
 119 |       </g>
 120 |      </g>
 121 |      <g id="text_2">
 122 |       <!-- 0 -->
 123 |       <g transform="translate(179.611407 322.182437)scale(0.1 -0.1)">
 124 |        <use xlink:href="#DejaVuSans-30"/>
 125 |       </g>
 126 |      </g>
 127 |     </g>
 128 |     <g id="xtick_3">
 129 |      <g id="line2d_5">
 130 |       <path d="M 246.939182 307.584 
 131 | L 246.939182 41.472 
 132 | " clip-path="url(#pdc4efacc78)" style="fill: none; stroke: #b0b0b0; stroke-width: 0.8; stroke-linecap: square"/>
 133 |      </g>
 134 |      <g id="line2d_6">
 135 |       <g>
 136 |        <use xlink:href="#m879b34f4ca" x="246.939182" y="307.584" style="stroke: #000000; stroke-width: 0.8"/>
 137 |       </g>
 138 |      </g>
 139 |      <g id="text_3">
 140 |       <!-- 10 -->
 141 |       <g transform="translate(240.576682 322.182437)scale(0.1 -0.1)">
 142 |        <use xlink:href="#DejaVuSans-31"/>
 143 |        <use xlink:href="#DejaVuSans-30" x="63.623047"/>
 144 |       </g>
 145 |      </g>
 146 |     </g>
 147 |     <g id="xtick_4">
 148 |      <g id="line2d_7">
 149 |       <path d="M 311.085708 307.584 
 150 | L 311.085708 41.472 
 151 | " clip-path="url(#pdc4efacc78)" style="fill: none; stroke: #b0b0b0; stroke-width: 0.8; stroke-linecap: square"/>
 152 |      </g>
 153 |      <g id="line2d_8">
 154 |       <g>
 155 |        <use xlink:href="#m879b34f4ca" x="311.085708" y="307.584" style="stroke: #000000; stroke-width: 0.8"/>
 156 |       </g>
 157 |      </g>
 158 |      <g id="text_4">
 159 |       <!-- 20 -->
 160 |       <g transform="translate(304.723208 322.182437)scale(0.1 -0.1)">
 161 |        <defs>
 162 |         <path id="DejaVuSans-32" d="M 1228 531 
 163 | L 3431 531 
 164 | L 3431 0 
 165 | L 469 0 
 166 | L 469 531 
 167 | Q 828 903 1448 1529 
 168 | Q 2069 2156 2228 2338 
 169 | Q 2531 2678 2651 2914 
 170 | Q 2772 3150 2772 3378 
 171 | Q 2772 3750 2511 3984 
 172 | Q 2250 4219 1831 4219 
 173 | Q 1534 4219 1204 4116 
 174 | Q 875 4013 500 3803 
 175 | L 500 4441 
 176 | Q 881 4594 1212 4672 
 177 | Q 1544 4750 1819 4750 
 178 | Q 2544 4750 2975 4387 
 179 | Q 3406 4025 3406 3419 
 180 | Q 3406 3131 3298 2873 
 181 | Q 3191 2616 2906 2266 
 182 | Q 2828 2175 2409 1742 
 183 | Q 1991 1309 1228 531 
 184 | z
 185 | " transform="scale(0.015625)"/>
 186 |        </defs>
 187 |        <use xlink:href="#DejaVuSans-32"/>
 188 |        <use xlink:href="#DejaVuSans-30" x="63.623047"/>
 189 |       </g>
 190 |      </g>
 191 |     </g>
 192 |     <g id="xtick_5">
 193 |      <g id="line2d_9">
 194 |       <path d="M 375.232233 307.584 
 195 | L 375.232233 41.472 
 196 | " clip-path="url(#pdc4efacc78)" style="fill: none; stroke: #b0b0b0; stroke-width: 0.8; stroke-linecap: square"/>
 197 |      </g>
 198 |      <g id="line2d_10">
 199 |       <g>
 200 |        <use xlink:href="#m879b34f4ca" x="375.232233" y="307.584" style="stroke: #000000; stroke-width: 0.8"/>
 201 |       </g>
 202 |      </g>
 203 |      <g id="text_5">
 204 |       <!-- 30 -->
 205 |       <g transform="translate(368.869733 322.182437)scale(0.1 -0.1)">
 206 |        <defs>
 207 |         <path id="DejaVuSans-33" d="M 2597 2516 
 208 | Q 3050 2419 3304 2112 
 209 | Q 3559 1806 3559 1356 
 210 | Q 3559 666 3084 287 
 211 | Q 2609 -91 1734 -91 
 212 | Q 1441 -91 1130 -33 
 213 | Q 819 25 488 141 
 214 | L 488 750 
 215 | Q 750 597 1062 519 
 216 | Q 1375 441 1716 441 
 217 | Q 2309 441 2620 675 
 218 | Q 2931 909 2931 1356 
 219 | Q 2931 1769 2642 2001 
 220 | Q 2353 2234 1838 2234 
 221 | L 1294 2234 
 222 | L 1294 2753 
 223 | L 1863 2753 
 224 | Q 2328 2753 2575 2939 
 225 | Q 2822 3125 2822 3475 
 226 | Q 2822 3834 2567 4026 
 227 | Q 2313 4219 1838 4219 
 228 | Q 1578 4219 1281 4162 
 229 | Q 984 4106 628 3988 
 230 | L 628 4550 
 231 | Q 988 4650 1302 4700 
 232 | Q 1616 4750 1894 4750 
 233 | Q 2613 4750 3031 4423 
 234 | Q 3450 4097 3450 3541 
 235 | Q 3450 3153 3228 2886 
 236 | Q 3006 2619 2597 2516 
 237 | z
 238 | " transform="scale(0.015625)"/>
 239 |        </defs>
 240 |        <use xlink:href="#DejaVuSans-33"/>
 241 |        <use xlink:href="#DejaVuSans-30" x="63.623047"/>
 242 |       </g>
 243 |      </g>
 244 |     </g>
 245 |     <g id="text_6">
 246 |      <!-- x -->
 247 |      <g transform="translate(234.384375 332.821187)scale(0.06 -0.06)">
 248 |       <defs>
 249 |        <path id="DejaVuSans-78" d="M 3513 3500 
 250 | L 2247 1797 
 251 | L 3578 0 
 252 | L 2900 0 
 253 | L 1881 1375 
 254 | L 863 0 
 255 | L 184 0 
 256 | L 1544 1831 
 257 | L 300 3500 
 258 | L 978 3500 
 259 | L 1906 2253 
 260 | L 2834 3500 
 261 | L 3513 3500 
 262 | z
 263 | " transform="scale(0.015625)"/>
 264 |       </defs>
 265 |       <use xlink:href="#DejaVuSans-78"/>
 266 |      </g>
 267 |     </g>
 268 |    </g>
 269 |    <g id="matplotlib.axis_2">
 270 |     <g id="ytick_1">
 271 |      <g id="line2d_11">
 272 |       <path d="M 57.6 299.37991 
 273 | L 414.72 299.37991 
 274 | " clip-path="url(#pdc4efacc78)" style="fill: none; stroke: #b0b0b0; stroke-width: 0.8; stroke-linecap: square"/>
 275 |      </g>
 276 |      <g id="line2d_12">
 277 |       <defs>
 278 |        <path id="m86ee47f39f" d="M 0 0 
 279 | L -3.5 0 
 280 | " style="stroke: #000000; stroke-width: 0.8"/>
 281 |       </defs>
 282 |       <g>
 283 |        <use xlink:href="#m86ee47f39f" x="57.6" y="299.37991" style="stroke: #000000; stroke-width: 0.8"/>
 284 |       </g>
 285 |      </g>
 286 |      <g id="text_7">
 287 |       <!-- −15 -->
 288 |       <g transform="translate(29.495313 303.179129)scale(0.1 -0.1)">
 289 |        <defs>
 290 |         <path id="DejaVuSans-35" d="M 691 4666 
 291 | L 3169 4666 
 292 | L 3169 4134 
 293 | L 1269 4134 
 294 | L 1269 2991 
 295 | Q 1406 3038 1543 3061 
 296 | Q 1681 3084 1819 3084 
 297 | Q 2600 3084 3056 2656 
 298 | Q 3513 2228 3513 1497 
 299 | Q 3513 744 3044 326 
 300 | Q 2575 -91 1722 -91 
 301 | Q 1428 -91 1123 -41 
 302 | Q 819 9 494 109 
 303 | L 494 744 
 304 | Q 775 591 1075 516 
 305 | Q 1375 441 1709 441 
 306 | Q 2250 441 2565 725 
 307 | Q 2881 1009 2881 1497 
 308 | Q 2881 1984 2565 2268 
 309 | Q 2250 2553 1709 2553 
 310 | Q 1456 2553 1204 2497 
 311 | Q 953 2441 691 2322 
 312 | L 691 4666 
 313 | z
 314 | " transform="scale(0.015625)"/>
 315 |        </defs>
 316 |        <use xlink:href="#DejaVuSans-2212"/>
 317 |        <use xlink:href="#DejaVuSans-31" x="83.789062"/>
 318 |        <use xlink:href="#DejaVuSans-35" x="147.412109"/>
 319 |       </g>
 320 |      </g>
 321 |     </g>
 322 |     <g id="ytick_2">
 323 |      <g id="line2d_13">
 324 |       <path d="M 57.6 267.306648 
 325 | L 414.72 267.306648 
 326 | " clip-path="url(#pdc4efacc78)" style="fill: none; stroke: #b0b0b0; stroke-width: 0.8; stroke-linecap: square"/>
 327 |      </g>
 328 |      <g id="line2d_14">
 329 |       <g>
 330 |        <use xlink:href="#m86ee47f39f" x="57.6" y="267.306648" style="stroke: #000000; stroke-width: 0.8"/>
 331 |       </g>
 332 |      </g>
 333 |      <g id="text_8">
 334 |       <!-- −10 -->
 335 |       <g transform="translate(29.495313 271.105866)scale(0.1 -0.1)">
 336 |        <use xlink:href="#DejaVuSans-2212"/>
 337 |        <use xlink:href="#DejaVuSans-31" x="83.789062"/>
 338 |        <use xlink:href="#DejaVuSans-30" x="147.412109"/>
 339 |       </g>
 340 |      </g>
 341 |     </g>
 342 |     <g id="ytick_3">
 343 |      <g id="line2d_15">
 344 |       <path d="M 57.6 235.233385 
 345 | L 414.72 235.233385 
 346 | " clip-path="url(#pdc4efacc78)" style="fill: none; stroke: #b0b0b0; stroke-width: 0.8; stroke-linecap: square"/>
 347 |      </g>
 348 |      <g id="line2d_16">
 349 |       <g>
 350 |        <use xlink:href="#m86ee47f39f" x="57.6" y="235.233385" style="stroke: #000000; stroke-width: 0.8"/>
 351 |       </g>
 352 |      </g>
 353 |      <g id="text_9">
 354 |       <!-- −5 -->
 355 |       <g transform="translate(35.857813 239.032604)scale(0.1 -0.1)">
 356 |        <use xlink:href="#DejaVuSans-2212"/>
 357 |        <use xlink:href="#DejaVuSans-35" x="83.789062"/>
 358 |       </g>
 359 |      </g>
 360 |     </g>
 361 |     <g id="ytick_4">
 362 |      <g id="line2d_17">
 363 |       <path d="M 57.6 203.160122 
 364 | L 414.72 203.160122 
 365 | " clip-path="url(#pdc4efacc78)" style="fill: none; stroke: #b0b0b0; stroke-width: 0.8; stroke-linecap: square"/>
 366 |      </g>
 367 |      <g id="line2d_18">
 368 |       <g>
 369 |        <use xlink:href="#m86ee47f39f" x="57.6" y="203.160122" style="stroke: #000000; stroke-width: 0.8"/>
 370 |       </g>
 371 |      </g>
 372 |      <g id="text_10">
 373 |       <!-- 0 -->
 374 |       <g transform="translate(44.2375 206.959341)scale(0.1 -0.1)">
 375 |        <use xlink:href="#DejaVuSans-30"/>
 376 |       </g>
 377 |      </g>
 378 |     </g>
 379 |     <g id="ytick_5">
 380 |      <g id="line2d_19">
 381 |       <path d="M 57.6 171.08686 
 382 | L 414.72 171.08686 
 383 | " clip-path="url(#pdc4efacc78)" style="fill: none; stroke: #b0b0b0; stroke-width: 0.8; stroke-linecap: square"/>
 384 |      </g>
 385 |      <g id="line2d_20">
 386 |       <g>
 387 |        <use xlink:href="#m86ee47f39f" x="57.6" y="171.08686" style="stroke: #000000; stroke-width: 0.8"/>
 388 |       </g>
 389 |      </g>
 390 |      <g id="text_11">
 391 |       <!-- 5 -->
 392 |       <g transform="translate(44.2375 174.886078)scale(0.1 -0.1)">
 393 |        <use xlink:href="#DejaVuSans-35"/>
 394 |       </g>
 395 |      </g>
 396 |     </g>
 397 |     <g id="ytick_6">
 398 |      <g id="line2d_21">
 399 |       <path d="M 57.6 139.013597 
 400 | L 414.72 139.013597 
 401 | " clip-path="url(#pdc4efacc78)" style="fill: none; stroke: #b0b0b0; stroke-width: 0.8; stroke-linecap: square"/>
 402 |      </g>
 403 |      <g id="line2d_22">
 404 |       <g>
 405 |        <use xlink:href="#m86ee47f39f" x="57.6" y="139.013597" style="stroke: #000000; stroke-width: 0.8"/>
 406 |       </g>
 407 |      </g>
 408 |      <g id="text_12">
 409 |       <!-- 10 -->
 410 |       <g transform="translate(37.875 142.812816)scale(0.1 -0.1)">
 411 |        <use xlink:href="#DejaVuSans-31"/>
 412 |        <use xlink:href="#DejaVuSans-30" x="63.623047"/>
 413 |       </g>
 414 |      </g>
 415 |     </g>
 416 |     <g id="ytick_7">
 417 |      <g id="line2d_23">
 418 |       <path d="M 57.6 106.940334 
 419 | L 414.72 106.940334 
 420 | " clip-path="url(#pdc4efacc78)" style="fill: none; stroke: #b0b0b0; stroke-width: 0.8; stroke-linecap: square"/>
 421 |      </g>
 422 |      <g id="line2d_24">
 423 |       <g>
 424 |        <use xlink:href="#m86ee47f39f" x="57.6" y="106.940334" style="stroke: #000000; stroke-width: 0.8"/>
 425 |       </g>
 426 |      </g>
 427 |      <g id="text_13">
 428 |       <!-- 15 -->
 429 |       <g transform="translate(37.875 110.739553)scale(0.1 -0.1)">
 430 |        <use xlink:href="#DejaVuSans-31"/>
 431 |        <use xlink:href="#DejaVuSans-35" x="63.623047"/>
 432 |       </g>
 433 |      </g>
 434 |     </g>
 435 |     <g id="ytick_8">
 436 |      <g id="line2d_25">
 437 |       <path d="M 57.6 74.867072 
 438 | L 414.72 74.867072 
 439 | " clip-path="url(#pdc4efacc78)" style="fill: none; stroke: #b0b0b0; stroke-width: 0.8; stroke-linecap: square"/>
 440 |      </g>
 441 |      <g id="line2d_26">
 442 |       <g>
 443 |        <use xlink:href="#m86ee47f39f" x="57.6" y="74.867072" style="stroke: #000000; stroke-width: 0.8"/>
 444 |       </g>
 445 |      </g>
 446 |      <g id="text_14">
 447 |       <!-- 20 -->
 448 |       <g transform="translate(37.875 78.66629)scale(0.1 -0.1)">
 449 |        <use xlink:href="#DejaVuSans-32"/>
 450 |        <use xlink:href="#DejaVuSans-30" x="63.623047"/>
 451 |       </g>
 452 |      </g>
 453 |     </g>
 454 |     <g id="ytick_9">
 455 |      <g id="line2d_27">
 456 |       <path d="M 57.6 42.793809 
 457 | L 414.72 42.793809 
 458 | " clip-path="url(#pdc4efacc78)" style="fill: none; stroke: #b0b0b0; stroke-width: 0.8; stroke-linecap: square"/>
 459 |      </g>
 460 |      <g id="line2d_28">
 461 |       <g>
 462 |        <use xlink:href="#m86ee47f39f" x="57.6" y="42.793809" style="stroke: #000000; stroke-width: 0.8"/>
 463 |       </g>
 464 |      </g>
 465 |      <g id="text_15">
 466 |       <!-- 25 -->
 467 |       <g transform="translate(37.875 46.593028)scale(0.1 -0.1)">
 468 |        <use xlink:href="#DejaVuSans-32"/>
 469 |        <use xlink:href="#DejaVuSans-35" x="63.623047"/>
 470 |       </g>
 471 |      </g>
 472 |     </g>
 473 |     <g id="text_16">
 474 |      <!-- y -->
 475 |      <g transform="translate(24.2475 176.303625)rotate(-90)scale(0.06 -0.06)">
 476 |       <defs>
 477 |        <path id="DejaVuSans-79" d="M 2059 -325 
 478 | Q 1816 -950 1584 -1140 
 479 | Q 1353 -1331 966 -1331 
 480 | L 506 -1331 
 481 | L 506 -850 
 482 | L 844 -850 
 483 | Q 1081 -850 1212 -737 
 484 | Q 1344 -625 1503 -206 
 485 | L 1606 56 
 486 | L 191 3500 
 487 | L 800 3500 
 488 | L 1894 763 
 489 | L 2988 3500 
 490 | L 3597 3500 
 491 | L 2059 -325 
 492 | z
 493 | " transform="scale(0.015625)"/>
 494 |       </defs>
 495 |       <use xlink:href="#DejaVuSans-79"/>
 496 |      </g>
 497 |     </g>
 498 |    </g>
 499 |    <g id="line2d_29">
 500 |     <path d="M 182.792657 190.330817 
 501 | L 188.831411 190.304517 
 502 | L 194.870806 190.277576 
 503 | L 200.90956 190.249993 
 504 | L 206.949597 190.221127 
 505 | L 212.988992 190.192261 
 506 | L 219.02967 190.162112 
 507 | L 225.069707 190.131322 
 508 | L 231.111027 190.099248 
 509 | L 237.152347 190.067175 
 510 | L 243.194949 190.03446 
 511 | L 249.237552 190.001746 
 512 | L 255.279513 189.967748 
 513 | L 261.318909 189.933109 
 514 | L 267.353814 189.896545 
 515 | L 273.381663 189.858699 
 516 | L 279.399248 189.818928 
 517 | L 285.411702 189.779157 
 518 | L 291.526149 189.781723 
 519 | L 297.577732 190.40202 
 520 | L 302.354082 193.584329 
 521 | L 305.987983 198.567231 
 522 | L 309.648825 203.252493 
 523 | L 314.412346 206.949258 
 524 | L 320.14448 208.732531 
 525 | L 325.946533 210.301555 
 526 | L 331.76334 211.788471 
 527 | L 342.262202 217.609769 
 528 | L 351.970137 224.739655 
 529 | L 361.884624 231.150459 
 530 | L 367.551969 233.695151 
 531 | L 373.504767 234.30326 
 532 | L 379.245881 232.559116 
 533 | L 383.866997 228.714174 
 534 | L 386.54768 223.393861 
 535 | L 388.203302 217.569998 
 536 | L 389.790928 211.739079 
 537 | L 391.349047 205.908801 
 538 | L 392.909091 200.079806 
 539 | L 394.500566 194.255302 
 540 | L 396.153622 188.43657 
 541 | L 397.762417 182.605651 
 542 | L 398.487273 176.681078 
 543 | L 395.75848 171.442231 
 544 | L 392.634544 166.299604 
 545 | L 392.973879 160.466761 
 546 | L 394.728286 154.611466 
 547 | L 394.893143 148.616332 
 548 | L 393.329892 142.837371 
 549 | L 390.237388 137.687688 
 550 | L 385.880556 133.506618 
 551 | L 380.626314 130.521238 
 552 | L 374.845429 128.929122 
 553 | L 368.845163 128.334483 
 554 | L 362.822446 127.841838 
 555 | L 356.806785 127.322893 
 556 | L 350.788558 126.820625 
 557 | L 344.769689 126.319641 
 558 | L 338.752104 125.80711 
 559 | L 332.735801 125.278543 
 560 | L 326.720782 124.735863 
 561 | L 320.707045 124.183562 
 562 | L 314.693308 123.623563 
 563 | L 308.680854 123.059715 
 564 | L 302.667759 122.493942 
 565 | L 296.655305 121.929453 
 566 | L 290.641568 121.369454 
 567 | L 284.627832 120.817152 
 568 | L 278.612812 120.274472 
 569 | L 272.597151 119.745905 
 570 | L 266.579565 119.23145 
 571 | L 260.561338 118.728541 
 572 | L 254.54247 118.233972 
 573 | L 248.52296 117.743251 
 574 | L 242.50345 117.253813 
 575 | L 236.483298 116.761167 
 576 | L 230.465071 116.261466 
 577 | L 224.448127 115.747652 
 578 | L 218.434391 115.216519 
 579 | L 212.403334 114.75274 
 580 | L 206.590376 115.543025 
 581 | L 202.023144 119.493168 
 582 | L 198.384111 124.311213 
 583 | L 194.777794 129.151069 
 584 | L 191.111178 133.947946 
 585 | L 186.647221 137.982762 
 586 | L 180.900976 139.430549 
 587 | L 174.901993 138.835911 
 588 | L 168.928027 137.957104 
 589 | L 162.964966 136.974379 
 590 | L 156.998697 136.032066 
 591 | L 151.027939 135.135298 
 592 | L 145.05718 134.236605 
 593 | L 139.088988 133.305198 
 594 | L 133.122078 132.363527 
 595 | L 127.153244 131.446231 
 596 | L 121.183126 130.545614 
 597 | L 115.213009 129.636658 
 598 | L 109.176821 129.671938 
 599 | L 104.007894 132.45269 
 600 | L 100.224532 137.164253 
 601 | L 96.76511 142.106742 
 602 | L 93.265276 147.038327 
 603 | L 89.733368 151.947461 
 604 | L 86.204668 156.838633 
 605 | L 82.708041 161.740711 
 606 | L 79.260806 166.707576 
 607 | L 75.940582 171.807225 
 608 | L 73.832727 177.325751 
 609 | L 74.419668 183.310622 
 610 | L 77.691141 188.344199 
 611 | L 83.163481 190.457186 
 612 | L 89.166954 190.724677 
 613 | L 95.221745 190.625891 
 614 | L 101.267555 190.58163 
 615 | L 107.308233 190.565594 
 616 | L 113.34827 190.554047 
 617 | L 119.387024 190.541218 
 618 | L 125.426419 190.526464 
 619 | L 131.465173 190.511069 
 620 | L 137.503927 190.493749 
 621 | L 143.542681 190.475788 
 622 | L 149.580793 190.456544 
 623 | L 155.619547 190.436018 
 624 | L 161.65766 190.414208 
 625 | L 167.696413 190.391756 
 626 | L 173.734526 190.368022 
 627 | " clip-path="url(#pdc4efacc78)" style="fill: none; stroke: #ff0000; stroke-opacity: 0.1; stroke-width: 4.2; stroke-linecap: square"/>
 628 |     <defs>
 629 |      <path id="m2238c895a1" d="M 0 1.5 
 630 | C 0.397805 1.5 0.77937 1.341951 1.06066 1.06066 
 631 | C 1.341951 0.77937 1.5 0.397805 1.5 0 
 632 | C 1.5 -0.397805 1.341951 -0.77937 1.06066 -1.06066 
 633 | C 0.77937 -1.341951 0.397805 -1.5 0 -1.5 
 634 | C -0.397805 -1.5 -0.77937 -1.341951 -1.06066 -1.06066 
 635 | C -1.341951 -0.77937 -1.5 -0.397805 -1.5 0 
 636 | C -1.5 0.397805 -1.341951 0.77937 -1.06066 1.06066 
 637 | C -0.77937 1.341951 -0.397805 1.5 0 1.5 
 638 | z
 639 | " style="stroke: #ff0000; stroke-opacity: 0.1"/>
 640 |     </defs>
 641 |     <g clip-path="url(#pdc4efacc78)">
 642 |      <use xlink:href="#m2238c895a1" x="182.792657" y="190.330817" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 643 |      <use xlink:href="#m2238c895a1" x="188.831411" y="190.304517" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 644 |      <use xlink:href="#m2238c895a1" x="194.870806" y="190.277576" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 645 |      <use xlink:href="#m2238c895a1" x="200.90956" y="190.249993" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 646 |      <use xlink:href="#m2238c895a1" x="206.949597" y="190.221127" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 647 |      <use xlink:href="#m2238c895a1" x="212.988992" y="190.192261" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 648 |      <use xlink:href="#m2238c895a1" x="219.02967" y="190.162112" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 649 |      <use xlink:href="#m2238c895a1" x="225.069707" y="190.131322" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 650 |      <use xlink:href="#m2238c895a1" x="231.111027" y="190.099248" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 651 |      <use xlink:href="#m2238c895a1" x="237.152347" y="190.067175" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 652 |      <use xlink:href="#m2238c895a1" x="243.194949" y="190.03446" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 653 |      <use xlink:href="#m2238c895a1" x="249.237552" y="190.001746" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 654 |      <use xlink:href="#m2238c895a1" x="255.279513" y="189.967748" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 655 |      <use xlink:href="#m2238c895a1" x="261.318909" y="189.933109" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 656 |      <use xlink:href="#m2238c895a1" x="267.353814" y="189.896545" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 657 |      <use xlink:href="#m2238c895a1" x="273.381663" y="189.858699" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 658 |      <use xlink:href="#m2238c895a1" x="279.399248" y="189.818928" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 659 |      <use xlink:href="#m2238c895a1" x="285.411702" y="189.779157" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 660 |      <use xlink:href="#m2238c895a1" x="291.526149" y="189.781723" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 661 |      <use xlink:href="#m2238c895a1" x="297.577732" y="190.40202" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 662 |      <use xlink:href="#m2238c895a1" x="302.354082" y="193.584329" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 663 |      <use xlink:href="#m2238c895a1" x="305.987983" y="198.567231" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 664 |      <use xlink:href="#m2238c895a1" x="309.648825" y="203.252493" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 665 |      <use xlink:href="#m2238c895a1" x="314.412346" y="206.949258" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 666 |      <use xlink:href="#m2238c895a1" x="320.14448" y="208.732531" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 667 |      <use xlink:href="#m2238c895a1" x="325.946533" y="210.301555" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 668 |      <use xlink:href="#m2238c895a1" x="331.76334" y="211.788471" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 669 |      <use xlink:href="#m2238c895a1" x="342.262202" y="217.609769" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 670 |      <use xlink:href="#m2238c895a1" x="351.970137" y="224.739655" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 671 |      <use xlink:href="#m2238c895a1" x="361.884624" y="231.150459" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 672 |      <use xlink:href="#m2238c895a1" x="367.551969" y="233.695151" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 673 |      <use xlink:href="#m2238c895a1" x="373.504767" y="234.30326" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 674 |      <use xlink:href="#m2238c895a1" x="379.245881" y="232.559116" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 675 |      <use xlink:href="#m2238c895a1" x="383.866997" y="228.714174" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 676 |      <use xlink:href="#m2238c895a1" x="386.54768" y="223.393861" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 677 |      <use xlink:href="#m2238c895a1" x="388.203302" y="217.569998" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 678 |      <use xlink:href="#m2238c895a1" x="389.790928" y="211.739079" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 679 |      <use xlink:href="#m2238c895a1" x="391.349047" y="205.908801" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 680 |      <use xlink:href="#m2238c895a1" x="392.909091" y="200.079806" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 681 |      <use xlink:href="#m2238c895a1" x="394.500566" y="194.255302" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 682 |      <use xlink:href="#m2238c895a1" x="396.153622" y="188.43657" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 683 |      <use xlink:href="#m2238c895a1" x="397.762417" y="182.605651" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 684 |      <use xlink:href="#m2238c895a1" x="398.487273" y="176.681078" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 685 |      <use xlink:href="#m2238c895a1" x="395.75848" y="171.442231" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 686 |      <use xlink:href="#m2238c895a1" x="392.634544" y="166.299604" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 687 |      <use xlink:href="#m2238c895a1" x="392.973879" y="160.466761" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 688 |      <use xlink:href="#m2238c895a1" x="394.728286" y="154.611466" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 689 |      <use xlink:href="#m2238c895a1" x="394.893143" y="148.616332" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 690 |      <use xlink:href="#m2238c895a1" x="393.329892" y="142.837371" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 691 |      <use xlink:href="#m2238c895a1" x="390.237388" y="137.687688" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 692 |      <use xlink:href="#m2238c895a1" x="385.880556" y="133.506618" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 693 |      <use xlink:href="#m2238c895a1" x="380.626314" y="130.521238" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 694 |      <use xlink:href="#m2238c895a1" x="374.845429" y="128.929122" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 695 |      <use xlink:href="#m2238c895a1" x="368.845163" y="128.334483" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 696 |      <use xlink:href="#m2238c895a1" x="362.822446" y="127.841838" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 697 |      <use xlink:href="#m2238c895a1" x="356.806785" y="127.322893" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 698 |      <use xlink:href="#m2238c895a1" x="350.788558" y="126.820625" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 699 |      <use xlink:href="#m2238c895a1" x="344.769689" y="126.319641" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 700 |      <use xlink:href="#m2238c895a1" x="338.752104" y="125.80711" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 701 |      <use xlink:href="#m2238c895a1" x="332.735801" y="125.278543" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 702 |      <use xlink:href="#m2238c895a1" x="326.720782" y="124.735863" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 703 |      <use xlink:href="#m2238c895a1" x="320.707045" y="124.183562" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 704 |      <use xlink:href="#m2238c895a1" x="314.693308" y="123.623563" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 705 |      <use xlink:href="#m2238c895a1" x="308.680854" y="123.059715" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 706 |      <use xlink:href="#m2238c895a1" x="302.667759" y="122.493942" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 707 |      <use xlink:href="#m2238c895a1" x="296.655305" y="121.929453" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 708 |      <use xlink:href="#m2238c895a1" x="290.641568" y="121.369454" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 709 |      <use xlink:href="#m2238c895a1" x="284.627832" y="120.817152" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 710 |      <use xlink:href="#m2238c895a1" x="278.612812" y="120.274472" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 711 |      <use xlink:href="#m2238c895a1" x="272.597151" y="119.745905" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 712 |      <use xlink:href="#m2238c895a1" x="266.579565" y="119.23145" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 713 |      <use xlink:href="#m2238c895a1" x="260.561338" y="118.728541" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 714 |      <use xlink:href="#m2238c895a1" x="254.54247" y="118.233972" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 715 |      <use xlink:href="#m2238c895a1" x="248.52296" y="117.743251" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 716 |      <use xlink:href="#m2238c895a1" x="242.50345" y="117.253813" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 717 |      <use xlink:href="#m2238c895a1" x="236.483298" y="116.761167" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 718 |      <use xlink:href="#m2238c895a1" x="230.465071" y="116.261466" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 719 |      <use xlink:href="#m2238c895a1" x="224.448127" y="115.747652" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 720 |      <use xlink:href="#m2238c895a1" x="218.434391" y="115.216519" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 721 |      <use xlink:href="#m2238c895a1" x="212.403334" y="114.75274" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 722 |      <use xlink:href="#m2238c895a1" x="206.590376" y="115.543025" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 723 |      <use xlink:href="#m2238c895a1" x="202.023144" y="119.493168" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 724 |      <use xlink:href="#m2238c895a1" x="198.384111" y="124.311213" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 725 |      <use xlink:href="#m2238c895a1" x="194.777794" y="129.151069" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 726 |      <use xlink:href="#m2238c895a1" x="191.111178" y="133.947946" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 727 |      <use xlink:href="#m2238c895a1" x="186.647221" y="137.982762" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 728 |      <use xlink:href="#m2238c895a1" x="180.900976" y="139.430549" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 729 |      <use xlink:href="#m2238c895a1" x="174.901993" y="138.835911" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 730 |      <use xlink:href="#m2238c895a1" x="168.928027" y="137.957104" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 731 |      <use xlink:href="#m2238c895a1" x="162.964966" y="136.974379" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 732 |      <use xlink:href="#m2238c895a1" x="156.998697" y="136.032066" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 733 |      <use xlink:href="#m2238c895a1" x="151.027939" y="135.135298" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 734 |      <use xlink:href="#m2238c895a1" x="145.05718" y="134.236605" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 735 |      <use xlink:href="#m2238c895a1" x="139.088988" y="133.305198" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 736 |      <use xlink:href="#m2238c895a1" x="133.122078" y="132.363527" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 737 |      <use xlink:href="#m2238c895a1" x="127.153244" y="131.446231" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 738 |      <use xlink:href="#m2238c895a1" x="121.183126" y="130.545614" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 739 |      <use xlink:href="#m2238c895a1" x="115.213009" y="129.636658" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 740 |      <use xlink:href="#m2238c895a1" x="109.176821" y="129.671938" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 741 |      <use xlink:href="#m2238c895a1" x="104.007894" y="132.45269" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 742 |      <use xlink:href="#m2238c895a1" x="100.224532" y="137.164253" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 743 |      <use xlink:href="#m2238c895a1" x="96.76511" y="142.106742" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 744 |      <use xlink:href="#m2238c895a1" x="93.265276" y="147.038327" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 745 |      <use xlink:href="#m2238c895a1" x="89.733368" y="151.947461" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 746 |      <use xlink:href="#m2238c895a1" x="86.204668" y="156.838633" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 747 |      <use xlink:href="#m2238c895a1" x="82.708041" y="161.740711" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 748 |      <use xlink:href="#m2238c895a1" x="79.260806" y="166.707576" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 749 |      <use xlink:href="#m2238c895a1" x="75.940582" y="171.807225" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 750 |      <use xlink:href="#m2238c895a1" x="73.832727" y="177.325751" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 751 |      <use xlink:href="#m2238c895a1" x="74.419668" y="183.310622" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 752 |      <use xlink:href="#m2238c895a1" x="77.691141" y="188.344199" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 753 |      <use xlink:href="#m2238c895a1" x="83.163481" y="190.457186" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 754 |      <use xlink:href="#m2238c895a1" x="89.166954" y="190.724677" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 755 |      <use xlink:href="#m2238c895a1" x="95.221745" y="190.625891" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 756 |      <use xlink:href="#m2238c895a1" x="101.267555" y="190.58163" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 757 |      <use xlink:href="#m2238c895a1" x="107.308233" y="190.565594" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 758 |      <use xlink:href="#m2238c895a1" x="113.34827" y="190.554047" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 759 |      <use xlink:href="#m2238c895a1" x="119.387024" y="190.541218" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 760 |      <use xlink:href="#m2238c895a1" x="125.426419" y="190.526464" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 761 |      <use xlink:href="#m2238c895a1" x="131.465173" y="190.511069" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 762 |      <use xlink:href="#m2238c895a1" x="137.503927" y="190.493749" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 763 |      <use xlink:href="#m2238c895a1" x="143.542681" y="190.475788" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 764 |      <use xlink:href="#m2238c895a1" x="149.580793" y="190.456544" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 765 |      <use xlink:href="#m2238c895a1" x="155.619547" y="190.436018" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 766 |      <use xlink:href="#m2238c895a1" x="161.65766" y="190.414208" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 767 |      <use xlink:href="#m2238c895a1" x="167.696413" y="190.391756" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 768 |      <use xlink:href="#m2238c895a1" x="173.734526" y="190.368022" style="fill: #ff0000; fill-opacity: 0.1; stroke: #ff0000; stroke-opacity: 0.1"/>
 769 |     </g>
 770 |    </g>
 771 |    <g id="patch_3">
 772 |     <path d="M 57.6 307.584 
 773 | L 57.6 41.472 
 774 | " style="fill: none; stroke: #000000; stroke-width: 0.8; stroke-linejoin: miter; stroke-linecap: square"/>
 775 |    </g>
 776 |    <g id="patch_4">
 777 |     <path d="M 414.72 307.584 
 778 | L 414.72 41.472 
 779 | " style="fill: none; stroke: #000000; stroke-width: 0.8; stroke-linejoin: miter; stroke-linecap: square"/>
 780 |    </g>
 781 |    <g id="patch_5">
 782 |     <path d="M 57.6 307.584 
 783 | L 414.72 307.584 
 784 | " style="fill: none; stroke: #000000; stroke-width: 0.8; stroke-linejoin: miter; stroke-linecap: square"/>
 785 |    </g>
 786 |    <g id="patch_6">
 787 |     <path d="M 57.6 41.472 
 788 | L 414.72 41.472 
 789 | " style="fill: none; stroke: #000000; stroke-width: 0.8; stroke-linejoin: miter; stroke-linecap: square"/>
 790 |    </g>
 791 |    <g id="text_17">
 792 |     <!-- sim_waypoints3 -->
 793 |     <g transform="translate(212.151563 35.472)scale(0.06 -0.06)">
 794 |      <defs>
 795 |       <path id="DejaVuSans-73" d="M 2834 3397 
 796 | L 2834 2853 
 797 | Q 2591 2978 2328 3040 
 798 | Q 2066 3103 1784 3103 
 799 | Q 1356 3103 1142 2972 
 800 | Q 928 2841 928 2578 
 801 | Q 928 2378 1081 2264 
 802 | Q 1234 2150 1697 2047 
 803 | L 1894 2003 
 804 | Q 2506 1872 2764 1633 
 805 | Q 3022 1394 3022 966 
 806 | Q 3022 478 2636 193 
 807 | Q 2250 -91 1575 -91 
 808 | Q 1294 -91 989 -36 
 809 | Q 684 19 347 128 
 810 | L 347 722 
 811 | Q 666 556 975 473 
 812 | Q 1284 391 1588 391 
 813 | Q 1994 391 2212 530 
 814 | Q 2431 669 2431 922 
 815 | Q 2431 1156 2273 1281 
 816 | Q 2116 1406 1581 1522 
 817 | L 1381 1569 
 818 | Q 847 1681 609 1914 
 819 | Q 372 2147 372 2553 
 820 | Q 372 3047 722 3315 
 821 | Q 1072 3584 1716 3584 
 822 | Q 2034 3584 2315 3537 
 823 | Q 2597 3491 2834 3397 
 824 | z
 825 | " transform="scale(0.015625)"/>
 826 |       <path id="DejaVuSans-69" d="M 603 3500 
 827 | L 1178 3500 
 828 | L 1178 0 
 829 | L 603 0 
 830 | L 603 3500 
 831 | z
 832 | M 603 4863 
 833 | L 1178 4863 
 834 | L 1178 4134 
 835 | L 603 4134 
 836 | L 603 4863 
 837 | z
 838 | " transform="scale(0.015625)"/>
 839 |       <path id="DejaVuSans-6d" d="M 3328 2828 
 840 | Q 3544 3216 3844 3400 
 841 | Q 4144 3584 4550 3584 
 842 | Q 5097 3584 5394 3201 
 843 | Q 5691 2819 5691 2113 
 844 | L 5691 0 
 845 | L 5113 0 
 846 | L 5113 2094 
 847 | Q 5113 2597 4934 2840 
 848 | Q 4756 3084 4391 3084 
 849 | Q 3944 3084 3684 2787 
 850 | Q 3425 2491 3425 1978 
 851 | L 3425 0 
 852 | L 2847 0 
 853 | L 2847 2094 
 854 | Q 2847 2600 2669 2842 
 855 | Q 2491 3084 2119 3084 
 856 | Q 1678 3084 1418 2786 
 857 | Q 1159 2488 1159 1978 
 858 | L 1159 0 
 859 | L 581 0 
 860 | L 581 3500 
 861 | L 1159 3500 
 862 | L 1159 2956 
 863 | Q 1356 3278 1631 3431 
 864 | Q 1906 3584 2284 3584 
 865 | Q 2666 3584 2933 3390 
 866 | Q 3200 3197 3328 2828 
 867 | z
 868 | " transform="scale(0.015625)"/>
 869 |       <path id="DejaVuSans-5f" d="M 3263 -1063 
 870 | L 3263 -1509 
 871 | L -63 -1509 
 872 | L -63 -1063 
 873 | L 3263 -1063 
 874 | z
 875 | " transform="scale(0.015625)"/>
 876 |       <path id="DejaVuSans-77" d="M 269 3500 
 877 | L 844 3500 
 878 | L 1563 769 
 879 | L 2278 3500 
 880 | L 2956 3500 
 881 | L 3675 769 
 882 | L 4391 3500 
 883 | L 4966 3500 
 884 | L 4050 0 
 885 | L 3372 0 
 886 | L 2619 2869 
 887 | L 1863 0 
 888 | L 1184 0 
 889 | L 269 3500 
 890 | z
 891 | " transform="scale(0.015625)"/>
 892 |       <path id="DejaVuSans-61" d="M 2194 1759 
 893 | Q 1497 1759 1228 1600 
 894 | Q 959 1441 959 1056 
 895 | Q 959 750 1161 570 
 896 | Q 1363 391 1709 391 
 897 | Q 2188 391 2477 730 
 898 | Q 2766 1069 2766 1631 
 899 | L 2766 1759 
 900 | L 2194 1759 
 901 | z
 902 | M 3341 1997 
 903 | L 3341 0 
 904 | L 2766 0 
 905 | L 2766 531 
 906 | Q 2569 213 2275 61 
 907 | Q 1981 -91 1556 -91 
 908 | Q 1019 -91 701 211 
 909 | Q 384 513 384 1019 
 910 | Q 384 1609 779 1909 
 911 | Q 1175 2209 1959 2209 
 912 | L 2766 2209 
 913 | L 2766 2266 
 914 | Q 2766 2663 2505 2880 
 915 | Q 2244 3097 1772 3097 
 916 | Q 1472 3097 1187 3025 
 917 | Q 903 2953 641 2809 
 918 | L 641 3341 
 919 | Q 956 3463 1253 3523 
 920 | Q 1550 3584 1831 3584 
 921 | Q 2591 3584 2966 3190 
 922 | Q 3341 2797 3341 1997 
 923 | z
 924 | " transform="scale(0.015625)"/>
 925 |       <path id="DejaVuSans-70" d="M 1159 525 
 926 | L 1159 -1331 
 927 | L 581 -1331 
 928 | L 581 3500 
 929 | L 1159 3500 
 930 | L 1159 2969 
 931 | Q 1341 3281 1617 3432 
 932 | Q 1894 3584 2278 3584 
 933 | Q 2916 3584 3314 3078 
 934 | Q 3713 2572 3713 1747 
 935 | Q 3713 922 3314 415 
 936 | Q 2916 -91 2278 -91 
 937 | Q 1894 -91 1617 61 
 938 | Q 1341 213 1159 525 
 939 | z
 940 | M 3116 1747 
 941 | Q 3116 2381 2855 2742 
 942 | Q 2594 3103 2138 3103 
 943 | Q 1681 3103 1420 2742 
 944 | Q 1159 2381 1159 1747 
 945 | Q 1159 1113 1420 752 
 946 | Q 1681 391 2138 391 
 947 | Q 2594 391 2855 752 
 948 | Q 3116 1113 3116 1747 
 949 | z
 950 | " transform="scale(0.015625)"/>
 951 |       <path id="DejaVuSans-6f" d="M 1959 3097 
 952 | Q 1497 3097 1228 2736 
 953 | Q 959 2375 959 1747 
 954 | Q 959 1119 1226 758 
 955 | Q 1494 397 1959 397 
 956 | Q 2419 397 2687 759 
 957 | Q 2956 1122 2956 1747 
 958 | Q 2956 2369 2687 2733 
 959 | Q 2419 3097 1959 3097 
 960 | z
 961 | M 1959 3584 
 962 | Q 2709 3584 3137 3096 
 963 | Q 3566 2609 3566 1747 
 964 | Q 3566 888 3137 398 
 965 | Q 2709 -91 1959 -91 
 966 | Q 1206 -91 779 398 
 967 | Q 353 888 353 1747 
 968 | Q 353 2609 779 3096 
 969 | Q 1206 3584 1959 3584 
 970 | z
 971 | " transform="scale(0.015625)"/>
 972 |       <path id="DejaVuSans-6e" d="M 3513 2113 
 973 | L 3513 0 
 974 | L 2938 0 
 975 | L 2938 2094 
 976 | Q 2938 2591 2744 2837 
 977 | Q 2550 3084 2163 3084 
 978 | Q 1697 3084 1428 2787 
 979 | Q 1159 2491 1159 1978 
 980 | L 1159 0 
 981 | L 581 0 
 982 | L 581 3500 
 983 | L 1159 3500 
 984 | L 1159 2956 
 985 | Q 1366 3272 1645 3428 
 986 | Q 1925 3584 2291 3584 
 987 | Q 2894 3584 3203 3211 
 988 | Q 3513 2838 3513 2113 
 989 | z
 990 | " transform="scale(0.015625)"/>
 991 |       <path id="DejaVuSans-74" d="M 1172 4494 
 992 | L 1172 3500 
 993 | L 2356 3500 
 994 | L 2356 3053 
 995 | L 1172 3053 
 996 | L 1172 1153 
 997 | Q 1172 725 1289 603 
 998 | Q 1406 481 1766 481 
 999 | L 2356 481 
1000 | L 2356 0 
1001 | L 1766 0 
1002 | Q 1100 0 847 248 
1003 | Q 594 497 594 1153 
1004 | L 594 3053 
1005 | L 172 3053 
1006 | L 172 3500 
1007 | L 594 3500 
1008 | L 594 4494 
1009 | L 1172 4494 
1010 | z
1011 | " transform="scale(0.015625)"/>
1012 |      </defs>
1013 |      <use xlink:href="#DejaVuSans-73"/>
1014 |      <use xlink:href="#DejaVuSans-69" x="52.099609"/>
1015 |      <use xlink:href="#DejaVuSans-6d" x="79.882812"/>
1016 |      <use xlink:href="#DejaVuSans-5f" x="177.294922"/>
1017 |      <use xlink:href="#DejaVuSans-77" x="227.294922"/>
1018 |      <use xlink:href="#DejaVuSans-61" x="309.082031"/>
1019 |      <use xlink:href="#DejaVuSans-79" x="370.361328"/>
1020 |      <use xlink:href="#DejaVuSans-70" x="429.541016"/>
1021 |      <use xlink:href="#DejaVuSans-6f" x="493.017578"/>
1022 |      <use xlink:href="#DejaVuSans-69" x="554.199219"/>
1023 |      <use xlink:href="#DejaVuSans-6e" x="581.982422"/>
1024 |      <use xlink:href="#DejaVuSans-74" x="645.361328"/>
1025 |      <use xlink:href="#DejaVuSans-73" x="684.570312"/>
1026 |      <use xlink:href="#DejaVuSans-33" x="736.669922"/>
1027 |     </g>
1028 |    </g>
1029 |   </g>
1030 |  </g>
1031 |  <defs>
1032 |   <clipPath id="pdc4efacc78">
1033 |    <rect x="57.6" y="41.472" width="357.12" height="266.112"/>
1034 |   </clipPath>
1035 |  </defs>
1036 | </svg>
1037 | 


--------------------------------------------------------------------------------
/img/sim_waypoints4_zalazone_uni_track.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/jkk-research/sim_wayp_plan_tools/15dd2b4977d004a2f74a936df6805c3681bbac82/img/sim_waypoints4_zalazone_uni_track.png


--------------------------------------------------------------------------------
/launch/all_in_once.launch.py:
--------------------------------------------------------------------------------
 1 | from launch import LaunchDescription
 2 | from launch_ros.actions import Node
 3 | from launch.actions import IncludeLaunchDescription, TimerAction
 4 | from launch.launch_description_sources import PythonLaunchDescriptionSource
 5 | from launch_ros.substitutions import FindPackageShare
 6 | 
 7 | 
 8 | def generate_launch_description():
 9 | 
10 |     return LaunchDescription(
11 |         [
12 |             IncludeLaunchDescription(
13 |                 PythonLaunchDescriptionSource([
14 |                     FindPackageShare("sim_wayp_plan_tools"), '/launch/', 'gazebo_bridge.launch.py'])
15 |             ),
16 |             IncludeLaunchDescription(
17 |                 PythonLaunchDescriptionSource([
18 |                     FindPackageShare("sim_wayp_plan_tools"), '/launch/', 'rviz1.launch.py'])
19 |             ),
20 |             IncludeLaunchDescription(
21 |                 PythonLaunchDescriptionSource([
22 |                     FindPackageShare("sim_wayp_plan_tools"), '/launch/', 'waypoint_loader.launch.py'])
23 |             ),
24 |             TimerAction(
25 |                 period=4.0, # delay / wait in seconds
26 |                 actions=[
27 |                     IncludeLaunchDescription(
28 |                         PythonLaunchDescriptionSource([
29 |                             FindPackageShare("sim_wayp_plan_tools"), '/launch/', 'waypoint_to_target.launch.py'])
30 |                     ),
31 |                 ]
32 |             ),    
33 |             TimerAction(
34 |                 period=5.0, # delay / wait in seconds
35 |                 actions=[
36 |                     IncludeLaunchDescription(
37 |                         PythonLaunchDescriptionSource([
38 |                             FindPackageShare("sim_wayp_plan_tools"), '/launch/', 'single_goal_pursuit.launch.py'])
39 |                     ),
40 |                 ]
41 |             ),     
42 |             # TimerAction(
43 |             #     period=5.0, # delay / wait in seconds
44 |             #     actions=[
45 |             #         IncludeLaunchDescription(
46 |             #             PythonLaunchDescriptionSource([
47 |             #                 FindPackageShare("sim_wayp_plan_tools"), '/launch/', 'stanley.launch.py'])
48 |             #         ),
49 |             #     ]
50 |             # ), 
51 |         ]
52 |     )


--------------------------------------------------------------------------------
/launch/all_in_once_save_metrics.launch.py:
--------------------------------------------------------------------------------
 1 | from launch import LaunchDescription
 2 | from launch_ros.actions import Node
 3 | from launch.actions import IncludeLaunchDescription, TimerAction
 4 | from launch.launch_description_sources import PythonLaunchDescriptionSource
 5 | from launch_ros.substitutions import FindPackageShare
 6 | 
 7 | 
 8 | def generate_launch_description():
 9 | 
10 |     return LaunchDescription(
11 |         [
12 |             IncludeLaunchDescription(
13 |                 PythonLaunchDescriptionSource([
14 |                     FindPackageShare("sim_wayp_plan_tools"), '/launch/', 'gazebo_bridge.launch.py'])
15 |             ),
16 |             IncludeLaunchDescription(
17 |                 PythonLaunchDescriptionSource([
18 |                     FindPackageShare("sim_wayp_plan_tools"), '/launch/', 'rviz1.launch.py'])
19 |             ),
20 |             IncludeLaunchDescription(
21 |                 PythonLaunchDescriptionSource([
22 |                     FindPackageShare("sim_wayp_plan_tools"), '/launch/', 'waypoint_loader.launch.py'])
23 |             ),
24 |             IncludeLaunchDescription(
25 |                 PythonLaunchDescriptionSource([
26 |                     FindPackageShare("sim_wayp_plan_tools"), '/launch/', 'csv_saver.launch.py'])
27 |             ),            
28 |             TimerAction(
29 |                 period=4.0, # delay / wait in seconds
30 |                 actions=[
31 |                     Node(
32 |                         package='wayp_plan_tools',
33 |                         executable='waypoint_to_target',
34 |                         #name='wayp_to_target',
35 |                         output='screen',
36 |                         namespace='sim1',
37 |                         parameters=[
38 |                             {"lookahead_min": 2.5},
39 |                             {"lookahead_max": 4.5},
40 |                             {"mps_alpha": 1.5},
41 |                             {"mps_beta": 4.5}, 
42 |                             {"waypoint_topic": "waypointarray"},
43 |                             {"tf_frame_id": "base_link"},
44 |                             {"tf_child_frame_id": "map"},
45 |                             {"interpolate": True},
46 |                         ],
47 |                     ),
48 |                 ]
49 |             ),    
50 |             TimerAction(
51 |                 period=5.0, # delay / wait in seconds
52 |                 actions=[
53 |                     Node(
54 |                         package='wayp_plan_tools',
55 |                         executable='single_goal_pursuit',
56 |                         namespace='sim1',
57 |                         output='screen',
58 |                         parameters=[
59 |                                 {"cmd_topic": "/model/vehicle_blue/cmd_vel"},
60 |                                 {"wheelbase": 1.0}, # from the /usr/share/ignition/ignition-gazebo6/worlds/ackermann_steering.sdf file wheel_base parameter
61 |                                 {"waypoint_topic": "targetpoints"},
62 |                                 {"max_angular_velocity": 1.0},
63 |                         ],
64 |                     ),
65 |                 ]   
66 |             ),
67 | 
68 |             # TimerAction(
69 |             #     period=5.0, # delay / wait in seconds
70 |             #     actions=[
71 |             #         IncludeLaunchDescription(
72 |             #             PythonLaunchDescriptionSource([
73 |             #                 FindPackageShare("sim_wayp_plan_tools"), '/launch/', 'stanley.launch.py'])
74 |             #         ),
75 |             #     ]
76 |             # ), 
77 |         ]
78 |     )


--------------------------------------------------------------------------------
/launch/csv_saver.launch.py:
--------------------------------------------------------------------------------
 1 | # https://github.com/jkk-research/sim_wayp_plan_tools/issues/1
 2 | # Benchmark CSV saving
 3 | from launch import LaunchDescription
 4 | from launch_ros.actions import Node
 5 | from ament_index_python.packages import get_package_share_directory
 6 | import os
 7 | 
 8 | 
 9 | def generate_launch_description():
10 | 
11 |     pkg_name = 'sim_wayp_plan_tools'
12 |     pkg_dir = get_package_share_directory(pkg_name)
13 |     file_prefix = "tmp"
14 |     csv_dir = os.path.expanduser("~") + "/ros2_ws/src/sim_wayp_plan_tools/csv"
15 |     i = 1
16 |     # iterate until we find a file that does not exist
17 |     while os.path.exists("%s/%s%02d_xypose.csv" % (csv_dir, file_prefix, i)):
18 |         i += 1
19 |     csv_file_name = "%s%02d" % (file_prefix, i)
20 | 
21 |     return LaunchDescription([
22 |         Node(
23 |             package='sim_wayp_plan_tools',
24 |             executable='csv_saver',
25 |             #name='wayp_saver',
26 |             output='screen',
27 |             namespace='sim1',
28 |             parameters=[
29 |                 # {"file_dir": pkg_dir + "/csv"},
30 |                 # {"file_dir": "$HOME/ros2_ws/src/sim_wayp_plan_tools/csv"},
31 |                 {"file_dir": csv_dir},
32 |                 # {"file_name": "tmp01"},
33 |                 {"file_name": csv_file_name},
34 |                 {"topic_based_saving": False},
35 |                 {"tf_frame_id": "map"},
36 |                 {"tf_child_frame_id": "base_link"},
37 |             ],
38 |         )
39 |     ])


--------------------------------------------------------------------------------
/launch/gazebo_bridge.launch.py:
--------------------------------------------------------------------------------
 1 | from launch import LaunchDescription
 2 | from launch_ros.actions import Node
 3 | 
 4 | def generate_launch_description():
 5 | 
 6 |     return LaunchDescription([
 7 |         # ros2 run ros_gz_bridge parameter_bridge /world/ackermann_steering/pose/info@geometry_msgs/msg/PoseArray[ignition.msgs.Pose_V /model/vehicle_blue/cmd_vel@geometry_msgs/msg/Twist]ignition.msgs.Twist
 8 |         Node(
 9 |             package='ros_gz_bridge',
10 |             executable='parameter_bridge',
11 |             #name='ros_gz_bridge_1',
12 |             arguments=[
13 |                 '/world/ackermann_steering/pose/info@geometry_msgs/msg/PoseArray[ignition.msgs.Pose_V',  
14 |                 '/model/vehicle_blue/cmd_vel@geometry_msgs/msg/Twist]ignition.msgs.Twist',  
15 |             ],
16 |             output='screen',
17 |         ),
18 |         Node(
19 |             package='sim_wayp_plan_tools',
20 |             executable='pose_arr_to_tf',
21 |             #name='pose_arr_to_tf1',
22 |             output='screen',
23 |             parameters=[
24 |                 {"pose_topic": "/world/ackermann_steering/pose/info"},
25 |             ],
26 |         )
27 |         
28 | 
29 |     ])


--------------------------------------------------------------------------------
/launch/rviz1.launch.py:
--------------------------------------------------------------------------------
 1 | from launch import LaunchDescription
 2 | from launch_ros.actions import Node
 3 | import os
 4 | from ament_index_python.packages import get_package_share_directory
 5 | 
 6 | 
 7 | def generate_launch_description():
 8 | 
 9 |     pkg_name = 'sim_wayp_plan_tools'
10 |     pkg_dir = get_package_share_directory(pkg_name)
11 |     #print(pkg_dir) # /home/he/ros2_ws/src/demo_jkk
12 | 
13 | 
14 |     return LaunchDescription([
15 |         Node(
16 |             package='rviz2',
17 |             namespace='',
18 |             executable='rviz2',
19 |             #name='rviz2',
20 |             arguments=['-d', [os.path.join(pkg_dir, 'rviz', 'rviz1.rviz')]]
21 |         )
22 |     ])


--------------------------------------------------------------------------------
/launch/single_goal_pursuit.launch.py:
--------------------------------------------------------------------------------
 1 | from launch import LaunchDescription
 2 | from launch_ros.actions import Node
 3 | 
 4 | def generate_launch_description():
 5 |     return LaunchDescription([
 6 |         Node(
 7 |             package='wayp_plan_tools',
 8 |             executable='single_goal_pursuit',
 9 |             #name='pure_pursuit',
10 |             namespace='sim1',
11 |             output='screen',
12 |             parameters=[
13 |                     {"cmd_topic": "/model/vehicle_blue/cmd_vel"},
14 |                     {"wheelbase": 1.0}, # from the /usr/share/ignition/ignition-gazebo6/worlds/ackermann_steering.sdf file wheel_base parameter
15 |                     {"waypoint_topic": "targetpoints"},
16 |                 ],
17 |         )
18 |     ])
19 | 


--------------------------------------------------------------------------------
/launch/stanley.launch.py:
--------------------------------------------------------------------------------
 1 | from launch import LaunchDescription
 2 | from launch_ros.actions import Node
 3 | 
 4 | def generate_launch_description():
 5 |     return LaunchDescription([
 6 |         Node(
 7 |             package='wayp_plan_tools',
 8 |             executable='stanley_control',
 9 |             #name='stanley',
10 |             namespace='sim1',
11 |             output='screen',
12 |             parameters=[
13 |                     {"cmd_topic": "/model/vehicle_blue/cmd_vel"},
14 |                     {"wheelbase": 1.0}, # from the /usr/share/ignition/ignition-gazebo6/worlds/ackermann_steering.sdf file wheel_base parameter
15 |                     {"waypoint_topic": "targetpoints"},
16 |                     {"heading_error_rate": 0.0},
17 |                     {"cross_track_err_rate": 0.05},
18 |                 ],
19 |         )
20 |     ])
21 | 


--------------------------------------------------------------------------------
/launch/waypoint_loader.launch.py:
--------------------------------------------------------------------------------
 1 | from launch import LaunchDescription
 2 | from launch_ros.actions import Node
 3 | from ament_index_python.packages import get_package_share_directory
 4 | 
 5 | 
 6 | def generate_launch_description():
 7 | 
 8 |     pkg_name = 'sim_wayp_plan_tools'
 9 |     pkg_dir = get_package_share_directory(pkg_name)
10 |     #print(pkg_dir)
11 | 
12 |     return LaunchDescription([
13 |         Node(
14 |             package='wayp_plan_tools',
15 |             executable='waypoint_loader',
16 |             #name='wayp_load',
17 |             output='screen',
18 |             namespace='sim1',
19 |             parameters=[
20 |                 #{"file_dir": "/mnt/bag/waypoints/"},
21 |                 {"file_dir": pkg_dir + "/csv"},
22 |                 {"file_name": "sim_waypoints3.csv"},
23 |             ],
24 |         )
25 |     ])


--------------------------------------------------------------------------------
/launch/waypoint_saver.launch.py:
--------------------------------------------------------------------------------
 1 | from launch import LaunchDescription
 2 | from launch_ros.actions import Node
 3 | from ament_index_python.packages import get_package_share_directory
 4 | 
 5 | def generate_launch_description():
 6 | 
 7 |     pkg_name = 'sim_wayp_plan_tools'
 8 |     pkg_dir = get_package_share_directory(pkg_name)
 9 |     #print(pkg_dir)
10 | 
11 |     return LaunchDescription([
12 |         Node(
13 |             package='wayp_plan_tools',
14 |             executable='waypoint_saver',
15 |             #name='wayp_saver',
16 |             output='screen',
17 |             namespace='sim1',
18 |             parameters=[
19 |                 {"file_dir": pkg_dir + "/csv"},
20 |                 {"file_name": "sim_waypoints99.csv"},
21 |                 {"topic_based_saving": False},
22 |                 {"tf_frame_id": "map"},
23 |                 {"tf_child_frame_id": "base_link"},
24 |             ],
25 |         )
26 |     ])


--------------------------------------------------------------------------------
/launch/waypoint_to_target.launch.py:
--------------------------------------------------------------------------------
 1 | from launch import LaunchDescription
 2 | from launch_ros.actions import Node
 3 | from ament_index_python.packages import get_package_share_directory
 4 | 
 5 | def generate_launch_description():
 6 | 
 7 |     #pkg_name = 'wayp_plan_tools'
 8 |     #pkg_dir = get_package_share_directory(pkg_name)
 9 |     #print(pkg_dir)
10 | 
11 |     return LaunchDescription([
12 |         Node(
13 |             package='wayp_plan_tools',
14 |             executable='waypoint_to_target',
15 |             #name='wayp_to_target',
16 |             output='screen',
17 |             namespace='sim1',
18 |             parameters=[
19 |                 {"lookahead_min": 2.5},
20 |                 {"lookahead_max": 4.5},
21 |                 {"mps_alpha": 1.5},
22 |                 {"mps_beta": 4.5}, 
23 |                 {"waypoint_topic": "waypointarray"},
24 |                 {"tf_frame_id": "base_link"},
25 |                 {"tf_child_frame_id": "map"},
26 |             ],
27 |         ),
28 |         Node(
29 |             package='sim_wayp_plan_tools',
30 |             executable='visuals',
31 |             output='screen',
32 |             namespace='sim1',
33 |             parameters=[
34 |                 {"marker_topic": "marker_steering"},
35 |                 {"mod_limit": 40}, # modulo limit for path size (publish every n-th message)
36 |                 {"path_size": 800},
37 |                 {"pose_frame": "base_link"}, 
38 |                 {"publish_steer_marker": True},
39 |             ],
40 |         ),
41 |     ])
42 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 3 | <package format="3">
 4 |   <name>sim_wayp_plan_tools</name>
 5 |   <version>0.0.0</version>
 6 |   <description>TODO: Package description</description>
 7 |   <maintainer email="horverno@gmail.com">he</maintainer>
 8 |   <license>TODO: License declaration</license>
 9 | 
10 |   <buildtool_depend>ament_cmake</buildtool_depend>
11 | 
12 |   <test_depend>ament_lint_auto</test_depend>
13 |   <test_depend>ament_lint_common</test_depend>
14 |   <exec_depend>ros2launch</exec_depend>
15 |   <depend>rclcpp</depend>
16 |   <depend>geometry_msgs</depend>
17 |   <depend>nav_msgs</depend>
18 |   <depend>visualization_msgs</depend>
19 |   <depend>tf2_msgs</depend>
20 |   <depend>tf2</depend>
21 |   <depend>tf2_ros</depend>
22 |   <depend>tf2_geometry_msgs</depend>
23 |   <depend>wayp_plan_tools</depend>
24 |   
25 |   <export>
26 |     <build_type>ament_cmake</build_type>
27 |   </export>
28 | </package>
29 | 


--------------------------------------------------------------------------------
/rviz/rviz1.rviz:
--------------------------------------------------------------------------------
  1 | Panels:
  2 |   - Class: rviz_common/Displays
  3 |     Help Height: 78
  4 |     Name: Displays
  5 |     Property Tree Widget:
  6 |       Expanded:
  7 |         - /Global Options1
  8 |         - /Status1
  9 |         - /Grid1
 10 |         - /MarkerArray1
 11 |         - /TF1
 12 |       Splitter Ratio: 0.5
 13 |     Tree Height: 365
 14 |   - Class: rviz_common/Selection
 15 |     Name: Selection
 16 |   - Class: rviz_common/Tool Properties
 17 |     Expanded:
 18 |       - /2D Goal Pose1
 19 |       - /Publish Point1
 20 |     Name: Tool Properties
 21 |     Splitter Ratio: 0.5886790156364441
 22 |   - Class: rviz_common/Views
 23 |     Expanded:
 24 |       - /Current View1
 25 |     Name: Views
 26 |     Splitter Ratio: 0.5
 27 |   - Class: rviz_common/Time
 28 |     Experimental: false
 29 |     Name: Time
 30 |     SyncMode: 0
 31 |     SyncSource: ""
 32 | Visualization Manager:
 33 |   Class: ""
 34 |   Displays:
 35 |     - Alpha: 0.20000000298023224
 36 |       Cell Size: 10
 37 |       Class: rviz_default_plugins/Grid
 38 |       Color: 160; 160; 164
 39 |       Enabled: true
 40 |       Line Style:
 41 |         Line Width: 0.029999999329447746
 42 |         Value: Lines
 43 |       Name: Grid
 44 |       Normal Cell Count: 0
 45 |       Offset:
 46 |         X: 0
 47 |         Y: 0
 48 |         Z: 0
 49 |       Plane: XY
 50 |       Plane Cell Count: 20
 51 |       Reference Frame: <Fixed Frame>
 52 |       Value: true
 53 |     - Class: rviz_default_plugins/MarkerArray
 54 |       Enabled: true
 55 |       Name: WaypointMarkerArray
 56 |       Namespaces:
 57 |         speed_kmph: true
 58 |         waypoints: true
 59 |       Topic:
 60 |         Depth: 5
 61 |         Durability Policy: Volatile
 62 |         History Policy: Keep Last
 63 |         Reliability Policy: Reliable
 64 |         Value: /sim1/waypointarray_marker
 65 |       Value: true
 66 |     - Class: rviz_default_plugins/Axes
 67 |       Enabled: false
 68 |       Length: 50
 69 |       Name: Axes
 70 |       Radius: 0.10000000149011612
 71 |       Reference Frame: <Fixed Frame>
 72 |       Value: false
 73 |     - Class: rviz_default_plugins/MarkerArray
 74 |       Enabled: true
 75 |       Name: MarkerArray
 76 |       Namespaces:
 77 |         "": true
 78 |         cross_track_marker: true
 79 |         pursuit_closest: true
 80 |         pursuit_goal: true
 81 |       Topic:
 82 |         Depth: 5
 83 |         Durability Policy: Volatile
 84 |         History Policy: Keep Last
 85 |         Reliability Policy: Reliable
 86 |         Value: /sim1/pursuitviz
 87 |       Value: true
 88 |     - Class: rviz_default_plugins/TF
 89 |       Enabled: true
 90 |       Frame Timeout: 15
 91 |       Frames:
 92 |         All Enabled: true
 93 |         base_link:
 94 |           Value: true
 95 |         map:
 96 |           Value: true
 97 |       Marker Scale: 10
 98 |       Name: TF
 99 |       Show Arrows: true
100 |       Show Axes: true
101 |       Show Names: false
102 |       Tree:
103 |         map:
104 |           base_link:
105 |             {}
106 |       Update Interval: 0
107 |       Value: true
108 |     - Alpha: 1
109 |       Buffer Length: 1
110 |       Class: rviz_default_plugins/Path
111 |       Color: 214; 214; 214
112 |       Enabled: true
113 |       Head Diameter: 0.30000001192092896
114 |       Head Length: 0.20000000298023224
115 |       Length: 0.30000001192092896
116 |       Line Style: Billboards
117 |       Line Width: 0.20000000298023224
118 |       Name: Path
119 |       Offset:
120 |         X: 0
121 |         Y: 0
122 |         Z: 0
123 |       Pose Color: 255; 85; 255
124 |       Pose Style: None
125 |       Radius: 0.029999999329447746
126 |       Shaft Diameter: 0.10000000149011612
127 |       Shaft Length: 0.10000000149011612
128 |       Topic:
129 |         Depth: 5
130 |         Durability Policy: Volatile
131 |         Filter size: 10
132 |         History Policy: Keep Last
133 |         Reliability Policy: Reliable
134 |         Value: /sim1/marker_path
135 |       Value: true
136 |     - Class: rviz_default_plugins/Marker
137 |       Enabled: true
138 |       Name: SteeringMarker
139 |       Namespaces:
140 |         steering_path: true
141 |       Topic:
142 |         Depth: 5
143 |         Durability Policy: Volatile
144 |         Filter size: 10
145 |         History Policy: Keep Last
146 |         Reliability Policy: Reliable
147 |         Value: /sim1/marker_steering
148 |       Value: true
149 |   Enabled: true
150 |   Global Options:
151 |     Background Color: 48; 48; 48
152 |     Fixed Frame: map
153 |     Frame Rate: 30
154 |   Name: root
155 |   Tools:
156 |     - Class: rviz_default_plugins/Interact
157 |       Hide Inactive Objects: true
158 |     - Class: rviz_default_plugins/MoveCamera
159 |     - Class: rviz_default_plugins/Select
160 |     - Class: rviz_default_plugins/FocusCamera
161 |     - Class: rviz_default_plugins/Measure
162 |       Line color: 128; 128; 0
163 |     - Class: rviz_default_plugins/SetInitialPose
164 |       Covariance x: 0.25
165 |       Covariance y: 0.25
166 |       Covariance yaw: 0.06853891909122467
167 |       Topic:
168 |         Depth: 5
169 |         Durability Policy: Volatile
170 |         History Policy: Keep Last
171 |         Reliability Policy: Reliable
172 |         Value: /initialpose
173 |     - Class: rviz_default_plugins/SetGoal
174 |       Topic:
175 |         Depth: 5
176 |         Durability Policy: Volatile
177 |         History Policy: Keep Last
178 |         Reliability Policy: Reliable
179 |         Value: /goal_pose
180 |     - Class: rviz_default_plugins/PublishPoint
181 |       Single click: true
182 |       Topic:
183 |         Depth: 5
184 |         Durability Policy: Volatile
185 |         History Policy: Keep Last
186 |         Reliability Policy: Reliable
187 |         Value: /clicked_point
188 |   Transformation:
189 |     Current:
190 |       Class: rviz_default_plugins/TF
191 |   Value: true
192 |   Views:
193 |     Current:
194 |       Class: rviz_default_plugins/ThirdPersonFollower
195 |       Distance: 49.51030731201172
196 |       Enable Stereo Rendering:
197 |         Stereo Eye Separation: 0.05999999865889549
198 |         Stereo Focal Distance: 1
199 |         Swap Stereo Eyes: false
200 |         Value: false
201 |       Focal Point:
202 |         X: 0.9502010345458984
203 |         Y: -16.014331817626953
204 |         Z: -6.522667536046356e-05
205 |       Focal Shape Fixed Size: false
206 |       Focal Shape Size: 0.05000000074505806
207 |       Invert Z Axis: false
208 |       Name: Current View
209 |       Near Clip Distance: 0.009999999776482582
210 |       Pitch: 1.5147963762283325
211 |       Target Frame: <Fixed Frame>
212 |       Value: ThirdPersonFollower (rviz_default_plugins)
213 |       Yaw: 4.510385513305664
214 |     Saved: ~
215 | Window Geometry:
216 |   Displays:
217 |     collapsed: true
218 |   Height: 601
219 |   Hide Left Dock: true
220 |   Hide Right Dock: true
221 |   QMainWindow State: 000000ff00000000fd0000000400000000000001f3000001bffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003b000001bf000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000231fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b00000231000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000002530000003efc0100000002fb0000000800540069006d00650100000000000002530000025300fffffffb0000000800540069006d0065010000000000000450000000000000000000000253000001bf00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
222 |   Selection:
223 |     collapsed: false
224 |   Time:
225 |     collapsed: false
226 |   Tool Properties:
227 |     collapsed: false
228 |   Views:
229 |     collapsed: true
230 |   Width: 595
231 |   X: 364
232 |   Y: 6
233 | 


--------------------------------------------------------------------------------
/src/csv_saver.cpp:
--------------------------------------------------------------------------------
  1 | // publishes nav_msgs/Path and steering marker and km/h
  2 | 
  3 | #include <iostream>
  4 | #include <vector>
  5 | #include "rclcpp/rclcpp.hpp"
  6 | #include "nav_msgs/msg/path.hpp"
  7 | #include "geometry_msgs/msg/twist.hpp"
  8 | #include "std_msgs/msg/float32_multi_array.hpp"
  9 | #include "visualization_msgs/msg/marker.hpp"
 10 | #include "tf2/LinearMath/Quaternion.h"
 11 | #include "tf2/LinearMath/Matrix3x3.h"
 12 | #include "tf2_ros/transform_listener.h"
 13 | #include "tf2_ros/buffer.h"
 14 | #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 15 | #include "tf2/convert.h"
 16 | #include <fstream>
 17 | #include "wayp_plan_tools/common.hpp"
 18 | 
 19 | using namespace std::chrono_literals;
 20 | using std::placeholders::_1;
 21 | 
 22 | class CsvSave : public rclcpp::Node
 23 | {
 24 |     rcl_interfaces::msg::SetParametersResult parametersCallback(const std::vector<rclcpp::Parameter> &parameters)
 25 |     {
 26 |         rcl_interfaces::msg::SetParametersResult result;
 27 |         result.successful = true;
 28 |         result.reason = "success";
 29 |         for (const auto &param : parameters)
 30 |         {
 31 |             RCLCPP_INFO_STREAM(this->get_logger(), "Param update: " << param.get_name().c_str() << ": " << param.value_to_string().c_str());
 32 |             if (param.get_name() == "pose_frame")
 33 |             {
 34 |                 pose_frame = param.as_string();
 35 |             }
 36 |             if (param.get_name() == "metrics_topic")
 37 |             {
 38 |                 metrics_topic = param.as_string();
 39 |             }
 40 |             if (param.get_name() == "file_name")
 41 |             {
 42 |                 file_name = param.as_string();
 43 |             }
 44 |             if (param.get_name() == "file_dir")
 45 |             {
 46 |                 file_dir = param.as_string();
 47 |             }
 48 |             if (param.get_name() == "mod_limit")
 49 |             {
 50 |                 mod_limit = param.as_int();
 51 |             }
 52 |             file_path_xy = file_dir + "/" + file_name + "_xypose.csv";
 53 |             file_path_metrics = file_dir + "/" + file_name + "_metrics.csv";
 54 |         }
 55 |         return result;
 56 |     }
 57 | 
 58 | public:
 59 |     CsvSave() : Node("csv_saver_node")
 60 |     {
 61 |         this->declare_parameter<std::string>("pose_frame", "base_link");
 62 |         this->declare_parameter<std::string>("file_name", "tmp01");
 63 |         this->declare_parameter<std::string>("file_dir", "/home/");
 64 |         this->declare_parameter<std::string>("metrics_topic", "marker_steering");
 65 |         this->declare_parameter<int>("mod_limit", 10); // modulo limit for path size
 66 | 
 67 |         this->get_parameter("pose_frame", pose_frame);
 68 |         this->get_parameter("metrics_topic", metrics_topic);
 69 |         this->get_parameter("file_name", file_name);
 70 |         this->get_parameter("file_dir", file_dir);
 71 |         this->get_parameter("mod_limit", mod_limit);
 72 | 
 73 |         file_path_xy = file_dir + "/" + file_name + "_xypose.csv";
 74 |         file_path_metrics = file_dir + "/" + file_name + "_metrics.csv";
 75 |         tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
 76 |         tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
 77 |         sub_twist_ = this->create_subscription<geometry_msgs::msg::Twist>("/model/vehicle_blue/cmd_vel", 10, std::bind(&CsvSave::vehicleTwistCallback, this, _1));
 78 |         sub_metrics_ = this->create_subscription<std_msgs::msg::Float32MultiArray>("metrics_wayp", 10, std::bind(&CsvSave::MetricsCallback, this, _1));
 79 |         // Call loop function 20 Hz (50 milliseconds)
 80 |         timer_ = this->create_wall_timer(std::chrono::milliseconds(50), std::bind(&CsvSave::loop, this));
 81 |         callback_handle_ = this->add_on_set_parameters_callback(std::bind(&CsvSave::parametersCallback, this, std::placeholders::_1));
 82 |         RCLCPP_INFO_STREAM(this->get_logger(), "Node started: " << this->get_name());
 83 |     }
 84 | 
 85 | private:
 86 |     // Callback for steering wheel and speed cmd messages
 87 |     void vehicleTwistCallback(const geometry_msgs::msg::Twist &vehicle_msg)
 88 |     {
 89 |         steering_angle = vehicle_msg.angular.z;
 90 |         vehicle_speed_mps = vehicle_msg.linear.x;
 91 |     }
 92 | 
 93 |     // get tf2 transform from map to base_link
 94 |     void vehiclePoseFromTransform()
 95 |     {
 96 |         geometry_msgs::msg::TransformStamped transformStamped;
 97 |         try
 98 |         {
 99 |             transformStamped = tf_buffer_->lookupTransform("map", pose_frame, tf2::TimePointZero);
100 |         }
101 | 
102 |         catch (const tf2::TransformException &ex)
103 |         {
104 |             // RCLCPP_WARN(this->get_logger(), "Could not get transform: %s", ex.what());
105 |             return;
106 |         }
107 |         actual_pose.pose.position.x = transformStamped.transform.translation.x;
108 |         actual_pose.pose.position.y = transformStamped.transform.translation.y;
109 |         actual_pose.pose.position.z = transformStamped.transform.translation.z + 1.0;
110 |         actual_pose.pose.orientation.x = transformStamped.transform.rotation.x;
111 |         actual_pose.pose.orientation.y = transformStamped.transform.rotation.y;
112 |         actual_pose.pose.orientation.z = transformStamped.transform.rotation.z;
113 |         actual_pose.pose.orientation.w = transformStamped.transform.rotation.w;
114 |         actual_pose.header.stamp = this->now();
115 |         actual_pose.header.frame_id = "map";
116 |         // RCLCPP_INFO_STREAM(this->get_logger(), "actual_pose: " << actual_pose.pose.position.x << ", " << actual_pose.pose.position.y << ", " << actual_pose.pose.position.z);
117 |     }
118 | 
119 |     void loop()
120 |     {
121 |         if (first_run)
122 |         {
123 |             RCLCPP_INFO_STREAM(this->get_logger(), "CSV xy path: " << file_path_xy);
124 |             start_time = this->now();
125 |             first_run = false;
126 |             std::ofstream file;
127 |             file.open(file_path_xy, std::ios_base::app);
128 |             file << "x,y,time,steering_angle,speed_mps,cur_lat_dist_abs,trg_way_lon_dist" << "\n";
129 |         }
130 |         vehiclePoseFromTransform();
131 |         geometry_msgs::msg::PoseStamped pose;
132 | 
133 |         if (loop_increment % mod_limit == 0)
134 |         {
135 |             // 3 decmial places for x, y
136 |             std::ofstream file;
137 |             file.open(file_path_xy, std::ios_base::app);
138 |             double actual_time = (this->now() - start_time).seconds();
139 |             file << std::fixed << std::setprecision(4) << actual_pose.pose.position.x << "," << actual_pose.pose.position.y << "," << actual_time << "," << steering_angle << "," << vehicle_speed_mps << "," << cur_lat_dist_abs << "," << trg_way_lon_dist << "\n";
140 |             // RCLCPP_INFO_STREAM(this->get_logger(), "XY:" << actual_pose.pose.position.x << "," << actual_pose.pose.position.y);
141 |         }
142 | 
143 |         loop_increment += 1;
144 |     }
145 | 
146 |     void MetricsCallback(const std_msgs::msg::Float32MultiArray &msg)
147 |     {
148 |         current_waypoint_id = msg.data[common_wpt::CUR_WAYPOINT_ID];
149 |         cur_lat_dist_abs = msg.data[common_wpt::CUR_LAT_DIST_ABS];
150 |         trg_way_lon_dist = msg.data[common_wpt::TRG_WAY_LON_DIST];
151 |         // RCLCPP_INFO_STREAM(this->get_logger(), "Max lateral distance: " << std::setprecision(2) << msg.data[common_wpt::MAX_LAT_DISTANCE] << " m");
152 |         // RCLCPP_INFO_STREAM(this->get_logger(), "Avg lateral distance: " << std::setprecision(2) << msg.data[common_wpt::AVG_LAT_DISTANCE] << " m");
153 |         // RCLCPP_INFO_STREAM(this->get_logger(), "Metrics callback || waypoint ID: " << current_waypoint_id << " - prev wayp ID: " << previous_waypoint_id);
154 |         if (previous_waypoint_id > current_waypoint_id and previous_waypoint_id != -1)
155 |         {
156 |             last_waypoint_reached_time = this->now();
157 |             RCLCPP_INFO_STREAM(this->get_logger(), "Last waypoint reached");
158 |         }
159 |         if ((last_waypoint_reached_time - this->now()).nanoseconds() / -1e9 > 15.0)
160 |         {
161 |             RCLCPP_INFO_STREAM(this->get_logger(), "Loop finished more than 15s ago, Finished CSV saving  || waypoint ID: " << current_waypoint_id << " - prev wayp ID: " << previous_waypoint_id);
162 |             RCLCPP_INFO_STREAM(this->get_logger(), "CSV metrics path: " << file_path_metrics);
163 |             std::ofstream file_m;
164 |             file_m.open(file_path_metrics, std::ios_base::app);
165 |             file_m << std::fixed << std::setprecision(4) << "MAX_LAT_DISTANCE," << msg.data[common_wpt::MAX_LAT_DISTANCE] << "\n";
166 |             file_m << std::fixed << std::setprecision(4) << "AVG_LAT_DISTANCE," << msg.data[common_wpt::AVG_LAT_DISTANCE] << "\n";
167 |             rclcpp::shutdown();
168 |         }
169 |         previous_waypoint_id = current_waypoint_id;
170 |     }
171 | 
172 |     rclcpp::TimerBase::SharedPtr timer_;
173 |     rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr sub_twist_;
174 |     rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr sub_metrics_;
175 |     OnSetParametersCallbackHandle::SharedPtr callback_handle_;
176 |     std::string pose_topic, metrics_topic, path_topic, pose_frame, file_name, file_dir, file_path_xy, file_path_metrics;
177 |     double steering_angle, vehicle_speed_mps, trg_way_lon_dist = 0.0, cur_lat_dist_abs = 0.0;
178 |     bool first_run = true;
179 |     geometry_msgs::msg::PoseStamped actual_pose;
180 |     rclcpp::Time start_time;
181 |     std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
182 |     std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};
183 |     int loop_increment = 0, mod_limit = 20, previous_waypoint_id = -1, current_waypoint_id = 0;
184 |     rclcpp::Time last_waypoint_reached_time = this->now() + rclcpp::Duration(36000, 0); // init with 10h in the future
185 | };
186 | 
187 | int main(int argc, char **argv)
188 | {
189 |     rclcpp::init(argc, argv);
190 |     rclcpp::spin(std::make_shared<CsvSave>());
191 |     rclcpp::shutdown();
192 |     return 0;
193 | }


--------------------------------------------------------------------------------
/src/pose_arr_to_tf.cpp:
--------------------------------------------------------------------------------
 1 | #include <chrono>
 2 | #include <functional>
 3 | #include <memory>
 4 | #include <string>
 5 | #include "rclcpp/rclcpp.hpp"
 6 | #include "geometry_msgs/msg/pose_array.hpp"
 7 | #include "tf2_ros/buffer.h"
 8 | #include "tf2/exceptions.h"
 9 | #include "tf2_ros/transform_broadcaster.h"
10 | #include "rcl_interfaces/msg/set_parameters_result.hpp"
11 | 
12 | using namespace std::chrono_literals;
13 | using std::placeholders::_1;
14 | 
15 | class PoseArrayToTf : public rclcpp::Node
16 | {
17 |     geometry_msgs::msg::Pose previous_pose;
18 |     float speed_mps;
19 |     rcl_interfaces::msg::SetParametersResult parametersCallback(const std::vector<rclcpp::Parameter> &parameters)
20 |     {
21 |         rcl_interfaces::msg::SetParametersResult result;
22 |         result.successful = true;
23 |         result.reason = "success";
24 |         for (const auto &param : parameters)
25 |         {
26 |             RCLCPP_INFO_STREAM(this->get_logger(), "Param update: " << param.get_name().c_str() << ": " << param.value_to_string().c_str());
27 |             if (param.get_name() == "pose_topic")
28 |             {
29 |                 pose_topic = param.as_string();
30 |             }
31 |         }
32 |         return result;
33 |     }
34 | 
35 | public:
36 |     PoseArrayToTf() : Node("waypoint_saver_node")
37 |     {
38 | 
39 |         this->declare_parameter<std::string>("pose_topic", "");
40 | 
41 |         this->get_parameter("pose_topic", pose_topic);
42 | 
43 |         callback_handle_ = this->add_on_set_parameters_callback(std::bind(&PoseArrayToTf::parametersCallback, this, std::placeholders::_1));
44 | 
45 |         sub_pose_array_ = this->create_subscription<geometry_msgs::msg::PoseArray>(pose_topic, 10, std::bind(&PoseArrayToTf::poseArrayCallback, this, _1));
46 |         tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
47 |     }
48 | 
49 | private:
50 |     void poseArrayCallback(const geometry_msgs::msg::PoseArray &current_pose_arr)
51 |     {
52 |          - previous_pose.position.x;
53 |         t.header.stamp = this->get_clock()->now();
54 |         t.header.frame_id = "map";
55 |         t.child_frame_id = "base_link";
56 |         t.transform.translation.x = current_pose_arr.poses[1].position.x;
57 |         t.transform.translation.y = current_pose_arr.poses[1].position.y;
58 |         t.transform.rotation = current_pose_arr.poses[1].orientation;
59 |         tf_broadcaster_->sendTransform(t);
60 |     }
61 | 
62 |     rclcpp::Subscription<geometry_msgs::msg::PoseArray>::SharedPtr sub_pose_array_;
63 |     std::string pose_topic;
64 |     std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
65 |     geometry_msgs::msg::TransformStamped t;
66 |     OnSetParametersCallbackHandle::SharedPtr callback_handle_;
67 | };
68 | 
69 | int main(int argc, char *argv[])
70 | {
71 |     rclcpp::init(argc, argv);
72 |     rclcpp::spin(std::make_shared<PoseArrayToTf>());
73 |     rclcpp::shutdown();
74 |     return 0;
75 | }


--------------------------------------------------------------------------------
/src/visuals.cpp:
--------------------------------------------------------------------------------
  1 | // publishes nav_msgs/Path and steering marker and km/h
  2 | 
  3 | #include <iostream>
  4 | #include <vector>
  5 | #include "rclcpp/rclcpp.hpp"
  6 | #include "nav_msgs/msg/path.hpp"
  7 | #include "geometry_msgs/msg/twist.hpp"
  8 | #include "visualization_msgs/msg/marker.hpp"
  9 | #include "tf2/LinearMath/Quaternion.h"
 10 | #include "tf2/LinearMath/Matrix3x3.h"
 11 | #include "tf2_ros/transform_listener.h"
 12 | #include "tf2_ros/buffer.h"
 13 | #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 14 | #include "tf2/convert.h"
 15 | 
 16 | using namespace std::chrono_literals;
 17 | using std::placeholders::_1;
 18 | 
 19 | class Visuals : public rclcpp::Node
 20 | {
 21 | public:
 22 |     Visuals() : Node("visuals_node")
 23 |     {
 24 |         this->declare_parameter<std::string>("pose_frame", "base_link");
 25 |         this->declare_parameter<std::string>("marker_topic", "marker_steering");
 26 |         this->declare_parameter<std::string>("path_topic", "marker_path");
 27 |         this->declare_parameter<bool>("publish_steer_marker", true);
 28 |         this->declare_parameter<int>("path_size", 1500);
 29 |         this->declare_parameter<int>("mod_limit", 10); // modulo limit for path size (publish every n-th message)
 30 | 
 31 |         this->get_parameter("pose_frame", pose_frame);
 32 |         this->get_parameter("marker_topic", marker_topic);
 33 |         this->get_parameter("path_topic", path_topic);
 34 |         this->get_parameter("publish_steer_marker", publish_steer_marker);
 35 |         this->get_parameter("path_size", path_size);
 36 |         this->get_parameter("mod_limit", mod_limit);
 37 | 
 38 |         tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
 39 |         tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
 40 |         marker_pub = this->create_publisher<visualization_msgs::msg::Marker>(marker_topic, 1);
 41 |         path_pub = this->create_publisher<nav_msgs::msg::Path>(path_topic, 1);
 42 |         sub_twist_ = this->create_subscription<geometry_msgs::msg::Twist>("/model/vehicle_blue/cmd_vel", 10, std::bind(&Visuals::vehicleTwistCallback, this, _1));
 43 |         // Call loop function 20 Hz (50 milliseconds)
 44 |         timer_ = this->create_wall_timer(std::chrono::milliseconds(50), std::bind(&Visuals::loop, this));
 45 |         RCLCPP_INFO_STREAM(this->get_logger(), "Node started: " << this->get_name());
 46 |     }
 47 | 
 48 | private:
 49 | 
 50 |     // Callback for steering wheel and speed cmd messages
 51 |     void vehicleTwistCallback(const geometry_msgs::msg::Twist &vehicle_msg)
 52 |     {
 53 |         steering_angle = vehicle_msg.angular.z;
 54 |         vehicle_speed_mps = vehicle_msg.linear.x;
 55 |     }
 56 | 
 57 |     // get tf2 transform from map to base_link
 58 |     void vehiclePoseFromTransform()
 59 |     {
 60 |         geometry_msgs::msg::TransformStamped transformStamped;
 61 |         try
 62 |         {
 63 |             transformStamped = tf_buffer_->lookupTransform("map", pose_frame, tf2::TimePointZero);
 64 |         }
 65 | 
 66 |         catch (const tf2::TransformException &ex)
 67 |         {
 68 |             // RCLCPP_WARN(this->get_logger(), "Could not get transform: %s", ex.what());
 69 |             return;
 70 |         }
 71 |         actual_pose.pose.position.x = transformStamped.transform.translation.x;
 72 |         actual_pose.pose.position.y = transformStamped.transform.translation.y;
 73 |         actual_pose.pose.position.z = transformStamped.transform.translation.z + 1.0;
 74 |         actual_pose.pose.orientation.x = transformStamped.transform.rotation.x;
 75 |         actual_pose.pose.orientation.y = transformStamped.transform.rotation.y;
 76 |         actual_pose.pose.orientation.z = transformStamped.transform.rotation.z;
 77 |         actual_pose.pose.orientation.w = transformStamped.transform.rotation.w;
 78 |         actual_pose.header.stamp = this->now();
 79 |         actual_pose.header.frame_id = "map";
 80 |         // RCLCPP_INFO_STREAM(this->get_logger(), "actual_pose: " << actual_pose.pose.position.x << ", " << actual_pose.pose.position.y << ", " << actual_pose.pose.position.z);
 81 |     }
 82 | 
 83 |     void loop()
 84 |     {
 85 |         vehiclePoseFromTransform();
 86 |         if (publish_steer_marker)
 87 |         {
 88 |             visualization_msgs::msg::Marker steer_marker;
 89 |             steer_marker.header.frame_id = "/base_link";
 90 |             steer_marker.header.stamp = this->now();
 91 |             steer_marker.ns = "steering_path";
 92 |             steer_marker.id = 0;
 93 |             steer_marker.type = steer_marker.LINE_STRIP;
 94 |             steer_marker.action = visualization_msgs::msg::Marker::MODIFY;
 95 |             steer_marker.pose.position.x = 0;
 96 |             steer_marker.pose.position.y = 0;
 97 |             steer_marker.pose.position.z = 0;
 98 |             steer_marker.pose.orientation.x = 0.0;
 99 |             steer_marker.pose.orientation.y = 0.0;
100 |             steer_marker.pose.orientation.z = 0.0;
101 |             steer_marker.pose.orientation.w = 1.0;
102 |             steer_marker.scale.x = 0.6;
103 |             // https://github.com/jkk-research/colors
104 |             steer_marker.color.r = 0.94f;
105 |             steer_marker.color.g = 0.83f;
106 |             steer_marker.color.b = 0.07f;
107 |             steer_marker.color.a = 1.0;
108 |             steer_marker.lifetime = rclcpp::Duration::from_seconds(0);
109 |             double marker_pos_x = 0.0, marker_pos_y = 0.0, theta = 0.0;
110 |             for (int i = 0; i < 80; i++)
111 |             {
112 |                 // Ackermann steering model
113 |                 marker_pos_x += 0.01 * 10 * cos(theta);
114 |                 marker_pos_y += 0.01 * 10 * sin(theta);
115 |                 theta += 0.01 * 10 / wheelbase * tan(steering_angle);
116 |                 geometry_msgs::msg::Point p;
117 |                 p.x = marker_pos_x;
118 |                 p.y = marker_pos_y;
119 |                 steer_marker.points.push_back(p);
120 |             }
121 |             marker_pub->publish(steer_marker);
122 |             steer_marker.points.clear();
123 |         }
124 |         geometry_msgs::msg::PoseStamped pose;
125 | 
126 |         pose.header.stamp = this->now();
127 |         pose.pose.position = actual_pose.pose.position;
128 |         pose.header.frame_id = "map";
129 |         path.header.frame_id = "map";
130 |         pose.pose.orientation = actual_pose.pose.orientation;
131 |         path.poses.push_back(pose);
132 |         path.header.stamp = this->now();
133 |         if (loop_increment % mod_limit == 0){
134 |             path.poses.push_back(pose);
135 |         }
136 |         // keep only the last n (path_size) path message
137 |         if (path.poses.size() > path_size)
138 |         {
139 |             int shift = path.poses.size() - path_size;
140 |             path.poses.erase(path.poses.begin(), path.poses.begin() + shift);
141 |         }
142 | 
143 |         path_pub->publish(path);
144 |         loop_increment += 1;
145 |     }
146 |     rclcpp::TimerBase::SharedPtr timer_;
147 |     rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr sub_twist_;
148 |     std::string pose_topic, marker_topic, path_topic, pose_frame;
149 |     const double wheelbase = 1.0;
150 |     double steering_angle, vehicle_speed_mps;
151 |     long unsigned int path_size;
152 |     bool steering_enabled;
153 |     bool first_run = true, publish_steer_marker, publish_kmph;
154 |     std::string marker_color;
155 |     rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr marker_pub;
156 |     rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_pub;
157 |     nav_msgs::msg::Path path;
158 |     geometry_msgs::msg::PoseStamped actual_pose;
159 |     std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
160 |     std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};
161 |     int loop_increment = 0, mod_limit = 20;
162 | };
163 | 
164 | int main(int argc, char **argv)
165 | {
166 |     rclcpp::init(argc, argv);
167 |     rclcpp::spin(std::make_shared<Visuals>());
168 |     rclcpp::shutdown();
169 |     return 0;
170 | }


--------------------------------------------------------------------------------