From 5ff7f6814c2ecf4e1f10f0b167df03507780554b Mon Sep 17 00:00:00 2001 From: Russ Webber Date: Thu, 23 Oct 2025 16:50:24 +1100 Subject: [PATCH 1/7] fix: use newer ServicesQoS --- adnav_driver/src/adnav_driver.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/adnav_driver/src/adnav_driver.cpp b/adnav_driver/src/adnav_driver.cpp index 7833959..18404a6 100644 --- a/adnav_driver/src/adnav_driver.cpp +++ b/adnav_driver/src/adnav_driver.cpp @@ -191,7 +191,7 @@ void Driver::createServices() { this, std::placeholders::_1, std::placeholders::_2), - rmw_qos_profile_services_default, + rclcpp::ServicesQoS(), service_group_); packet_period_timer_srv_ = this->create_service( @@ -201,7 +201,7 @@ void Driver::createServices() { this, std::placeholders::_1, std::placeholders::_2), - rmw_qos_profile_services_default, + rclcpp::ServicesQoS(), service_group_); request_packet_srv_ = this->create_service( @@ -211,7 +211,7 @@ void Driver::createServices() { this, std::placeholders::_1, std::placeholders::_2), - rmw_qos_profile_services_default, + rclcpp::ServicesQoS(), service_group_); ntrip_srv_ = this->create_service( @@ -221,7 +221,7 @@ void Driver::createServices() { this, std::placeholders::_1, std::placeholders::_2), - rmw_qos_profile_services_default, + rclcpp::ServicesQoS(), service_group_); } From 22f735ea5a311f245ef3a7517faab78c02b128d2 Mon Sep 17 00:00:00 2001 From: Russ Webber Date: Thu, 23 Oct 2025 17:04:04 +1100 Subject: [PATCH 2/7] fix: nest topics and services under nodename --- .../include/adnav_driver/adnav_driver.h | 3 -- adnav_driver/src/adnav_driver.cpp | 32 ++++++++----------- 2 files changed, 14 insertions(+), 21 deletions(-) diff --git a/adnav_driver/include/adnav_driver/adnav_driver.h b/adnav_driver/include/adnav_driver/adnav_driver.h index 9855254..394ad96 100644 --- a/adnav_driver/include/adnav_driver/adnav_driver.h +++ b/adnav_driver/include/adnav_driver/adnav_driver.h @@ -141,9 +141,6 @@ class Driver : public rclcpp::Node // Inheriting gives every "this->" as a poin // String to hold frame_id std::string frame_id_ = "imu_link"; - // String to hold node name. - std::string node_name_; - // device communication settings std::unique_ptr communicator_; adnav_connections_data_t comms_data_; diff --git a/adnav_driver/src/adnav_driver.cpp b/adnav_driver/src/adnav_driver.cpp index 18404a6..5fcf939 100644 --- a/adnav_driver/src/adnav_driver.cpp +++ b/adnav_driver/src/adnav_driver.cpp @@ -53,10 +53,6 @@ Driver::Driver(): rclcpp::Node("adnav_driver"), msg_write_done_(false) // Setup parameters for the node setupParamService(); - // Get the name of the node - node_name_ = this->get_name(); - RCLCPP_INFO(this->get_logger(), "\nNamespace: %s\n", node_name_.c_str()); - // Create timers for callbacks publish_timer_ = this->create_wall_timer( publish_timer_interval_ , std::bind(&Driver::publishTimerCallback, this), publishing_group_); @@ -168,16 +164,16 @@ void Driver::requestDeviceInfo() { */ void Driver::createPublishers() { // Creating the ROS2 Publishers - imu_pub_ = this->create_publisher(std::string(node_name_ + "/imu"), 10); - imu_raw_pub_ = this->create_publisher(std::string(node_name_ + "/imu_raw"), 10); - nav_sat_fix_pub_ = this->create_publisher(std::string(node_name_ + "/nav_sat_fix"), 10); - magnetic_field_pub_ = this->create_publisher(std::string(node_name_ + "/magnetic_field"), 10); - barometric_pressure_pub_ = this->create_publisher(std::string(node_name_ + "/barometric_pressure"), 10); - temperature_pub_ = this->create_publisher(std::string(node_name_ + "/temperature"), 10); - twist_pub_ = this->create_publisher(std::string(node_name_ + "/twist"), 10); - pose_pub_ = this->create_publisher(std::string(node_name_ + "/pose"), 10); - system_status_pub_ = this->create_publisher(std::string(node_name_ + "/system_status"), 10); - filter_status_pub_ = this->create_publisher(std::string(node_name_ + "/filter_status"), 10); + imu_pub_ = this->create_publisher("~/imu", 10); + imu_raw_pub_ = this->create_publisher("~/imu_raw", 10); + nav_sat_fix_pub_ = this->create_publisher("~/nav_sat_fix", 10); + magnetic_field_pub_ = this->create_publisher("~/magnetic_field", 10); + barometric_pressure_pub_ = this->create_publisher("~/barometric_pressure", 10); + temperature_pub_ = this->create_publisher("~/temperature", 10); + twist_pub_ = this->create_publisher("~/twist", 10); + pose_pub_ = this->create_publisher("~/pose", 10); + system_status_pub_ = this->create_publisher("~/system_status", 10); + filter_status_pub_ = this->create_publisher("~/filter_status", 10); } /** @@ -185,7 +181,7 @@ void Driver::createPublishers() { */ void Driver::createServices() { packet_period_srv_ = this->create_service( - (node_name_ + "/packet_periods"), + "~/packet_periods", std::bind( &Driver::srvPacketPeriods, this, @@ -195,7 +191,7 @@ void Driver::createServices() { service_group_); packet_period_timer_srv_ = this->create_service( - (node_name_ + "/packet_timer_period"), + "~/packet_timer_period", std::bind( &Driver::srvPacketTimerPeriod, this, @@ -205,7 +201,7 @@ void Driver::createServices() { service_group_); request_packet_srv_ = this->create_service( - (node_name_ + "/request_packet"), + "~/request_packet", std::bind( &Driver::srvRequestPackets, this, @@ -215,7 +211,7 @@ void Driver::createServices() { service_group_); ntrip_srv_ = this->create_service( - (node_name_ + "/ntrip"), + "~/ntrip", std::bind( &Driver::srvNtrip, this, From ebe2741c36409f4979d2f9690baeb8c01e71d7b6 Mon Sep 17 00:00:00 2001 From: Russ Webber Date: Mon, 27 Oct 2025 10:53:35 +1100 Subject: [PATCH 3/7] feat: add TwistStamped, GeoPoseStamped topics --- adnav_driver/CMakeLists.txt | 2 + .../include/adnav_driver/adnav_driver.h | 12 ++++++ adnav_driver/package.xml | 1 + adnav_driver/src/adnav_driver.cpp | 43 +++++++++++++++---- 4 files changed, 50 insertions(+), 8 deletions(-) diff --git a/adnav_driver/CMakeLists.txt b/adnav_driver/CMakeLists.txt index b4faf4e..da63c56 100644 --- a/adnav_driver/CMakeLists.txt +++ b/adnav_driver/CMakeLists.txt @@ -28,6 +28,7 @@ find_package(std_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(diagnostic_msgs REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(geographic_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(tf2 REQUIRED) find_package(adnav_interfaces REQUIRED) @@ -55,6 +56,7 @@ set(DEPENDENCIES "geometry_msgs" "tf2" "adnav_interfaces" + "geographic_msgs" ) set(EXECUTABLE_NAME "adnav_driver") diff --git a/adnav_driver/include/adnav_driver/adnav_driver.h b/adnav_driver/include/adnav_driver/adnav_driver.h index 394ad96..8c8f6bc 100644 --- a/adnav_driver/include/adnav_driver/adnav_driver.h +++ b/adnav_driver/include/adnav_driver/adnav_driver.h @@ -67,12 +67,16 @@ #include #include #include +#include #include +#include #include #include #include #include #include +#include +#include #if defined(WIN32) || defined(_WIN32) #pragma comment(lib, "ws2_32.lib") // Winsock Library @@ -167,7 +171,11 @@ class Driver : public rclcpp::Node // Inheriting gives every "this->" as a poin sensor_msgs::msg::FluidPressure baro_msg_; sensor_msgs::msg::Temperature temp_msg_; geometry_msgs::msg::Twist twist_msg_; + geometry_msgs::msg::TwistStamped twist_stamped_msg_; + geometry_msgs::msg::PoseStamped pose_stamped_msg_; geometry_msgs::msg::Pose pose_msg_; + geographic_msgs::msg::GeoPose geo_pose_msg_; + geographic_msgs::msg::GeoPoseStamped geo_pose_stamped_msg_; diagnostic_msgs::msg::DiagnosticStatus system_status_msg_; diagnostic_msgs::msg::DiagnosticStatus filter_status_msg_; @@ -179,7 +187,11 @@ class Driver : public rclcpp::Node // Inheriting gives every "this->" as a poin rclcpp::Publisher::SharedPtr barometric_pressure_pub_; rclcpp::Publisher::SharedPtr temperature_pub_; rclcpp::Publisher::SharedPtr twist_pub_; + rclcpp::Publisher::SharedPtr twist_stamped_pub_; rclcpp::Publisher::SharedPtr pose_pub_; + rclcpp::Publisher::SharedPtr pose_stamped_pub_; + rclcpp::Publisher::SharedPtr geo_pose_pub_; + rclcpp::Publisher::SharedPtr geo_pose_stamped_pub_; rclcpp::Publisher::SharedPtr system_status_pub_; rclcpp::Publisher::SharedPtr filter_status_pub_; diff --git a/adnav_driver/package.xml b/adnav_driver/package.xml index f90e8fb..b25936c 100644 --- a/adnav_driver/package.xml +++ b/adnav_driver/package.xml @@ -14,6 +14,7 @@ sensor_msgs diagnostic_msgs geometry_msgs + geographic_msgs tf2 std_srvs adnav_interfaces diff --git a/adnav_driver/src/adnav_driver.cpp b/adnav_driver/src/adnav_driver.cpp index 5fcf939..6d1ccde 100644 --- a/adnav_driver/src/adnav_driver.cpp +++ b/adnav_driver/src/adnav_driver.cpp @@ -103,6 +103,20 @@ Driver::~Driver() { communicator_->close(); } +rclcpp::Time time_from_state_packet(const system_state_packet_t& state_packet) { + // Create a ROS time from the system state packet timestamp + auto sec = state_packet.unix_time_seconds; + auto nanosecs = state_packet.microseconds * 1000; + + // Handle possible rollover of nanoseconds + if (nanosecs >= 1000000000) { + sec += nanosecs / 1000000000; + nanosecs = nanosecs % 1000000000; + } + + return {static_cast(sec), nanosecs, RCL_ROS_TIME}; +} + /** * @brief Function to ask for device information from a Advanced navigation device and wait for its response. */ @@ -171,9 +185,11 @@ void Driver::createPublishers() { barometric_pressure_pub_ = this->create_publisher("~/barometric_pressure", 10); temperature_pub_ = this->create_publisher("~/temperature", 10); twist_pub_ = this->create_publisher("~/twist", 10); + twist_stamped_pub_ = this->create_publisher("~/twist_stamped", 10); pose_pub_ = this->create_publisher("~/pose", 10); - system_status_pub_ = this->create_publisher("~/system_status", 10); - filter_status_pub_ = this->create_publisher("~/filter_status", 10); + pose_stamped_pub_ = this->create_publisher("~/pose_stamped", 10); + geo_pose_pub_ = this->create_publisher("~/geopose", 10); + geo_pose_stamped_pub_ = this->create_publisher("~/geopose_stamped", 10); } /** @@ -548,6 +564,7 @@ void Driver::publishTimerCallback() { // PUBLISH MESSAGES nav_sat_fix_pub_->publish(nav_fix_msg_); twist_pub_->publish(twist_msg_); + twist_stamped_pub_->publish(twist_stamped_msg_); imu_pub_->publish(imu_msg_); imu_raw_pub_->publish(imu_raw_msg_); system_status_pub_->publish(system_status_msg_); @@ -556,6 +573,9 @@ void Driver::publishTimerCallback() { barometric_pressure_pub_->publish(baro_msg_); temperature_pub_->publish(temp_msg_); pose_pub_->publish(pose_msg_); + pose_stamped_pub_->publish(pose_stamped_msg_); + geo_pose_pub_->publish(geo_pose_msg_); + geo_pose_stamped_pub_->publish(geo_pose_stamped_msg_); RCLCPP_DEBUG(this->get_logger(), "Pub: \t\tMutex: U\tAccess: %d", pub_num_++); @@ -1588,7 +1608,7 @@ void Driver::systemStateRosDecoder(an_packet_t* an_packet) { nav_fix_msg_.position_covariance = { pow(system_state_packet.standard_deviation[1], 2), 0.0, 0.0, 0.0, pow(system_state_packet.standard_deviation[0], 2), 0.0, 0.0, 0.0, pow(system_state_packet.standard_deviation[2], 2)}; - nav_fix_msg_.position_covariance_type = nav_fix_msg_.COVARIANCE_TYPE_DIAGONAL_KNOWN; + nav_fix_msg_.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; llh_.latitude = system_state_packet.latitude * RADIANS_TO_DEGREES; llh_.longitude = system_state_packet.longitude * RADIANS_TO_DEGREES; @@ -1604,7 +1624,8 @@ void Driver::systemStateRosDecoder(an_packet_t* an_packet) { 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]; - + twist_stamped_msg_.twist = twist_msg_; + twist_stamped_msg_.header = nav_fix_msg_.header; // IMU imu_msg_.header.stamp.sec = system_state_packet.unix_time_seconds; @@ -1622,10 +1643,9 @@ void Driver::systemStateRosDecoder(an_packet_t* an_packet) { 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]; + pose_msg_.orientation = imu_msg_.orientation; + pose_stamped_msg_.pose = pose_msg_; + pose_stamped_msg_.header = nav_fix_msg_.header; 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]; @@ -1636,6 +1656,13 @@ void Driver::systemStateRosDecoder(an_packet_t* an_packet) { imu_msg_.linear_acceleration.y = system_state_packet.body_acceleration[1]; imu_msg_.linear_acceleration.z = system_state_packet.body_acceleration[2]; + geo_pose_msg_.orientation = imu_msg_.orientation; + geo_pose_msg_.position.latitude = nav_fix_msg_.latitude; + geo_pose_msg_.position.longitude = nav_fix_msg_.longitude; + geo_pose_msg_.position.altitude = nav_fix_msg_.altitude; + geo_pose_stamped_msg_.pose = geo_pose_msg_; + geo_pose_stamped_msg_.header = nav_fix_msg_.header; + // SYSTEM STATUS system_status_msg_.message = ""; system_status_msg_.level = diagnostic_msgs::msg::DiagnosticStatus::OK; // default OK state From 0b58010f2eb7ba54d3ebbb94f8c7fcf8ff6cc271 Mon Sep 17 00:00:00 2001 From: Russ Webber Date: Mon, 27 Oct 2025 12:12:27 +1100 Subject: [PATCH 4/7] fix: launch files --- adnav_launch/launch/adnav_serial.launch.py | 2 -- adnav_launch/launch/adnav_tcp_client.launch.py | 2 -- adnav_launch/launch/adnav_tcp_server.launch.py | 2 -- adnav_launch/launch/adnav_udp_client.launch.py | 2 -- 4 files changed, 8 deletions(-) diff --git a/adnav_launch/launch/adnav_serial.launch.py b/adnav_launch/launch/adnav_serial.launch.py index 1f3ae93..e624652 100644 --- a/adnav_launch/launch/adnav_serial.launch.py +++ b/adnav_launch/launch/adnav_serial.launch.py @@ -22,7 +22,5 @@ def generate_launch_description(): parameters = [config] ) - ld.add_action - ld.add_action(node) return ld \ No newline at end of file diff --git a/adnav_launch/launch/adnav_tcp_client.launch.py b/adnav_launch/launch/adnav_tcp_client.launch.py index cbf9389..6b3d769 100644 --- a/adnav_launch/launch/adnav_tcp_client.launch.py +++ b/adnav_launch/launch/adnav_tcp_client.launch.py @@ -22,7 +22,5 @@ def generate_launch_description(): parameters = [config] ) - ld.add_action - ld.add_action(node) return ld \ No newline at end of file diff --git a/adnav_launch/launch/adnav_tcp_server.launch.py b/adnav_launch/launch/adnav_tcp_server.launch.py index 24dfdfd..fe9c51b 100644 --- a/adnav_launch/launch/adnav_tcp_server.launch.py +++ b/adnav_launch/launch/adnav_tcp_server.launch.py @@ -22,7 +22,5 @@ def generate_launch_description(): parameters = [config] ) - ld.add_action - ld.add_action(node) return ld \ No newline at end of file diff --git a/adnav_launch/launch/adnav_udp_client.launch.py b/adnav_launch/launch/adnav_udp_client.launch.py index 1888f11..38de42c 100644 --- a/adnav_launch/launch/adnav_udp_client.launch.py +++ b/adnav_launch/launch/adnav_udp_client.launch.py @@ -22,7 +22,5 @@ def generate_launch_description(): parameters = [config] ) - ld.add_action - ld.add_action(node) return ld \ No newline at end of file From e83e726132396666b505272d89fa364449c98576 Mon Sep 17 00:00:00 2001 From: Russ Webber Date: Wed, 29 Oct 2025 10:14:46 +1100 Subject: [PATCH 5/7] feat: use diagnostic_updater for diagnostics --- adnav_driver/CMakeLists.txt | 15 +- .../include/adnav_driver/adnav_driver.h | 15 +- adnav_driver/package.xml | 2 +- adnav_driver/src/adnav_driver.cpp | 337 ++++++++---------- adnav_driver/src/main.cpp | 5 +- adnav_launch/config/adnav_udp_client.yaml | 2 +- 6 files changed, 156 insertions(+), 220 deletions(-) diff --git a/adnav_driver/CMakeLists.txt b/adnav_driver/CMakeLists.txt index da63c56..5e72907 100644 --- a/adnav_driver/CMakeLists.txt +++ b/adnav_driver/CMakeLists.txt @@ -1,5 +1,4 @@ cmake_minimum_required(VERSION 3.5) -set(CMAKE_BUILD_TYPE release) project(adnav_driver) # Default to C99 @@ -9,7 +8,7 @@ endif() # Default to C++14 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") @@ -26,7 +25,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(sensor_msgs REQUIRED) -find_package(diagnostic_msgs REQUIRED) +find_package(diagnostic_updater REQUIRED) find_package(geometry_msgs REQUIRED) find_package(geographic_msgs REQUIRED) find_package(std_srvs REQUIRED) @@ -52,7 +51,7 @@ set(DEPENDENCIES "std_msgs" "std_srvs" "sensor_msgs" - "diagnostic_msgs" + "diagnostic_updater" "geometry_msgs" "tf2" "adnav_interfaces" @@ -67,18 +66,10 @@ add_executable(${EXECUTABLE_NAME} src/main.cpp) target_link_libraries(${EXECUTABLE_NAME} ${PROJECT_NAME}_lib) ament_target_dependencies(${EXECUTABLE_NAME} ${DEPENDENCIES}) -# add_executable(srv_test src/srv_test.cpp) -# target_link_libraries(srv_test ${PROJECT_NAME}_lib) -# ament_target_dependencies(srv_test ${DEPENDENCIES}) - install(TARGETS ${EXECUTABLE_NAME} DESTINATION lib/${PROJECT_NAME} ) -# install(TARGETS srv_test -# DESTINATION lib/${PROJECT_NAME} -# ) - install( DIRECTORY include/ DESTINATION include diff --git a/adnav_driver/include/adnav_driver/adnav_driver.h b/adnav_driver/include/adnav_driver/adnav_driver.h index 8c8f6bc..f6a8c38 100644 --- a/adnav_driver/include/adnav_driver/adnav_driver.h +++ b/adnav_driver/include/adnav_driver/adnav_driver.h @@ -60,6 +60,7 @@ // ROS2 Packages, Services, Messages #include +#include "diagnostic_updater/diagnostic_updater.hpp" #include #include #include @@ -70,7 +71,6 @@ #include #include #include -#include #include #include #include @@ -160,7 +160,8 @@ class Driver : public rclcpp::Node // Inheriting gives every "this->" as a poin // ANPP Packet variables acknowledge_packet_t acknowledge_packet_; // only access with protection of acknowledge_mutex_ - device_information_packet_t device_information_packet_; + std::optional device_information_packet_; + std::optional system_state_packet_; // Msgs. Only access with protection of messages_mutex_ tf2::Quaternion orientation_; @@ -176,8 +177,8 @@ class Driver : public rclcpp::Node // Inheriting gives every "this->" as a poin geometry_msgs::msg::Pose pose_msg_; geographic_msgs::msg::GeoPose geo_pose_msg_; geographic_msgs::msg::GeoPoseStamped geo_pose_stamped_msg_; - diagnostic_msgs::msg::DiagnosticStatus system_status_msg_; - diagnostic_msgs::msg::DiagnosticStatus filter_status_msg_; + + std::shared_ptr diagnostic_updater_; // Publishers rclcpp::Publisher::SharedPtr imu_pub_; @@ -192,8 +193,6 @@ class Driver : public rclcpp::Node // Inheriting gives every "this->" as a poin rclcpp::Publisher::SharedPtr pose_stamped_pub_; rclcpp::Publisher::SharedPtr geo_pose_pub_; rclcpp::Publisher::SharedPtr geo_pose_stamped_pub_; - rclcpp::Publisher::SharedPtr system_status_pub_; - rclcpp::Publisher::SharedPtr filter_status_pub_; // ~~~~~~~~~~~~~~~ Callback handles and parameters // Callback groups Allows the callbacks to be processed on a different thread by @@ -260,8 +259,8 @@ class Driver : public rclcpp::Node // Inheriting gives every "this->" as a poin //~~~~~~ Logging Functions void openLogFile(); - void statusErrLog(const std::string& errmsg); - void statusWarnLog(const std::string& warnmsg); + void system_status_diagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat); + void filter_status_diagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat); //~~~~~~ ROS Services void srvPacketPeriods(const std::shared_ptr request, diff --git a/adnav_driver/package.xml b/adnav_driver/package.xml index b25936c..a7be35f 100644 --- a/adnav_driver/package.xml +++ b/adnav_driver/package.xml @@ -12,7 +12,7 @@ rclcpp std_msgs sensor_msgs - diagnostic_msgs + diagnostic_updater geometry_msgs geographic_msgs tf2 diff --git a/adnav_driver/src/adnav_driver.cpp b/adnav_driver/src/adnav_driver.cpp index 6d1ccde..cd06aa7 100644 --- a/adnav_driver/src/adnav_driver.cpp +++ b/adnav_driver/src/adnav_driver.cpp @@ -50,6 +50,8 @@ Driver::Driver(): rclcpp::Node("adnav_driver"), msg_write_done_(false) // Group for completing incoming services. service_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + diagnostic_updater_ = std::make_shared(this); + // Setup parameters for the node setupParamService(); @@ -79,6 +81,9 @@ Driver::Driver(): rclcpp::Node("adnav_driver"), msg_write_done_(false) // Send current setup of timer period, and packet periods to the device. deviceSetup(); + diagnostic_updater_->add("System Status", this, &Driver::system_status_diagnostic); + diagnostic_updater_->add("Filter Status", this, &Driver::filter_status_diagnostic); + RCLCPP_INFO(this->get_logger(), "Your Advanced Navigation ROS driver is currently running\nPress Ctrl-C to interrupt\n"); } @@ -567,8 +572,6 @@ void Driver::publishTimerCallback() { twist_stamped_pub_->publish(twist_stamped_msg_); imu_pub_->publish(imu_msg_); imu_raw_pub_->publish(imu_raw_msg_); - system_status_pub_->publish(system_status_msg_); - filter_status_pub_->publish(filter_status_msg_); magnetic_field_pub_->publish(mag_field_msg_); barometric_pressure_pub_->publish(baro_msg_); temperature_pub_->publish(temp_msg_); @@ -611,34 +614,128 @@ void Driver::RestartReader() { read_timer_interval_, std::bind(&Driver::recievePackets, this), reading_group_); } -//~~~~~~ Logging Functions +//~~~~~~ Diagnostic Functions -/** - * @brief Function to log an error message into the ROS system status and to the Error Logger - * - * This function assumes it is being called from a function with thread safe access to the ROS messages. - * - * @param errmsg string containing the message to be put into the logger. - */ -void Driver::statusErrLog(const std::string& errmsg) { +void Driver::system_status_diagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat) +{ + if (system_state_packet_.has_value()) + { + auto last_rx_time = time_from_state_packet(system_state_packet_.value()); + + if (this->get_clock()->now() - last_rx_time < rclcpp::Duration(1, 0)) + { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "System State packet timeout"); + return; + } + + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + + if (system_state_packet_->system_status.b.system_failure) { + stat.add("System", "Fail"); + stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + } + if (system_state_packet_->system_status.b.accelerometer_sensor_failure) { + stat.add("Accelerometer", "Fail"); + stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + } + if (system_state_packet_->system_status.b.gyroscope_sensor_failure) { + stat.add("Gyroscope", "Fail"); + stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + } + if (system_state_packet_->system_status.b.magnetometer_sensor_failure) { + stat.add("Magnetometer", "Fail"); + stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + } + if (system_state_packet_->system_status.b.pressure_sensor_failure) { + stat.add("Pressure Sensor", "Fail"); + stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + } + if (system_state_packet_->system_status.b.gnss_failure) { + stat.add("GNSS", "Fail"); + stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + } + if (system_state_packet_->system_status.b.accelerometer_over_range) { + stat.add("Accelerometer Over Range", "Fail"); + stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + } + if (system_state_packet_->system_status.b.gyroscope_over_range) { + stat.add("Gyroscope Over Range", "Fail"); + stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + } + if (system_state_packet_->system_status.b.magnetometer_over_range) { + stat.add("Magnetometer", " Over Range"); + stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + } + if (system_state_packet_->system_status.b.pressure_over_range) { + stat.add("Pressure Sensor", " Over Range"); + stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + } + if (system_state_packet_->system_status.b.minimum_temperature_alarm) { + stat.add("Temperature Alarm", "Minimum Temperature Alarm"); + stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + } + if (system_state_packet_->system_status.b.maximum_temperature_alarm) { + stat.add("Temperature Alarm", "Maximum Temperature Alarm"); + stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + } + if (system_state_packet_->system_status.b.internal_data_logging_error) { + stat.add("Data Logging", "Internal Error"); + stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + } + if (system_state_packet_->system_status.b.high_voltage_alarm) { + stat.add("Power Supply", "High Voltage Alarm"); + stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + } + if (system_state_packet_->system_status.b.gnss_antenna_fault) { + stat.add("GNSS Antenna", "Fault Detected"); + stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + } + if (system_state_packet_->system_status.b.serial_port_overflow_alarm) { + stat.add("Serial Port", "Data Overflow"); + stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + } - system_status_msg_.level = 2; // ERROR state - system_status_msg_.message += errmsg; - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 10000, errmsg.c_str()); + if (stat.level == diagnostic_msgs::msg::DiagnosticStatus::OK) { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "OK"); + } else + { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "System Failure Detected"); + } + } else { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "System State packet not received"); + } } -/** - * @brief Function to log an warning message into the ROS filter status and to the warning Logger - * - * This function assumes it is being called from a function with thread safe access to the ROS messages. - * - * @param warnmsg string containing the message to be put into the logger. - */ -void Driver::statusWarnLog(const std::string& warnmsg) { +void Driver::filter_status_diagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat) +{ + if (system_state_packet_.has_value()) + { + auto last_rx_time = time_from_state_packet(system_state_packet_.value()); - filter_status_msg_.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; // WARN state - filter_status_msg_.message += warnmsg; - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 10000, warnmsg.c_str()); + if (this->get_clock()->now() - last_rx_time < rclcpp::Duration(1, 0)) + { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "System State packet timeout"); + return; + } + + stat.add("Orientation Filter Initialised", system_state_packet_->filter_status.b.orientation_filter_initialised ? "Yes" : "No"); + stat.add("Navigation Filter Initialised", system_state_packet_->filter_status.b.ins_filter_initialised ? "Yes" : "No"); + stat.add("Heading Initialised", system_state_packet_->filter_status.b.heading_initialised ? "Yes" : "No"); + stat.add("UTC Time Initialised", system_state_packet_->filter_status.b.utc_time_initialised ? "Yes" : "No"); + stat.add("Internal GNSS Enabled", system_state_packet_->filter_status.b.internal_gnss_enabled ? "Yes" : "No"); + stat.add("Dual Antenna Heading Active", system_state_packet_->filter_status.b.dual_antenna_heading_active ? "Yes" : "No"); + stat.add("Velocity Heading Enabled", system_state_packet_->filter_status.b.velocity_heading_enabled ? "Yes" : "No"); + stat.add("Atmospheric Altitude Enabled", system_state_packet_->filter_status.b.atmospheric_altitude_enabled ? "Yes" : "No"); + stat.add("External Position Active", system_state_packet_->filter_status.b.external_position_active ? "Yes" : "No"); + stat.add("External Velocity Active", system_state_packet_->filter_status.b.external_velocity_active ? "Yes" : "No"); + stat.add("External Heading Active", system_state_packet_->filter_status.b.external_heading_active ? "Yes" : "No"); + + // TODO parameters need to be added to inform what is considered abnormal operation + + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "OK"); + } else { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "System State packet not received"); + } } //~~~~~~ Service Functions @@ -1538,21 +1635,28 @@ void Driver::acknowledgeDecoder(an_packet_t* an_packet) { */ void Driver::deviceInfoDecoder(an_packet_t* an_packet) { // Decode packet and warn if error. - if(decode_device_information_packet(&device_information_packet_, an_packet)) - { + device_information_packet_t device_information_packet; + if(decode_device_information_packet(&device_information_packet, an_packet)) + { RCLCPP_WARN(this->get_logger(), "Error decoding device information Packet"); + } else + { + std::stringstream serial_num; + serial_num << std::hex << device_information_packet.serial_number[0] << + device_information_packet.serial_number[1] << device_information_packet.serial_number[2]; + diagnostic_updater_->setHardwareID(serial_num.str()); + device_information_packet_ = device_information_packet; } // since multiple packets may be requested before the device responds. ensure only one gets printed per second. RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Device Information:\n" - << "Device ID: " << device_information_packet_.device_id << + << "Device ID: " << device_information_packet.device_id << "\nVersion:" << - "\n Software: " << device_information_packet_.software_version << - "\n Hardware: " << device_information_packet_.hardware_revision << - "\nSerial Number: " << std::hex << device_information_packet_.serial_number[0] << - device_information_packet_.serial_number[1] << device_information_packet_.serial_number[2] + "\n Software: " << device_information_packet.software_version << + "\n Hardware: " << device_information_packet.hardware_revision << + "\nSerial Number: " << std::hex << device_information_packet.serial_number[0] << + device_information_packet.serial_number[1] << device_information_packet.serial_number[2] << std::endl ); - } /** @@ -1575,8 +1679,7 @@ void Driver::systemStateRosDecoder(an_packet_t* an_packet) { if(decode_system_state_packet(&system_state_packet, an_packet) == 0) { // NAVSATFIX - nav_fix_msg_.header.stamp.sec = system_state_packet.unix_time_seconds; - nav_fix_msg_.header.stamp.nanosec = system_state_packet.microseconds*1000; + nav_fix_msg_.header.stamp = time_from_state_packet(system_state_packet); nav_fix_msg_.header.frame_id = frame_id_; if ((system_state_packet.filter_status.b.gnss_fix_type == gnss_fix_2d) || (system_state_packet.filter_status.b.gnss_fix_type == gnss_fix_3d)) @@ -1628,9 +1731,7 @@ void Driver::systemStateRosDecoder(an_packet_t* an_packet) { twist_stamped_msg_.header = nav_fix_msg_.header; // 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_; + imu_msg_.header = nav_fix_msg_.header; // Using the RPY orientation as done by cosama orientation_.setRPY( system_state_packet.orientation[0], @@ -1663,158 +1764,14 @@ void Driver::systemStateRosDecoder(an_packet_t* an_packet) { geo_pose_stamped_msg_.pose = geo_pose_msg_; geo_pose_stamped_msg_.header = nav_fix_msg_.header; - // SYSTEM STATUS - system_status_msg_.message = ""; - system_status_msg_.level = diagnostic_msgs::msg::DiagnosticStatus::OK; // default OK state - std::stringstream serial_num; - serial_num << std::hex << device_information_packet_.serial_number[0] << - device_information_packet_.serial_number[1] << device_information_packet_.serial_number[2]; - system_status_msg_.hardware_id = serial_num.str(); - if (system_state_packet.system_status.b.system_failure) { - ss << "\n0. SYSTEM FAILURE DETECTED."; - } - if (system_state_packet.system_status.b.accelerometer_sensor_failure) { - ss << "\n1. ACCELEROMETER SENSOR FAILURE."; - } - if (system_state_packet.system_status.b.gyroscope_sensor_failure) { - ss << "\n2. GYROSCOPE SENSOR FAILURE."; - } - if (system_state_packet.system_status.b.magnetometer_sensor_failure) { - ss << "\n3. MAGNETOMETER SENSOR FAILURE."; - } - if (system_state_packet.system_status.b.pressure_sensor_failure) { - ss << "\n4. PRESSURE SENSOR FAILURE."; - } - if (system_state_packet.system_status.b.gnss_failure) { - ss << "\n5. GNSS FAILURE."; - } - if (system_state_packet.system_status.b.accelerometer_over_range) { - ss << "\n6. ACCELEROMETER OVER RANGE."; - } - if (system_state_packet.system_status.b.gyroscope_over_range) { - ss << "\n7. GYROSCOPE OVER RANGE."; - } - if (system_state_packet.system_status.b.magnetometer_over_range) { - ss << "\n8. MAGNETOMETER OVER RANGE."; - } - if (system_state_packet.system_status.b.pressure_over_range) { - ss << "\n9. PRESSURE OVER RANGE."; - } - if (system_state_packet.system_status.b.minimum_temperature_alarm) { - ss << "\n10. MINIMUM TEMPERATURE ALARM."; - } - if (system_state_packet.system_status.b.maximum_temperature_alarm) { - ss << "\n11. MAXIMUM TEMPERATURE ALARM."; - } - if (system_state_packet.system_status.b.internal_data_logging_error) { - ss << "\n12. INTERNAL DATA LOGGING ERROR."; - } - if (system_state_packet.system_status.b.high_voltage_alarm) { - ss << "\n13. HIGH VOLTAGE ALARM."; - } - if (system_state_packet.system_status.b.gnss_antenna_fault) { - ss << "\n14. GNSS ANTENNA FAULT."; - } - if (system_state_packet.system_status.b.serial_port_overflow_alarm) { - ss << "\n15. SERIAL PORT DATA OVERFLOW."; - } - - // If an error occured log it - if(!ss.str().empty()) { - ss << std::endl; - statusErrLog(ss.str()); - // empty the stringstream - ss.str(""); - } + system_state_packet_ = system_state_packet; - // FILTER STATUS - filter_status_msg_.message = ""; - filter_status_msg_.level = diagnostic_msgs::msg::DiagnosticStatus::OK;; // default OK state - filter_status_msg_.hardware_id = serial_num.str(); - if (system_state_packet.filter_status.b.orientation_filter_initialised) { - filter_status_msg_.message += "\n0. Orientation Filter Initialised."; - } - else { - ss << "\n0. Orientation Filter NOT Initialised."; - } - if (system_state_packet.filter_status.b.ins_filter_initialised) { - filter_status_msg_.message += "\n1. Navigation Filter Initialised."; - } - else { - ss << "\n1. Navigation Filter NOT Initialised."; - } - if (system_state_packet.filter_status.b.heading_initialised) { - filter_status_msg_.message += "\n2. Heading Initialised."; - } - else { - ss << "\n2. Heading NOT Initialised."; - } - if (system_state_packet.filter_status.b.utc_time_initialised) { - filter_status_msg_.message += "\n3. UTC Time Initialised."; - } - else { - ss << "\n3. UTC Time NOT Initialised."; - } if (system_state_packet.filter_status.b.event1_flag) { - ss << "\n7. Event 1 Occured."; - } - else { - filter_status_msg_.message += "\n7. Event 1 NOT Occured."; - } - if (system_state_packet.filter_status.b.event2_flag) { - ss << "\n8. Event 2 Occured."; - } - else { - filter_status_msg_.message += "\n8. Event 2 NOT Occured."; - } - if (system_state_packet.filter_status.b.internal_gnss_enabled) { - filter_status_msg_.message += "\n9. Internal GNSS Enabled."; - } - else { - ss << "\n9. Internal GNSS NOT Enabled."; - } - if (system_state_packet.filter_status.b.dual_antenna_heading_active) { - filter_status_msg_.message += "\n10. Dual Antenna Heading Active."; - } - else { - ss << "\n10. Dual Antenna Heading NOT Active."; - } - if (system_state_packet.filter_status.b.velocity_heading_enabled) { - filter_status_msg_.message += "\n11. Velocity Heading Enabled."; - } - else { - ss << "\n11. Velocity Heading NOT Enabled."; - } - if (system_state_packet.filter_status.b.atmospheric_altitude_enabled) { - filter_status_msg_.message += "\n12. Atmospheric Altitude Enabled."; - } - else { - ss << "\n12. Atmospheric Altitude NOT Enabled."; - } - if (system_state_packet.filter_status.b.external_position_active) { - filter_status_msg_.message += "\n13. External Position Active."; - } - else { - ss << "\n13. External Position NOT Active."; - } - if (system_state_packet.filter_status.b.external_velocity_active) { - filter_status_msg_.message += "\n14. External Velocity Active."; - } - else { - ss << "\n14. External Velocity NOT Active."; - } - if (system_state_packet.filter_status.b.external_heading_active) { - filter_status_msg_.message += "\n15. External Heading Active."; - } - else { - ss << "\n16. External Heading NOT Active."; + RCLCPP_INFO(this->get_logger(), "Event 1 Occurred."); } - // If a warning has occued log it - if(!ss.str().empty()) { - ss << std::endl; - statusWarnLog(ss.str()); - ss.str(""); + if (system_state_packet.filter_status.b.event2_flag) { + RCLCPP_INFO(this->get_logger(), "Event 1 Occurred."); } } // Now that work is complete notify an update for the publisher. @@ -1937,12 +1894,4 @@ void Driver::rawSensorsRosDecoder(an_packet_t* an_packet) { auto diff = this->get_clock().get()->now().nanoseconds() - time; RCLCPP_DEBUG(this->get_logger(), "Packet 28:\tMutex: U\tAccess: %d\tTimeLock: %ld μs", P28_num_++, diff/1000); } - - - }// namespace adnav - - - - - diff --git a/adnav_driver/src/main.cpp b/adnav_driver/src/main.cpp index 033abbc..a78c552 100644 --- a/adnav_driver/src/main.cpp +++ b/adnav_driver/src/main.cpp @@ -41,10 +41,7 @@ int main(int argc, char * argv[]) // Add the driver node to the executor and spin it. executor.add_node(node); - - while(rclcpp::ok()) { - executor.spin(); - } + executor.spin(); rclcpp::shutdown(); return 0; diff --git a/adnav_launch/config/adnav_udp_client.yaml b/adnav_launch/config/adnav_udp_client.yaml index b8699a0..57e610d 100644 --- a/adnav_launch/config/adnav_udp_client.yaml +++ b/adnav_launch/config/adnav_udp_client.yaml @@ -24,7 +24,7 @@ adnav_node: # This is the minimum period for reading and publishing data. # Warning on some systems this is performance limited. - # The configuration will be rejected if publissh_us < read_us + # The configuration will be rejected if publish_us < read_us # Units are in μs. 20000 = 50Hz | 1000 = 1000Hz etc. publish_us: 20000 read_us: 1000 From 093df0ad2e9b08e05ded55ecbe322137a5663c43 Mon Sep 17 00:00:00 2001 From: Russ Webber Date: Mon, 10 Nov 2025 13:20:22 +1100 Subject: [PATCH 6/7] feat: add ability to transform twist to FLU --- adnav_driver/CMakeLists.txt | 2 + .../include/adnav_driver/adnav_driver.h | 3 + adnav_driver/package.xml | 1 + adnav_driver/src/adnav_driver.cpp | 60 ++++++++++++++++--- 4 files changed, 59 insertions(+), 7 deletions(-) diff --git a/adnav_driver/CMakeLists.txt b/adnav_driver/CMakeLists.txt index 5e72907..7b2aae4 100644 --- a/adnav_driver/CMakeLists.txt +++ b/adnav_driver/CMakeLists.txt @@ -30,6 +30,7 @@ find_package(geometry_msgs REQUIRED) find_package(geographic_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) find_package(adnav_interfaces REQUIRED) # Build @@ -54,6 +55,7 @@ set(DEPENDENCIES "diagnostic_updater" "geometry_msgs" "tf2" + "tf2_geometry_msgs" "adnav_interfaces" "geographic_msgs" ) diff --git a/adnav_driver/include/adnav_driver/adnav_driver.h b/adnav_driver/include/adnav_driver/adnav_driver.h index f6a8c38..ecdef73 100644 --- a/adnav_driver/include/adnav_driver/adnav_driver.h +++ b/adnav_driver/include/adnav_driver/adnav_driver.h @@ -145,6 +145,9 @@ class Driver : public rclcpp::Node // Inheriting gives every "this->" as a poin // String to hold frame_id std::string frame_id_ = "imu_link"; + // Whether to convert twist from ENU to FLU + bool convert_twist_enu_to_flu_ = false; + // device communication settings std::unique_ptr communicator_; adnav_connections_data_t comms_data_; diff --git a/adnav_driver/package.xml b/adnav_driver/package.xml index a7be35f..9c037b4 100644 --- a/adnav_driver/package.xml +++ b/adnav_driver/package.xml @@ -16,6 +16,7 @@ geometry_msgs geographic_msgs tf2 + tf2_geometry_msgs std_srvs adnav_interfaces diff --git a/adnav_driver/src/adnav_driver.cpp b/adnav_driver/src/adnav_driver.cpp index cd06aa7..15eefb5 100644 --- a/adnav_driver/src/adnav_driver.cpp +++ b/adnav_driver/src/adnav_driver.cpp @@ -28,6 +28,8 @@ #include "adnav_driver.h" +#include + namespace adnav { /** * @brief Constructor for the Advanced Navigation Driver node @@ -513,7 +515,14 @@ void Driver::setupParams() { commRange.step = 1; comms_select_description.integer_range.push_back(commRange); this->declare_parameter("comm_select", adnav::CONNECTION_SERIAL, comms_select_description); - comms_data_.method = (int) this->get_parameter("comm_select").as_int(); + comms_data_.method = this->get_parameter("comm_select").as_int(); + + rcl_interfaces::msg::ParameterDescriptor convert_desc; + convert_desc.description = "Convert ENU Twist to FLU Twist.\n" + " Default: false\n" + " If true, the ENU Twist will be converted to FLU Twist before publishing."; + convert_desc.read_only = true; + convert_twist_enu_to_flu_ = this->declare_parameter("convert_twist_enu_to_flu", false, convert_desc); } //~~~~~~ Control Functions @@ -1659,6 +1668,35 @@ void Driver::deviceInfoDecoder(an_packet_t* an_packet) { ); } + geometry_msgs::msg::Twist transformTwistEnuToFlu( + const geometry_msgs::msg::Twist & twist_enu, + const tf2::Quaternion & orientation) + { + // Inverse orientation to transform from ENU to FLU + tf2::Quaternion q_enu_to_flu = orientation.inverse(); + + tf2::Vector3 twist_vel, twist_angular; + tf2::fromMsg(twist_enu.linear, twist_vel); + tf2::fromMsg(twist_enu.angular, twist_angular); + + twist_vel.setY(-twist_vel.y()); + twist_vel.setZ(-twist_vel.z()); + + // Rotate the ENU twist into the FLU frame + tf2::Vector3 transformed_velocity = tf2::quatRotate(q_enu_to_flu, twist_vel); + tf2::Vector3 transformed_angular = tf2::quatRotate(q_enu_to_flu, twist_angular); + + geometry_msgs::msg::Twist twist_flu; + twist_flu.linear.x = -transformed_velocity.y(); + twist_flu.linear.y = transformed_velocity.x(); + twist_flu.linear.z = transformed_velocity.z(); + twist_flu.angular.x = -transformed_angular.y(); + twist_flu.angular.y = transformed_angular.x(); + twist_flu.angular.z = transformed_angular.z(); + + return twist_flu; + } + /** * @brief Function to decode the System State ANPP Packet (ANPP.20). * @@ -1720,6 +1758,14 @@ void Driver::systemStateRosDecoder(an_packet_t* an_packet) { if(ntrip_client_.get() != nullptr) { ntrip_client_->set_location(llh_.latitude, llh_.longitude, llh_.height); } + + // 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 + ); + // TWIST twist_msg_.linear.x = system_state_packet.velocity[0]; twist_msg_.linear.y = system_state_packet.velocity[1]; @@ -1727,17 +1773,17 @@ void Driver::systemStateRosDecoder(an_packet_t* an_packet) { 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]; + + if (convert_twist_enu_to_flu_) { + twist_msg_ = transformTwistEnuToFlu(twist_msg_, orientation_); + } + twist_stamped_msg_.twist = twist_msg_; twist_stamped_msg_.header = nav_fix_msg_.header; // IMU imu_msg_.header = nav_fix_msg_.header; - // 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]; From e7cb0c71fe276111eecef3a9c6b911e0b4cc2bd7 Mon Sep 17 00:00:00 2001 From: Russ Webber Date: Mon, 10 Nov 2025 13:21:31 +1100 Subject: [PATCH 7/7] fix: don't try to disconnect if connector not inited --- adnav_driver/src/adnav_driver.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/adnav_driver/src/adnav_driver.cpp b/adnav_driver/src/adnav_driver.cpp index 15eefb5..7f84fd6 100644 --- a/adnav_driver/src/adnav_driver.cpp +++ b/adnav_driver/src/adnav_driver.cpp @@ -95,19 +95,22 @@ Driver::Driver(): rclcpp::Node("adnav_driver"), msg_write_done_(false) * Closes the Logfiles and gives name to the user. Shutsdown the communication method */ Driver::~Driver() { - RCLCPP_INFO(this->get_logger(), "Destructing Adnav_Driver node"); + RCLCPP_INFO(this->get_logger(), "Deconstructing Adnav_Driver node"); anpp_logger_.closeFile(); // if the Ntrip client is initialised and running or the logger is open. - if (ntrip_client_.get() != nullptr && (ntrip_client_->service_running() || rtcm_logger_.is_open())) { + if (ntrip_client_ && (ntrip_client_->service_running() || rtcm_logger_.is_open())) { ntrip_client_->stop(); // Close the logfile rtcm_logger_.closeFile(); } - communicator_->close(); + if (communicator_) + { + communicator_->close(); + } } rclcpp::Time time_from_state_packet(const system_state_packet_t& state_packet) {