From 94d5d652d78e6b51a989e4e3b9d87d2350223df4 Mon Sep 17 00:00:00 2001
From: Rooholla-KhorramBakht <learn.kho@gmail.com>
Date: Tue, 16 Apr 2024 00:05:02 -0400
Subject: [PATCH] switch to lowlevel mode key added to the bridge

---
 deploy/robot_ws/src/go2py_node/src/bridge.cpp | 25 +++++++++++++++--
 req.py                                        | 28 +++++++++++++++++++
 2 files changed, 50 insertions(+), 3 deletions(-)
 create mode 100644 req.py

diff --git a/deploy/robot_ws/src/go2py_node/src/bridge.cpp b/deploy/robot_ws/src/go2py_node/src/bridge.cpp
index 6b70a5e..b438a54 100644
--- a/deploy/robot_ws/src/go2py_node/src/bridge.cpp
+++ b/deploy/robot_ws/src/go2py_node/src/bridge.cpp
@@ -24,7 +24,7 @@
 #include "unitree_api/msg/request.hpp"
 #include "common/ros2_sport_client.h"
 #include "joystick.h"
-
+#include "unitree_api/msg/request.hpp"
 class Custom: public rclcpp::Node
 {
     public:
@@ -53,6 +53,7 @@ class Custom: public rclcpp::Node
             "/go2/lowcmd", 1, std::bind(&Custom::lowcmd_callback, this, std::placeholders::_1));
 
             lowcmd_puber = this->create_publisher<unitree_go::msg::LowCmd>("/lowcmd", 10);
+            api_publisher = this->create_publisher<unitree_api::msg::Request>("/api/robot_state/request", 10);
 
         }
     private:
@@ -89,6 +90,7 @@ class Custom: public rclcpp::Node
         unitree_go::msg::LowCmd lowcmd_msg;
         uint64_t last_lowcmd_stamp = 0;
         xRockerBtnDataStruct _keyData;
+        rclcpp::Publisher<unitree_api::msg::Request>::SharedPtr api_publisher;
 
         void init_lowcmd()
         {
@@ -190,11 +192,28 @@ void Custom::lowstate_callback(unitree_go::msg::LowState::SharedPtr data)
     {
         Estop = 1;
     }
-    if ((_keyData.btn.components.R1 == 1 && _keyData.btn.components.L1 == 1)||
-        (_keyData.btn.components.L2 == 1 && _keyData.btn.components.A == 1))
+    if ((_keyData.btn.components.L2 == 1 && _keyData.btn.components.A == 1))
     {
         Estop = 0;
     }
+    if ((_keyData.btn.components.L2 == 1 && _keyData.btn.components.L1 == 1))
+    {
+        auto msg = std::make_shared<unitree_api::msg::Request>();
+        // Populate the message fields
+        msg->header.identity.id = 80005;
+        msg->header.identity.api_id = 1001;
+        msg->parameter = "{\"name\":\"sport_mode\",\"switch\":0}";
+        api_publisher->publish(*msg);
+    }
+    if ((_keyData.btn.components.R2 == 1 && _keyData.btn.components.R1 == 1))
+    {
+        auto msg = std::make_shared<unitree_api::msg::Request>();
+        // Populate the message fields
+        msg->header.identity.id = 80005;
+        msg->header.identity.api_id = 1001;
+        msg->parameter = "{\"name\":\"sport_mode\",\"switch\":1}";
+        api_publisher->publish(*msg);
+    }
     // std::cout << "Estop: " << Estop << std::endl;
 }
 
diff --git a/req.py b/req.py
new file mode 100644
index 0000000..9eebd63
--- /dev/null
+++ b/req.py
@@ -0,0 +1,28 @@
+import rclpy
+from rclpy.node import Node
+from unitree_api.msg import Request
+
+class PublisherNode(Node):
+    def __init__(self):
+        super().__init__('publisher_node')
+        self.publisher_ = self.create_publisher(Request, '/api/robot_state/request', 10)
+        self.timer = self.create_timer(1, self.publish_message)
+
+    def publish_message(self):
+        msg = Request()
+        # Populate the message fields
+        msg.header.identity.id=80005
+        msg.header.identity.api_id=1001
+        msg.parameter = '{"name":"sport_mode","switch":0}'
+        self.publisher_.publish(msg)
+        self.get_logger().info('Published message')
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = PublisherNode()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()