Merge pull request #3 from KazuyaOguma18/feat-math-expression

feat: 数式表現によりshared, axis, buttonの値を抽出可能に

.devcontainer/devcontainer.json

@@ -1,12 +1,12 @@
 {
   "name": "ros 2",
-  "image": "mcr.microsoft.com/devcontainers/cpp:jammy",
+  "image": "mcr.microsoft.com/devcontainers/cpp:noble",
   "features": {
     "ghcr.io/adamlm/devcontainer-features/ros2:0": {
-      "distro": "humble"
+      "distro": "jazzy"
     },
     "ghcr.io/devcontainers-contrib/features/apt-packages:1": {
-      "packages": "ros-humble-foxglove-bridge,python3-colcon-mixin,mold,ccache,cpplint,clang-format,cmake-format,doxygen,python3-rosdep,yamllint",
+      "packages": "ros-jazzy-foxglove-bridge,python3-colcon-mixin,mold,ccache,cpplint,clang-format,cmake-format,doxygen,python3-rosdep,yamllint",
       "installsAfter": ["ghcr.io/adamlm/devcontainer-features/ros2"]
     },
     "ghcr.io/devcontainers-contrib/features/pipx-package:1": {
@@ -17,7 +17,7 @@
   },
   "forwardPorts": [9090],
   "remoteEnv": {
-    "ROS_DISTRO": "humble"
+    "ROS_DISTRO": "jazzy"
   },
   "containerEnv": {
     "TZ": "Asia/Tokyo"

.github/workflows/build-and-test-differential.yaml

@@ -5,16 +5,16 @@ on:
 
 jobs:
   build-and-test-differential:
-    runs-on: ubuntu-22.04
+    runs-on: ubuntu-24.04
     container: ${{ matrix.container }}
     strategy:
       fail-fast: false
       matrix:
         rosdistro:
-          - humble
+          - jazzy
         include:
-          - rosdistro: humble
-            container: ros:humble
+          - rosdistro: jazzy
+            container: ros:jazzy
             build-depends-repos: build_depends.repos
     steps:
       - name: Generate token

.github/workflows/build-and-test.yaml

@@ -9,16 +9,16 @@ on:
 jobs:
   build-and-test:
     if: ${{ github.event_name != 'push' || github.ref_name == github.event.repository.default_branch }}
-    runs-on: ubuntu-22.04
+    runs-on: ubuntu-24.04
     container: ${{ matrix.container }}
     strategy:
       fail-fast: false
       matrix:
         rosdistro:
-          - humble
+          - jazzy
         include:
-          - rosdistro: humble
-            container: ros:humble
+          - rosdistro: jazzy
+            container: ros:jazzy
             build-depends-repos: build_depends.repos
     steps:
       - name: Generate token

.github/workflows/cancel-previous-workflows.yaml

@@ -5,7 +5,7 @@ on:
 
 jobs:
   cancel-previous-workflows:
-    runs-on: ubuntu-22.04
+    runs-on: ubuntu-24.04
     steps:
       - name: Cancel previous runs
         uses: styfle/cancel-workflow-action@0.12.1

.github/workflows/deploy-docs.yaml

@@ -8,9 +8,9 @@ on:
 
 jobs:
   get-paths:
-    runs-on: ubuntu-22.04
+    runs-on: ubuntu-24.04
     if: github.event_name == 'push' || github.event.pull_request.merged == true
-    container: ros:humble
+    container: ros:jazzy
     outputs:
       self-package-paths: ${{ steps.get-self-package-paths.outputs.paths }}
     steps:
@@ -22,7 +22,7 @@ jobs:
         uses: Closer-Robotics/closer-github-actions/get-self-package-paths@v1
 
   build:
-    runs-on: ubuntu-22.04
+    runs-on: ubuntu-24.04
     needs: get-paths
     steps:
       - name: Check out repository
@@ -35,7 +35,7 @@ jobs:
 
   deploy:
     needs: build
-    runs-on: ubuntu-22.04
+    runs-on: ubuntu-24.04
     concurrency:
       group: ${{ github.workflow }}-${{ github.ref }}
     environment:

.github/workflows/pre-commit-optional.yaml

@@ -5,7 +5,7 @@ on:
 
 jobs:
   pre-commit-optional:
-    runs-on: ubuntu-22.04
+    runs-on: ubuntu-24.04
     steps:
       - name: Check out repository
         uses: actions/checkout@v4

.github/workflows/pre-commit.yaml

@@ -6,7 +6,7 @@ on:
 jobs:
   pre-commit:
     if: ${{ github.event.repository.private }} # Use pre-commit.ci for public repositories
-    runs-on: ubuntu-22.04
+    runs-on: ubuntu-24.04
     steps:
       - name: Generate token
         id: generate-token

mofpy/mofpy/action/publish.py

@@ -7,6 +7,7 @@
 import yaml
 
 from .action import Action
+from ..math_expression import MathExpression
 
 
 class Publish(Action):
@@ -47,6 +48,15 @@ def create_publisher(self, topic_name, topic_type, qos_profile):
     def execute(self, named_joy=None):
         yaml_vals = yaml.load(str(self.__values), Loader=yaml.FullLoader)
         msg = self.__msg_class()
+
+        # 数式表現があれば数式展開する．失敗した場合は次の処理は行わない
+        yaml_vals, success = MathExpression.expressions(
+            yaml_vals, named_buttons=named_joy["buttons"], named_axes=named_joy["axes"]
+        )
+        if not success:
+            rclpy.logging.get_logger("mofpy.Publish").error("Failed to expand math expression")
+            return
+
         try:
             timestamp_fields = set_message_fields(
                 msg, yaml_vals, expand_header_auto=True, expand_time_now=True

mofpy/mofpy/action/shared_values.py → mofpy/mofpy/action/shared_list.py

@@ -4,11 +4,11 @@
 from ..shared import Shared
 
 
-class SharedValues(Action):
-    NAME = "shared_values"
+class SharedList(Action):
+    NAME = "shared_list"
 
     def __init__(self, definition, node):
-        super(SharedValues, self).__init__(definition, node)
+        super(SharedList, self).__init__(definition, node)
         Action.actions[self.__class__.NAME] = self.__class__
 
         self.__key = self.get_required("key")
@@ -46,7 +46,7 @@ def execute(self, named_joy=None):
         index = self.__next_index__()
         value = self.__select__(index)
 
-        rclpy.logging.get_logger("shared_values").info("{0} : {1}".format(self.__key, value))
+        rclpy.logging.get_logger("shared_list").info("{0} : {1}".format(self.__key, value))
 
     def __select__(self, index):
         Shared.add(self.__shared_index_key, index)
@@ -70,4 +70,4 @@ def __next_index__(self):
         return next_index
 
 
-Action.register_preset(SharedValues)
+Action.register_preset(SharedList)

mofpy/mofpy/action/shared_value.py

@@ -0,0 +1,54 @@
+import rclpy.logging
+
+from .action import Action
+from ..shared import Shared
+
+
+class SharedValue(Action):
+    NAME = "shared_value"
+
+    def __init__(self, definition, node):
+        super(SharedValue, self).__init__(definition, node)
+        Action.actions[self.__class__.NAME] = self.__class__
+
+        self.__key = self.get_required("key")
+
+        self.__value = self.get("value")
+
+        self.__step = self.get("step", 0)
+
+        self.__enable_button = self.get("enable_button")
+
+        if self.has("initial"):
+            Shared.add(self.__key, self.get("initial", self.__value))
+
+    def execute(self, named_joy=None):
+        if not named_joy or not self.__enable_button:
+            Shared.update(self.__key, self.__value)
+            rclpy.logging.get_logger("shared_value").info(
+                "{0} : {1}".format(self.__key, self.__value)
+            )
+            return
+
+        named_buttons = named_joy["buttons"]
+        active_button = (
+            named_buttons[self.__enable_button].value
+            if self.__enable_button in named_joy["buttons"]
+            else None
+        )
+        if active_button:
+            value = self.__next_value__()
+            Shared.update(self.__key, value)
+
+            rclpy.logging.get_logger("shared_value").info("{0} : {1}".format(self.__key, value))
+
+    def __next_value__(self):
+        if self.__value:
+            return self.__value
+
+        current_val = Shared.get(self.__key)
+
+        return current_val + self.__step
+
+
+Action.register_preset(SharedValue)

mofpy/mofpy/definition.py

@@ -11,11 +11,12 @@ def __init__(self):
 
     @staticmethod
     def parse(filepaths):
-        # node.get_logger().info(str(filepaths))
         for filepath in filepaths:
             with open(filepath, "r") as yml:
                 definition = yaml.safe_load(yml)
-                Definitions.definitions = merge_dicts(Definitions.definitions, definition)
+                Definitions.definitions = Definitions.__merge_dicts__(
+                    Definitions.definitions, definition
+                )
 
     @staticmethod
     def get(key: str, default_val=None):
@@ -32,18 +33,16 @@ def get(key: str, default_val=None):
 
         return d
 
-
-def merge_dicts(d1, d2):
-    merged = copy.deepcopy(d1)  # d1の内容をコピー
-    for key, value in d2.items():
-        if key in merged:
-            # 両方が辞書なら、再帰的にマージ
-            if isinstance(merged[key], dict) and isinstance(value, dict):
-                merged[key] = merge_dicts(merged[key], value)
+    @staticmethod
+    def __merge_dicts__(d1, d2):
+        merged = copy.deepcopy(d1)
+        for key, value in d2.items():
+            if key in merged:
+                # If both are dictionaries, recursively merge
+                if isinstance(merged[key], dict) and isinstance(value, dict):
+                    merged[key] = Definitions.__merge_dicts__(merged[key], value)
+                else:
+                    merged[key] = value
             else:
-                # d2 の値で上書き
                 merged[key] = value
-        else:
-            merged[key] = value
-    # rclpy.logging.get_logger("merge_dicts").info(f"merge: {str(merged)}, {str(d2)}")
-    return merged
+        return merged

mofpy/mofpy/math_expression.py

@@ -0,0 +1,56 @@
+from functools import partial
+import re
+
+import sympy
+
+from .shared import Shared
+
+
+class MathExpression:
+
+    def __init__(self):
+        pass
+
+    @staticmethod
+    def expressions(values: dict, named_buttons, named_axes) -> dict:
+        for key, value in values.items():
+            success = True
+            if isinstance(value, dict):
+                values[key], success = MathExpression.expressions(value, named_buttons, named_axes)
+            if isinstance(value, str):
+                values[key], success = MathExpression.__expression__(
+                    value, named_buttons, named_axes
+                )
+            if not success:
+                return values, False
+        return values, True
+
+    @staticmethod
+    def __shared__(key):
+        return Shared.get(str(key))
+
+    @staticmethod
+    def __axis__(axis, named_axes):
+        axis_str = str(axis)
+        return named_axes[axis_str].value if axis_str in named_axes else 0
+
+    @staticmethod
+    def __button__(button, named_buttons):
+        return 1 if named_buttons[str(button)].value else 0
+
+    @staticmethod
+    def __expression__(value, named_buttons, named_axes):
+        variables = {
+            "shared": MathExpression.__shared__,
+            "axis": partial(MathExpression.__axis__, named_axes=named_axes),
+            "button": partial(MathExpression.__button__, named_buttons=named_buttons),
+        }
+
+        match = re.match(r"\${(.+)}", value)
+        if match:
+            try:
+                expr = sympy.sympify(match.group(1), locals=variables)
+                return expr, True
+            except (sympy.SympifyError, ValueError, AttributeError):
+                return value, False
+        return value, True

mofpy/package.xml

@@ -7,13 +7,15 @@
   <maintainer email="kzy.basect@gmail.com">Kazuya Oguma</maintainer>
   <license>MIT</license>
 
+  <depend>control_msgs</depend>
   <depend>geometry_msgs</depend>
+  <depend>joy</depend>
   <depend>moveit_msgs</depend>
   <depend>moveit_py</depend>
+  <depend>python3-sympy</depend>
   <depend>rclpy</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
-  <exec_depend>joy</exec_depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>