Closed osrf-migration closed 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.
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.
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.
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.
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.
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…
For the hector_gazebo_ros_gps plugin, the referenceHeading parameter defines the 2D rotation from the Gazebo +X coordinate to the HectorGPS +Latitude coordinate - in degrees.
The hector_gazebo_ros_gps fix_velocity publication provides the velocity in NWU (north, west, up) coordinates.
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.
Original comment by Brian Bingham (Bitbucket: brian_bingham).
Let me know if this resolves the issue for you?
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.
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?
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.
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
Original comment by Brian Bingham (Bitbucket: brian_bingham).
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 inwamv_gps.xacro
shows that thereferenceHeading
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.