East North Up parameter added #16

szenergy · Apr 11, 2022 · f00726f · f00726f
1 parent 67db9dc
commit f00726f
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 6 deletions.
diff --git a/launch/duro_example.launch b/launch/duro_example.launch
@@ -13,6 +13,8 @@
             orig: the original Z provided by Duro / Piksi
             -->
             <param name="z_coord_ref_switch" value="zero_based"/>
+            <!-- default: false, East North Up - see: issue 16 -->
+            <param name="orientation_enu" type="bool" value="false" />
         </node>
     </group>
 </launch>
diff --git a/src/duro.cpp b/src/duro.cpp
@@ -46,6 +46,7 @@ std::string gps_receiver_frame_id;
 std::string imu_frame_id;
 std::string utm_frame_id;
 std::string z_coord_ref_switch;
+bool orientation_enu;
 static sbp_msg_callbacks_node_t heartbeat_callback_node;
 static sbp_msg_callbacks_node_t pos_ll_callback_node;
 static sbp_msg_callbacks_node_t orientation_callback_node;
@@ -239,12 +240,21 @@ void orientation_callback(u16 sender_id, u8 len, u8 msg[], void *context)
   double z = orimsg->z * pow(2, -31);
   tf2::Quaternion tf_orig(x, y, z, w);
   tf2::Quaternion tf_rot, tf_aligned;
-  tf_rot.setRPY(0.0, 0.0, -M_PI_2); // left-handerd / right handed rotation
-  tf_aligned = tf_rot * tf_orig;    // left-handerd / right handed rotation
-  pose_msg.pose.orientation.w = tf_aligned.w() * -1;
-  pose_msg.pose.orientation.x = tf_aligned.y();      // left-handerd / right handed orientation
-  pose_msg.pose.orientation.y = tf_aligned.x() * -1; // left-handerd / right handed orientation
-  pose_msg.pose.orientation.z = tf_aligned.z();      // left-handerd / right handed orientation
+  // East North Up orientation
+  if(orientation_enu){ 
+    pose_msg.pose.orientation.w = w; 
+    pose_msg.pose.orientation.x = x;
+    pose_msg.pose.orientation.y = y; 
+    pose_msg.pose.orientation.z = z;   
+  }
+  else{
+    tf_rot.setRPY(0.0, 0.0, -M_PI_2);
+    tf_aligned = tf_rot * tf_orig;
+    pose_msg.pose.orientation.w = tf_aligned.w() * -1;
+    pose_msg.pose.orientation.x = tf_aligned.y(); 
+    pose_msg.pose.orientation.y = tf_aligned.x() * -1; 
+    pose_msg.pose.orientation.z = tf_aligned.z();
+  }
 }
 
 void time_callback(u16 sender_id, u8 len, u8 msg[], void *context)
@@ -413,7 +423,9 @@ int main(int argc, char **argv)
   n_private.param<std::string>("imu_frame_id", imu_frame_id, gps_receiver_frame_id);
   n_private.param<std::string>("utm_frame_id", utm_frame_id, "utm");
   n_private.param<std::string>("z_coord_ref_switch", z_coord_ref_switch, "zero");
+  n_private.param<bool>("orientation_enu", orientation_enu, false);
   ROS_INFO("Connecting to duro on %s:%d", tcp_ip_addr.c_str(), tcp_ip_port);
+  //ROS_INFO_STREAM("Orientation East North Up: " << std::boolalpha << orientation_enu);
 
   setup_socket();
   sbp_state_init(&s);