Adding the tf rotation and offset feature

.gitignore

@@ -10,4 +10,12 @@ rqt.repos
 __pycache__/
 .vs/
 Log_*
-cpplint_*
+cpplint_*
+
+**/.obj*
+
+adnav/debian/
+adnav_driver/debian/
+adnav_launch/debian/
+adnav_interfaces/debian/
+

adnav/CMakeLists.txt

@@ -0,0 +1,5 @@
+cmake_minimum_required(VERSION 3.16)
+project(adnav LANGUAGES NONE)
+
+find_package(ament_cmake REQUIRED)
+ament_package()

adnav/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>adnav</name>
+  <version>1.0.0</version>
+  <description>Metapackage for adnav packages</description>
+
+  <maintainer email="j.justin@acfr.usyd.edu.au">Jerome Justin</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <exec_depend>adnav_driver</exec_depend>
+  <exec_depend>adnav_interfaces</exec_depend>
+  <exec_depend>adnav_launch</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

adnav_driver/include/adnav_driver/adnav_driver.h

@@ -61,6 +61,7 @@
 // ROS2 Packages, Services, Messages
 #include <rclcpp/rclcpp.hpp>
 #include <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Matrix3x3.h>
 #include <rcl_interfaces/msg/set_parameters_result.hpp>
 #include <std_msgs/msg/string.hpp>
 #include <sensor_msgs/msg/nav_sat_fix.hpp>
@@ -132,6 +133,11 @@ class Driver : public rclcpp::Node  // Inheriting gives every "this->" as a poin
     ~Driver();
 
  private:
+    double roll_, pitch_, yaw_;
+    double yaw_offset_, roll_offset_, pitch_offset_;
+    tf2::Quaternion rotation_q_;
+    tf2::Matrix3x3 rotation_matrix_;
+
     // Debug variables
     int pub_num_ = 0, P28_num_ = 0, P20_num_ = 0, P27_num_ = 0, P33_num_ = 0, P0_num_ = 0;
 

adnav_driver/src/adnav_driver.cpp

@@ -497,6 +497,23 @@ void Driver::setupParams() {
 	comms_select_description.integer_range.push_back(commRange);
 	this->declare_parameter<int>("comm_select", adnav::CONNECTION_SERIAL, comms_select_description);
 	comms_data_.method = (int) this->get_parameter("comm_select").as_int();
+
+	// Add parameters for static rotation
+    this->declare_parameter<double>("rotation_x_angle", 0.0);
+    this->declare_parameter<double>("rotation_y_angle", 0.0);
+    this->declare_parameter<double>("rotation_z_angle", 0.0);
+	this->declare_parameter<double>("roll_offset", 0.0);
+	this->declare_parameter<double>("pitch_offset", 0.0);	
+	this->declare_parameter<double>("yaw_offset", 0.0);
+
+	this->get_parameter<double>("rotation_x_angle", roll_);
+	this->get_parameter<double>("rotation_y_angle", pitch_);
+	this->get_parameter<double>("rotation_z_angle", yaw_);
+	this->get_parameter<double>("pitch_offset", pitch_offset_);
+	this->get_parameter<double>("yaw_offset", yaw_offset_);
+
+    rotation_q_.setRPY(roll_, pitch_, yaw_);
+    rotation_matrix_.setRotation(rotation_q_);
 }
 
 //~~~~~~ Control Functions
@@ -1602,43 +1619,50 @@ void Driver::systemStateRosDecoder(an_packet_t* an_packet) {
 				ntrip_client_->set_location(llh_.latitude, llh_.longitude, llh_.height);
 			}
 			// TWIST
-			twist_msg_.linear.x = system_state_packet.velocity[0];
-			twist_msg_.linear.y = system_state_packet.velocity[1];
-			twist_msg_.linear.z = system_state_packet.velocity[2];
-			twist_msg_.angular.x = system_state_packet.angular_velocity[0];
-			twist_msg_.angular.y = system_state_packet.angular_velocity[1];
-			twist_msg_.angular.z = system_state_packet.angular_velocity[2];
-
+            tf2::Vector3 original_vel(system_state_packet.velocity[0], system_state_packet.velocity[1], system_state_packet.velocity[2]);
+            tf2::Vector3 rotated_vel = rotation_matrix_ * original_vel;
+            twist_msg_.linear.x = rotated_vel.x();
+            twist_msg_.linear.y = rotated_vel.y();
+            twist_msg_.linear.z = rotated_vel.z();
+
+            tf2::Vector3 original_ang_vel(system_state_packet.angular_velocity[0], system_state_packet.angular_velocity[1], system_state_packet.angular_velocity[2]);
+            tf2::Vector3 rotated_ang_vel = rotation_matrix_ * original_ang_vel;
+            twist_msg_.angular.x = rotated_ang_vel.x();
+            twist_msg_.angular.y = rotated_ang_vel.y();
+            twist_msg_.angular.z = rotated_ang_vel.z();
 
 			// IMU
 			imu_msg_.header.stamp.sec = system_state_packet.unix_time_seconds;
 			imu_msg_.header.stamp.nanosec = system_state_packet.microseconds*1000;
 			imu_msg_.header.frame_id = frame_id_;
 			// Using the RPY orientation as done by cosama
-			orientation_.setRPY(
-				system_state_packet.orientation[0],
-				system_state_packet.orientation[1],
-				M_PI/2.0f - system_state_packet.orientation[2] // REP 103
-			);
-			imu_msg_.orientation.x = orientation_[0];
-			imu_msg_.orientation.y = orientation_[1];
-			imu_msg_.orientation.z = orientation_[2];
-			imu_msg_.orientation.w = orientation_[3];
-
-			// POSE Orientation
-			pose_msg_.orientation.x = orientation_[0];
-			pose_msg_.orientation.y = orientation_[1];
-			pose_msg_.orientation.z = orientation_[2];
-			pose_msg_.orientation.w = orientation_[3];
-
-			imu_msg_.angular_velocity.x = system_state_packet.angular_velocity[0]; // These the same as the TWIST msg values
-			imu_msg_.angular_velocity.y = system_state_packet.angular_velocity[1];
-			imu_msg_.angular_velocity.z = system_state_packet.angular_velocity[2];
-
-			// The IMU linear acceleration is now coming from the RAW Sensors Accelerometer
-			imu_msg_.linear_acceleration.x = system_state_packet.body_acceleration[0];
-			imu_msg_.linear_acceleration.y = system_state_packet.body_acceleration[1];
-			imu_msg_.linear_acceleration.z = system_state_packet.body_acceleration[2];
+            tf2::Quaternion sensor_orientation;
+            sensor_orientation.setRPY(
+                system_state_packet.orientation[0] + (roll_offset_ * M_PI / 180.0),
+                system_state_packet.orientation[1] + (pitch_offset_ * M_PI / 180.0),
+                M_PI/2.0f - system_state_packet.orientation[2] + (yaw_offset_ * M_PI / 180.0)// REP 103
+            );
+
+			// Apply the static rotation by multiplying the quaternions
+            tf2::Quaternion final_orientation = rotation_q_ * sensor_orientation;
+            final_orientation.normalize(); // It's good practice to normalize after multiplication
+
+            // Assign the final, rotated orientation to the IMU and Pose messages
+            imu_msg_.orientation.x = final_orientation.x();
+            imu_msg_.orientation.y = final_orientation.y();
+            imu_msg_.orientation.z = final_orientation.z();
+            imu_msg_.orientation.w = final_orientation.w();
+
+            // POSE Orientation
+            pose_msg_.orientation.x = final_orientation.x();
+            pose_msg_.orientation.y = final_orientation.y();
+            pose_msg_.orientation.z = final_orientation.z();
+            pose_msg_.orientation.w = final_orientation.w();
+
+            // The angular velocity is already rotated, so this is correct
+            imu_msg_.angular_velocity.x = rotated_ang_vel.x();
+            imu_msg_.angular_velocity.y = rotated_ang_vel.y();
+            imu_msg_.angular_velocity.z = rotated_ang_vel.z();
 
 			// SYSTEM STATUS
 			system_status_msg_.message = "";
@@ -1883,19 +1907,26 @@ void Driver::rawSensorsRosDecoder(an_packet_t* an_packet) {
 	if(decode_raw_sensors_packet(&raw_sensors_packet, an_packet) == 0) {
 
 		// RAW MAGNETICFIELD VALUE FROM IMU
-		mag_field_msg_.header.frame_id = frame_id_;
-		mag_field_msg_.magnetic_field.x = raw_sensors_packet.magnetometers[0];
-		mag_field_msg_.magnetic_field.y = raw_sensors_packet.magnetometers[1];
-		mag_field_msg_.magnetic_field.z = raw_sensors_packet.magnetometers[2];
-
-		imu_raw_msg_.header.frame_id = frame_id_;
-		imu_raw_msg_.orientation_covariance[0] = -1; // Tell recievers that no orientation is sent.
-		imu_raw_msg_.linear_acceleration.x = raw_sensors_packet.accelerometers[0];
-		imu_raw_msg_.linear_acceleration.y = raw_sensors_packet.accelerometers[1];
-		imu_raw_msg_.linear_acceleration.z = raw_sensors_packet.accelerometers[2];
-		imu_raw_msg_.angular_velocity.x = raw_sensors_packet.gyroscopes[0];
-		imu_raw_msg_.angular_velocity.y = raw_sensors_packet.gyroscopes[1];
-		imu_raw_msg_.angular_velocity.z = raw_sensors_packet.gyroscopes[2];
+        mag_field_msg_.header.frame_id = frame_id_;
+        tf2::Vector3 original_mag(raw_sensors_packet.magnetometers[0], raw_sensors_packet.magnetometers[1], raw_sensors_packet.magnetometers[2]);
+        tf2::Vector3 rotated_mag = rotation_matrix_ * original_mag;
+        mag_field_msg_.magnetic_field.x = rotated_mag.x();
+        mag_field_msg_.magnetic_field.y = rotated_mag.y();
+        mag_field_msg_.magnetic_field.z = rotated_mag.z();
+
+        imu_raw_msg_.header.frame_id = frame_id_;
+        imu_raw_msg_.orientation_covariance[0] = -1; // Tell recievers that no orientation is sent.
+        tf2::Vector3 original_raw_accel(raw_sensors_packet.accelerometers[0], raw_sensors_packet.accelerometers[1], raw_sensors_packet.accelerometers[2]);
+        tf2::Vector3 rotated_raw_accel = rotation_matrix_ * original_raw_accel;
+        imu_raw_msg_.linear_acceleration.x = rotated_raw_accel.x();
+        imu_raw_msg_.linear_acceleration.y = rotated_raw_accel.y();
+        imu_raw_msg_.linear_acceleration.z = rotated_raw_accel.z();
+
+        tf2::Vector3 original_raw_gyro(raw_sensors_packet.gyroscopes[0], raw_sensors_packet.gyroscopes[1], raw_sensors_packet.gyroscopes[2]);
+        tf2::Vector3 rotated_raw_gyro = rotation_matrix_ * original_raw_gyro;
+        imu_raw_msg_.angular_velocity.x = rotated_raw_gyro.x();
+        imu_raw_msg_.angular_velocity.y = rotated_raw_gyro.y();
+        imu_raw_msg_.angular_velocity.z = rotated_raw_gyro.z();
 
 		// BAROMETRIC PRESSURE
 		baro_msg_.header.frame_id = frame_id_;

adnav_launch/config/adnav_serial.yaml

@@ -45,4 +45,16 @@ adnav_node:
 
     # The size of the packet period in Microseconds. 
     # Bounded to [1000-65535] [1000Hz-15.29Hz]
-    packet_timer_period: 1000
+    packet_timer_period: 1000
+
+    # Optional
+    # Only do this if you have not already set the orientation and offset in the JAVA App
+    # Add TF rotation to the raw RPY values of the IMU based on your robot's body coordinate system
+    rotation_x_angle: 0.0
+    rotation_y_angle: 0.0
+    rotation_z_angle: 0.0
+
+    # Add an offset to the raw RPY values of the IMU based on your robot's body coordinate system
+    yaw_offset: 0.0
+    roll_offset: 0.0
+    pitch_offset: 0.0