diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md new file mode 100644 index 0000000..a85b791 --- /dev/null +++ b/.github/pull_request_template.md @@ -0,0 +1,27 @@ +## Jira Link: + +## Description + +Describe problems, if any, clearly and concisely. +Describe any changes that have been made in this pull request. + +## Type of Change + +- [ ] Bugfix +- [ ] Enhancement +- [ ] New feature +- [ ] Breaking change (fix or feature that would cause the existing functionality to not work as expected) + +## How Has This Been Tested? + +- [ ] New unit tests added. +- [ ] Manual tested. + +## Checklist: + +- [ ] Using Branch Name Convention + - `feature/JIRA-ID-SHORT-DESCRIPTION` if has a JIRA ticket + - `enhancement/SHORT-DESCRIPTION` if has/has no JIRA ticket and contain enhancement + - `hotfix/SHORT-DESCRIPTION` if the change doesn't need to be tested (urgent) +- [ ] I have commented on my code, particularly in hard-to-understand areas. +- [ ] I have made the documentation for the corresponding changes. \ No newline at end of file diff --git a/.github/workflows/build-and-test-nightly.yml b/.github/workflows/build-and-test-nightly.yml new file mode 100644 index 0000000..9b96fee --- /dev/null +++ b/.github/workflows/build-and-test-nightly.yml @@ -0,0 +1,28 @@ +name: Build and Test Nightly +on: + workflow_dispatch: + pull_request: + branches: [master] + push: + branches: [master] +jobs: + build-and-test-nightly: + runs-on: ubuntu-latest + steps: + - name: Checkout this repository + uses: actions/checkout@v2.3.4 + with: + path: ninshiki_py + + - name: Add nightly Debian repository and rosdep sources list + run: | + sudo apt update && sudo apt install curl + curl -s http://repository.ichiro-its.org/debian/setup-nightly.bash | bash -s + curl -s http://repository.ichiro-its.org/rosdep/setup.bash | bash -s + + - name: Install Modules + run: | + pip install wget tflite-runtime tflite-support + + - name: Build and test workspace + uses: ichiro-its/ros2-build-and-test-action@main diff --git a/.github/workflows/build-and-test-stable.yml b/.github/workflows/build-and-test-stable.yml new file mode 100644 index 0000000..1afdb37 --- /dev/null +++ b/.github/workflows/build-and-test-stable.yml @@ -0,0 +1,26 @@ +name: Build and Test Stable +on: + workflow_dispatch: + push: + branches: [master] +jobs: + build-and-test-stable: + runs-on: ubuntu-latest + steps: + - name: Checkout this repository + uses: actions/checkout@v2.3.4 + with: + path: gyakuenki + + - name: Add stable Debian repository and rosdep sources list + run: | + sudo apt update && sudo apt install curl + curl -s http://repository.ichiro-its.org/debian/setup.bash | bash -s + curl -s http://repository.ichiro-its.org/rosdep/setup.bash | bash -s + + - name: Install Modules + run: | + pip install wget tflite-runtime tflite-support + + - name: Build and test workspace + uses: ichiro-its/ros2-build-and-test-action@main diff --git a/.github/workflows/build-debian-nightly.yml b/.github/workflows/build-debian-nightly.yml new file mode 100644 index 0000000..acb99be --- /dev/null +++ b/.github/workflows/build-debian-nightly.yml @@ -0,0 +1,46 @@ +name: Deploy Debian Nightly +on: + workflow_dispatch: + push: + branches: [master] +jobs: + deploy-debian-nightly: + runs-on: ubuntu-latest + steps: + - name: Checkout this repository + uses: actions/checkout@v2.3.4 + with: + path: gyakuenki + + - name: Add nightly Debian repository and rosdep sources list + run: | + sudo apt update && sudo apt install curl + curl -s http://repository.ichiro-its.org/debian/setup-nightly.bash | bash -s + curl -s http://repository.ichiro-its.org/rosdep/setup.bash | bash -s + - name: Build nightly Debian package + uses: ichiro-its/ros2-build-debian-action@main + with: + unique-version: true + + - name: Deploy nightly Debian package to server + uses: appleboy/scp-action@master + with: + host: ${{ secrets.SSH_HOST }} + port: ${{ secrets.SSH_PORT }} + username: ${{ secrets.SSH_USER }} + password: ${{ secrets.SSH_PASS }} + source: "package/*.deb" + target: "~/temp/nightly/gyakuenki/" + rm: true + + - name: Prepare nightly Debian package in the server + uses: appleboy/ssh-action@master + with: + host: ${{ secrets.SSH_HOST }} + port: ${{ secrets.SSH_PORT }} + username: ${{ secrets.SSH_USER }} + password: ${{ secrets.SSH_PASS }} + script: | + cd ${{ secrets.SERVER_REPO_DIR }}/debian + reprepro includedeb nightly ~/temp/nightly/gyakuenki/package/*.deb + rm -rf ~/temp/nightly/gyakuenki/ diff --git a/.github/workflows/build-debian-stable.yml b/.github/workflows/build-debian-stable.yml new file mode 100644 index 0000000..d987089 --- /dev/null +++ b/.github/workflows/build-debian-stable.yml @@ -0,0 +1,44 @@ +name: Deploy Debian Stable +on: + workflow_dispatch: + release: + types: [created] +jobs: + deploy-debian-stable: + runs-on: ubuntu-latest + steps: + - name: Checkout this repository + uses: actions/checkout@v2.3.4 + with: + path: gyakuenki + + - name: Add stable Debian repository and rosdep sources list + run: | + sudo apt update && sudo apt install curl + curl -s http://repository.ichiro-its.org/debian/setup.bash | bash -s + curl -s http://repository.ichiro-its.org/rosdep/setup.bash | bash -s + - name: Build stable Debian package + uses: ichiro-its/ros2-build-debian-action@main + + - name: Deploy stable Debian package to server + uses: appleboy/scp-action@master + with: + host: ${{ secrets.SSH_HOST }} + port: ${{ secrets.SSH_PORT }} + username: ${{ secrets.SSH_USER }} + password: ${{ secrets.SSH_PASS }} + source: "package/*.deb" + target: "~/temp/stable/gyakuenki/" + rm: true + + - name: Prepare stable Debian package in the server + uses: appleboy/ssh-action@master + with: + host: ${{ secrets.SSH_HOST }} + port: ${{ secrets.SSH_PORT }} + username: ${{ secrets.SSH_USER }} + password: ${{ secrets.SSH_PASS }} + script: | + cd ${{ secrets.SERVER_REPO_DIR }}/debian + reprepro includedeb stable ~/temp/stable/gyakuenki/package/*.deb + rm -rf ~/temp/stable/gyakuenki/ diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..b2dd736 --- /dev/null +++ b/.gitignore @@ -0,0 +1,14 @@ +.* + +!.git* + +build +log +install + +__pycache__ +*.pyc + +*.cfg +*.names +*.weights diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 0000000..9eee72f --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,3 @@ +Any contribution that you make to this repository will +be under the MIT license, as dictated by that +[license](https://opensource.org/licenses/MIT). diff --git a/README.md b/README.md new file mode 100644 index 0000000..d336382 --- /dev/null +++ b/README.md @@ -0,0 +1,9 @@ +# Gyakuenki +[![latest version](https://img.shields.io/github/v/release/ichiro-its/ninshiki.svg)](https://github.com/ichiro-its/gyakuenki/releases/) +[![license](https://img.shields.io/github/license/ichiro-its/gyakuenki.svg)](./LICENSE) + +This package implements Inverse Perspective Mapping for the [ROS 2](https://docs.ros.org/en/foxy/index.html) soccer project using Python. + +## License + +This project is licensed under [the MIT License](./LICENSE). diff --git a/gyakuenki/__init__.py b/gyakuenki/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/gyakuenki/gyakuenki_main.py b/gyakuenki/gyakuenki_main.py new file mode 100644 index 0000000..6ac7db6 --- /dev/null +++ b/gyakuenki/gyakuenki_main.py @@ -0,0 +1,18 @@ +import rclpy +from rclpy.node import Node + +from gyakuenki.node.gyakuenki_node import GyakuenkiNode + +def main(): + rclpy.init() + + node = Node('gyakuenki') + gyakuenki_node = GyakuenkiNode(node) + + rclpy.spin(gyakuenki_node.node) + + gyakuenki_node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/gyakuenki/node/gyakuenki_node.py b/gyakuenki/node/gyakuenki_node.py new file mode 100644 index 0000000..423f1ab --- /dev/null +++ b/gyakuenki/node/gyakuenki_node.py @@ -0,0 +1,74 @@ +import rclpy +import tf2_ros as tf2 + +from rclpy.duration import Duration +from ipm_library.ipm import IPM +from gyakuenki.gyakuenki.utils.projections import map_detected_objects +from ninshiki_interfaces.msg import DetectedObjects, Contours +from gyakuenki_interfaces.msg import ProjectedObjects + +class GyakuenkiNode: + def __init__(self, node: rclpy.node.Node): + self.node = node + self.projected_objects = [] + self.time_stamp = self.node.get_clock().now() + + # Parameters + self.declare_parameter('gaze_frame', 'gaze') + self.declare_parameter('base_footprint_frame', 'base_footprint') + self.declare_parameter('detection_topic_dnn', 'ninshiki_cpp/dnn_detection') + self.declare_parameter('detection_topic_color', 'ninshiki_cpp/color_detection') + + # Subscribers and Publishers + self.dnn_objects_subscriber = self.node.create_subscription(DetectedObjects, self.get_parameter('detection_topic_dnn').value, self.dnn_detection_callback, 10) + self.color_objects_subscriber = self.node.create_subscription(Contours, self.get_parameter('detection_topic_color').value, self.color_detection_callback, 10) + self.projected_objects_publisher = self.node.create_publisher(ProjectedObjects, 'projected_objects', 10) # TODO: determine published data + + self.get_logger().info('Subscribed to ' + self.get_parameter('detection_topic').value) + + # TF2 + self.tf_buffer = tf2.Buffer(cache_time=Duration(seconds=30.0)) + self.tf_listener = tf2.TransformListener(self.tf_buffer, self.node) + + # Create the IPM instance + self.ipm = IPM() + + # Pipelines + # Callback for dnn detection subscriber + def dnn_detection_callback(self, msg: DetectedObjects): + detection_type = 'dnn' + + projected_dnn_objects = map_detected_objects( + msg, + detection_type, + self.time_stamp, + self.ipm, + self.get_parameter('base_footprint_frame').value, + self.get_parameter('gaze_frame').value, + self.get_logger()) + + self.projected_objects.extend(projected_dnn_objects) + + # Callback for color detection subscriber + def color_detection_callback(self, msg: Contours): + detection_type = 'color' + + projected_color_objects = map_detected_objects( + msg, + detection_type, + self.time_stamp, + self.ipm, + self.get_parameter('base_footprint_frame').value, + self.get_parameter('gaze_frame').value, + self.get_logger()) + + self.projected_objects.extend(projected_color_objects) + + # Publishes the projected objects + def publish_projected_objects(self): + projected_objects_msg = ProjectedObjects() + projected_objects_msg.objects = self.projected_objects + + self.projected_objects_publisher.publish(projected_objects_msg) + # remove all elements in projected_objects + self.projected_objects = [] diff --git a/gyakuenki/utils/projections.py b/gyakuenki/utils/projections.py new file mode 100644 index 0000000..f50362b --- /dev/null +++ b/gyakuenki/utils/projections.py @@ -0,0 +1,65 @@ +from ipm_library.ipm import IPM +from rclpy.impl.rcutils_logger import RcutilsLogger +from soccer_ipm.utils import create_horizontal_plane +from ninshiki_interfaces.msg import DetectedObject, DetectedObjects +from builtin_interfaces.msg import Time +from rclpy.impl.rcutils_logger import RcutilsLogger +from gyakuenki.utils.utils import create_horizontal_plane, get_object_center +from gyakuenki_interfaces import ProjectedObject, ProjectedObjects + +def map_detected_objects( + detected_objects: DetectedObjects, + detection_type: str, + time_stamp: Time, + ipm: IPM, + base_footprint_frame: str, + gaze_frame: str, + logger: RcutilsLogger) -> ProjectedObjects: + """ + Map a given array of 2D ball detections onto the ground plane. + + :param detected_objects: The 2D message that should be mapped + :param ipm: An instance of the IPM mapping utility + :param base_footprint: The tf frame of the field + :param gaze_frame: The tf frame of the gaze + :param logger: A ros logger to display warnings etc. + :param ball_diameter: The diameter of the balls that are mapped + :returns: The balls as 3D cartesian detections in the output_frame + """ + object_relative = ProjectedObject() + objects_relative = ProjectedObjects() + + detected_object: DetectedObject + for detected_object in detected_objects: + try: + if detected_object.score > 0.5: + if detected_object.label == 'ball': + object_diameter = 0.153 + elif detected_object.label == 'marking': # TODO: check marking label + object_diameter = 0.0 + + object_center = get_object_center(detected_object, detection_type) + elevated_field = create_horizontal_plane(object_diameter / 2) + + transformed_object = ipm.map_point( + elevated_field, + object_center, + time_stamp, + base_footprint_frame, + gaze_frame, + ) + + object_relative.center.x = transformed_object.point.x + object_relative.center.y = transformed_object.point.y + object_relative.center.z = transformed_object.point.z + object_relative.confidence = detected_object.score + if detection_type == 'dnn': + object_relative.label = detected_object.label + else: + object_relative.label = detected_object.name + + objects_relative.object.append(object_relative) + except: + logger.warn("Failed to map object") + + return objects_relative diff --git a/gyakuenki/utils/utils.py b/gyakuenki/utils/utils.py new file mode 100644 index 0000000..011f720 --- /dev/null +++ b/gyakuenki/utils/utils.py @@ -0,0 +1,32 @@ +from ninshiki_interfaces.msg import DetectedObject +from shape_msgs.msg import Plane +from vision_msgs.msg import Point2D + +def create_horizontal_plane( + height_offset: float = 0.0) -> Plane: + """Create a plane message for a given frame at a given time, with a given height offset.""" + plane = Plane() + plane.coef[2] = 1.0 # Normal in z direction + plane.coef[3] = -height_offset # Distance above the ground + return plane + +def get_object_center( + object: DetectedObject, + detection_type: str) -> Point2D: + """Get the center of a detected object.""" + + if detection_type == 'dnn': + x = (object.right - object.left) / 2 + y = (object.bottom - object.top) / 2 + else: + contour = object.contour + + min_x = min(contour, key=lambda p: p.x).x + max_x = max(contour, key=lambda p: p.x).x + min_y = min(contour, key=lambda p: p.y).y + max_y = max(contour, key=lambda p: p.y).y + + x = (max_x - min_x) / 2 + y = (max_y - min_y) / 2 + + return Point2D(x=x, y=y) diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..ef2152d --- /dev/null +++ b/package.xml @@ -0,0 +1,32 @@ + + + + gyakuenki + 0.0.0 + Inverse Perspective Mapping package for soccer legacy code. + Hanun Shaka + MIT License + + geometry_msgs + ipm_library + sensor_msgs + shape_msgs + soccer_vision_2d_msgs + soccer_vision_3d_msgs + std_msgs + tf2_geometry_msgs + tf2_sensor_msgs + tf2 + vision_msgs + ninshiki_interfaces + gyakuenki_interfaces + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/resource/gyakuenki b/resource/gyakuenki new file mode 100644 index 0000000..e69de29 diff --git a/setup.py b/setup.py new file mode 100644 index 0000000..7cee746 --- /dev/null +++ b/setup.py @@ -0,0 +1,26 @@ +from setuptools import find_packages, setup + +package_name = 'gyakuenki' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='nuna', + maintainer_email='hanunshaka02@gmail.com', + description='Inverse Perspective Mapping package for soccer legacy code.', + license='MIT License', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'gyakuenki = gyakuenki.gyakuenki_main:main', + ], + }, +) diff --git a/test/test_copyright.py b/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/test/test_copyright.py @@ -0,0 +1,25 @@ +# 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/test/test_flake8.py b/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/test/test_flake8.py @@ -0,0 +1,25 @@ +# 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/test/test_pep257.py b/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/test/test_pep257.py @@ -0,0 +1,23 @@ +# 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'