From 1218ad3d280ac66f5d7dd378ecae72ea6a32f608 Mon Sep 17 00:00:00 2001
From: Vladimir Kupryukhin <vladimirkupryukhin@gmail.com>
Date: Thu, 26 Sep 2024 17:45:10 +0000
Subject: [PATCH 1/2] Added service for moteus motor data in robot_info.py

---
 container_launch.sh                       | 12 ++++++------
 src/custom_interfaces/CMakeLists.txt      |  1 +
 src/custom_interfaces/srv/MoteusState.srv |  3 +++
 src/lib/interface/robot_info.py           | 16 ++++++++++++++++
 4 files changed, 26 insertions(+), 6 deletions(-)
 create mode 100644 src/custom_interfaces/srv/MoteusState.srv

diff --git a/container_launch.sh b/container_launch.sh
index e85c3279..496a2d50 100755
--- a/container_launch.sh
+++ b/container_launch.sh
@@ -39,17 +39,17 @@ fi
 # --- Handle the Docker image ---
 
 # If the image does NOT exist OR we force the image to be built
-if [ -z "$(docker images -q ${trickire_container}:latest 2> /dev/null)" ] || [ "$b_flag" = true ]; then
+if [ -z "$(docker images -q ${trickfire_image}:latest 2> /dev/null)" ] || [ "$b_flag" = true ]; then
   # Should we build without cache?
   if [ "$no_cache_flag" = true ]; then
-    echo -e "${BLUE}$(tput bold)[${text_helper}] Building \"${trickire_container}\" without cache${NC}"
-    docker build --no-cache -t ${trickire_container} -f .devcontainer/Dockerfile . 
+    echo -e "${BLUE}$(tput bold)[${text_helper}] Building \"${trickfire_image}\" without cache${NC}"
+    docker build --no-cache -t ${trickfire_image} -f .devcontainer/Dockerfile . 
   else
-    echo -e "${BLUE}$(tput bold)[${text_helper}] Building \"${trickire_container}\" with cache${NC}"
-    docker build -t ${trickire_container} -f .devcontainer/Dockerfile . 
+    echo -e "${BLUE}$(tput bold)[${text_helper}] Building \"${trickfire_image}\" with cache${NC}"
+    docker build -t ${trickfire_image} -f .devcontainer/Dockerfile . 
   fi
 else
-    echo -e "${BLUE}$(tput bold)[${text_helper}] Image \"${trickire_container}\" exists. Skipping${NC}"
+    echo -e "${BLUE}$(tput bold)[${text_helper}] Image \"${trickfire_image}\" exists. Skipping${NC}"
 fi
 
 
diff --git a/src/custom_interfaces/CMakeLists.txt b/src/custom_interfaces/CMakeLists.txt
index 9d3734ea..fb7e816c 100644
--- a/src/custom_interfaces/CMakeLists.txt
+++ b/src/custom_interfaces/CMakeLists.txt
@@ -15,6 +15,7 @@ find_package(rosidl_default_generators REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/ArmMode.srv"
+  "srv/MoteusState.srv"
  )
 
 if(BUILD_TESTING)
diff --git a/src/custom_interfaces/srv/MoteusState.srv b/src/custom_interfaces/srv/MoteusState.srv
new file mode 100644
index 00000000..bcd2edf6
--- /dev/null
+++ b/src/custom_interfaces/srv/MoteusState.srv
@@ -0,0 +1,3 @@
+int64 target_can_id
+---
+string json_payload
\ No newline at end of file
diff --git a/src/lib/interface/robot_info.py b/src/lib/interface/robot_info.py
index ed8b10b0..78bfc079 100644
--- a/src/lib/interface/robot_info.py
+++ b/src/lib/interface/robot_info.py
@@ -3,10 +3,13 @@
 motor on the robot.
 """
 
+from typing import Any
+
 from rclpy.node import Node
 from rclpy.subscription import Subscription
 from std_msgs.msg import String
 
+from custom_interfaces.srv import MoteusState
 from lib.configs import MoteusMotorConfig, MotorConfigs
 from lib.moteus_motor_state import MoteusMotorState
 
@@ -21,6 +24,10 @@ def __init__(self, ros_node: Node):
         self.sub_list: list[Subscription] = []  # empty array
         self.can_id_to_json: dict[int, MoteusMotorState] = {}  # Dict
 
+        self.get_moteus_state_service = self._ros_node.create_service(
+            MoteusState, "get_moteus_motor_state", self._handleGetMoteusState
+        )
+
         for motor_config in MotorConfigs.getAllMotors():
             self._ros_node.create_subscription(
                 String, motor_config.getCanTopicName(), self._subCallback, 10
@@ -31,6 +38,15 @@ def _subCallback(self, msg: String) -> None:
         state = MoteusMotorState.fromJsonMsg(msg)
         self.can_id_to_json[state.can_id] = state
 
+    def _handleGetMoteusState(
+        self, request: MoteusState.Request, response: MoteusState.Response
+    ) -> MoteusState.Response:
+
+        can_id = request.target_can_id
+        string_message = self.can_id_to_json[can_id].toMsg()
+        response.json_payload = str(string_message.data)
+        return response
+
     def getMotorState(self, motor: MoteusMotorConfig) -> MoteusMotorState:
         """
         Gets the state of the motor with the given can_id.

From 08390a35639d8bcc73c1ec191fd91b8bbed8cc1c Mon Sep 17 00:00:00 2001
From: TrickFire Robotics <trickfirerobotics@gmail.com>
Date: Mon, 30 Sep 2024 18:41:14 -0700
Subject: [PATCH 2/2] Remove some debugging prints

---
 src/lib/interface/robot_info.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/lib/interface/robot_info.py b/src/lib/interface/robot_info.py
index 78bfc079..d7fe68cc 100644
--- a/src/lib/interface/robot_info.py
+++ b/src/lib/interface/robot_info.py
@@ -45,6 +45,7 @@ def _handleGetMoteusState(
         can_id = request.target_can_id
         string_message = self.can_id_to_json[can_id].toMsg()
         response.json_payload = str(string_message.data)
+    
         return response
 
     def getMotorState(self, motor: MoteusMotorConfig) -> MoteusMotorState: