Skip to content

Additional topics, diagnostics, ENU/FLU conversion, fixes #17

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
russkel wants to merge 7 commits into
base: master
Choose a base branch
from
Open

Additional topics, diagnostics, ENU/FLU conversion, fixes #17

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
5ff7f68
fix: use newer ServicesQoS
russkel Oct 23, 2025
22f735e
fix: nest topics and services under nodename
russkel Oct 23, 2025
ebe2741
feat: add TwistStamped, GeoPoseStamped topics
russkel Oct 26, 2025
0b58010
fix: launch files
russkel Oct 27, 2025
e83e726
feat: use diagnostic_updater for diagnostics
russkel Oct 28, 2025
093df0a
feat: add ability to transform twist to FLU
russkel Nov 10, 2025
e7cb0c7
fix: don't try to disconnect if connector not inited
russkel Nov 10, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
19 changes: 7 additions & 12 deletions adnav_driver/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,4 @@
cmake_minimum_required(VERSION 3.5)
set(CMAKE_BUILD_TYPE release)
project(adnav_driver)

# Default to C99
Expand All @@ -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")
Expand All @@ -26,10 +25,12 @@ 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)
find_package(tf2 REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(adnav_interfaces REQUIRED)

# Build
Expand All @@ -51,10 +52,12 @@ set(DEPENDENCIES
"std_msgs"
"std_srvs"
"sensor_msgs"
"diagnostic_msgs"
"diagnostic_updater"
"geometry_msgs"
"tf2"
"tf2_geometry_msgs"
"adnav_interfaces"
"geographic_msgs"
)

set(EXECUTABLE_NAME "adnav_driver")
Expand All @@ -65,18 +68,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
Expand Down
31 changes: 21 additions & 10 deletions adnav_driver/include/adnav_driver/adnav_driver.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,19 +60,23 @@

// ROS2 Packages, Services, Messages
#include <rclcpp/rclcpp.hpp>
#include "diagnostic_updater/diagnostic_updater.hpp"
#include <tf2/LinearMath/Quaternion.h>
#include <rcl_interfaces/msg/set_parameters_result.hpp>
#include <std_msgs/msg/string.hpp>
#include <sensor_msgs/msg/nav_sat_fix.hpp>
#include <sensor_msgs/msg/time_reference.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <diagnostic_msgs/msg/diagnostic_array.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <sensor_msgs/msg/magnetic_field.hpp>
#include <sensor_msgs/msg/temperature.hpp>
#include <sensor_msgs/msg/fluid_pressure.hpp>
#include <std_srvs/srv/empty.hpp>
#include <geographic_msgs/msg/geo_pose.hpp>
#include <geographic_msgs/msg/geo_pose_stamped.hpp>

#if defined(WIN32) || defined(_WIN32)
#pragma comment(lib, "ws2_32.lib") // Winsock Library
Expand Down Expand Up @@ -141,8 +145,8 @@ 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_;
// Whether to convert twist from ENU to FLU
bool convert_twist_enu_to_flu_ = false;

// device communication settings
std::unique_ptr<adnav::Communicator> communicator_;
Expand All @@ -159,7 +163,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_t> device_information_packet_;
std::optional<system_state_packet_t> system_state_packet_;

// Msgs. Only access with protection of messages_mutex_
tf2::Quaternion orientation_;
Expand All @@ -170,9 +175,13 @@ 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_;
diagnostic_msgs::msg::DiagnosticStatus system_status_msg_;
diagnostic_msgs::msg::DiagnosticStatus filter_status_msg_;
geographic_msgs::msg::GeoPose geo_pose_msg_;
geographic_msgs::msg::GeoPoseStamped geo_pose_stamped_msg_;

std::shared_ptr<diagnostic_updater::Updater> diagnostic_updater_;

// Publishers
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
Expand All @@ -182,9 +191,11 @@ class Driver : public rclcpp::Node // Inheriting gives every "this->" as a poin
rclcpp::Publisher<sensor_msgs::msg::FluidPressure>::SharedPtr barometric_pressure_pub_;
rclcpp::Publisher<sensor_msgs::msg::Temperature>::SharedPtr temperature_pub_;
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub_;
rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_stamped_pub_;
rclcpp::Publisher<geometry_msgs::msg::Pose>::SharedPtr pose_pub_;
rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticStatus>::SharedPtr system_status_pub_;
rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticStatus>::SharedPtr filter_status_pub_;
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_stamped_pub_;
rclcpp::Publisher<geographic_msgs::msg::GeoPose>::SharedPtr geo_pose_pub_;
rclcpp::Publisher<geographic_msgs::msg::GeoPoseStamped>::SharedPtr geo_pose_stamped_pub_;

// ~~~~~~~~~~~~~~~ Callback handles and parameters
// Callback groups Allows the callbacks to be processed on a different thread by
Expand Down Expand Up @@ -251,8 +262,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<adnav_interfaces::srv::PacketPeriods::Request> request,
Expand Down
4 changes: 3 additions & 1 deletion adnav_driver/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,9 +12,11 @@
<depend>rclcpp</depend>
<depend>std_msgs</depend>
<depend>sensor_msgs</depend>
<depend>diagnostic_msgs</depend>
<depend>diagnostic_updater</depend>
<depend>geometry_msgs</depend>
<depend>geographic_msgs</depend>
<depend>tf2</depend>
<depend>tf2_geometry_msgs</depend>
<depend>std_srvs</depend>
<depend>adnav_interfaces</depend>

Expand Down
Loading