Added ins origin logic. Rotated tf2 dvl frame

Author: jorgenfj

auv_setup/description/nautilus.urdf.xacro

@@ -25,7 +25,7 @@
   <joint name="base_to_dvl" type="fixed">
     <parent link="base_link"/>
     <child link="dvl_link"/>
-    <origin xyz="-0.130 0.003 0.2414" rpy="0.0 0.0 0.0"/>
+    <origin xyz="-0.130 0.003 0.2414" rpy="0.0 0.0 3.14159"/>
   </joint>
 
   <!-- base to pressure sensor -->

auv_setup/launch/nucleus_odom_transformer.launch.py

@@ -46,10 +46,10 @@ def launch_setup(context, *args, **kwargs):
         parameters=[
             {
                 "frame_id": f"{namespace}/nucleus_frame",
+                "qos": "best_effort",
                 "connection_params.remote_ip": "10.0.0.42",
                 "connection_params.data_remote_port": 9000,
                 "connection_params.password": "",
-                "reset_pose_on_start": True,
                 "enable_imu": True,
                 "enable_ins_odom": True,
                 "enable_dvl": True,
@@ -78,7 +78,7 @@ def launch_setup(context, *args, **kwargs):
                 "altimeter_settings.power_level": 0,  # 0=default
                 "magnetometer_settings.freq": 75,
                 "magnetometer_settings.mode": 0,
-                "instrument_settings.rotxy": 180.0,  # Transform currently not working. Maybe fix later
+                "instrument_settings.rotxy": 0.0,  # Transform currently not working. Maybe fix later
                 "instrument_settings.rotyz": 0.0,
                 "instrument_settings.rotxz": 0.0,
             },
@@ -98,7 +98,6 @@ def launch_setup(context, *args, **kwargs):
                 "publish_tf": True,
                 "publish_pose": True,
                 "publish_twist": True,
-                "rotate_yaw_180": True,
                 "topics.input": f"/{namespace}/nucleus/odom",
                 "topics.output": f"/{namespace}/odom",
                 "topics.pose": f"/{namespace}/pose",

auv_setup/launch/state_estimation.launch.py

@@ -151,10 +151,10 @@ def launch_setup(context, *args, **kwargs):
                 parameters=[
                     {
                         "frame_id": f"/{namespace}/nucleus_frame",
+                        "qos": "best_effort",
                         "connection_params.remote_ip": "10.0.0.42",
                         "connection_params.data_remote_port": 9000,
                         "connection_params.password": "",
-                        "reset_pose_on_start": True,
                         "enable_imu": False,
                         "enable_ins_odom": False,
                         "enable_dvl": True,
@@ -183,7 +183,7 @@ def launch_setup(context, *args, **kwargs):
                         "fast_pressure_settings.sampling_rate": 16,
                         "magnetometer_settings.freq": 75,
                         "magnetometer_settings.mode": 0,
-                        "instrument_settings.rotxy": 180.0,  # Transform currently not working. Maybe fix later
+                        "instrument_settings.rotxy": 0.0,  # Transform currently not working. Maybe fix later
                         "instrument_settings.rotyz": 0.0,
                         "instrument_settings.rotxz": 0.0,
                     }

auv_setup/launch/topside.launch.py

@@ -3,11 +3,13 @@
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch.actions import (
+    DeclareLaunchArgument,
     IncludeLaunchDescription,
     OpaqueFunction,
     SetEnvironmentVariable,
 )
 from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
 
 from auv_setup.launch_arg_common import (
@@ -19,6 +21,7 @@
 def launch_setup(context, *args, **kwargs):
     """Set up the topside nodes with drone-specific namespace."""
     drone, namespace = resolve_drone_and_namespace(context)
+    orientation_mode = LaunchConfiguration("orientation_mode").perform(context)
 
     joy_node = Node(
         package="joy",
@@ -43,6 +46,7 @@ def launch_setup(context, *args, **kwargs):
         launch_arguments={
             "drone": drone,
             "namespace": namespace,
+            "orientation_mode": orientation_mode,
         }.items(),
     )
 
@@ -68,5 +72,12 @@ def generate_launch_description() -> LaunchDescription:
     return LaunchDescription(
         [set_env_var]
         + declare_drone_and_namespace_args()
-        + [OpaqueFunction(function=launch_setup)]
+        + [
+            DeclareLaunchArgument(
+                "orientation_mode",
+                default_value="euler",
+                description="Reference orientation representation: 'euler' (ReferenceFilter) or 'quat' (ReferenceFilterQuat)",
+            ),
+            OpaqueFunction(function=launch_setup),
+        ]
     )

navigation/odom_transformer/config/odom_transformer_params.yaml

@@ -5,7 +5,6 @@
       publish_tf: true
       publish_pose: true
       publish_twist: true
-      rotate_yaw_180: false
       topics:
         input: "nucleus/odom"
         output: "odom"

navigation/odom_transformer/include/odom_transformer/odom_transformer.hpp

@@ -45,13 +45,17 @@ class OdomTransformer : public rclcpp::Node {
     Eigen::Matrix3d R_base_sensor_ = Eigen::Matrix3d::Identity();
     Eigen::Vector3d t_base_sensor_ = Eigen::Vector3d::Zero();
 
+    // Odom origin — first received base_link pose in the sensor's world frame
+    bool origin_set_{false};
+    Eigen::Matrix3d R_origin_ = Eigen::Matrix3d::Identity();
+    Eigen::Vector3d p_origin_ = Eigen::Vector3d::Zero();
+
     std::string frame_prefix_;
     std::string sensor_frame_;
     bool tf_loaded_{false};
     bool publish_tf_{false};
     bool publish_pose_{false};
     bool publish_twist_{false};
-    bool rotate_yaw_180_{false};
 };
 
 #endif  // ODOM_TRANSFORMER__ODOM_TRANSFORMER_HPP_

navigation/odom_transformer/src/odom_transformer.cpp

@@ -22,8 +22,6 @@ OdomTransformer::OdomTransformer(const rclcpp::NodeOptions& options)
     this->declare_parameter<std::string>("topics.output");
     this->declare_parameter<std::string>("topics.pose");
     this->declare_parameter<std::string>("topics.twist");
-    rotate_yaw_180_ = this->declare_parameter<bool>("rotate_yaw_180");
-
     tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
     tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
     tf_timer_ = this->create_wall_timer(
@@ -40,10 +38,15 @@ void OdomTransformer::lookup_static_transforms() {
 
         tf_loaded_ = true;
         tf_timer_->cancel();
+
+        Eigen::Vector3d rpy =
+            R_base_sensor_.eulerAngles(2, 1, 0).reverse() * 180.0 / M_PI;
         RCLCPP_INFO(get_logger(),
-                    "Loaded static transform: %s -> %s  t=(%.3f, %.3f, %.3f)",
+                    "Loaded static transform: %s -> %s  "
+                    "t=(%.3f, %.3f, %.3f)  rpy=(%.1f, %.1f, %.1f) deg",
                     frame("base_link").c_str(), frame(sensor_frame_).c_str(),
-                    t_base_sensor_.x(), t_base_sensor_.y(), t_base_sensor_.z());
+                    t_base_sensor_.x(), t_base_sensor_.y(), t_base_sensor_.z(),
+                    rpy.x(), rpy.y(), rpy.z());
         complete_initialization();
     } catch (const tf2::TransformException& ex) {
         RCLCPP_WARN(get_logger(), "TF lookup failed (will retry): %s",
@@ -96,32 +99,30 @@ void OdomTransformer::odom_callback(
                                  msg->twist.twist.angular.y,
                                  msg->twist.twist.angular.z);
 
-    if (rotate_yaw_180_) {
-        // 180 deg yaw flips X and Y, leaves Z unchanged
-        q_odom_sensor = Eigen::Quaterniond(
-                            Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ())) *
-                        q_odom_sensor;
-        v_sensor.x() = -v_sensor.x();
-        v_sensor.y() = -v_sensor.y();
-        omega_sensor.x() = -omega_sensor.x();
-        omega_sensor.y() = -omega_sensor.y();
-        msg->pose.pose.position.x = -msg->pose.pose.position.x;
-        msg->pose.pose.position.y = -msg->pose.pose.position.y;
-    }
-
     Eigen::Matrix3d R_odom_sensor = q_odom_sensor.toRotationMatrix();
 
     // Orientation: R_odom_base = R_odom_sensor * R_base_sensor^-1
     Eigen::Matrix3d R_odom_base = R_odom_sensor * R_base_sensor_.transpose();
-    Eigen::Quaterniond q_odom_base(R_odom_base);
-    q_odom_base.normalize();
 
     // Position: p_base = p_sensor - R_odom_base * t_base_sensor
     Eigen::Vector3d p_sensor(msg->pose.pose.position.x,
                              msg->pose.pose.position.y,
                              msg->pose.pose.position.z);
     Eigen::Vector3d p_base = p_sensor - R_odom_base * t_base_sensor_;
 
+    // Capture the first base_link pose as the odom frame origin
+    if (!origin_set_) {
+        R_origin_ = R_odom_base;
+        p_origin_ = p_base;
+        origin_set_ = true;
+    }
+
+    // Express pose relative to origin so t=0 is identity
+    Eigen::Matrix3d R_out = R_origin_.transpose() * R_odom_base;
+    Eigen::Vector3d p_out = R_origin_.transpose() * (p_base - p_origin_);
+    Eigen::Quaterniond q_out(R_out);
+    q_out.normalize();
+
     // Angular velocity: rotate from sensor frame to base_link frame
     Eigen::Vector3d omega_base = R_base_sensor_ * omega_sensor;
 
@@ -136,13 +137,13 @@ void OdomTransformer::odom_callback(
     out->header.frame_id = frame("odom");
     out->child_frame_id = frame("base_link");
 
-    out->pose.pose.position.x = p_base.x();
-    out->pose.pose.position.y = p_base.y();
-    out->pose.pose.position.z = p_base.z();
-    out->pose.pose.orientation.w = q_odom_base.w();
-    out->pose.pose.orientation.x = q_odom_base.x();
-    out->pose.pose.orientation.y = q_odom_base.y();
-    out->pose.pose.orientation.z = q_odom_base.z();
+    out->pose.pose.position.x = p_out.x();
+    out->pose.pose.position.y = p_out.y();
+    out->pose.pose.position.z = p_out.z();
+    out->pose.pose.orientation.w = q_out.w();
+    out->pose.pose.orientation.x = q_out.x();
+    out->pose.pose.orientation.y = q_out.y();
+    out->pose.pose.orientation.z = q_out.z();
 
     out->twist.twist.linear.x = v_base.x();
     out->twist.twist.linear.y = v_base.y();
@@ -176,9 +177,9 @@ void OdomTransformer::odom_callback(
         tf_msg.header.stamp = msg->header.stamp;
         tf_msg.header.frame_id = frame("odom");
         tf_msg.child_frame_id = frame("base_link");
-        tf_msg.transform.translation.x = p_base.x();
-        tf_msg.transform.translation.y = p_base.y();
-        tf_msg.transform.translation.z = p_base.z();
+        tf_msg.transform.translation.x = p_out.x();
+        tf_msg.transform.translation.y = p_out.y();
+        tf_msg.transform.translation.z = p_out.z();
         tf_msg.transform.rotation = out->pose.pose.orientation;
         tf_broadcaster_->sendTransform(tf_msg);
     }