Gazebo: correct Gazebo ENU to ArduPilot NED transform

- Correct orientation and transform for truck - quadplane landing examples. Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>
ArduPilot · Jan 11, 2024 · cfaec90 · cfaec90
1 parent 07df90c
commit cfaec90
Show file tree

Hide file tree

Showing 9 changed files with 19 additions and 36 deletions.
diff --git a/Gazebo/models/alti_transition_quad/model.sdf b/Gazebo/models/alti_transition_quad/model.sdf
@@ -1122,18 +1122,8 @@
       <connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>
       <lock_step>1</lock_step>
 
-      <!-- Frame conventions
-        modelXYZToAirplaneXForwardZDown:
-          - transforms body frame from orientation in Gazebo to NED
-
-        gazeboXYZToNED
-          - transforms world from Gazebo convention xyz = N -E -D
-            to ArduPilot convention xyz = NED
-
-        VTOL is oriented x-forward, y-left, z-up
-      -->
-      <modelXYZToAirplaneXForwardZDown>0 0 0 3.141593 0 0</modelXYZToAirplaneXForwardZDown>
-      <gazeboXYZToNED>0 0 0 3.141593 0 0</gazeboXYZToNED>
+      <modelXYZToAirplaneXForwardZDown degrees="true">0 0 0 180 0 0</modelXYZToAirplaneXForwardZDown>
+      <gazeboXYZToNED degrees="true">0 0 0 180 0 90</gazeboXYZToNED>
 
       <!-- Sensors -->
       <imuName>imu_sensor</imuName>

diff --git a/Gazebo/models/alti_transition_quad/model.sdf.erb b/Gazebo/models/alti_transition_quad/model.sdf.erb
@@ -1143,18 +1143,8 @@ DO NOT EDIT. This file is generated from an ERB template.
     <connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>
     <lock_step>1</lock_step>
 
-    <!-- Frame conventions
-      modelXYZToAirplaneXForwardZDown:
-        - transforms body frame from orientation in Gazebo to NED
-
-      gazeboXYZToNED
-        - transforms world from Gazebo convention xyz = N -E -D
-          to ArduPilot convention xyz = NED
-
-      VTOL is oriented x-forward, y-left, z-up
-    -->
-    <modelXYZToAirplaneXForwardZDown>0 0 0 3.141593 0 0</modelXYZToAirplaneXForwardZDown>
-    <gazeboXYZToNED>0 0 0 3.141593 0 0</gazeboXYZToNED>
+    <modelXYZToAirplaneXForwardZDown degrees="true">0 0 0 180 0 0</modelXYZToAirplaneXForwardZDown>
+    <gazeboXYZToNED degrees="true">0 0 0 180 0 90</gazeboXYZToNED>
 
     <!-- Sensors -->
     <imuName>imu_sensor</imuName>

diff --git a/Gazebo/models/daf_xf_450_flatbed_trailer/model.sdf b/Gazebo/models/daf_xf_450_flatbed_trailer/model.sdf
@@ -602,8 +602,8 @@
     <fdm_addr>127.0.0.1</fdm_addr>
     <fdm_port_in>9012</fdm_port_in>
     <connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>
-    <modelXYZToAirplaneXForwardZDown>0 0 0 3.141593 0 0</modelXYZToAirplaneXForwardZDown>
-    <gazeboXYZToNED>0 0 0 3.141593 0 0</gazeboXYZToNED>
+    <modelXYZToAirplaneXForwardZDown degrees="true">0 0 0 180 0 0</modelXYZToAirplaneXForwardZDown>
+    <gazeboXYZToNED degrees="true">0 0 0 180 0 90</gazeboXYZToNED>
     <imuName>imu_sensor</imuName>
   </plugin>
 

diff --git a/Gazebo/models/daf_xf_450_flatbed_trailer/model.sdf.erb b/Gazebo/models/daf_xf_450_flatbed_trailer/model.sdf.erb
@@ -625,8 +625,8 @@ DO NOT EDIT. This file is generated from an ERB template.
     <fdm_addr><%= fdm_addr %></fdm_addr>
     <fdm_port_in><%= fdm_port_in %></fdm_port_in>
     <connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>
-    <modelXYZToAirplaneXForwardZDown>0 0 0 3.141593 0 0</modelXYZToAirplaneXForwardZDown>
-    <gazeboXYZToNED>0 0 0 3.141593 0 0</gazeboXYZToNED>
+    <modelXYZToAirplaneXForwardZDown degrees="true">0 0 0 180 0 0</modelXYZToAirplaneXForwardZDown>
+    <gazeboXYZToNED degrees="true">0 0 0 180 0 90</gazeboXYZToNED>
     <imuName>imu_sensor</imuName>
   </plugin>
 

diff --git a/Gazebo/models/daf_xf_450_tractor/model.sdf b/Gazebo/models/daf_xf_450_tractor/model.sdf
@@ -692,8 +692,8 @@
     <fdm_addr>127.0.0.1</fdm_addr>
     <fdm_port_in>9002</fdm_port_in>
     <connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>
-    <modelXYZToAirplaneXForwardZDown>0 0 0 3.141593 0 0</modelXYZToAirplaneXForwardZDown>
-    <gazeboXYZToNED>0 0 0 3.141593 0 0</gazeboXYZToNED>
+    <modelXYZToAirplaneXForwardZDown degrees="true">0 0 0 180 0 0</modelXYZToAirplaneXForwardZDown>
+    <gazeboXYZToNED degrees="true">0 0 0 180 0 90</gazeboXYZToNED>
 
     <!-- Sensors -->
     <!-- <imuName>imu_link::imu_sensor</imuName> -->

diff --git a/Gazebo/models/daf_xf_450_tractor/model.sdf.erb b/Gazebo/models/daf_xf_450_tractor/model.sdf.erb
@@ -722,8 +722,8 @@ DO NOT EDIT. This file is generated from an ERB template.
     <fdm_addr><%= fdm_addr %></fdm_addr>
     <fdm_port_in><%= fdm_port_in %></fdm_port_in>
     <connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>
-    <modelXYZToAirplaneXForwardZDown>0 0 0 3.141593 0 0</modelXYZToAirplaneXForwardZDown>
-    <gazeboXYZToNED>0 0 0 3.141593 0 0</gazeboXYZToNED>
+    <modelXYZToAirplaneXForwardZDown degrees="true">0 0 0 180 0 0</modelXYZToAirplaneXForwardZDown>
+    <gazeboXYZToNED degrees="true">0 0 0 180 0 90</gazeboXYZToNED>
 
     <!-- Sensors -->
     <!-- <imuName>imu_link::imu_sensor</imuName> -->

diff --git a/Gazebo/worlds/alti_transition_runway.sdf b/Gazebo/worlds/alti_transition_runway.sdf
@@ -35,11 +35,12 @@
     </light>
 
     <include>
+      <pose degrees="true">-29 545 0 0 0 363</pose>
       <uri>model://runway</uri>
     </include>
 
     <include>
-      <pose>0 0 0.35 0 0 0</pose>
+      <pose degrees="true">0 0 0.35 0 0 90</pose>
       <uri>model://alti_transition_quad</uri>
     </include>
 

diff --git a/Gazebo/worlds/daf_truck_runway.sdf b/Gazebo/worlds/daf_truck_runway.sdf
@@ -35,11 +35,12 @@
     </light>
 
     <include>
+      <pose degrees="true">-29 545 0 0 0 363</pose>
       <uri>model://runway</uri>
     </include>
 
     <include>
-      <pose>0 0 1.0 0 0 0</pose>
+      <pose degrees="true">0 0 1.0 0 0 90</pose>
       <uri>model://daf_xf_450_tractor</uri>
     </include>
 

diff --git a/Gazebo/worlds/truck_quadplane_landing.sdf.erb b/Gazebo/worlds/truck_quadplane_landing.sdf.erb
@@ -29,12 +29,12 @@
   trailer_model = make_trailer_model('trailer', trailer_pose, 9012)
 
   # make the tractor model and inject trailer
-  tractor_pose = '0 0 1.0 0 0 0'
+  tractor_pose = '0 0 1.0 0 0 1.57079632'
   tractor_model = make_tractor_model('tractor', tractor_pose, 9002,
     trailer_joint, trailer_model)
 
   # make the quadplane and place on the trailer
-  quadplane_pose = '-8.0 0 1.6 0 0 0'
+  quadplane_pose = '0 -8.0 1.6 0 0 1.57079632'
   quadplane_model = make_quadplane_model('quadplane', quadplane_pose, 9022)
 %>
 
@@ -75,6 +75,7 @@
     </light>
 
     <include>
+      <pose degrees="true">-29 545 0 0 0 363</pose>
       <uri>model://runway</uri>
     </include>