Merge pull request #2 from NortekSupport/add_ins_to_ros2_driver

added INS support to ros2 driver

nucleus_driver/setup.cfg

@@ -1,6 +1,6 @@
 [metadata]
 name = nucleus_driver
-version = 1.5.10
+version = 1.5.11
 author = Martin Bergene Johansen
 author_email = [email protected]
 description = driver for the Nortek Nucleus

ros2/examples/example.py

@@ -23,6 +23,7 @@
 from nucleus_subscribers.current_profile_packets import SubscriberCurrentProfilePackets
 from nucleus_subscribers.field_calibration_packets import SubscriberFieldCalibrationPackets
 from nucleus_subscribers.imu_packets import SubscriberImuPackets
+from nucleus_subscribers.ins_packets import SubscriberInsPackets
 from nucleus_subscribers.magnetometer_packets import SubscriberMagnetometerPackets
 from nucleus_subscribers.water_track_packets import SubscriberWaterTrackPackets
 
@@ -37,6 +38,7 @@ def __init__(self) -> None:
         self.current_profile_queue = Queue()
         self.field_calibration_queue = Queue()
         self.imu_queue = Queue()
+        self.ins_queue = Queue()
         self.mag_queue = Queue()
         self.water_track_queue = Queue()
 
@@ -46,6 +48,7 @@ def __init__(self) -> None:
         self.current_profile_subscriber = SubscriberCurrentProfilePackets(callback_function=self.current_profile_queue.put)
         self.field_calibration_subscriber = SubscriberFieldCalibrationPackets(callback_function=self.field_calibration_queue.put)
         self.imu_subscriber = SubscriberImuPackets(callback_function=self.imu_queue.put)
+        self.ins_subscriber = SubscriberInsPackets(callback_function=self.ins_queue.put)
         self.mag_subscriber = SubscriberMagnetometerPackets(callback_function=self.mag_queue.put)
         self.water_track_subscriber = SubscriberWaterTrackPackets(callback_function=self.water_track_queue.put)
 
@@ -60,6 +63,7 @@ def start_subscribers(self):
         self.subscriber_executor.add_node(self.current_profile_subscriber)
         self.subscriber_executor.add_node(self.field_calibration_subscriber)
         self.subscriber_executor.add_node(self.imu_subscriber)
+        self.subscriber_executor.add_node(self.ins_subscriber)
         self.subscriber_executor.add_node(self.ahrs_subscriber)
         self.subscriber_executor.add_node(self.mag_subscriber)
         self.subscriber_executor.add_node(self.water_track_subscriber)
@@ -77,6 +81,7 @@ def stop_subscribers(self):
         self.current_profile_subscriber.destroy_node()
         self.field_calibration_subscriber.destroy_node()
         self.imu_subscriber.destroy_node()
+        self.ins_subscriber.destroy_node()
         self.mag_subscriber.destroy_node()
         self.water_track_subscriber.destroy_node()
 

ros2/src/interfaces/CMakeLists.txt

@@ -40,6 +40,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/CurrentProfile.msg"
   "msg/FieldCalibration.msg"
   "msg/IMU.msg"
+  "msg/INS.msg"
   "msg/Magnetometer.msg"
 )
 

ros2/src/interfaces/msg/INS.msg

@@ -0,0 +1,48 @@
+bool posix_time
+uint32 timestamp
+uint32 microseconds
+
+uint32 serial_number
+uint8 operation_mode
+float32 fom_ahrs
+float32 fom_fc1
+float32 roll
+float32 pitch
+float32 heading
+float32 quaternion_w
+float32 quaternion_x
+float32 quaternion_y
+float32 quaternion_z
+float32 rotation_matrix_0
+float32 rotation_matrix_1
+float32 rotation_matrix_2
+float32 rotation_matrix_3
+float32 rotation_matrix_4
+float32 rotation_matrix_5
+float32 rotation_matrix_6
+float32 rotation_matrix_7
+float32 rotation_matrix_8
+float32 declination
+float32 depth
+
+float32 fom_ins
+bool lat_long_is_valid
+float32 course_over_ground
+float32 temperature
+float32 pressure
+float32 altitude
+float64 latitude
+float64 longitude
+float32 position_frame_x
+float32 position_frame_y
+float32 position_frame_z
+float32 velocity_ned_x
+float32 velocity_ned_y
+float32 velocity_ned_z
+float32 velocity_nucleus_x
+float32 velocity_nucleus_y
+float32 velocity_nucleus_z
+float32 speed_over_ground
+float32 turn_rate_x
+float32 turn_rate_y
+float32 turn_rate_z