diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index c1b172e..44358a9 100644 --- a/.devcontainer/devcontainer.json +++ b/.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" diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index 266b3ac..bc1f3f2 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.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 diff --git a/.github/workflows/build-and-test.yaml b/.github/workflows/build-and-test.yaml index 345e328..622af24 100644 --- a/.github/workflows/build-and-test.yaml +++ b/.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 diff --git a/.github/workflows/cancel-previous-workflows.yaml b/.github/workflows/cancel-previous-workflows.yaml index bd2463d..b1c4d85 100644 --- a/.github/workflows/cancel-previous-workflows.yaml +++ b/.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 diff --git a/.github/workflows/deploy-docs.yaml b/.github/workflows/deploy-docs.yaml index 521fffe..e7d2135 100644 --- a/.github/workflows/deploy-docs.yaml +++ b/.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: diff --git a/.github/workflows/pre-commit-optional.yaml b/.github/workflows/pre-commit-optional.yaml index 12f536c..20ed486 100644 --- a/.github/workflows/pre-commit-optional.yaml +++ b/.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 diff --git a/.github/workflows/pre-commit.yaml b/.github/workflows/pre-commit.yaml index 4d005e8..9d6a342 100644 --- a/.github/workflows/pre-commit.yaml +++ b/.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 diff --git a/mofpy/mofpy/action/publish.py b/mofpy/mofpy/action/publish.py index df697bd..2f2059c 100644 --- a/mofpy/mofpy/action/publish.py +++ b/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 diff --git a/mofpy/mofpy/action/shared_values.py b/mofpy/mofpy/action/shared_list.py similarity index 89% rename from mofpy/mofpy/action/shared_values.py rename to mofpy/mofpy/action/shared_list.py index 5819e23..9a76267 100644 --- a/mofpy/mofpy/action/shared_values.py +++ b/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) diff --git a/mofpy/mofpy/action/shared_value.py b/mofpy/mofpy/action/shared_value.py new file mode 100644 index 0000000..5904315 --- /dev/null +++ b/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) diff --git a/mofpy/mofpy/definition.py b/mofpy/mofpy/definition.py index 51d0ad9..cedfdb9 100644 --- a/mofpy/mofpy/definition.py +++ b/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 diff --git a/mofpy/mofpy/math_expression.py b/mofpy/mofpy/math_expression.py new file mode 100644 index 0000000..177e3a7 --- /dev/null +++ b/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 diff --git a/mofpy/package.xml b/mofpy/package.xml index ea04c1f..26d3c51 100644 --- a/mofpy/package.xml +++ b/mofpy/package.xml @@ -7,13 +7,15 @@ Kazuya Oguma MIT + control_msgs geometry_msgs + joy moveit_msgs moveit_py + python3-sympy rclpy sensor_msgs std_msgs - joy ament_copyright ament_flake8 diff --git a/mofpy/test/test_copyright.py b/mofpy/test/test_copyright.py deleted file mode 100644 index 95f0381..0000000 --- a/mofpy/test/test_copyright.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_copyright.main import main -import pytest - - -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason="No copyright header has been placed in the generated source file.") -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=[".", "test"]) - assert rc == 0, "Found errors" diff --git a/mofpy/test/test_flake8.py b/mofpy/test/test_flake8.py deleted file mode 100644 index 49c1644..0000000 --- a/mofpy/test/test_flake8.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, "Found %d code style errors / warnings:\n" % len(errors) + "\n".join(errors) diff --git a/mofpy/test/test_pep257.py b/mofpy/test/test_pep257.py deleted file mode 100644 index a2c3deb..0000000 --- a/mofpy/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=[".", "test"]) - assert rc == 0, "Found code style errors / warnings" diff --git a/mofpy_demo/CMakeLists.txt b/mofpy_demo/CMakeLists.txt index 7d97bd6..0c50307 100644 --- a/mofpy_demo/CMakeLists.txt +++ b/mofpy_demo/CMakeLists.txt @@ -15,16 +15,4 @@ install( DIRECTORY config launch DESTINATION share/mofpy_demo) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - ament_package() diff --git a/mofpy_demo/config/presets/common.yaml b/mofpy_demo/config/presets/common.yaml index 54b6d4f..55b15f5 100644 --- a/mofpy_demo/config/presets/common.yaml +++ b/mofpy_demo/config/presets/common.yaml @@ -7,22 +7,31 @@ presets: switch_state_to_chassis: trigger: [OP, OP] action: - - type: shared_values + - type: shared_value key: state - initial: 0 + initial: chassis value: chassis switch_state_to_arm: trigger: [SH, SH] action: - - type: shared_values + - type: shared_value key: state value: arm switch_state_to_arm_fk: trigger: [[OP, SH], [OP, SH]] action: - - type: shared_values + - type: shared_value key: state value: arm-fk + state_pub: + trigger: always + action: + - type: publish + topic: + name: state + type: std_msgs/String + values: + data: ${shared(state)} super: # Dummy trigger: [C_U, C_U, C_D, C_D, C_L, C_R, C_L, C_R, X, O] @@ -44,9 +53,9 @@ presets: values: data: Hello sample_twist: - # Publish twist with a fixed value + # Joyの値を用いてtwistトピックを配信 enabled_states: chassis - trigger: Q + trigger: always action: - type: publish topic: @@ -58,6 +67,31 @@ presets: frame_id: base_link twist: linear: - x: 1 + x: ${axis(LSV) * 0.5} angular: - z: 0.2 + z: ${axis(RSH) * 0.2} + + sample_pub_float_inc: + enabled_states: chassis + trigger: always + action: + - type: shared_value + key: float_data + step: 0.1 + enable_button: C_U + initial: 0.5 + - type: publish + topic: + name: float + type: std_msgs/Float32 + values: + data: ${shared(float_data)} + + sample_pub_float_dec: + enabled_states: chassis + trigger: always + action: + - type: shared_value + key: float_data + step: -0.1 + enable_button: C_D diff --git a/mofpy_demo/package.xml b/mofpy_demo/package.xml index d29c8e7..5457f12 100644 --- a/mofpy_demo/package.xml +++ b/mofpy_demo/package.xml @@ -9,13 +9,13 @@ ament_cmake - mofpy - moveit_core - moveit_ros_planning - moveit_resources_panda_moveit_config - moveit_configs_utils - moveit_servo - ament_lint_auto + mofpy + moveit_configs_utils + moveit_core + moveit_py_configs_utils + moveit_resources_panda_moveit_config + moveit_ros_planning + moveit_servo ament_lint_common diff --git a/moveit_py_configs_utils/package.xml b/moveit_py_configs_utils/package.xml index a91d7f4..df5eed8 100644 --- a/moveit_py_configs_utils/package.xml +++ b/moveit_py_configs_utils/package.xml @@ -7,11 +7,12 @@ Kazuya Oguma BSD-3-Clause - ament_index_python - launch - launch_param_builder - launch_ros - srdfdom + ament_index_python + launch + launch_param_builder + launch_ros + moveit_configs_utils + srdfdom ament_python diff --git a/moveit_py_configs_utils/test/test_copyright.py b/moveit_py_configs_utils/test/test_copyright.py deleted file mode 100644 index 95f0381..0000000 --- a/moveit_py_configs_utils/test/test_copyright.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_copyright.main import main -import pytest - - -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason="No copyright header has been placed in the generated source file.") -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=[".", "test"]) - assert rc == 0, "Found errors" diff --git a/moveit_py_configs_utils/test/test_flake8.py b/moveit_py_configs_utils/test/test_flake8.py deleted file mode 100644 index 49c1644..0000000 --- a/moveit_py_configs_utils/test/test_flake8.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, "Found %d code style errors / warnings:\n" % len(errors) + "\n".join(errors) diff --git a/moveit_py_configs_utils/test/test_pep257.py b/moveit_py_configs_utils/test/test_pep257.py deleted file mode 100644 index a2c3deb..0000000 --- a/moveit_py_configs_utils/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=[".", "test"]) - assert rc == 0, "Found code style errors / warnings"