diff --git a/spacenav/include/spacenav/spacenav.hpp b/spacenav/include/spacenav/spacenav.hpp index b95e36db..6616b3d5 100644 --- a/spacenav/include/spacenav/spacenav.hpp +++ b/spacenav/include/spacenav/spacenav.hpp @@ -34,6 +34,7 @@ #include #include +#include #include #include @@ -60,6 +61,8 @@ class Spacenav final : public rclcpp::Node rclcpp::Publisher::SharedPtr publisher_rot_offset; + rclcpp::Publisher::SharedPtr publisher_twist_stamped; + rclcpp::Publisher::SharedPtr publisher_twist; rclcpp::Publisher::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; diff --git a/spacenav/src/spacenav.cpp b/spacenav/src/spacenav.cpp index b0991872..e2a32577 100644 --- a/spacenav/src/spacenav.cpp +++ b/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,8 @@ Spacenav::Spacenav(const rclcpp::NodeOptions & options) // translation, or both. this->declare_parameter(SPACENAV_STATIC_TRANS_DEADBAND_PARAM_S, 0.1); this->declare_parameter(SPACENAV_STATIC_ROT_DEADBAND_PARAM_S, 0.1); + this->declare_parameter(SPACENAV_USE_TWIST_STAMPED_PARAM_S, false); + this->get_parameter(SPACENAV_USE_TWIST_STAMPED_PARAM_S, use_twist_stamped); auto param_change_callback = [this]( std::vector parameters) { @@ -117,8 +120,13 @@ Spacenav::Spacenav(const rclcpp::NodeOptions & options) "spacenav/offset", 10); publisher_rot_offset = this->create_publisher( "spacenav/rot_offset", 10); - publisher_twist = - this->create_publisher("spacenav/twist", 10); + if (use_twist_stamped) { + publisher_twist_stamped = + this->create_publisher("spacenav/twist", 10); + } else { + publisher_twist = + this->create_publisher("spacenav/twist", 10); + } publisher_joy = this->create_publisher("spacenav/joy", 10); @@ -288,7 +296,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(); + 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;