VRichardJP
commented
8 months ago One strange behavior I observe is that when the path is broken while the vehicle is asked to go reverse, the trajectory orientation is reversed.
Here the generation is correct:
Custom freespace planner trajectory:
header:
stamp:
sec: 1703640772
nanosec: 956512242
frame_id: map
points:
- time_from_start:
sec: 0
nanosec: 0
pose:
position:
x: 3725.650146484375
y: 73740.2265625
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.20226494505023526
w: 0.979330838891447
longitudinal_velocity_mps: -0.25
lateral_velocity_mps: 0.0
acceleration_mps2: 0.10000000149011612
heading_rate_rps: 0.05014345049858093
front_wheel_angle_rad: 0.0
rear_wheel_angle_rad: 0.0
[...]motion_velocity_smoother trajectory:
header:
stamp:
sec: 1703640773
nanosec: 356973420
frame_id: map
points:
- time_from_start:
sec: 0
nanosec: 0
pose:
position:
x: 3725.650146484375
y: 73740.2265625
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.20226494505023526
w: 0.979330838891447
longitudinal_velocity_mps: -0.6013365983963013
lateral_velocity_mps: 0.0
acceleration_mps2: -0.7235000133514404
heading_rate_rps: 0.05264103040099144
front_wheel_angle_rad: 0.0
rear_wheel_angle_rad: 0.0As you can see, orientation field is the same in both case. However when the path is broken I get this:
freespace planner trajectory:
header:
stamp:
sec: 1703640202
nanosec: 83022253
frame_id: map
points:
- time_from_start:
sec: 0
nanosec: 342219741
pose:
position:
x: 3707.8560223578856
y: 73733.6997291461
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.6741909033788862
w: 0.7385571242640351
longitudinal_velocity_mps: -0.1900017261505127
lateral_velocity_mps: 0.0
acceleration_mps2: -1.0888986587524414
heading_rate_rps: 0.012745929881930351
front_wheel_angle_rad: -0.0
rear_wheel_angle_rad: 0.0motion_velocity_smoother trajectory:
header:
stamp:
sec: 1703640201
nanosec: 683058660
frame_id: map
points:
- time_from_start:
sec: 0
nanosec: 342219741
pose:
position:
x: 3707.8560223578856
y: 73733.6997291461
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.7288375537869278
w: 0.6846866583992178
longitudinal_velocity_mps: -0.0
lateral_velocity_mps: 0.0
acceleration_mps2: 0.10000000149011612
heading_rate_rps: 0.012745929881930351
front_wheel_angle_rad: 0.0
rear_wheel_angle_rad: 0.0The position is exactly the same, but orientation has been reversed:
stale[bot]
Checklist
Description
freespace_planner trajectory:
Velocity smoother trajectory:
Expected behavior
velocity smoother should output an acceptable trajectory?
Actual behavior
the trajectory contains many points, but it looks broken. Autoware does not want to drive
Steps to reproduce
freespace planner +
Versions
No response
Possible causes
It may be related to the "partial trajectory" logic implemented in the freespace planner.
I observe this behavior typically happens in the following situation:
where the freespace algorithm wants to drive slightly forward first in order to adjust the vehicle orientation, and then drive backward to the goal point. If the turn point happen to be close to the current location (based on
th_arrived_distance_mparameter), then the freespace planner decide to skip the forward part and only send the backward trajectory. I have added a couple printf in the code to verify that. In particular I print for each point of the trajectory whetheris_backis set (so here from index 0 to 4 we are supposed to drive forward), and the indices of the partial trajectory generated:So here the freespace planner decided to skip the small forward section.
I am not sure why this would cause an issue for the velocity smoother:
I am not sure this is the problem though, because I still get the issue when I reduce
th_arrived_distance_m.any idea?
Additional context
No response