feat: initial commit

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

.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/[email protected]
+        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

.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/[email protected]
+        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

.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/[email protected]
+        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/

.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/[email protected]
+        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/

.gitignore

@@ -0,0 +1,14 @@
+.*
+
+!.git*
+
+build
+log
+install
+
+__pycache__
+*.pyc
+
+*.cfg
+*.names
+*.weights

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).

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).

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()

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 = []

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

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)