Sensor spawn at wrong position / in wrong tf

[Roosstef]

Hey, i noticed that in the latest commit the sensors are spawned in a wrong tf. See pictures. Same sensor definition file in different commits. Top: Commit c9f33e0 Bottom: 9a9eb9b (current master head)

[Roosstef]

Any updates on this issue? It seems that this problem still occurs in the latest master.

[lasuomela]

Seems to me there are multiple issues with the sensors and tf2.

I'm running the ros-bridge @ 9a9eb9b with ROS Foxy.

In objects.json, I have defined an rgb camera as follows:

{ "type": "sensor.camera.rgb", "id": "rgb_front", "spawn_point": {"x": 2.0, "y": 0.0, "z": 2.0, "roll": 0.0, "pitch": 0.0, "yaw": 270.0}, "image_size_x": 800, "image_size_y": 600, "fov": 90.0 }

Now, if I run the carla simulator and ros-bridge, and start the ego_vehicle at x=150, y=-130 (/map frame coordinates), and check the transformations with

ros2 run tf2_ros tf2_echo map ego_vehicle At time 9.540466212 -Translation: [150.000, -133.000, 0.001] -Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]

which is fine.

Now, the transformation for the 'rgb_front' is

ros2 run tf2_ros tf2_echo ego_vehicle ego_vehicle/rgb_front At time 10.440466226 -Translation: [0.000, 0.000, 0.000] -Rotation: in Quaternion [-0.500, 0.500, -0.500, 0.500] ``

which is not correct. What's more, if we start the simulator and the ros-bridge again, and start the vehicle at the same position as before (x=150, y=-133)

ros2 run tf2_ros tf2_echo ego_vehicle ego_vehicle/rgb_front At time 31.666939977 -Translation: [-14.213, -87.714, -0.285] -Rotation: in Quaternion [-0.707, -0.000, 0.000, 0.707]

which in addition to not being correct is also inconsistent with the earlier run. Restarting the simulator and ros-bridge multiple times, the output of 'ros2 run tf2_ros tf2_echo ego_vehicle ego_vehicle/rgb_front' changes for each run. Shouldn't the transform ego_vehicle -> ego_vehicle/rgb_front be the offset between the vehicle body and the sensor, which we specify in the objects.json? Now it seems like the ego_vehicle -> ego_vehicle/sensor transformations take on random values. I have no idea on how to start fixing this, though.

Also, /tf_static is still not publishing the static transforms between 'ego_vehicle' and 'ego_vehicle/sensor' frames, as pointed out in #338. Based on discussion in #23, I understand publishing the relationships between sensors and vehicle frame though /tf_static would be the desired behavior .

[KevinLADLee]

After reviewing the code, I think this problem is relative to the 'relative_spawn_pose' variable. So I changed the actor_factory, make it directly get from the sensor definition file. It seems to work for me. Could you please help me to confirm that this is fixing the issue? Thanks. @Roosstef @lasuomela

[lasuomela]

Hi,

Thanks for the quick response @KevinLADLee. I checked the commit, and seems like it partly fixes the problem. For the IMU and GNSS sensors, it seems like the problem is now fixed, eg.

{
            "type": "sensor.other.imu",`
            "id": "imu",
            "spawn_point": {"x": 2.0, "y": 0.0, "z": 2.0, "roll": 0.0, "pitch": 0.0, "yaw": 270.0},
             "noise_accel_stddev_x": 0.0, "noise_accel_stddev_y": 0.0, "noise_accel_stddev_z": 0.0,
             "noise_gyro_stddev_x": 0.0, "noise_gyro_stddev_y": 0.0, "noise_gyro_stddev_z": 0.0,
             "noise_gyro_bias_x": 0.0, "noise_gyro_bias_y": 0.0, "noise_gyro_bias_z": 0.0
}

results in

ros2 run tf2_ros tf2_echo ego_vehicle ego_vehicle/imu
At time 643.900159741
- Translation: [2.000, 0.000, 2.000]
- Rotation: in Quaternion [0.000, 0.000, -0.707, 0.707]

which is correct! Nice. Same is true for the GNSS.

But for the camera sensors

                {
                    "type": "sensor.camera.rgb",
                    "id": "rgb_front",
                    "spawn_point": {"x": 2.0, "y": 0.0, "z": 2.0, "roll": 0.0, "pitch": 0.0, "yaw": 270.0},
                    "image_size_x": 800,
                    "image_size_y": 600,
                    "fov": 90.0
                },

->

ros2 run tf2_ros tf2_echo ego_vehicle ego_vehicle/rgb_front
At time 800.300162072
- Translation: [2.000, 0.000, 2.000]
- Rotation: in Quaternion [-0.000, -0.707, 0.707, 0.000]

So now the translation is correct, but the rotation is off. With camera spec

                {
                    "type": "sensor.camera.rgb",
                    "id": "rgb_front",
                    "spawn_point": {"x": 4.0, "y": 0.0, "z": 2.0, "roll": 0.0, "pitch": 0.0, "yaw": 0.0},
                    "image_size_x": 800,
                    "image_size_y": 600,
                    "fov": 90.0
                }

the result is

ros2 run tf2_ros tf2_echo ego_vehicle ego_vehicle/rgb_front
At time 800.300162072
- Translation: [4.000, 0.000, 2.000]
- Rotation: in Quaternion [-0.500, 0.500, -0.500, 0.500]

Which also has the correct translation but wrong orientation.

For a camera sensor used for visualization purposes,

                {
                    "type": "sensor.camera.rgb",
                    "id": "rgb_view",
                    "spawn_point": {"x": -4.5, "y": 0.0, "z": 2.8, "roll": 0.0, "pitch": 20.0, "yaw": 0.0},
                    "image_size_x": 800,
                    "image_size_y": 600,
                    "fov": 90.0,
                    "attached_objects":
                    [
                        {
                            "type": "actor.pseudo.control",
                            "id": "control"
                        }
                    ]
                },

the results are

ros2 run tf2_ros tf2_echo ego_vehicle ego_vehicle/rgb_view
At time 800.300162072
- Translation: [-11.950, 0.373, 7.050]
- Rotation: in Quaternion [-0.567, 0.585, -0.416, 0.403]

which is incorrect in both translation and rotation.

For the same camera with the spec

                {
                    "type": "sensor.camera.rgb",
                    "id": "rgb_view",
                    "spawn_point": {"x": -3.0, "y": 0.0, "z": 1.0, "roll": 0.0, "pitch": 20.0, "yaw": 0.0},
                    "image_size_x": 800,
                    "image_size_y": 600,
                    "fov": 90.0,
                    "attached_objects":
                    [
                        {
                            "type": "actor.pseudo.control",
                            "id": "control"
                        }
                    ]
                },

the output, weirdly, is

At time 806.950162171
- Translation: [-11.950, 0.373, 7.050]
- Rotation: in Quaternion [-0.567, 0.585, -0.416, 0.403]

which is incorrect but also the same as the output with the previous sensor spec. So seems that the rgb_view is getting its configuration somewhere else than the objects.json file that we define in the carla_example_ego_vehicle.launch.py launch parameters.

It's weird that the issue seems to be affecting the rgb cameras but not the other sensors that I tested.

[KevinLADLee]

Hi, @lasuomela . I have also tested the code by running the following commands.

# For ros1
roslaunch carla_ros_bridge carla_ros_bridge_with_example_ego_vehicle.launch 
# For ros2
ros2 launch carla_ros_bridge carla_ros_bridge_with_example_ego_vehicle.launch.py

The first issue you mentioned about camera sensor rotation is not wrong. Because the definition of the camera coordinate system is different from lidar, GNSS, and IMU sensors in ROS. For more information about this, you could have a look at #511 and #516 .

Also, for rgb_view camera, the output of tf is:

- Translation: [-4.500, 0.000, 2.800]
- Rotation: in Quaternion [-0.579, 0.579, -0.406, 0.406]

I think this output is correct. Maybe there is some other config file in your workspace or launch file?

[lasuomela]

@KevinLADLee, yes it seems that the different orientation of the camera coordinate system was the cause of the orientation difference. And you're probably right about the rgb_view issue.

So I can confirm the fix is working, nice job! Thanks.