Improvement of the profile criterion

[jara001]

Currently, the profile criterion is a little bit off. Predicted time does not correspond to the real value (the car drives using the trajectory faster than the estimated time). It could be a problem of the trajectory tracker, however, it also shows that it is possible to drive faster.

Therefore, it is required to do some testing on the car.

Possible steps:

• Build a racing track.
• Create a map of the track (Cartographer).
• Run the optimization on the track.
• Obtain the best / any solution in order to test parameters.
• Tune the parameters on the offline profiler (i.e., 'profile_trajectory') to match the real situation.

[jara001]

Small update: In 0366d00 a YAML support was added to 'ng_generate_data'. Therefore it should be much faster and easier to prepare data for optimization.

[jara001]

Testing on the car

tmux new-session \; \
	rename-window "Sensors" \; \
    send-keys "auto sensors launch lidar" Enter \; \
    split-window -h \; \
    send-keys "auto sensors launch imu" Enter \; \
	new-window \; \
    rename-window "Vehicle Platform" \; \
    send-keys "auto components launch vesc" Enter \; \
    split-window \; \
    send-keys "auto components launch drive_api_vesc" Enter \; \
    select-pane -U \; \
    split-window -h \; \
    send-keys "auto components launch teensy" Enter \; \
    new-window \; \
    rename-window "Mapping" \; \
    send-keys "auto components launch cartographer_mapping" Enter \; \
    split-window \; \
    send-keys "history -s auto save_slam" Enter \; \
    new-window \; \
    rename-window "Localization" \; \
    send-keys "history -s roslaunch cartographer_slam localization.launch state_name:=" Enter \; \
    new-window \; \
    rename-window "Path" \; \
    send-keys "history -s roslaunch simple_trajectory start.launch" Enter \; \
    split-window \; \
    send-keys "history -s roslaunch profile_trajectory2 start.launch remap:=true path:=/reference_path/path" Enter \; \
    new-window \; \
    rename-window "Trajectory (Do not use)" \; \
    send-keys "history -s rosrun follow_trajectory main.py /tf/converted:=/odom" Enter \; \
    new-window \; \
    rename-window "Trajectory (with converter)" \; 
    send-keys "history -s roslaunch tf_converter start.launch" Enter \; \
    split-window \; \
    send-keys "history -s rosrun follow_trajectory main.py" Enter \; \
    select-pane -U \; \
    split-window -h \; \
    send-keys "history -s roslaunch trajectory_visualization start.launch" Enter \; \
    next-window \;

Obtain data from rosbag

Filter

rosbag filter $BAG.bag $BAG.tf.converted.bag "topic == '/tf/converted'"
rosbag filter $BAG.bag $BAG.sensors.core.bag "topic == '/sensors/core'"
rosbag filter $BAG.bag $BAG.command.bag "topic == '/command'"
rosbag filter $BAG.bag $BAG.trajectory.bag "topic == '/trajectory'"

Obtain data

rosbag filter -p "'%010d.%09d %f %f' % (t.secs, t.nsecs, m.pose.pose.position.x, m.pose.pose.position.y)" $BAG.tf.converted.bag /dev/null "True" | grep MATCH | sed 's/.*MATCH //g' > $BAG.tf.converted.data.stamped
rosbag filter -p "'%010d.%09d %f' % (t.secs, t.nsecs, m.state.speed)" $BAG.sensors.core.bag /dev/null "True" | grep MATCH | sed 's/.*MATCH //g' > $BAG.sensors.core.data.stamped
rosbag filter -p "'\nMATCH '.join(['%010d.%09d %s %s %f' % (t.secs, t.nsecs, a.command, a.parameters[0].parameter, a.parameters[0].value) for a in m.commands])" $BAG.command.bag /dev/null True | grep MATCH | sed 's/.*MATCH //g' > $BAG.command.data.stamped
rosbag filter -p "'\nMATCH '.join(['%f' % s for s in m.velocities])" $BAG.trajectory.bag /dev/null True | grep MATCH | sed 's/.*MATCH //g' > $BAG.trajectory.data

Filter command data

cat $BAG.command.data.stamped | grep steer > $BAG.command.steer.data.stamped
cat $BAG.command.data.stamped | grep speed > $BAG.command.speed.data.stamped

Timeshift

cat $BAG*stamped | cut -d' ' -f1 | sort | head -n1 | cut -d. -f1 > $BAG.timeshift

Plot

gnuplot -p -e "plot '$BAG.sensors.core.data.stamped' using (\$1-`cat $BAG.timeshift`):(\$2/4100) with lines title 'Speed (VESC)', '$BAG.command.speed.data.stamped' using (\$1-`cat $BAG.timeshift`):4 with lines title 'Speed (Command)'"
gnuplot -p -e "plot '$BAG.trajectory.data' with lines title 'Speed (Plan)'"

[jara001]

Model improvement ideas:

• Remeasure the acceleration limits.
• Use different friction for deacceleration.
• Take the delays of the platform into the account.
• Plan / execute the trajectory while respecting the delays.

Alternative model ideas:

• https://f1tenth-gym.readthedocs.io/en/latest/api/dynamic_models.html
• https://f1tenth-gym.readthedocs.io/en/latest/customized_usage.html#changing-parameters-in-vehicle-dynamics
• https://gitlab.lrz.de/tum-cps/commonroad-vehicle-models/-/blob/master/PYTHON/vehiclemodels/vehicle_dynamics_st.py

[jara001]

Evaluating the results

For plotting the plan we need to obtain the trajectory time (as it is not yet contained in the message).

Prepare the workstation

In here, use the real values.

roslaunch profile_trajectory2 start.launch path:=/reference_path/path remap:=true v_lim:=4 a_acc_max:=4 a_break_max:=4

Play the data

Modify the profile trajectory to print out the _t array before sending the message.

rosbag play $BAG.bag --topics /reference_path/path

Copy the printed data to $BAG.trajectory.time.

Merge the data

paste $BAG.trajectory.time $BAG.trajectory.data > $BAG.trajectory.data.stamped

Plot with plan

First, set variable `TIMESHIFT to the real value (start with 0).

TIMESHIFT=

gnuplot -p -e "plot '$BAG.sensors.core.data.stamped' using (\$1-`cat $BAG.timeshift`):(\$2/4100) with lines title 'Speed (VESC)', '$BAG.command.speed.data.stamped' using (\$1-`cat $BAG.timeshift`):4 with lines title 'Speed (Command)', '$BAG.trajectory.data.stamped' using (\$1+$TIMESHIFT):(\$2) with lines title 'Speed (Plan)'"