task-1 #3

base_move/move_base_hori_line.py

@@ -245,15 +245,17 @@ def move_to_hori_line(ctrl, msg, target_distance=0.1, observe=False):
     返回:
     bool: 是否成功到达目标位置
     """
-    # # 首先校准到水平
-    # aligned = align_to_horizontal_line(ctrl, msg, observe=False)
+    # 首先校准到水平
+    print("校准到横向线水平")
+    aligned = align_to_horizontal_line(ctrl, msg, observe=observe)
     
-    # if not aligned:
-    #     print("无法校准到横向线水平，停止移动")
-    #     return False
+    if not aligned:
+        print("无法校准到横向线水平，停止移动")
+        return False
     
     # 检测横向线
-    image = cv2.imread("current_image.jpg")  # ctrl.image_processor.get_current_image()
+    # image = cv2.imread("current_image.jpg")  # TEST
+    image = ctrl.image_processor.get_current_image()
     edge_point, edge_info = detect_horizontal_track_edge(image, observe=False)
     
     if edge_point is None or edge_info is None:
@@ -280,8 +282,6 @@ def move_to_hori_line(ctrl, msg, target_distance=0.1, observe=False):
         print("已经达到目标距离，无需移动")
         return True
     
-    return True
-    
     # 设置移动命令
     msg.mode = 11  # Locomotion模式
     msg.gait_id = 26  # 自变频步态

main.py

@@ -86,8 +86,8 @@ class Robot_Ctrl(object):
 
     def msg_handler_o(self, channel, data):
         self.odo_msg = localization_lcmt().decode(data)
-        if self.odo_msg.timestamp % 100 == 0:
-            print(self.odo_msg.rpy)
+        # if self.odo_msg.timestamp % 100 == 0:
+        #     print(self.odo_msg.rpy)
 
     def rec_responce(self):
         while self.runing: