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..d7fe68cc 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,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.