Merge pull request #20 from NASA-JSC-Robotics/ndunkelb/use-services-for-mockup-states

Use services for mockup states

Author: ndunkelb-nasa

mockup_msgs/CMakeLists.txt

@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.16)
+project(mockup_msgs)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(sensor_msgs REQUIRED)
+
+set(service_files
+  "srv/SetJointState.srv"
+)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  ${service_files}
+  DEPENDENCIES
+    sensor_msgs
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+
+if(BUILD_TESTING)
+endif()
+
+ament_package()

mockup_msgs/README.md

@@ -0,0 +1,4 @@
+# Mockup messages
+
+Services to help with handling mockup states.
+For now, this just includes SetJointState, which can be used with a mockup_state_manager to set the state of joints to be published

mockup_msgs/package.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>mockup_msgs</name>
+  <version>1.0.0</version>
+  <description>ROS2 interface declaration for managing mockups</description>
+  <maintainer email="nathan.b.dunkelberger@nasa.gov">ndunkelb</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <build_depend>sensor_msgs</build_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

mockup_msgs/srv/SetJointState.srv

@@ -0,0 +1,7 @@
+# This service takes in a joint state, and uses that to update an internal representation of joint state,
+# returning success if the set was successful, and optionally returning a message
+
+sensor_msgs/JointState joint_state
+---
+bool success
+string message

mockups_launch_common/package.xml

@@ -17,6 +17,7 @@
   <exec_depend>launch</exec_depend>
   <exec_depend>launch_ros</exec_depend>
   <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>mockup_msgs</exec_depend>
 
   <test_depend>ament_cmake_pytest</test_depend>
   <test_depend>python3-pytest</test_depend>

mockups_launch_common/scripts/mockup_state_manager.py

@@ -1,21 +1,24 @@
 #!/usr/bin/env python3
 
 import rclpy
+from rclpy.executors import ExternalShutdownException
 from rclpy.node import Node
-from std_msgs.msg import Float64
 from sensor_msgs.msg import JointState
-import copy
-import threading
+from mockup_msgs.srv import SetJointState
 
 
 # Class that stores the information for each mockup config
 class MockupConfig:
-    def __init__(self, topic_name, min_position, max_position, initial_position, joint_name):
-        self.topic_name = topic_name
+    def __init__(self, min_position, max_position, initial_position, joint_name):
         self.joint_name = joint_name
         self.min_position = min_position
         self.max_position = max_position
         self.position = initial_position
+        self.velocity = 0.0
+        self.effort = 0.0
+
+    def set_position(self, desired_position):
+        self.position = max(self.min_position, min(desired_position, self.max_position))
 
 
 class MockupStateManager(Node):
@@ -34,99 +37,98 @@ def __init__(self):
         # create the joint state publisher
         self.publisher_ = self.create_publisher(JointState, "joint_states", 10)
 
-        subscriptions = []
-        for index, mockup_config in enumerate(self.mockup_configs):
-            # this was a bit funky for copying in index
-            # see https://github.com/ros2/rclpy/issues/629#issuecomment-1542151499 for reference
-            subscriptions.append(
-                self.create_subscription(
-                    Float64,
-                    self.prefix + mockup_config.topic_name,
-                    lambda msg, idx=index: self.position_cb(msg, idx),
-                    10,
-                )
-            )
+        self.set_joint_state_service = self.create_service(SetJointState, "~/set_joint_state", self.set_joint_state_cb)
 
         # create the timer for joint state publisher callback
         timer_period_sec = 0.5  # unit: seconds
         self.timer = self.create_timer(timer_period_sec, self.joint_state_cb)
 
-        self.lock = threading.Lock()
-
     def load_mockup_configs(self):
         """loads the parameters provided with each of the relevaant joints and populates self.mockup_configs"""
         # get the list of topic names first
 
-        topic_params = self.get_parameters_by_prefix("topics")
+        joints_params = self.get_parameters_by_prefix("joints")
 
-        topic_names = {key.split(".")[0] for key in topic_params.keys()}
+        joints = {key.split(".")[0] for key in joints_params.keys()}
 
         # puopulate self.mockup_configs based on loaded parameters
-        self.mockup_configs = []
-        for topic in topic_names:
-            self.get_logger().info(f"Loading: {self.prefix + topic}")
+        self.mockup_configs = dict()
+        for joint in joints:
+            self.get_logger().info(f"Loading: {self.prefix + joint}")
 
-            joint_name = topic_params[f"{topic}.joint_name"].value
-            min_position = topic_params[f"{topic}.min_position"].value
-            max_position = topic_params[f"{topic}.max_position"].value
-            initial_position = topic_params[f"{topic}.initial_position"].value
+            joint_name = joint
+            min_position = joints_params[f"{joint}.min_position"].value
+            max_position = joints_params[f"{joint}.max_position"].value
+            initial_position = joints_params[f"{joint}.initial_position"].value
 
             # add mockups to the member variable
-            self.mockup_configs.append(MockupConfig(topic, min_position, max_position, initial_position, joint_name))
+            self.mockup_configs[joint_name] = MockupConfig(min_position, max_position, initial_position, joint_name)
 
     def joint_state_cb(self):
         """Publisher for the manager which publishes the joint state info."""
         msg = JointState()
         msg.header.stamp = self.get_clock().now().to_msg()
-        # with mutex locking, add all of the joint states
-        with self.lock:
-            for mockup_config in self.mockup_configs:
-                msg.name.append(self.prefix + mockup_config.joint_name)
-                msg.position.append(mockup_config.position)
-                msg.velocity.append(0.0)
-                msg.effort.append(0.0)
+        # add all of the joint states
+        for mockup_config in self.mockup_configs.values():
+            msg.name.append(self.prefix + mockup_config.joint_name)
+            msg.position.append(mockup_config.position)
+            msg.velocity.append(mockup_config.position)
+            msg.effort.append(mockup_config.effort)
 
         self.publisher_.publish(msg)
 
-        self.get_logger().debug(
-            f"This is the hatch joint state message: {msg}"
-        )  # this will fill in the string with formatted msg data
+    def set_joint_state_cb(self, req: SetJointState.Request, res: SetJointState.Response):
+        """
+        Callback for the setting internal joint states which will then be published.
+        """
 
-    def position_cb(self, msg: Float64, index):
-        """callback for the position setting
+        # Check first to make sure that data is the right size.
+        # Any non-empty lists must be of the same size of names
+        error_msg = ""
+        msg_size = len(req.joint_state.name)
+        valid = True
+
+        for field in ("position", "velocity", "effort"):
+            values = getattr(req.joint_state, field)
+            if values and (len(values) != msg_size):
+                error_msg += (
+                    f"The size of `{field}` ({len(values)}) does not match the size of"
+                    "`name` ({msg_size}) in SetJointState. "
+                )
+                valid = False
 
-        Args:
-            msg (Float64): This message transmits the joint position (in radians or meters)
-            index (int): the index that we are accessing (to ineracte with self.mockup_configs
-        """
-        self.get_logger().debug(
-            f"This is the new position of {self.prefix + self.mockup_configs[index].joint_name}: {msg}"
-        )
-        if self.mockup_configs[index].min_position <= msg.data and msg.data <= self.mockup_configs[index].max_position:
-            with self.lock:
-                self.mockup_configs[index].position = msg.data
-        elif self.mockup_configs[index].min_position >= msg.data:
-            with self.lock:
-                self.mockup_configs[index].position = copy.deepcopy(self.mockup_configs[index].min_position)
-        elif self.mockup_configs[index].max_position <= msg.data:
-            with self.lock:
-                self.mockup_configs[index].position = copy.deepcopy(self.mockup_configs[index].max_position)
-        else:  # I don't think this will ever happen 0.o
-            raise Exception(self.mockup_configs[index].joint_name + " joint angle out of range")
+        # return early if not valid
+        if not valid:
+            res.message = error_msg
+            res.success = False
+            return res
+
+        for i, name in enumerate(req.joint_state.name):
+            mockup_config = self.mockup_configs[name]
+            # position is special because we have to clamp it, so it uses a member function
+            if req.joint_state.position:
+                mockup_config.set_position(req.joint_state.position[i])
+            if req.joint_state.velocity:
+                mockup_config.velocity = req.joint_state.velocity[i]
+            if req.joint_state.effort:
+                mockup_config.effort = req.joint_state.effort[i]
+
+        res.success = True
+        return res
 
 
 def main(args=None):
     rclpy.init(args=args)
 
     mockup_state_manager = MockupStateManager()
 
-    rclpy.spin(mockup_state_manager)
-
-    # Destroy the node explicitly
-    # (optional - otherwise it will be done automatically
-    # when the garbage collector destroys the node object)
-    mockup_state_manager.destroy_node()
-    rclpy.shutdown()
+    try:
+        rclpy.spin(mockup_state_manager)
+    except (KeyboardInterrupt, ExternalShutdownException):
+        pass
+    finally:
+        mockup_state_manager.destroy_node()
+        rclpy.try_shutdown()
 
 
 if __name__ == "__main__":