Fix publishing of spacenav button values (#243)

* Fix publishing of spacenav button values

We now always publish the two default button states, in which
0 = not pressed
1 = pressed.

Additional buttons emerge and stay in that list upon being pressed for
the first time.  This supports more advanced spacenav devices with a
yet unknown number of buttons.

Fixes #223

* Fix format

* Switch back to shared memory transport

We now only keep the buttons' memory as class member.

Author: stefanscherzinger

spacenav/include/spacenav/spacenav.hpp

@@ -30,6 +30,7 @@
 #ifndef SPACENAV__SPACENAV_HPP_
 #define SPACENAV__SPACENAV_HPP_
 
+#include <vector>
 #include <rclcpp/rclcpp.hpp>
 
 #include <geometry_msgs/msg/twist.hpp>
@@ -84,6 +85,9 @@ class Spacenav final : public rclcpp::Node
   double normed_wx = 0;
   double normed_wy = 0;
   double normed_wz = 0;
+
+  // We'll resize dynamically to support spacenav devices with more buttons.
+  std::vector<int> joystick_buttons = {0, 0};
 };
 
 }  // namespace spacenav

spacenav/src/spacenav.cpp

@@ -246,10 +246,20 @@ void Spacenav::poll_spacenav()
         break;
 
       case SPNAV_EVENT_BUTTON:
-        if (sev.button.bnum >= static_cast<int>(msg_joystick->buttons.size())) {
-          msg_joystick->buttons.resize(sev.button.bnum + 1);
+
+        if (sev.button.bnum < 0) {
+          RCLCPP_WARN(
+            get_logger(), "Negative spacenav buttons not supported. Got %i", sev.button.bnum);
+          break;
+        }
+        if (sev.button.bnum < static_cast<int>(joystick_buttons.size())) {
+          // Update known buttons
+          joystick_buttons[sev.button.bnum] = sev.button.press;
+        } else {
+          // Enlarge, fill up with zeros, and support the new button
+          joystick_buttons.resize(sev.button.bnum + 1, 0);
+          joystick_buttons[sev.button.bnum] = sev.button.press;
         }
-        msg_joystick->buttons[sev.button.bnum] = sev.button.press;
         joy_stale = true;
         break;
 
@@ -293,6 +303,7 @@ void Spacenav::poll_spacenav()
       msg_joystick->axes[3] = normed_wx;
       msg_joystick->axes[4] = normed_wy;
       msg_joystick->axes[5] = normed_wz;
+      msg_joystick->buttons = joystick_buttons;
       publisher_joy->publish(std::move(msg_joystick));
     }
   }