add option to use TwistStamped (#251)

Signed-off-by: Borong Yuan <[email protected]>

Author: borongyuan

spacenav/CMakeLists.txt

@@ -19,14 +19,6 @@ find_package(rclcpp_components REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(SPNAV REQUIRED)
 
-# Convenience variable for dependencies
-set(THIS_PACKAGE_INCLUDE_DEPENDS
-  geometry_msgs
-  rclcpp
-  rclcpp_components
-  sensor_msgs
-)
-
 add_library(spacenav
   SHARED
     src/spacenav.cpp)
@@ -36,12 +28,15 @@ target_include_directories(spacenav PUBLIC
   $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
   ${spnav_INCLUDE_DIR})
 
-ament_target_dependencies(spacenav
-  ${THIS_PACKAGE_INCLUDE_DEPENDS}
+target_link_libraries(spacenav PUBLIC
+  ${geometry_msgs_TARGETS}
+  rclcpp::rclcpp
+  ${sensor_msgs_TARGETS}
+  spnav
 )
 
-target_link_libraries(spacenav
-  spnav
+target_link_libraries(spacenav PRIVATE
+  rclcpp_components::component
 )
 
 install(TARGETS spacenav EXPORT export_spacenav

spacenav/include/spacenav/spacenav.hpp

@@ -34,6 +34,7 @@
 #include <rclcpp/rclcpp.hpp>
 
 #include <geometry_msgs/msg/twist.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
 #include <geometry_msgs/msg/vector3.hpp>
 #include <sensor_msgs/msg/joy.hpp>
 
@@ -60,6 +61,8 @@ class Spacenav final : public rclcpp::Node
 
   rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr publisher_rot_offset;
 
+  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr publisher_twist_stamped;
+
   rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_twist;
 
   rclcpp::Publisher<sensor_msgs::msg::Joy>::SharedPtr publisher_joy;
@@ -74,6 +77,7 @@ class Spacenav final : public rclcpp::Node
   bool zero_when_static;
   double static_trans_deadband;
   double static_rot_deadband;
+  bool use_twist_stamped;
 
   spnav_event sev;
   bool joy_stale = false;

spacenav/src/spacenav.cpp

@@ -56,6 +56,7 @@
 #define SPACENAV_ZERO_WHEN_STATIC_PARAM_S "zero_when_static"
 #define SPACENAV_STATIC_TRANS_DEADBAND_PARAM_S "static_trans_deadband"
 #define SPACENAV_STATIC_ROT_DEADBAND_PARAM_S "static_rot_deadband"
+#define SPACENAV_USE_TWIST_STAMPED_PARAM_S "use_twist_stamped"
 
 using namespace std::chrono_literals;
 
@@ -91,6 +92,7 @@ Spacenav::Spacenav(const rclcpp::NodeOptions & options)
   // translation, or both.
   this->declare_parameter<double>(SPACENAV_STATIC_TRANS_DEADBAND_PARAM_S, 0.1);
   this->declare_parameter<double>(SPACENAV_STATIC_ROT_DEADBAND_PARAM_S, 0.1);
+  use_twist_stamped = this->declare_parameter<bool>(SPACENAV_USE_TWIST_STAMPED_PARAM_S, false);
 
   auto param_change_callback = [this](
     std::vector<rclcpp::Parameter> parameters) {
@@ -117,8 +119,13 @@ Spacenav::Spacenav(const rclcpp::NodeOptions & options)
     "spacenav/offset", 10);
   publisher_rot_offset = this->create_publisher<geometry_msgs::msg::Vector3>(
     "spacenav/rot_offset", 10);
-  publisher_twist =
-    this->create_publisher<geometry_msgs::msg::Twist>("spacenav/twist", 10);
+  if (use_twist_stamped) {
+    publisher_twist_stamped =
+      this->create_publisher<geometry_msgs::msg::TwistStamped>("spacenav/twist_stamped", 10);
+  } else {
+    publisher_twist =
+      this->create_publisher<geometry_msgs::msg::Twist>("spacenav/twist", 10);
+  }
   publisher_joy =
     this->create_publisher<sensor_msgs::msg::Joy>("spacenav/joy", 10);
 
@@ -288,7 +295,14 @@ void Spacenav::poll_spacenav()
 
       publisher_offset->publish(std::move(msg_offset));
       publisher_rot_offset->publish(std::move(msg_rot_offset));
-      publisher_twist->publish(std::move(msg_twist));
+      if (use_twist_stamped) {
+        auto msg_twist_stamped = std::make_unique<geometry_msgs::msg::TwistStamped>();
+        msg_twist_stamped->header.stamp = msg_joystick->header.stamp;
+        msg_twist_stamped->twist = *msg_twist;
+        publisher_twist_stamped->publish(std::move(msg_twist_stamped));
+      } else {
+        publisher_twist->publish(std::move(msg_twist));
+      }
 
       no_motion_count = 0;
       motion_stale = false;