Orientation issues with CARLA ego-vehicle spawning?

[rohanb2018]

Hello, I noticed that there seemed to be two issues in the processing of spawn points passed to the CARLA ego-vehicle node.

Issue 1: Refers to the code here.

In the code snippet I referenced, it seems that spawn_point_param (provided through the spawn_point ROS parameter) is intended to be in the CARLA default (left-handed) coordinate system, and that self.actor_spawnpoint is intended to be in the ROS ENU (right-handed) coordinate system. When self.actor_spawnpoint is being created, however, it seems that although the position is being converted appropriately, the orientation is not being transformed into the ROS coordinate system.

To fix the problem, I ended up applying the carla_rotation_to_ros_quaternion utility function located in carla_ros_bridge/transforms.py (here) to the RPY values coming from the user.

Are my assumptions correct about what coordinate frames spawn_point_param and self.actor_spawnpoint are in?

Issue 2: Refers to the code here.

It seems that the roll and pitch values are being ignored entirely when spawning the vehicle - is this intentional?

I noticed that if the vehicle is given an initial spawn point (through the spawn-point ROS parameter) with RPY = (0, -180, 0), then the spawn_point variable actually ends up having a yaw angle of +180. I checked the RPY values given by euler_from_quaternion and they seem to be RPY = (180, 0, 180), which explains things.

My hypothesis is that this occurs because of gimbal lock, which makes it possible for multiple sets of RPY angles to correspond to the same 3D orientation in space. The issue is when we convert from RPY to quaternion and then back to RPY, we may not land up with the same RPY set that we started with. So when we truncate the roll and pitch angles, this might actually corrupt our final rotational state. This only seemed to happen when the pitch was sufficiently high, which is not a very realistic orientation for a vehicle.

My solution was to add the roll and pitch angles back in, so that at least the original 3D orientation would always be preserved (even if the RPY values change). I also again tried to account for the fact that self.actor_spawnpoint is in the ROS ENU (right-handed) frame, while spawn_point should be in the left-handed CARLA coordinate frame, by inverting the transform found here, which actually ends up being the same transformation.