From 03a3300beb72e26b7cc336089f7a3c68a2a2d412 Mon Sep 17 00:00:00 2001
From: Raffaello Bonghi <raffaello@rnext.it>
Date: Thu, 29 Sep 2022 22:18:13 +0100
Subject: [PATCH] Improve eye detection

---
 Dockerfile                                    |  2 +-
 nanosaur_ei/launch/edge_impulse.launch.py     | 10 ++-
 nanosaur_ei/rosinstall/ei.rosinstall          |  2 +-
 .../nanosaur_ei_controller/ei_controller.py   | 67 +++++++++++++++++++
 4 files changed, 76 insertions(+), 5 deletions(-)

diff --git a/Dockerfile b/Dockerfile
index 5e7629a..ed8a29c 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -60,7 +60,7 @@ WORKDIR $ROS_WS
 
 # Build nanosaur edge impulse package
 RUN . /opt/ros/$ROS_DISTRO/setup.sh && \
-    colcon build --symlink-install \
+    colcon build --symlink-install --merge-install \
     --cmake-args \
     -DCMAKE_BUILD_TYPE=Release
 
diff --git a/nanosaur_ei/launch/edge_impulse.launch.py b/nanosaur_ei/launch/edge_impulse.launch.py
index 65644c9..d3649b9 100644
--- a/nanosaur_ei/launch/edge_impulse.launch.py
+++ b/nanosaur_ei/launch/edge_impulse.launch.py
@@ -35,16 +35,20 @@ def generate_launch_description():
         package='edgeimpulse_ros',
         executable='image_classification',
         name='image_classification',
-        namespace='nanosaur',
+        namespace='nanosaur/edge_impulse',
         parameters=[{
             'model.filepath': '/opt/ei/models/ssd/person_detection/person-detection-linux-aarch64-v42.eim',
         }],
-        remappings=[('/detection/input/image', 'camera/color/image_raw')],
+        remappings=[('/detection/input/image', '/nanosaur/camera/color/image_raw'),
+                    ('/detection/output/results', 'results'),
+                    ('/detection/output/info', 'info'),
+                    ('/detection/output/image', 'image'),
+                    ],
     )
     
     ei_controller_node = Node(
         package='nanosaur_ei_controller',
-        namespace='nanosaur',
+        namespace='nanosaur/edge_impulse',
         executable='nanosaur_ei_controller',
         name='nanosaur_ei_controller',
     )
diff --git a/nanosaur_ei/rosinstall/ei.rosinstall b/nanosaur_ei/rosinstall/ei.rosinstall
index 675b83a..93855ba 100644
--- a/nanosaur_ei/rosinstall/ei.rosinstall
+++ b/nanosaur_ei/rosinstall/ei.rosinstall
@@ -4,5 +4,5 @@
     uri: https://github.com/rnanosaur/nanosaur.git
 - git:
     local-name: edgeimpulse_ros
-    version: 0.0.3
+    version: 0.0.4
     uri: https://github.com/gbr1/edgeimpulse_ros.git
\ No newline at end of file
diff --git a/nanosaur_ei_controller/nanosaur_ei_controller/ei_controller.py b/nanosaur_ei_controller/nanosaur_ei_controller/ei_controller.py
index 09bea37..8ffb860 100644
--- a/nanosaur_ei_controller/nanosaur_ei_controller/ei_controller.py
+++ b/nanosaur_ei_controller/nanosaur_ei_controller/ei_controller.py
@@ -26,6 +26,10 @@
 
 import rclpy
 from rclpy.node import Node
+from rclpy.qos import QoSProfile
+from nanosaur_msgs.msg import Eyes
+from geometry_msgs.msg import Twist
+from vision_msgs.msg import Detection2DArray
 
 
 class Controller(Node):
@@ -34,6 +38,68 @@ def __init__(self):
         super().__init__('nanosaur_ei_controller')
         # Node started
         self.get_logger().info("Hello nanosaur_ei_controller!")
+        # Get frame size to follow
+        self.declare_parameter("frame.width", 320.0)
+        self.frame_width = self.get_parameter("frame.width").value
+        self.declare_parameter("frame.height", 320.0)
+        self.frame_height = self.get_parameter("frame.height").value
+        # Gain eyes message
+        self.declare_parameter("gain.eyes.x", 1)
+        self.gain_eyes_x = self.get_parameter("gain.eyes.x").value
+        self.declare_parameter("gain.eyes.y", 1)
+        self.gain_eyes_y = self.get_parameter("gain.eyes.y").value
+        #Init QoS
+        qos_profile = QoSProfile(depth=5)
+        # Create command Twist publisher
+        self.pub_nav_ = self.create_publisher(Twist, '/nanosaur/nav_vel', qos_profile)
+        # Create command eyes publisher
+        self.pub_eyes_ = self.create_publisher(Eyes, '/nanosaur/eyes', qos_profile)
+        # Subscribe to AprilTag Detection message
+        self.subscription = self.create_subscription(
+            Detection2DArray,
+            'results',
+            self.april_tag,
+            1)
+
+    def follower_stop(self):
+        self.pub_nav_.publish(Twist())
+        self.pub_eyes_.publish(Eyes())
+
+    def move_eyes(self, error_x, error_y):
+        eyes_msg = Eyes()
+        # Convert center to eye movement
+        eyes_msg.position.x = self.gain_eyes_x * error_x
+        eyes_msg.position.y = self.gain_eyes_y * error_y
+        # self.get_logger().info(f"[{eyes_msg.x:.0f}, {eyes_msg.y:.0f}]")
+        # Wrap to Eyes message
+        self.pub_eyes_.publish(eyes_msg)
+
+    def detect_person(self, detections):
+        person = {"detect": False, "px": 0., "py": 0., "size": 0.}
+        for detection in detections:
+            results = detection.results
+            bbox = detection.bbox
+            center = bbox.center
+            for result in results:
+                if result.id == "person":
+                    person["detect"] = True
+                    score = result.score
+                    person["px"] = center.x #-(center.x - self.frame_width / 2.0)
+                    person["py"] = center.y # (center.y - self.frame_height / 2.0)
+                    # Size tag
+                    # person["size"] = self.size_tag(detect.corners)
+                    self.get_logger().info(f"{bbox}")
+                    break
+        return person
+
+    def april_tag(self, msg):
+        person = self.detect_person(msg.detections)
+        self.get_logger().info(f"{person}")
+        px = person["px"]
+        py = person["py"]
+        # Update eyes
+        self.move_eyes(px, py)
+        return
 
 
 def main(args=None):
@@ -44,6 +110,7 @@ def main(args=None):
         rclpy.spin(controller)
     except (KeyboardInterrupt, SystemExit):
         pass
+    controller.follower_stop()
     # Destroy the node explicitly
     controller.destroy_node()
     rclpy.shutdown()