Fix feedback and status message not being published after a while

Fixes #34

ds4_driver/config/params.yaml

@@ -25,3 +25,6 @@ ds4_driver:
     # Amount by which the joystick has to move before it is considered to be
     # off-center.
     deadzone: 0.1
+
+    # Maximum rate in Hz to publish Status messages.
+    max_status_rate: 100.0

ds4_driver/ds4_driver/controller_ros.py

@@ -25,17 +25,21 @@ def __init__(self, node):
         self.node.declare_parameter("frame_id", "ds4")
         self.node.declare_parameter("imu_frame_id", "ds4_imu")
         self.node.declare_parameter("autorepeat_rate", 0.0)
+        self.node.declare_parameter("max_status_rate", 100.0)
 
         self.use_standard_msgs = self.node.get_parameter("use_standard_msgs").value
         self.deadzone = self.node.get_parameter("deadzone").value
         self.frame_id = self.node.get_parameter("frame_id").value
         self.imu_frame_id = self.node.get_parameter("imu_frame_id").value
         # Only publish Joy messages on change
         self._autorepeat_rate = self.node.get_parameter("autorepeat_rate").value
+        self._max_status_rate = self.node.get_parameter("max_status_rate").value
         self._prev_joy = None
 
         self.stop_rumble_timer = None
 
+        self._last_status_publish_time = None
+
         # Use ROS-standard messages (like sensor_msgs/Joy)
         if self.use_standard_msgs:
             self.pub_report = self.node.create_publisher(Report, "raw_report", 0)
@@ -61,9 +65,15 @@ def cb_report(self, report):
         :param report:
         :return:
         """
+        now = self.node.get_clock().now()
+        if self._max_status_rate > 0 and self._last_status_publish_time is not None:
+            dt = (now - self._last_status_publish_time).nanoseconds / 1e9
+            if dt < (1 / self._max_status_rate):
+                return
+
         report_msg = Report()
         report_msg.header.frame_id = self.frame_id
-        report_msg.header.stamp = self.node.get_clock().now().to_msg()
+        report_msg.header.stamp = now.to_msg()
         for attr in dir(report):
             if attr.startswith("_"):
                 continue
@@ -100,6 +110,8 @@ def cb_report(self, report):
         else:
             self.pub_status.publish(status_msg)
 
+        self._last_status_publish_time = now
+
     def cb_feedback(self, msg):
         """
         Callback method for ds4_driver/Feedback

ds4_driver/nodes/demo.py

@@ -28,10 +28,8 @@ def cb_status(self, msg):
         """
         now = self._node.get_clock().now()
 
-        # Commenting out this check for now because to_sec() does not seem to work for rclpy Duration objects
-        # This checks to see if the min interval for a callback has been violated.
-        # if (now - self._last_pub_time).to_sec() < self._min_interval:
-        #     return
+        if (now - self._last_pub_time).nanoseconds / 1e9 < self._min_interval:
+            return
 
         feedback = Feedback()