From c1abfffb8f3fd35ea15fd9f8847a085ae20abeac Mon Sep 17 00:00:00 2001 From: Jasprit Singh Gill Date: Fri, 8 Jan 2021 18:43:35 -0800 Subject: [PATCH] Ros 2.0 port for the publisher classes --- CMakeLists.txt | 90 ++++++++++--------- package.xml | 4 +- param/xsens_mti_node.yaml | 67 +++++++------- src/main.cpp | 37 ++++---- src/messagepublishers/accelerationpublisher.h | 21 +++-- .../angularvelocitypublisher.h | 21 +++-- .../freeaccelerationpublisher.h | 21 +++-- src/messagepublishers/gnsspublisher.h | 27 +++--- src/messagepublishers/imupublisher.h | 78 ++++++++-------- .../magneticfieldpublisher.h | 21 +++-- .../orientationincrementspublisher.h | 20 ++--- src/messagepublishers/orientationpublisher.h | 21 +++-- src/messagepublishers/packetcallback.h | 4 +- src/messagepublishers/pressurepublisher.h | 22 ++--- src/messagepublishers/temperaturepublisher.h | 20 ++--- .../timereferencepublisher.h | 16 ++-- src/messagepublishers/transformpublisher.h | 9 +- src/messagepublishers/twistpublisher.h | 21 +++-- .../velocityincrementpublisher.h | 21 +++-- src/xdainterface.cpp | 19 +++- src/xdainterface.h | 5 +- 21 files changed, 296 insertions(+), 269 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d304f9c..3fbac4c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 2.8.3) project(xsens_mti_driver) +cmake_policy(SET CMP0057 NEW) + # Default to C++14 if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) @@ -10,40 +12,14 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(ament_cmake REQUIRED COMPONENTS - rclcpp - tf2 - tf2_ros - std_msgs - geometry_msgs - sensor_msgs -) - - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES xsens_mti_driver - CATKIN_DEPENDS - roscpp - tf2 - tf2_ros - sensor_msgs - std_msgs - geometry_msgs -# DEPENDS system_lib -) +# Find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) ########### ## Build ## @@ -51,11 +27,11 @@ catkin_package( ## Specify additional locations of header files ## Your package locations should be listed before other locations -include_directories( -# include - lib/xspublic - ${catkin_INCLUDE_DIRS} -) +#include_directories( +## include +# lib/xspublic +# ${catkin_INCLUDE_DIRS} +#) link_directories(lib/xspublic/xscontroller lib/xspublic/xscommon lib/xspublic/xstypes) add_executable( @@ -64,6 +40,20 @@ add_executable( src/xdainterface.cpp src/xdacallback.cpp ) + +target_include_directories(xsens_mti_node PUBLIC + $ + $) + +ament_target_dependencies(xsens_mti_node + rclcpp + tf2 + tf2_ros + std_msgs + geometry_msgs + sensor_msgs +) + target_link_libraries( xsens_mti_node xscontroller @@ -71,15 +61,27 @@ target_link_libraries( xstypes pthread dl - ${catkin_LIBRARIES} +# ${catkin_LIBRARIES} ) install(TARGETS xsens_mti_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + EXPORT export_${PROJECT_NAME} + DESTINATION lib/${PROJECT_NAME} ) install(DIRECTORY launch param rviz urdf - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME} ) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/package.xml b/package.xml index 85f23f5..33f46ae 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ xsens_mti_driver - 1.0.0 + 1.0.1 ROS driver for Xsens MTi IMU sensors Xsens BSD @@ -27,6 +27,8 @@ std_msgs geometry_msgs sensor_msgs + ament_lint_auto + ament_lint_common ament_cmake diff --git a/param/xsens_mti_node.yaml b/param/xsens_mti_node.yaml index c1bf3e5..041d07e 100644 --- a/param/xsens_mti_node.yaml +++ b/param/xsens_mti_node.yaml @@ -1,36 +1,39 @@ -## Device settings, provide one of the following: -## - device_id -## - port + baudrate information -## - nothing: the driver scans for devices and selects the first found. -# device_id: '077007EF' # uppercase hex string -# port: '/dev/ttyUSB0' -# baudrate: 921600 # non necessary for some devices +/**: + ros__parameters: -## Log file (optional), placed in ~/.ros/ otherwise use absolute path -# log_file: log.mtb + ## Device settings, provide one of the following: + ## - device_id + ## - port + baudrate information + ## - nothing: the driver scans for devices and selects the first found. + # device_id: '077007EF' # uppercase hex string + # port: '/dev/ttyUSB0' + # baudrate: 921600 # non necessary for some devices -publisher_queue_size: 5 + ## Log file (optional), placed in ~/.ros/ otherwise use absolute path + # log_file: log.mtb -# Message publishers -pub_imu: true -pub_quaternion: true -pub_mag: true -pub_angular_velocity: true -pub_acceleration: true -pub_free_acceleration: true -pub_dq: true -pub_dv: true -pub_sampletime: true -pub_temperature: true -pub_pressure: true -pub_gnss: true -pub_twist: true -pub_transform: true + publisher_queue_size: 5 -## Sensor standard deviation [x,y,z] (optional) -## This value is used to override the covariance matrix in sensor_msgs/Imu and -## sensor_msgs/MagneticField messages. -# linear_acceleration_stddev: [0, 0, 0] # [m/s^2] -# angular_velocity_stddev: [0, 0, 0] # [rad/s] -# orientation_stddev: [0, 0, 0] # [rad] -# magnetic_field_stddev: [0, 0, 0] # [Tesla] + # Message publishers + pub_imu: true + pub_quaternion: true + pub_mag: true + pub_angular_velocity: true + pub_acceleration: true + pub_free_acceleration: true + pub_dq: true + pub_dv: true + pub_sampletime: true + pub_temperature: true + pub_pressure: true + pub_gnss: true + pub_twist: true + pub_transform: true + + ## Sensor standard deviation [x,y,z] (optional) + ## This value is used to override the covariance matrix in sensor_msgs/Imu and + ## sensor_msgs/MagneticField messages. + # linear_acceleration_stddev: [0, 0, 0] # [m/s^2] + # angular_velocity_stddev: [0, 0, 0] # [rad/s] + # orientation_stddev: [0, 0, 0] # [rad] + # magnetic_field_stddev: [0, 0, 0] # [Tesla] diff --git a/src/main.cpp b/src/main.cpp index eadfc1b..23876fc 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -21,8 +21,8 @@ // ==> Icon Creative Commons 3.0: https://creativecommons.org/licenses/by/3.0/legalcode <== // -#include -#include "xdainterface.h" +#include +// #include "xdainterface.h" #include #include @@ -35,27 +35,32 @@ Journaller *gJournal = 0; int main(int argc, char *argv[]) { - ros::init(argc, argv, "xsens_driver"); - ros::NodeHandle node; + // ros::init(argc, argv, "xsens_driver"); + rclcpp::init(argc, argv); + rclcpp::executors::SingleThreadedExecutor exec; - XdaInterface *xdaInterface = new XdaInterface(); + // auto xdaInterface = std::make_shared(); + // exec.add_node(xdaInterface); - xdaInterface->registerPublishers(node); + // xdaInterface->registerPublishers(node); - if (!xdaInterface->connectDevice()) - return -1; + // if (!xdaInterface->connectDevice()) + // return -1; - if (!xdaInterface->prepare()) - return -1; - - while (ros::ok()) + // if (!xdaInterface->prepare()) + // return -1; + auto nh = std::make_shared("xsens_test"); + exec.add_node(nh); + while (rclcpp::ok()) { - xdaInterface->spinFor(milliseconds(100)); - - ros::spinOnce(); + // xdaInterface->spinFor(milliseconds(100)); + std::cout << "test\n"; + // ros::spinOnce(); + exec.spin_once(); } - xdaInterface->close(); + // xdaInterface->close(); + rclcpp::shutdown(); return 0; } diff --git a/src/messagepublishers/accelerationpublisher.h b/src/messagepublishers/accelerationpublisher.h index 6f723f5..0c1231b 100644 --- a/src/messagepublishers/accelerationpublisher.h +++ b/src/messagepublishers/accelerationpublisher.h @@ -25,27 +25,26 @@ #define ACCELERATIONPUBLISHER_H #include "packetcallback.h" -#include +#include struct AccelerationPublisher : public PacketCallback { - ros::Publisher pub; + rclcpp::Publisher::SharedPtr pub; + std::string frame_id = DEFAULT_FRAME_ID; - AccelerationPublisher(ros::NodeHandle &node) + AccelerationPublisher(rclcpp::Node &node) { int pub_queue_size = 5; - ros::param::get("~publisher_queue_size", pub_queue_size); - pub = node.advertise("/imu/acceleration", pub_queue_size); + node->get_parameter("publisher_queue_size", pub_queue_size); + pub = node->create_publisher("/imu/acceleration", pub_queue_size); + node->get_parameter("frame_id", frame_id); } - void operator()(const XsDataPacket &packet, ros::Time timestamp) + void operator()(const XsDataPacket &packet, rclcpp::Time timestamp) { if (packet.containsCalibratedAcceleration()) { - geometry_msgs::Vector3Stamped msg; - - std::string frame_id = DEFAULT_FRAME_ID; - ros::param::getCached("~frame_id", frame_id); + geometry_msgs::msg::Vector3Stamped msg; msg.header.stamp = timestamp; msg.header.frame_id = frame_id; @@ -56,7 +55,7 @@ struct AccelerationPublisher : public PacketCallback msg.vector.y = accel[1]; msg.vector.z = accel[2]; - pub.publish(msg); + pub->publish(msg); } } }; diff --git a/src/messagepublishers/angularvelocitypublisher.h b/src/messagepublishers/angularvelocitypublisher.h index 8d920fa..3e36d92 100644 --- a/src/messagepublishers/angularvelocitypublisher.h +++ b/src/messagepublishers/angularvelocitypublisher.h @@ -25,27 +25,26 @@ #define ANGULARVELOCITYPUBLISHER_H #include "packetcallback.h" -#include +#include struct AngularVelocityPublisher : public PacketCallback { - ros::Publisher pub; + rclcpp::Publisher::SharedPtr pub; + std::string frame_id = DEFAULT_FRAME_ID; - AngularVelocityPublisher(ros::NodeHandle &node) + AngularVelocityPublisher(rclcpp::Node &node) { int pub_queue_size = 5; - ros::param::get("~publisher_queue_size", pub_queue_size); - pub = node.advertise("/imu/angular_velocity", pub_queue_size); + node->get_parameter("publisher_queue_size", pub_queue_size); + pub = node->create_publisher("/imu/angular_velocity", pub_queue_size); + node->get_parameter("frame_id", frame_id); } - void operator()(const XsDataPacket &packet, ros::Time timestamp) + void operator()(const XsDataPacket &packet, rclcpp::Time timestamp) { if (packet.containsCalibratedGyroscopeData()) { - geometry_msgs::Vector3Stamped msg; - - std::string frame_id = DEFAULT_FRAME_ID; - ros::param::getCached("~frame_id", frame_id); + geometry_msgs::msg::Vector3Stamped msg; msg.header.stamp = timestamp; msg.header.frame_id = frame_id; @@ -56,7 +55,7 @@ struct AngularVelocityPublisher : public PacketCallback msg.vector.y = gyro[1]; msg.vector.z = gyro[2]; - pub.publish(msg); + pub->publish(msg); } } }; diff --git a/src/messagepublishers/freeaccelerationpublisher.h b/src/messagepublishers/freeaccelerationpublisher.h index 3e54917..43796c0 100644 --- a/src/messagepublishers/freeaccelerationpublisher.h +++ b/src/messagepublishers/freeaccelerationpublisher.h @@ -25,27 +25,26 @@ #define FREEACCELERATIONPUBLISHER_H #include "packetcallback.h" -#include +#include struct FreeAccelerationPublisher : public PacketCallback { - ros::Publisher pub; + rclcpp::Publisher::SharedPtr pub; + std::string frame_id = DEFAULT_FRAME_ID; - FreeAccelerationPublisher(ros::NodeHandle &node) + FreeAccelerationPublisher(rclcpp::Node &node) { int pub_queue_size = 5; - ros::param::get("~publisher_queue_size", pub_queue_size); - pub = node.advertise("/filter/free_acceleration", pub_queue_size); + node->get_parameter("publisher_queue_size", pub_queue_size); + pub = node->create_publisher("/filter/free_acceleration", pub_queue_size); + node->get_parameter("frame_id", frame_id); } - void operator()(const XsDataPacket &packet, ros::Time timestamp) + void operator()(const XsDataPacket &packet, rclcpp::Time timestamp) { if (packet.containsFreeAcceleration()) { - geometry_msgs::Vector3Stamped msg; - - std::string frame_id = DEFAULT_FRAME_ID; - ros::param::getCached("~frame_id", frame_id); + geometry_msgs::msg::Vector3Stamped msg; msg.header.stamp = timestamp; msg.header.frame_id = frame_id; @@ -56,7 +55,7 @@ struct FreeAccelerationPublisher : public PacketCallback msg.vector.y = accel[1]; msg.vector.z = accel[2]; - pub.publish(msg); + pub->publish(msg); } } }; diff --git a/src/messagepublishers/gnsspublisher.h b/src/messagepublishers/gnsspublisher.h index e57f78c..8bc8ef0 100644 --- a/src/messagepublishers/gnsspublisher.h +++ b/src/messagepublishers/gnsspublisher.h @@ -25,7 +25,7 @@ #define GNSSPUBLISHER_H #include "packetcallback.h" -#include +#include #define FIX_TYPE_2D_FIX (2) #define FIX_TYPE_3D_FIX (3) @@ -33,23 +33,22 @@ struct GnssPublisher : public PacketCallback { - ros::Publisher pub; + rclcpp::Publisher::SharedPtr pub; + std::string frame_id = DEFAULT_FRAME_ID; - GnssPublisher(ros::NodeHandle &node) + GnssPublisher(rclcpp::Node &node) { int pub_queue_size = 5; - ros::param::get("~publisher_queue_size", pub_queue_size); - pub = node.advertise("/gnss", pub_queue_size); + node->get_parameter("publisher_queue_size", pub_queue_size); + pub = node->create_publisher("/gnss", pub_queue_size); + node->get_parameter("frame_id", frame_id); } - void operator()(const XsDataPacket &packet, ros::Time timestamp) + void operator()(const XsDataPacket &packet, rclcpp::Time timestamp) { if (packet.containsRawGnssPvtData()) { - sensor_msgs::NavSatFix msg; - - std::string frame_id = DEFAULT_FRAME_ID; - ros::param::getCached("~frame_id", frame_id); + sensor_msgs::msg::NavSatFix msg; msg.header.stamp = timestamp; msg.header.frame_id = frame_id; @@ -63,21 +62,21 @@ struct GnssPublisher : public PacketCallback double sh = ((double)gnss.m_hAcc * 1e-3); double sv = ((double)gnss.m_vAcc * 1e-3); msg.position_covariance = {sh * sh, 0, 0, 0, sh * sh, 0, 0, 0, sv * sv}; - msg.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; + msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; switch (gnss.m_fixType) { case FIX_TYPE_2D_FIX: // fall through case FIX_TYPE_3D_FIX: // fall through case FIX_TYPE_GNSS_AND_DEAD_RECKONING: - msg.status.status = sensor_msgs::NavSatStatus::STATUS_FIX; + msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX; break; default: - msg.status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX; + msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX; } msg.status.service = 0; // unknown - pub.publish(msg); + pub->publish(msg); } } }; diff --git a/src/messagepublishers/imupublisher.h b/src/messagepublishers/imupublisher.h index d155ffc..299c8b2 100644 --- a/src/messagepublishers/imupublisher.h +++ b/src/messagepublishers/imupublisher.h @@ -25,56 +25,41 @@ #define IMUPUBLISHER_H #include "packetcallback.h" -#include +#include -static void variance_from_stddev_param(std::string param, double *variance_out) -{ - std::vector stddev; - if (ros::param::get(param, stddev)) - { - if (stddev.size() == 3) - { - auto squared = [](double x) { return x * x; }; - std::transform(stddev.begin(), stddev.end(), variance_out, squared); - } - else - { - ROS_WARN("Wrong size of param: %s, must be of size 3", param.c_str()); - } - } - else - { - memset(variance_out, 0, 3 * sizeof(double)); - } -} struct ImuPublisher : public PacketCallback { - ros::Publisher pub; - + rclcpp::Publisher::SharedPtr pub; double orientation_variance[3]; double linear_acceleration_variance[3]; double angular_velocity_variance[3]; + rclcpp::Node& node_handle; - ImuPublisher(ros::NodeHandle &node) + ImuPublisher(rclcpp::Node &node) + : node_handle(node) { + node->declare_parameter("orientation_stddev", orientation_variance); + node->declare_parameter("angular_velocity_stddev", angular_velocity_variance); + node->declare_parameter("linear_acceleration_stddev", linear_acceleration_variance); + int pub_queue_size = 5; - ros::param::get("~publisher_queue_size", pub_queue_size); - pub = node.advertise("/imu/data", pub_queue_size); + node->get_parameter("publisher_queue_size", pub_queue_size); + pub = node->create_publisher("/imu/data", pub_queue_size); // REP 145: Conventions for IMU Sensor Drivers (http://www.ros.org/reps/rep-0145.html) - variance_from_stddev_param("~orientation_stddev", orientation_variance); - variance_from_stddev_param("~angular_velocity_stddev", angular_velocity_variance); - variance_from_stddev_param("~linear_acceleration_stddev", linear_acceleration_variance); + variance_from_stddev_param("orientation_stddev", orientation_variance); + variance_from_stddev_param("angular_velocity_stddev", angular_velocity_variance); + variance_from_stddev_param("linear_acceleration_stddev", linear_acceleration_variance); } - void operator()(const XsDataPacket &packet, ros::Time timestamp) + void operator()(const XsDataPacket &packet, rclcpp::Time timestamp) { bool quaternion_available = packet.containsOrientation(); bool gyro_available = packet.containsCalibratedGyroscopeData(); bool accel_available = packet.containsCalibratedAcceleration(); - geometry_msgs::Quaternion quaternion; + geometry_msgs::msg::Quaternion quaternion; if (quaternion_available) { XsQuaternion q = packet.orientationQuaternion(); @@ -85,7 +70,7 @@ struct ImuPublisher : public PacketCallback quaternion.z = q.z(); } - geometry_msgs::Vector3 gyro; + geometry_msgs::msg::Vector3 gyro; if (gyro_available) { XsVector g = packet.calibratedGyroscopeData(); @@ -94,7 +79,7 @@ struct ImuPublisher : public PacketCallback gyro.z = g[2]; } - geometry_msgs::Vector3 accel; + geometry_msgs::msg::Vector3 accel; if (accel_available) { XsVector a = packet.calibratedAcceleration(); @@ -106,10 +91,10 @@ struct ImuPublisher : public PacketCallback // Imu message, publish if any of the fields is available if (quaternion_available || accel_available || gyro_available) { - sensor_msgs::Imu msg; + sensor_msgs::msg::Imu msg; std::string frame_id = DEFAULT_FRAME_ID; - ros::param::get("~frame_id", frame_id); + node_handle->get_parameter("frame_id", frame_id); msg.header.stamp = timestamp; msg.header.frame_id = frame_id; @@ -150,7 +135,28 @@ struct ImuPublisher : public PacketCallback msg.linear_acceleration_covariance[0] = -1; // mark as not available } - pub.publish(msg); + pub->publish(msg); + } + } + + void variance_from_stddev_param(std::string param, double *variance_out) + { + std::vector stddev; + if (node_handle->get_parameter(param, stddev)) + { + if (stddev.size() == 3) + { + auto squared = [](double x) { return x * x; }; + std::transform(stddev.begin(), stddev.end(), variance_out, squared); + } + else + { + node_handle->RCLCPP_WARN("Wrong size of param: %s, must be of size 3", param.c_str()); + } + } + else + { + memset(variance_out, 0, 3 * sizeof(double)); } } }; diff --git a/src/messagepublishers/magneticfieldpublisher.h b/src/messagepublishers/magneticfieldpublisher.h index f6974b1..7acc07f 100644 --- a/src/messagepublishers/magneticfieldpublisher.h +++ b/src/messagepublishers/magneticfieldpublisher.h @@ -25,31 +25,30 @@ #define MAGNETICFIELDPUBLISHER_H #include "packetcallback.h" -#include +#include struct MagneticFieldPublisher : public PacketCallback { - ros::Publisher pub; + rclcpp::Publisher::SharedPtr pub; + std::string frame_id = DEFAULT_FRAME_ID; // double magnetic_field_variance[3]; - MagneticFieldPublisher(ros::NodeHandle &node) + MagneticFieldPublisher(rclcpp::Node &node) { int pub_queue_size = 5; - ros::param::get("~publisher_queue_size", pub_queue_size); - pub = node.advertise("/imu/mag", pub_queue_size); + node->get_parameter("publisher_queue_size", pub_queue_size); + pub = node->create_publisher("/imu/mag", pub_queue_size); + node->get_parameter("frame_id", frame_id); // variance_from_stddev_param("~magnetic_field_stddev", magnetic_field_variance); } - void operator()(const XsDataPacket &packet, ros::Time timestamp) + void operator()(const XsDataPacket &packet, rclcpp::Time timestamp) { if (packet.containsCalibratedMagneticField()) { // TODO: Use sensor_msgs::MagneticField // Problem: Sensor gives normalized magnetic field vector with unknown units - geometry_msgs::Vector3Stamped msg; - - std::string frame_id = DEFAULT_FRAME_ID; - ros::param::getCached("~frame_id", frame_id); + geometry_msgs::msg::Vector3Stamped msg; msg.header.stamp = timestamp; msg.header.frame_id = frame_id; @@ -64,7 +63,7 @@ struct MagneticFieldPublisher : public PacketCallback // msg.magnetic_field_covariance[4] = magnetic_field_variance[1]; // msg.magnetic_field_covariance[8] = magnetic_field_variance[2]; - pub.publish(msg); + pub->publish(msg); } } }; diff --git a/src/messagepublishers/orientationincrementspublisher.h b/src/messagepublishers/orientationincrementspublisher.h index 47ca3d8..f77a51a 100644 --- a/src/messagepublishers/orientationincrementspublisher.h +++ b/src/messagepublishers/orientationincrementspublisher.h @@ -25,27 +25,27 @@ #define ORIENTATIONINCREMENTSPUBLISHER_H #include "packetcallback.h" -#include +#include struct OrientationIncrementsPublisher : public PacketCallback { - ros::Publisher pub; + rclcpp::Publisher::SharedPtr pub; + std::string frame_id = DEFAULT_FRAME_ID; - OrientationIncrementsPublisher(ros::NodeHandle &node) + OrientationIncrementsPublisher(rclcpp::Node &node) { int pub_queue_size = 5; ros::param::get("~publisher_queue_size", pub_queue_size); - pub = node.advertise("/imu/dq", pub_queue_size); + node->get_parameter("publisher_queue_size", pub_queue_size); + pub = node->create_publisher("/imu/dq", pub_queue_size); + node->get_parameter("frame_id", frame_id); } - void operator()(const XsDataPacket &packet, ros::Time timestamp) + void operator()(const XsDataPacket &packet, rclcpp::Time timestamp) { if (packet.containsOrientationIncrement()) { - geometry_msgs::QuaternionStamped msg; - - std::string frame_id = DEFAULT_FRAME_ID; - ros::param::getCached("~frame_id", frame_id); + geometry_msgs::msg::QuaternionStamped msg; msg.header.stamp = timestamp; msg.header.frame_id = frame_id; @@ -57,7 +57,7 @@ struct OrientationIncrementsPublisher : public PacketCallback msg.quaternion.y = dq.y(); msg.quaternion.z = dq.z(); - pub.publish(msg); + pub->publish(msg); } } }; diff --git a/src/messagepublishers/orientationpublisher.h b/src/messagepublishers/orientationpublisher.h index 85fd96b..63774c1 100644 --- a/src/messagepublishers/orientationpublisher.h +++ b/src/messagepublishers/orientationpublisher.h @@ -25,27 +25,30 @@ #define ORIENTATIONPUBLISHER_H #include "packetcallback.h" -#include +#include struct OrientationPublisher : public PacketCallback { - ros::Publisher pub; + rclcpp::Publisher::SharedPtr pub; + std::string frame_id = DEFAULT_FRAME_ID; - OrientationPublisher(ros::NodeHandle &node) + OrientationPublisher(rclcpp::Node &node) + : node_handle(node) { int pub_queue_size = 5; - ros::param::get("~publisher_queue_size", pub_queue_size); - pub = node.advertise("/filter/quaternion", pub_queue_size); + node->get_parameter("publisher_queue_size", pub_queue_size); + pub = node->create_publisher("/filter/quaternion", pub_queue_size); + node->get_parameter("frame_id", frame_id); } - void operator()(const XsDataPacket &packet, ros::Time timestamp) + void operator()(const XsDataPacket &packet, rclcpp::Time timestamp) { if (packet.containsOrientation()) { - geometry_msgs::QuaternionStamped msg; + geometry_msgs::msg::QuaternionStamped msg; std::string frame_id = DEFAULT_FRAME_ID; - ros::param::getCached("~frame_id", frame_id); + node_handle->get_parameter("frame_id", frame_id); msg.header.stamp = timestamp; msg.header.frame_id = frame_id; @@ -57,7 +60,7 @@ struct OrientationPublisher : public PacketCallback msg.quaternion.y = q.y(); msg.quaternion.z = q.z(); - pub.publish(msg); + pub->publish(msg); } } }; diff --git a/src/messagepublishers/packetcallback.h b/src/messagepublishers/packetcallback.h index 6fe32a7..dc695cf 100644 --- a/src/messagepublishers/packetcallback.h +++ b/src/messagepublishers/packetcallback.h @@ -24,7 +24,7 @@ #ifndef PACKETCALLBACK_H #define PACKETCALLBACK_H -#include +#include #include const char* DEFAULT_FRAME_ID = "imu_link"; @@ -32,7 +32,7 @@ const char* DEFAULT_FRAME_ID = "imu_link"; class PacketCallback { public: - virtual void operator()(const XsDataPacket &, ros::Time) = 0; + virtual void operator()(const XsDataPacket &, rclcpp::Time) = 0; }; #endif diff --git a/src/messagepublishers/pressurepublisher.h b/src/messagepublishers/pressurepublisher.h index 4e88e8e..0219fd1 100644 --- a/src/messagepublishers/pressurepublisher.h +++ b/src/messagepublishers/pressurepublisher.h @@ -25,27 +25,27 @@ #define PRESSUREPUBLISHER_H #include "packetcallback.h" -#include +#include struct PressurePublisher : public PacketCallback { - ros::Publisher pub; + rclcpp::Publisher::SharedPtr pub; + std::string frame_id = DEFAULT_FRAME_ID; - PressurePublisher(ros::NodeHandle &node) + PressurePublisher(rclcpp::Node &node) { int pub_queue_size = 5; - ros::param::get("~publisher_queue_size", pub_queue_size); - pub = node.advertise("/pressure", pub_queue_size); + node->get_parameter("publisher_queue_size", pub_queue_size); + pub = node->create_publisher("/pressure", pub_queue_size); + node->get_parameter("frame_id", frame_id); + pub = node.advertise("/pressure", pub_queue_size); } - void operator()(const XsDataPacket &packet, ros::Time timestamp) + void operator()(const XsDataPacket &packet, rclcpp::Time timestamp) { if (packet.containsPressure()) { - sensor_msgs::FluidPressure msg; - - std::string frame_id = DEFAULT_FRAME_ID; - ros::param::getCached("~frame_id", frame_id); + sensor_msgs::msg::FluidPressure msg; msg.header.stamp = timestamp; msg.header.frame_id = frame_id; @@ -55,7 +55,7 @@ struct PressurePublisher : public PacketCallback msg.variance = 0; // unknown // unused sample.m_pressureAge, age in samples - pub.publish(msg); + pub->publish(msg); } } }; diff --git a/src/messagepublishers/temperaturepublisher.h b/src/messagepublishers/temperaturepublisher.h index 6393d63..f709a84 100644 --- a/src/messagepublishers/temperaturepublisher.h +++ b/src/messagepublishers/temperaturepublisher.h @@ -25,27 +25,27 @@ #define TEMPERATUREPUBLISHER_H #include "packetcallback.h" -#include +#include struct TemperaturePublisher : public PacketCallback { - ros::Publisher pub; + rclcpp::Publisher::SharedPtr pub; + std::string frame_id = DEFAULT_FRAME_ID; - TemperaturePublisher(ros::NodeHandle &node) + TemperaturePublisher(rclcpp::Node &node) { int pub_queue_size = 5; ros::param::get("~publisher_queue_size", pub_queue_size); - pub = node.advertise("/temperature", pub_queue_size); + node->get_parameter("publisher_queue_size", pub_queue_size); + pub = node->create_publisher("/temperature", pub_queue_size); + node->get_parameter("frame_id", frame_id); } - void operator()(const XsDataPacket &packet, ros::Time timestamp) + void operator()(const XsDataPacket &packet, rclcpp::Time timestamp) { if (packet.containsTemperature()) { - sensor_msgs::Temperature msg; - - std::string frame_id = DEFAULT_FRAME_ID; - ros::param::getCached("~frame_id", frame_id); + sensor_msgs::msg::Temperature msg; msg.header.stamp = timestamp; msg.header.frame_id = frame_id; @@ -53,7 +53,7 @@ struct TemperaturePublisher : public PacketCallback msg.temperature = packet.temperature(); msg.variance = 0; // unknown - pub.publish(msg); + pub->publish(msg); } } }; diff --git a/src/messagepublishers/timereferencepublisher.h b/src/messagepublishers/timereferencepublisher.h index 397acb2..eac22ed 100644 --- a/src/messagepublishers/timereferencepublisher.h +++ b/src/messagepublishers/timereferencepublisher.h @@ -25,27 +25,27 @@ #define TIMEREFERENCEPUBLISHER_H #include "packetcallback.h" -#include +#include struct TimeReferencePublisher : public PacketCallback { - ros::Publisher pub; + rclcpp::Publisher::SharedPtr pub; - TimeReferencePublisher(ros::NodeHandle &node) + TimeReferencePublisher(rclcpp::Node &node) { int pub_queue_size = 5; - ros::param::get("~publisher_queue_size", pub_queue_size); - pub = node.advertise("/imu/time_ref", pub_queue_size); + node->get_parameter("publisher_queue_size", pub_queue_size); + pub = node->create_publisher("/imu/time_ref", pub_queue_size); } - void operator()(const XsDataPacket &packet, ros::Time timestamp) + void operator()(const XsDataPacket &packet, rclcpp::Time timestamp) { if (packet.containsSampleTimeFine()) { const uint32_t SAMPLE_TIME_FINE_HZ = 10000UL; const uint32_t ONE_GHZ = 1000000000UL; uint32_t sec, nsec, t_fine; - sensor_msgs::TimeReference msg; + sensor_msgs::msg::TimeReference msg; t_fine = packet.sampleTimeFine(); sec = t_fine / SAMPLE_TIME_FINE_HZ; @@ -63,7 +63,7 @@ struct TimeReferencePublisher : public PacketCallback msg.time_ref = sample_time; // msg.source = optional - pub.publish(msg); + pub->publish(msg); } } }; diff --git a/src/messagepublishers/transformpublisher.h b/src/messagepublishers/transformpublisher.h index 9ede2e9..5ed475d 100644 --- a/src/messagepublishers/transformpublisher.h +++ b/src/messagepublishers/transformpublisher.h @@ -25,25 +25,24 @@ #define TRANSFORMPUBLISHER_H #include "packetcallback.h" -#include +#include #include struct TransformPublisher : public PacketCallback { tf2_ros::TransformBroadcaster tf_broadcaster; + std::string frame_id = DEFAULT_FRAME_ID; TransformPublisher(ros::NodeHandle &node) : tf_broadcaster() { + node->get_parameter("frame_id", frame_id); } void operator()(const XsDataPacket &packet, ros::Time timestamp) { if (packet.containsOrientation()) { - geometry_msgs::TransformStamped tf; - - std::string frame_id = DEFAULT_FRAME_ID; - ros::param::getCached("~frame_id", frame_id); + geometry_msgs::msg::TransformStamped tf; XsQuaternion q = packet.orientationQuaternion(); diff --git a/src/messagepublishers/twistpublisher.h b/src/messagepublishers/twistpublisher.h index 0637b43..9c1c6c7 100644 --- a/src/messagepublishers/twistpublisher.h +++ b/src/messagepublishers/twistpublisher.h @@ -25,27 +25,26 @@ #define TWISTPUBLISHER_H #include "packetcallback.h" -#include +#include struct TwistPublisher : public PacketCallback { - ros::Publisher pub; + rclcpp::Publisher::SharedPtr pub; + std::string frame_id = DEFAULT_FRAME_ID; - TwistPublisher(ros::NodeHandle &node) + TwistPublisher(rclcpp::Node &node) { int pub_queue_size = 5; - ros::param::get("~publisher_queue_size", pub_queue_size); - pub = node.advertise("/filter/twist", pub_queue_size); + node->get_parameter("publisher_queue_size", pub_queue_size); + pub = node->create_publisher("/filter/twist", pub_queue_size); + node->get_parameter("frame_id", frame_id); } - void operator()(const XsDataPacket &packet, ros::Time timestamp) + void operator()(const XsDataPacket &packet, rclcpp::Time timestamp) { if (packet.containsVelocity() && packet.containsCalibratedGyroscopeData()) { - geometry_msgs::TwistStamped msg; - - std::string frame_id = DEFAULT_FRAME_ID; - ros::param::getCached("~frame_id", frame_id); + geometry_msgs::msg::TwistStamped msg; msg.header.stamp = timestamp; msg.header.frame_id = frame_id; @@ -61,7 +60,7 @@ struct TwistPublisher : public PacketCallback msg.twist.angular.y = gyro[1]; msg.twist.angular.z = gyro[2]; - pub.publish(msg); + pub->publish(msg); } } }; diff --git a/src/messagepublishers/velocityincrementpublisher.h b/src/messagepublishers/velocityincrementpublisher.h index 1ea1022..1c85c5c 100644 --- a/src/messagepublishers/velocityincrementpublisher.h +++ b/src/messagepublishers/velocityincrementpublisher.h @@ -25,27 +25,26 @@ #define VELOCITYINCREMENTPUBLISHER_H #include "packetcallback.h" -#include +#include struct VelocityIncrementPublisher : public PacketCallback { - ros::Publisher pub; + rclcpp::Publisher::SharedPtr pub; + std::string frame_id = DEFAULT_FRAME_ID; - VelocityIncrementPublisher(ros::NodeHandle &node) + VelocityIncrementPublisher(rclcpp::Node &node) { int pub_queue_size = 5; - ros::param::get("~publisher_queue_size", pub_queue_size); - pub = node.advertise("/imu/dv", pub_queue_size); + node->get_parameter("publisher_queue_size", pub_queue_size); + pub = node->create_publisher("/imu/dv", pub_queue_size); + node->get_parameter("frame_id", frame_id); } - void operator()(const XsDataPacket &packet, ros::Time timestamp) + void operator()(const XsDataPacket &packet, rclcpp::Time timestamp) { if (packet.containsVelocityIncrement()) { - geometry_msgs::Vector3Stamped msg; - - std::string frame_id = DEFAULT_FRAME_ID; - ros::param::getCached("~frame_id", frame_id); + geometry_msgs::msg::Vector3Stamped msg; msg.header.stamp = timestamp; msg.header.frame_id = frame_id; @@ -56,7 +55,7 @@ struct VelocityIncrementPublisher : public PacketCallback msg.vector.y = dv[1]; msg.vector.z = dv[2]; - pub.publish(msg); + pub->publish(msg); } } }; diff --git a/src/xdainterface.cpp b/src/xdainterface.cpp index 16ffd4f..0e6d987 100644 --- a/src/xdainterface.cpp +++ b/src/xdainterface.cpp @@ -46,9 +46,11 @@ #define XS_DEFAULT_BAUDRATE (115200) XdaInterface::XdaInterface() - : m_device(nullptr) + : Node("xsens_driver", rclcpp::NodeOptions()) + , m_device(nullptr) { - ROS_INFO("Creating XsControl object..."); + declareCommonParameters(); + RCLCPP_INFO(get_logger(), "Creating XsControl object..."); m_control = XsControl::construct(); assert(m_control != 0); } @@ -72,9 +74,10 @@ void XdaInterface::spinFor(std::chrono::milliseconds timeout) } } -void XdaInterface::registerPublishers(ros::NodeHandle &node) +void XdaInterface::registerPublishers() { bool should_publish; + rclcpp::Node& node = *this; if (ros::param::get("~pub_imu", should_publish) && should_publish) { @@ -239,3 +242,13 @@ bool XdaInterface::handleError(std::string error) ROS_ERROR("%s", error.c_str()); return false; } + +void XdaInterface::declareCommonParameters() +{ + // Declare ROS parameters common to all the publishers + std::string frame_id = DEFAULT_FRAME_ID; + node->declare_parameter("frame_id", frame_id); + + int pub_queue_size = 5; + node->declare_parameter("publisher_queue_size", pub_queue_size); +} \ No newline at end of file diff --git a/src/xdainterface.h b/src/xdainterface.h index 8bd0510..bab9875 100644 --- a/src/xdainterface.h +++ b/src/xdainterface.h @@ -24,7 +24,7 @@ #ifndef XDAINTERFACE_H #define XDAINTERFACE_H -#include +#include #include "xdacallback.h" #include @@ -37,7 +37,7 @@ struct XsDevice; class PacketCallback; -class XdaInterface +class XdaInterface : public rclcpp::Node { public: XdaInterface(); @@ -53,6 +53,7 @@ class XdaInterface private: void registerCallback(PacketCallback *cb); bool handleError(std::string error); + void declareCommonParameters(); XsControl *m_control; XsDevice *m_device;