search

osrf / vrx

Virtual RobotX (VRX) resources.
Apache License 2.0
429 stars 194 forks source link

Possible frame error in simulated GPS velocity generation #64

Closed osrf-migration closed 5 years ago

osrf-migration commented 5 years ago

Original report (archived issue) by Jonathan Wheare (Bitbucket: Jonathan Wheare).

Hi There,

I have been experimenting with the state estimation in the VRX simulation, and believe I have found a frame error in the generation of velocity information by the simulated GPS sensor. ROS and Gazebo both appear to use a right-handed East-North-Up coordinate system, defined in REP-015 and also specified in the spherical_coordinates tag of the sand island world.

However, if a wam-v is placed in the VRX world, aligned with the x-axis and driven forwards, the world frame velocity reported by the fix_velocity topic will be negative y, Similarly, when aligned with the y-axis, the velocity will be positive x. Examination of the simulated GPS sensor in wamv_gps.xacro shows that the referenceHeading tag has been used to rotate the GPS frame to align the latitude and longitude values with the ENU frame.

This may be caused by the way that the underlying hector_gazebo gps plugin operates. With a zero referenceHeading, the plugin appears to use a North-West-Up frame to generate latitude and longitude. Rotating this into East-North-Up causes the velocity data to be similarly rotated.

Steps to reproduce;

Select sensor wamv in the vmrc.launch file, then run the keydrive examples.

rostopic echo /gps/fix_velocity

Use keydrive to move the wamv forward. fix_velocity should show a negative y value as the largest component.

Rotate the wam-v to align with the y-axis and move the wam-v forward. fix_velocity should show a positive x value as the largest component.

Regards, Jonathan Wheare.

osrf-migration commented 5 years ago

Original comment by Carlos Agüero (Bitbucket: caguero, GitHub: caguero).

@brian_bingham , do you have time to take a look at this issue reported by Jonathan?

osrf-migration commented 5 years ago

Original comment by Jonathan Wheare (Bitbucket: Jonathan Wheare).

Hi Carlos,

Would it be useful for me to develop a gazebo world that demonstrated the issue?

Regards,

Jonathan.

osrf-migration commented 5 years ago

Original comment by Carlos Agüero (Bitbucket: caguero, GitHub: caguero).

That would be very helpful!

osrf-migration commented 5 years ago

Original comment by Brian Bingham (Bitbucket: brian_bingham).

Agreed - making it simple to reproduce would be a big help.

I’m just seeing Carlos' note about taking a look. I’m wrapping up my teaching quarter so should have more time to work on this.

osrf-migration commented 5 years ago

Original comment by Brian Bingham (Bitbucket: brian_bingham).

Hi There,

I have been experimenting with the state estimation in the VRX simulation, and believe I have found a frame error in the generation of velocity information by the simulated GPS sensor. ROS and Gazebo both appear to use a right-handed East-North-Up coordinate system, defined in REP-015 and also specified in the spherical_coordinates tag of the sand island world.

However, if a wam-v is placed in the VRX world, aligned with the x-axis and driven forwards, the world frame velocity reported by the fix_velocity topic will be negative y, Similarly, when aligned with the y-axis, the velocity will be positive x. Examination of the simulated GPS sensor in wamv_gps.xacro shows that the referenceHeading tag has been used to rotate the GPS frame to align the latitude and longitude values with the ENU frame.

This may be caused by the way that the underlying hector_gazebo gps plugin operates. With a zero referenceHeading, the plugin appears to use a North-West-Up frame to generate latitude and longitude. Rotating this into East-North-Up causes the velocity data to be similarly rotated.

Steps to reproduce;

Select sensor wamv in the vmrc.launch file, then run the keydrive examples.

rostopic echo /gps/fix_velocity

Use keydrive to move the wamv forward. fix_velocity should show a negative y value as the largest component.

Rotate the wam-v to align with the y-axis and move the wam-v forward. fix_velocity should show a positive x value as the largest component.

Regards, Jonathan Wheare.

osrf-migration commented 5 years ago

Original comment by Jonathan Wheare (Bitbucket: Jonathan Wheare).

To illustrate the issue, I have developed two plugins in a separate branch of my repository (https://bitbucket.org/JonathanWheare/vrx/branch/gps_plugin). These plugins are;

I have modified the GPS configuration of the default wamv by adding both GPS sensors and the velocity plugin. The topics published by the system are;

Turning on Gazebo’s axes show that the x-axis appears to be pointing towards the east, with the y-axis pointing north. This appears to be correlated with the view of google maps.

I started off by moving the wamv vehicle north and south of the axes, plotting the latitudes of the two sensors, I got the following graph;

I then repeated this with longitude;

From this, it appears the hector pluging is producing ENU oriented data, the gazebo GPS sensor appears to be reversed. From this I moved on to testing velocities. To do this, the simulated wamv was driven using the usv_keydrive launch file;

the v3d output should be a ground truth, gps2 the gazebo GPS sensor appears to be generating a negative value. Simultaneously, I was plotting the y values;

The velocities of GPS sensor 1 seem be swapped, the x axis correlated with the y-axis of the v3d plugin. the y-axis correlates with the reverse of the x-axis of the v3d plugin.

Notes:

Conclusion

There appears to be an error in frame transformation in both the hector gazebo GPS sensor, and gazebo’s built in GPS sensor. I found this quite surprising, and I would appreciate it if you could check my results.

Regards,

Jonathan.

osrf-migration commented 5 years ago

Original comment by Jonathan Wheare (Bitbucket: Jonathan Wheare).

The issue with coordinate generation in the built in GPS sensor may be related to an issue with spherical coordinates. The following gazebo answers post shows a possible issue in the conversion of local to spherical coordinates:

http://answers.gazebosim.org/question/13820/bug-in-sphericalcoordinatessphericalfromlocal/

In that post a test program that converts from local to spherical coordinates is used to test some simple values. Positive x and y values generate negative longitudes and latitudes respectively, implying that the coordinates are being reversed during the transform. I have tried the code sample, and my calculated values do seem to match those in the post.

Examining the linked source code, which shows the local to spherical transformation of both coordinates and velocities, the SphericalCoordinates class appears to start by reversing the sign of the x and y components. This could be the source of the coordinate reversal. Checking the ignition version of the code it seems identical, implying that later version of gazebo that use the ignition library are also likely to be affected.

Regards,

Jonathan.

osrf-migration commented 5 years ago

Original comment by Brian Bingham (Bitbucket: brian_bingham).

Thank you for doing such a great job of documenting all of this and setting up the test cases.

With regards to the coordinate frames for Gazebo and the hector_gazebo_ros_gps plugin, I believe this is what is going on…

So from my testing, I think there is consistency between the Gazebo coordinates and the Hector GPS plugin. The NWU convention for reporting fix velocities isn’t necessarily intuitive.

I haven’t had much experience with the built-in Gazebo GPS sensor, so I’m not sure why it would provide lat/lon values that are 180 degrees out from the Hector plugin.

osrf-migration commented 5 years ago

Original comment by Brian Bingham (Bitbucket: brian_bingham).

Let me know if this resolves the issue for you?

osrf-migration commented 5 years ago

Original comment by Jonathan Wheare (Bitbucket: Jonathan Wheare).

Hi Brian,

While the coordinates generated by the GPS appear correct, the velocities created by the nmea_navsat_driver recommended by the robot_localisation package are in the ENU frame. As such, a system developed using GPS velocities from the VRX simulation will not operate correctly if moved over to a real vehicle.

That said the issue seems to be with upstream. I can look into opening an issue with the appropriate packages.

Regards,

Jonathan.

osrf-migration commented 5 years ago

Original comment by Brian Bingham (Bitbucket: brian_bingham).

Hi Jonathan,

I think I am tracking now. I believe this issue is that…

Would one work around be to have a small translator node that converted the fix_velocity topic (Vector3 stamped) from the Gazebo plugin in NWU, to the vel topic (Twist) in ENU? Then it the message types and coord. frame conventions would be compatible with robot_localization?

osrf-migration commented 5 years ago

Original comment by Jonathan Wheare (Bitbucket: Jonathan Wheare).

Hi Brian,

Yes, you are correct in your summary.

The documentation of nmea_navsat_driver doesn’t claim to use ENU for velocity, but according to ROS Rep-103, ENU is the preferred coordinate frame for short-range cartesian coordinate frames. Examination of the nmea_navsat_driver code shows that the frame should be ENU. The GPVTG NMEA message outputs in the NED frame, and the axes are flipped into ENU.

Your suggestion could work, however the NWU topic will still be visible, as such it would be possible to subscribe to it by accident. The GPS velcoity message could be prepended with a namespace such as “/ignore”. This may prevent teams from using it.

Regards,

Jonathan.

osrf-migration commented 5 years ago

Original comment by Brian Bingham (Bitbucket: brian_bingham).

Jonathan,

That all sounds good to me. I’m putting in a PR#134 to add a comment to the xacro to make it explicit that the plugin puts velocity in NWU - and a mention of this issue. Hopefully this will remind folks.

Thank you for bringing this up and documenting it. I imagine this discussion will benefit a number of users.

I’m going to go ahead a mark as resolved for now.

Brian

osrf-migration commented 5 years ago

Original comment by Brian Bingham (Bitbucket: brian_bingham).