New line following algo (#104)

* add new algo for line_following

* fix checks

* fix checks

* fix checks

* fix checks

* add mode for recognition

* further edit

* adjust speed

* adjust speed

* finalise

* solve import error

* further fix structure

* fix type issue

* fix structrue

* fix bug

* finalise

* finalise

* fix check

README.md

@@ -355,7 +355,7 @@ ros2 launch mini_pupper_navigation navigation.launch.py map:=$HOME/map.yaml
 ### 2.2.4 Test dance
 Please refer to the README.md inside package "mini_pupper_dance".
 
-### 2.2.5 AI Image Recognition
+### 2.2.5 Image Recognition
 
 This package base on camera, and is tested utilising a package called v4l2 camera that focuses on Raspberry Pi Camera for most systems.
 
@@ -389,7 +389,7 @@ ros2 launch mini_pupper_bringup bringup.launch.py
 ```sh
 # Terminal 2 (ssh or PC)
 . ~/ros2_ws/install/setup.bash
-ros2 launch mini_pupper_recognition recognition.launch.py
+ros2 launch mini_pupper_recognition recognition.launch.py #pid:=false if want to disable pid
 ```
 
 ## License

mini_pupper_interfaces/CMakeLists.txt

@@ -17,6 +17,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/DanceCommand.srv"
   "srv/PlayMusic.srv"
   "srv/StopMusic.srv"
+  "msg/LineDetectionResult.msg"
+  "msg/AiLineRecognitionResult.msg"
   DEPENDENCIES std_msgs # Add packages that above messages depend on, in this case geometry_msgs for Sphere.msg
 )
 

mini_pupper_interfaces/msg/AiLineRecognitionResult.msg

@@ -0,0 +1,3 @@
+string direction
+string extent
+string orientation

mini_pupper_interfaces/msg/LineDetectionResult.msg

@@ -0,0 +1,2 @@
+string linear
+string angular

mini_pupper_recognition/launch/cloud_line_demo.launch.py

@@ -0,0 +1,39 @@
+#!/usr/bin/env python3
+
+# SPDX-License-Identifier: Apache-2.0
+#
+# Copyright (c) 2024 MangDang
+#
+# 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 launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    cloud_line_recognition_node = Node(
+            package="mini_pupper_recognition",
+            namespace="",
+            executable="cloud_line_recognition_node",
+            name="cloud_line_recognition_node",
+        )
+    cloud_line_following_node = Node(
+            package="mini_pupper_recognition",
+            namespace="",
+            executable="cloud_line_following_node",
+            name="cloud_line_following_node",
+        )
+    return LaunchDescription([
+        cloud_line_recognition_node,
+        cloud_line_following_node
+    ])

mini_pupper_recognition/launch/recognition.launch.py

@@ -17,23 +17,47 @@
 # limitations under the License.
 
 from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch.conditions import IfCondition, UnlessCondition
 from launch_ros.actions import Node
 
 
 def generate_launch_description():
-    ai_image_recognition_node = Node(
-            package="mini_pupper_recognition",
-            namespace="",
-            executable="ai_image_recognition_node",
-            name="ai_image_recognition_node",
-        )
-    line_following_node = Node(
-            package="mini_pupper_recognition",
-            namespace="",
-            executable="line_following_node",
-            name="line_following_node",
-        )
+    pid = LaunchConfiguration('pid')
+
+    pid_arg = DeclareLaunchArgument(
+        'pid',
+        default_value='true',
+        description='Enable pid if true'
+    )
+
+    line_detection_node = Node(
+        package="mini_pupper_recognition",
+        namespace="",
+        executable="line_detection_node",
+        name="line_detection_node",
+    )
+
+    pid_line_following_node = Node(
+        package="mini_pupper_recognition",
+        namespace="",
+        executable="pid_line_following_node",
+        name="pid_line_following_node",
+        condition=IfCondition(pid),
+    )
+
+    normal_line_following_node = Node(
+        package="mini_pupper_recognition",
+        namespace="",
+        executable="normal_line_following_node",
+        name="normal_line_following_node",
+        condition=UnlessCondition(pid),
+    )
+
     return LaunchDescription([
-        ai_image_recognition_node,
-        line_following_node
+        pid_arg,
+        line_detection_node,
+        pid_line_following_node,
+        normal_line_following_node
     ])

mini_pupper_recognition/mini_pupper_recognition/line_following_node.py → mini_pupper_recognition/mini_pupper_recognition/cloud_line_following_node.py

@@ -18,95 +18,73 @@
 
 import rclpy
 from rclpy.node import Node
-from std_msgs.msg import String
 from geometry_msgs.msg import Twist
 import time
+from mini_pupper_interfaces.msg import AiLineRecognitionResult
 
 
-class LineFollowingNode(Node):
+class CloudLineFollowingNode(Node):
     def __init__(self):
-        super().__init__('line_following_node')
-        self.extent = None
-        self.orientation = None
+        super().__init__('cloud_line_following_node')
         self.interval = 2.0
         self.speed = 0.0
         self.previous_direction = 'center'
         self.walk_previous_direction = 0
         self.previous_orientation = ''
 
-        self.direction_sub = self.create_subscription(
-            String,
-            'direction',
-            self._direction_callback,
-            10
-        )
-
-        self.extent_sub = self.create_subscription(
-            String,
-            'extent_of_movement',
-            self._extent_callback,
-            10
-        )
-
-        self.orientation_sub = self.create_subscription(
-            String,
-            'orientation_of_movement',
-            self._orientation_callback,
+        self.image_sub = self.create_subscription(
+            AiLineRecognitionResult,
+            'image',
+            self._image_callback,
             10
         )
 
         self.vel_publisher_ = self.create_publisher(Twist, 'cmd_vel', 10)
 
-    def _extent_callback(self, msg):
-        self.extent = msg.data
-
-    def _orientation_callback(self, msg):
-        self.orientation = msg.data
-
-    def _direction_callback(self, direction):
+    def _image_callback(self, msg):
 
         velocity_cmd = Twist()
 
-        if direction.data == 'left' or direction.data == 'right':
-            self.previous_direction = direction.data
+        if msg.direction == 'left' or msg.direction == 'right':
+            self.previous_direction = msg.direction
 
-        if direction.data == 'left' or direction.data == 'right':
-            if self.orientation == 'vertical' or self.orientation == 'slanted':
-                self.previous_orientation = self.orientation
+        if msg.direction == 'left' or msg.direction == 'right':
+            if msg.orientation == 'vertical' or msg.orientation == 'slanted':
+                self.previous_orientation = msg.orientation
 
-        if direction.data == 'left':
-            if self.orientation == 'vertical':
+        if msg.direction == 'left':
+            if msg.orientation == 'vertical':
                 self.speed = 0.04
-                velocity_cmd.linear.y = 0.025 * float(self.extent)
+                velocity_cmd.linear.y = 0.025 * float(msg.extent)
                 velocity_cmd.angular.z = 0.0
-            elif self.orientation == 'slanted':
+            elif msg.orientation == 'slanted':
                 self.speed = 0.03
                 velocity_cmd.linear.y = 0.0
-                velocity_cmd.angular.z = 0.4 * float(self.extent)
+                velocity_cmd.angular.z = 0.4 * float(msg.extent)
             self.vel_publisher_.publish(velocity_cmd)
             time.sleep(self.interval)
             self.walk_previous_direction = 0
 
-        elif direction.data == 'right':
-            if self.orientation == 'vertical':
+        elif msg.direction == 'right':
+            if msg.orientation == 'vertical':
                 self.speed = 0.04
-                velocity_cmd.linear.y = -0.025 * float(self.extent)
+                velocity_cmd.linear.y = -0.025 * float(msg.extent)
                 velocity_cmd.angular.z = 0.0
-            elif self.orientation == 'slanted':
+            elif msg.orientation == 'slanted':
                 self.speed = 0.03
                 velocity_cmd.linear.y = 0.0
-                velocity_cmd.angular.z = -0.4 * float(self.extent)
+                velocity_cmd.angular.z = -0.4 * float(msg.extent)
             self.vel_publisher_.publish(velocity_cmd)
             time.sleep(self.interval)
             self.walk_previous_direction = 0
 
-        elif direction.data == 'center':
+        elif msg.direction == 'center':
             self.speed = 0.04
             self.vel_publisher_.publish(velocity_cmd)
             time.sleep(1)
             self.walk_previous_direction = 0
 
-        elif direction.data == '' and self.walk_previous_direction < 4:
+        elif msg.direction == '' and self.walk_previous_direction < 4:
 
             self.walk_previous_direction += 1
 
@@ -128,7 +106,7 @@ def _direction_callback(self, direction):
                 self.vel_publisher_.publish(velocity_cmd)
                 time.sleep(self.interval)
 
-        elif direction.data == '' and self.walk_previous_direction >= 4:
+        elif msg.direction == '' and self.walk_previous_direction >= 4:
 
             velocity_cmd.angular.z = 0.0
             self.vel_publisher_.publish(velocity_cmd)
@@ -156,7 +134,7 @@ def _direction_callback(self, direction):
 
 def main(args=None):
     rclpy.init(args=args)
-    node = LineFollowingNode()
+    node = CloudLineFollowingNode()
     rclpy.spin(node)
     node.destroy_node()
     rclpy.shutdown()

mini_pupper_recognition/mini_pupper_recognition/ai_image_recognition_node.py → mini_pupper_recognition/mini_pupper_recognition/cloud_line_recognition_node.py

@@ -26,7 +26,7 @@
 from langchain_google_vertexai import ChatVertexAI
 from langchain_core.messages import HumanMessage
 from io import BytesIO
-from std_msgs.msg import String
+from mini_pupper_interfaces.msg import AiLineRecognitionResult
 
 
 def extract_keyword_constant(input_string):
@@ -48,7 +48,7 @@ def extract_keyword_constant(input_string):
     return keyword, proportion, orientation
 
 
-def ai_image_response(llm, image, text):
+def cloud_image_response(llm, image, text):
     buffered = BytesIO()
     image.save(buffered, format="JPEG")
     image_bytes = buffered.getvalue()
@@ -72,13 +72,11 @@ def ai_image_response(llm, image, text):
     return result
 
 
-class AiImageResponse(Node):
+class CloudLineResponse(Node):
     def __init__(self):
-        super().__init__('mini_pupper_maze_service')
+        super().__init__('cloud_line_response')
         self.sub = self.create_subscription(Image, '/image_raw', self.image_recognition, 10)
-        self.direction_publisher_ = self.create_publisher(String, 'direction', 10)
-        self.extent_publisher_ = self.create_publisher(String, 'extent_of_movement', 10)
-        self.orientation_publisher_ = self.create_publisher(String, 'orientation_of_movement', 10)
+        self.image_publisher_ = self.create_publisher(AiLineRecognitionResult, 'image', 10)
 
     def image_recognition(self, msg):
         direction_input_prompt = """
@@ -145,29 +143,20 @@ def image_recognition(self, msg):
         image = PIL.Image.fromarray(cv2.cvtColor(cv_img, cv2.COLOR_BGR2RGB))
 
         direction_response = extract_keyword_constant(
-            ai_image_response(multi_model, image=image, text=direction_input_prompt)
+            cloud_image_response(multi_model, image=image, text=direction_input_prompt)
         )
         self.get_logger().info(f"Direction response: {direction_response}")
 
-        message1 = String()
-        direction = direction_response[0]
-        message1.data = direction
-        self.direction_publisher_.publish(message1)
-
-        message2 = String()
-        extent = str(direction_response[1])
-        message2.data = extent
-        self.extent_publisher_.publish(message2)
-
-        message3 = String()
-        orientation = direction_response[2]
-        message3.data = orientation
-        self.orientation_publisher_.publish(message3)
+        message = AiLineRecognitionResult()
+        message.direction = direction_response[0]
+        message.extent = str(direction_response[1])
+        message.orientation = direction_response[2]
+        self.image_publisher_.publish(message)
 
 
 def main():
     rclpy.init()
-    minimal_service = AiImageResponse()
+    minimal_service = CloudLineResponse()
     rclpy.spin(minimal_service)