Open jara001 opened 2 years ago
Small update: In 0366d00f2cee5652abc7818375fa6f803c4383c0 a YAML support was added to 'ng_generate_data'. Therefore it should be much faster and easier to prepare data for optimization.
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 \;
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'"
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
cat $BAG.command.data.stamped | grep steer > $BAG.command.steer.data.stamped
cat $BAG.command.data.stamped | grep speed > $BAG.command.speed.data.stamped
cat $BAG*stamped | cut -d' ' -f1 | sort | head -n1 | cut -d. -f1 > $BAG.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)'"
gnuplot -p -e "plot '$BAG.trajectory.data' with lines title 'Speed (Plan)'"
Model improvement ideas:
Alternative model ideas:
For plotting the plan we need to obtain the trajectory time (as it is not yet contained in the message).
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
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
.
paste $BAG.trajectory.time $BAG.trajectory.data > $BAG.trajectory.data.stamped
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)'"
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: