Ros 2.0 port for the publisher classes

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,52 +12,26 @@ 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 ##
 ###########
 
 ## 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,22 +40,48 @@ add_executable(
 	src/xdainterface.cpp
 	src/xdacallback.cpp
 )
+
+target_include_directories(xsens_mti_node PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/lib/xspublic>
+  $<INSTALL_INTERFACE:include>)
+
+ament_target_dependencies(xsens_mti_node 
+  rclcpp
+  tf2
+  tf2_ros
+  std_msgs
+  geometry_msgs
+  sensor_msgs
+)
+
 target_link_libraries(
 	xsens_mti_node
 	xscontroller
 	xscommon
 	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()

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>xsens_mti_driver</name>
-  <version>1.0.0</version>
+  <version>1.0.1</version>
   <description>ROS driver for Xsens MTi IMU sensors</description>
   <maintainer email="support@xsens.com">Xsens</maintainer>
   <license>BSD</license>
@@ -27,6 +27,8 @@
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>sensor_msgs</exec_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
   <export>
     <build_type>ament_cmake</build_type>
   </export>

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]

src/main.cpp

@@ -21,8 +21,8 @@
 //  ==> Icon Creative Commons 3.0: https://creativecommons.org/licenses/by/3.0/legalcode <==
 //  
 
-#include <ros/ros.h>
-#include "xdainterface.h"
+#include <rclcpp/rclcpp.hpp>
+// #include "xdainterface.h"
 
 #include <iostream>
 #include <stdexcept>
@@ -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<XdaInterface>();
+	// 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<rclcpp::Node>("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;
 }

src/messagepublishers/accelerationpublisher.h

@@ -25,27 +25,26 @@
 #define ACCELERATIONPUBLISHER_H
 
 #include "packetcallback.h"
-#include <geometry_msgs/Vector3Stamped.h>
+#include <geometry_msgs/msg/vector3_stamped.hpp>
 
 struct AccelerationPublisher : public PacketCallback
 {
-    ros::Publisher pub;
+    rclcpp::Publisher<geometry_msgs::msg::Vector3Stamped>::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<geometry_msgs::Vector3Stamped>("/imu/acceleration", pub_queue_size);
+        node->get_parameter("publisher_queue_size", pub_queue_size);
+        pub = node->create_publisher<geometry_msgs::msg::Vector3Stamped>("/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);
         }
     }
 };

src/messagepublishers/angularvelocitypublisher.h

@@ -25,27 +25,26 @@
 #define ANGULARVELOCITYPUBLISHER_H
 
 #include "packetcallback.h"
-#include <geometry_msgs/Vector3Stamped.h>
+#include <geometry_msgs/msg/vector3_stamped.hpp>
 
 struct AngularVelocityPublisher : public PacketCallback
 {
-    ros::Publisher pub;
+    rclcpp::Publisher<geometry_msgs::msg::Vector3Stamped>::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<geometry_msgs::Vector3Stamped>("/imu/angular_velocity", pub_queue_size);
+        node->get_parameter("publisher_queue_size", pub_queue_size);
+        pub = node->create_publisher<geometry_msgs::msg::Vector3Stamped>("/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);
         }
     }
 };