Update Telemetry to Use Services Rather than Publishers

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
 
 

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)

src/custom_interfaces/srv/MoteusState.srv

@@ -0,0 +1,3 @@
+int64 target_can_id
+---
+string json_payload

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,16 @@ 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.