GSS and Odometry do not match

[zernchri]

Hi, I face an issue when connecting the simulation to the autonomous software of our car. The velocity (fsds/gss) matches the velocity estimated by our SLAM. but there is a huge missmatch of the pose between the simulation and the SLAM estimation. The SLAM pose is way faster than the pose in the simulator.

Has anyone encountered a similiar issue?

[wouter-heerwegh]

Hi @zernchri, I haven't had this happen before, velocities seem to be correct. However, in the current version, the GSS velocities are not aligned with the car frame, but rather with the map frame. Meaning that if the car turns, the velocity in X decreases while it increases in the Y direction. You can take the absolute velocity by combining both vectors. Could you check if this fixes your problems?

[zernchri]

No, unfortunately that does not fix the problem. I have the velocity vectors already combined.

[wouter-heerwegh]

Could you verify that the velocity given by the GSS and the velocity given on the fsds/testing/odom topic are the same? I haven't used the GSS yet, but the odom velocities seem to be correct.

[zernchri]

There is a small difference between the combined gss velocity vectors and the velocity in fsds/testing/odom. But I don't think this is the problem, I tried to use the fsds/testing/odom velocity instead the gss and this also did not work.

[wouter-heerwegh]

I've talked to my team and for them the position is more or less the same if they don't add any noise. Could you try to test the same thing on the acceleration track? This shoulde be 75m long, and a bit longer for the stop zone.

[zernchri]

I did a lot of testing the last few days, but I couldn't get it running. The position should be correct in the Simulation, I tested that with the acceleration track.
I also got the estimated position of the SLAM matching with the Simulation, by adding a factor to the calculation of the Wheel-revs. But then the velocities are wrong. They are way higher in the Simulation than in the SLAM.
One strange thing I recognized is that the velocity in the simulation is increasing very fast to a value of around 20 m/s, even if there is only a very low control_command and it doesn't look that fast in the Visualization. Actually it seems like the velocity is in km/h, but it should be in m/s, right?

[wouter-heerwegh]

I made an example to test this. Keep in mind that it does not take a couple of things into account and is only an approximation.
With this, after 75m, both are around 1m which is within a respectable range (I'm guessing yours is couple of 10s of meters off). The velocity is indeed in 20m/s this is where the car settings are physically limited.

import math
import rospy
from fs_msgs.msg import ControlCommand
from nav_msgs.msg import Odometry

total_distance = 0
prev_stamp = None
def callback(data: Odometry):
    global prev_stamp, total_distance, reference_distance, prev_position
    stamp = data.header.stamp
    # Get approximation of velocity (disregard slip)
    velocity = math.sqrt(data.twist.twist.linear.x * data.twist.twist.linear.x + data.twist.twist.linear.y * data.twist.twist.linear.y)
    
    if prev_stamp == None:
        prev_stamp = stamp
        reference_distance = 0
        prev_position = data.pose.pose.position
        return
    
    # Calculate driven distance
    diff: rospy.Duration = stamp - prev_stamp
    total_distance = total_distance + diff.to_sec() * velocity
    
    # Calculate odom distance
    diff = abs(data.pose.pose.position.x - prev_position.x) + abs(data.pose.pose.position.y - prev_position.y)
    reference_distance = reference_distance + diff

    # Print this info
    rospy.loginfo("Distance: %f, ref: %f", total_distance, reference_distance)
    prev_stamp = stamp
    prev_position = data.pose.pose.position

if __name__ == '__main__':
    try:
        rospy.init_node('test', anonymous=True)
        pub = rospy.Publisher('/fsds/control_command', ControlCommand, queue_size=10)
        sub = rospy.Subscriber("/fsds/testing_only/odom", Odometry, callback)
        
        rate = rospy.Rate(100)
        
        while not rospy.is_shutdown():
            command = ControlCommand()
            command.throttle = 1
            pub.publish(command)
            rate.sleep()

    except rospy.ROSInterruptException:
        pass

[wouter-heerwegh]

I did some more tests and it seems to variate a lot. I did find however that when not using the timestamps in the headers, but rather the time at which the message arrived at my node, both distances were a lot closer together. I'll look at this more closely this evening

[zernchri]

Okay, when I use this script, the total_distance (calculated with the velocities) is way higher than the reference_distance. When ref is at 75m at the end of the track, the driven distance is at about 373m.

[wouter-heerwegh]

It seems to be highly performance related. If you don't use camera's disable them. I noticed that if I don't use them, at 100m, the total_distance and ref are only 0.3m apart, while with camera's on, this will be more than 10m. Do you use a very camera heavy settings.json?

I've made an issue for the camera problem a while ago on the Airsim repo (microsoft/AirSim#4101, we're using an older and modified version), but it seems like they haven't gotten around to looking at it yet. At the moment I'm holding off on updating the Airsim version in this repo, as this will require quite some time.

[zernchri]

I can confirm that it is high performance related. I don't use cameras just one lidar, but with pretty high settings. Lowering them makes a huge difference and the two values are now very close together. Another thing making the performance better is switching to "NoDisplay" mode in the settings.json file.
Only problem is that a Lidar with that low settings does not detect cones as good as it should be. That could be a problem when it comes to testing the actual software.

[wouter-heerwegh]

Might be fixed by future improvements, see #128.

[ayrmoney]

Hi @wouter-heerwegh, @zernchri, has there been any update to this, or any work around that I can use? I need the cameras enabled.

I am facing a similar issue, where the motion model of the localization module (which takes input from GSS & IMU, using ROS Bridge with ApproximateTimeSynchronizer) suffers greatly when I try to run the simulator with cameras enabled (even when switching to ‘NoDisplay’ mode).
With the cameras disabled the estimation from the motion model is as expected.
Any suggestions?

OS: Ubuntu 18.04 LTS
CPU: Core i7-10750H
GPU: Geforce GTX 1650
RAM: 8GB

[wouter-heerwegh]

Hi @ayrmoney

At the moment not really. The problem stems from the total load of calculations that need to be taken during 1 frame. So this also means that the time in the simulation is different than real life.

Hence causing the shift in time between measurements.

Best way to mitigate the issue as much as possible for now is limit the load on the simulator (lower the resolution of the cameras, the simulator screen, lidar point clouds, running your autonomous system on a seperate system,...). I will also release version 2.2 soon which will contain shipping builds. These should be more optimized and thus cause better results.

[baradejoo]

Hi @ayrmoney
I think your calculations are incorrect. The proper way to get velocity vector in frame of bolide is multiplication of rotation matrix (for example: state of your kaman filter or calculated from imu orientations) with velocity vector in frame of fsds/map. Then you can use it as velocities of body frame, it means: fsds/FSCar.