fixed some bug on contours

catkin_ws/src/asclinic_pkg/src/find_target2.py

@@ -61,15 +61,16 @@ class TargetFinder:
             self.servo_control.publish(msg)
             sleep(2)
             number[i] = self.analysis(i)
-            
-        area_max = max(number, key=lambda x: x[0])
-        i = number.index(area_max)
-        self.targets_xy.append([area_max[1], area_max[2]])
-        msgn = ServoPulseWidth(channel=3, pulse_width_in_microseconds=self.angles[i])
-        self.servo_control.publish(msgn)
-        sleep(2)
-        ##Then take picture
-        self.save_picture_control.publish(UInt32(1))
+        
+        if sum(map(lambda x: x[0], number)) != 0:  # Check if there is any non-zero area
+            area_max = max(number, key=lambda x: x[0])
+            i = number.index(area_max)
+            self.targets_xy.append([area_max[1], area_max[2]])
+            msgn = ServoPulseWidth(channel=3, pulse_width_in_microseconds=self.angles[i])
+            self.servo_control.publish(msgn)
+            sleep(2)
+            # Then take picture
+            self.save_picture_control.publish(UInt32(1))
         
         #image_now = self.cv_image
         #cv2.imshow("Image", image_now)
@@ -124,8 +125,11 @@ class TargetFinder:
                     else:
                         contour_data.append([0, rx, ry])
         
-        max_contour = max(contour_data, key=lambda x: x[0])
-        return max_contour
+            if contour_data:
+                max_contour = max(contour_data, key=lambda x: x[0])
+                return max_contour
+            
+        return [0, 0, 0]
 
 
     def calculate_coordinate(self, cx, cy, cphi):

catkin_ws/src/asclinic_pkg/src/position_plotter.py

@@ -60,8 +60,9 @@ def main():
 
     while not rospy.is_shutdown():
         ax.clear()
-        ax.plot(x_data, y_data, 'b-', label='Path')
         ax.plot(ox, oy, 'ro', label='Obstacles')  # Plot obstacles
+        ax.plot(x_data, y_data, 'b-', label='Path')
+        
         ax.set_xlabel('X Position')
         ax.set_ylabel('Y Position')
         ax.set_title('Robot Position Plot')