From b9a763e9a4720f0e473992f9976c9f1d5cf48fe8 Mon Sep 17 00:00:00 2001
From: "marcin.mirski" <marcin.mirski@sanctuary.ai>
Date: Wed, 19 Feb 2025 19:43:26 -0800
Subject: [PATCH] Add left and right game controller rumble channels

---
 joy/include/joy/game_controller.hpp |  5 ++++
 joy/src/game_controller.cpp         | 38 +++++++++++++++++++++++------
 2 files changed, 36 insertions(+), 7 deletions(-)

diff --git a/joy/include/joy/game_controller.hpp b/joy/include/joy/game_controller.hpp
index c58a718d..2e0cc4f0 100644
--- a/joy/include/joy/game_controller.hpp
+++ b/joy/include/joy/game_controller.hpp
@@ -37,6 +37,7 @@
 #include <memory>
 #include <string>
 #include <thread>
+#include <utility>
 
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/joy.hpp>
@@ -75,6 +76,10 @@ class GameController final : public rclcpp::Node
   double scale_{0.0};
   double autorepeat_rate_{0.0};
   int autorepeat_interval_ms_{0};
+  std::pair<builtin_interfaces::msg::Time, uint16_t> rumble_intensity_left_stamped_{
+    builtin_interfaces::msg::Time(), 0};
+  std::pair<builtin_interfaces::msg::Time, uint16_t> rumble_intensity_right_stamped_{
+    builtin_interfaces::msg::Time(), 0};
   bool sticky_buttons_{false};
   bool publish_soon_{false};
   rclcpp::Time publish_soon_time_;
diff --git a/joy/src/game_controller.cpp b/joy/src/game_controller.cpp
index b17793bd..78ef3f64 100644
--- a/joy/src/game_controller.cpp
+++ b/joy/src/game_controller.cpp
@@ -131,26 +131,50 @@ GameController::~GameController()
 
 void GameController::feedbackCb(const std::shared_ptr<sensor_msgs::msg::JoyFeedback> msg)
 {
+  uint32_t duration_ms = 1000;
+
   if (msg->type != sensor_msgs::msg::JoyFeedback::TYPE_RUMBLE) {
     // We only support rumble
     return;
   }
 
-  if (msg->id != 0) {
-    // There can be only one (rumble)
-    // TODO(Rod Taylor): Support high and low frequency rumble channels.
+  if (msg->intensity < 0.0 || msg->intensity > 1.0) {
+    // We only accept intensities between 0 and 1.
     return;
   }
 
-  if (msg->intensity < 0.0 || msg->intensity > 1.0) {
-    // We only accept intensities between 0 and 1.
+  uint16_t intensity = static_cast<uint16_t>(msg->intensity * 0xFFFF);
+  rclcpp::Time now = this->now();
+
+  // 0: Both left motor (low frequency) and right motor (high frequency) rumble
+  // 1: Left rumble
+  // 2: Right rumble
+  if (msg->id == 0) {
+    rumble_intensity_left_stamped_ = {now, intensity};
+    rumble_intensity_right_stamped_ = {now, intensity};
+  } else if (msg->id == 1) {
+    rumble_intensity_left_stamped_ = {now, intensity};
+    if ((now - rumble_intensity_right_stamped_.first) >=
+      rclcpp::Duration::from_seconds(duration_ms / 1000.0))
+    {
+      rumble_intensity_right_stamped_ = {now, 0};
+    }
+  } else if (msg->id == 2) {
+    rumble_intensity_right_stamped_ = {now, intensity};
+    if ((now - rumble_intensity_left_stamped_.first) >=
+      rclcpp::Duration::from_seconds(duration_ms / 1000.0))
+    {
+      rumble_intensity_left_stamped_ = {now, 0};
+    }
+  } else {
     return;
   }
 
   if (game_controller_ != nullptr) {
     // We purposely ignore the return value; if it fails, what can we do?
-    uint16_t intensity = static_cast<uint16_t>(msg->intensity * 0xFFFF);
-    SDL_GameControllerRumble(game_controller_, intensity, intensity, 1000);
+    SDL_GameControllerRumble(
+      game_controller_, rumble_intensity_left_stamped_.second,
+      rumble_intensity_right_stamped_.second, duration_ms);
   }
 }