experimental feature: ball prediction added

catkin_ws/src/ball_tracking/ball_tracking_exp.launch

@@ -0,0 +1,23 @@
+<launch>
+    <!-- ArUco Tag Detection Node -->
+    <node name="camera_publisher" pkg="ball_tracking" type="camera_publisher.py" output="screen" />
+
+    <!-- ArUco Tag Detection Node -->
+    <node name="aruco_area" pkg="ball_tracking" type="aruco_area.py" output="screen" />
+
+    <!-- Ball Tracking Node -->
+    <node name="tracker_aruco" pkg="ball_tracking" type="tracker_aruco.py" output="screen" />
+    
+    <!-- Ball Tracking Node -->
+    <node name="tracker_aruco_idk" pkg="ball_tracking" type="tracker_aruco_idk.py" output="screen" />
+    
+    <!-- Ball Tracking Node -->
+    <node name="paddle_action" pkg="ball_tracking" type="paddle_action.py" output="screen" />
+    
+    <!-- Ball Tracking Node -->
+    <node name="ball_prediction" pkg="ball_tracking" type="ball_prediction.py" output="screen" />
+    
+    <!-- Ball Tracking Node -->
+    <node name="prediction_monitor" pkg="ball_tracking" type="prediction_monitor.py" output="screen" />
+</launch>
+

catkin_ws/src/ball_tracking/src/experimental/ball_prediction.py

@@ -0,0 +1,58 @@
+#!/usr/bin/env python3
+
+import rospy
+from geometry_msgs.msg import Point
+from std_msgs.msg import Header
+import numpy as np
+
+class BallMovementPredictor:
+    def __init__(self):
+        rospy.init_node('ball_movement_predictor', anonymous=True)
+        
+        self.ball_positions = []
+        self.forecast_length = 5  # Predict the ball's position 5 steps into the future
+        
+        # Subscriber to the ball's current position
+        self.ball_sub = rospy.Subscriber('/ball_coordinates', Point, self.ball_callback)
+        
+        # Publisher for the ball's forecasted future position
+        self.forecast_pub = rospy.Publisher('/ball_forecast', Point, queue_size=10)
+
+    def ball_callback(self, msg):
+        # Update the list of recent ball positions
+        self.ball_positions.append((msg.x, msg.y))
+        
+        # Keep only the last N positions for prediction
+        if len(self.ball_positions) > 5:
+            self.ball_positions.pop(0)
+        
+        # Predict the ball's future position and publish it
+        forecasted_position = self.predict_future_position()
+        if forecasted_position is not None:
+            forecast_msg = Point(x=forecasted_position[0], y=forecasted_position[1], z=0)
+            self.forecast_pub.publish(forecast_msg)
+
+    def predict_future_position(self):
+        if len(self.ball_positions) < 2:
+            return None  # Not enough data to predict
+
+        # Simple linear prediction based on the last two positions
+        x_positions, y_positions = zip(*self.ball_positions)
+        x_velocity = x_positions[-1] - x_positions[-2]
+        y_velocity = y_positions[-1] - y_positions[-2]
+
+        forecasted_x = x_positions[-1] + x_velocity * self.forecast_length
+        forecasted_y = y_positions[-1] + y_velocity * self.forecast_length
+
+        return (forecasted_x, forecasted_y)
+
+    def run(self):
+        rospy.spin()
+
+if __name__ == '__main__':
+    try:
+        predictor = BallMovementPredictor()
+        predictor.run()
+    except rospy.ROSInterruptException:
+        pass
+

catkin_ws/src/ball_tracking/src/experimental/prediction_monitor.py

@@ -0,0 +1,54 @@
+#!/usr/bin/env python3
+
+import rospy
+import cv2
+from cv_bridge import CvBridge
+from sensor_msgs.msg import Image
+from geometry_msgs.msg import Point
+
+class BallPredictionVisualizer:
+    def __init__(self):
+        rospy.init_node('ball_prediction_visualizer', anonymous=True)
+
+        self.bridge = CvBridge()
+        self.forecasted_position_aruco = None
+        self.aruco_origin = None
+
+        self.image_sub = rospy.Subscriber('/camera_image', Image, self.image_callback)
+        self.forecast_sub = rospy.Subscriber('/ball_forecast', Point, self.forecast_callback)
+        self.origin_sub = rospy.Subscriber('/aruco_origin', Point, self.origin_callback)
+
+    def forecast_callback(self, msg):
+        self.forecasted_position_aruco = (int(msg.x), int(msg.y))
+
+    def origin_callback(self, msg):
+        self.aruco_origin = (int(msg.x), int(msg.y))
+
+    def image_callback(self, msg):
+        cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
+
+        if self.aruco_origin:
+            # Draw the ArUco origin for verification
+            cv2.circle(cv_image, self.aruco_origin, 10, (0, 255, 0), -1)  # Green dot for the origin
+
+        if self.forecasted_position_aruco and self.aruco_origin:
+            # Adjust for the origin and possibly invert the y-axis
+            adjusted_position = (
+                self.forecasted_position_aruco[0] + self.aruco_origin[0],
+                self.forecasted_position_aruco[1] + self.aruco_origin[1]
+            )
+            cv2.circle(cv_image, adjusted_position, 10, (0, 0, 255), -1)  # Red dot for the predicted position
+
+        cv2.imshow("Ball Prediction", cv_image)
+        cv2.waitKey(1)
+
+    def run(self):
+        rospy.spin()
+
+if __name__ == '__main__':
+    try:
+        visualizer = BallPredictionVisualizer()
+        visualizer.run()
+    except rospy.ROSInterruptException:
+        cv2.destroyAllWindows()
+