test: #2

main.py

@@ -16,8 +16,8 @@ import rclpy
 from utils.robot_control_cmd_lcmt import robot_control_cmd_lcmt
 from utils.robot_control_response_lcmt import robot_control_response_lcmt
 from utils.localization_lcmt import localization_lcmt
-from utils.image_raw import ImageProcessor
 from utils.base_msg import BaseMsg
+# from utils.image_raw import ImageProcessor
 # from utils.marker_client import MarkerRunner
 
 from task_1.task_1 import run_task_1
@@ -29,11 +29,13 @@ from task_2_5.task_2_5 import run_task_2_5, run_task_2_5_back
 from task_4.task_4 import run_task_4, run_task_4_back
 from task_test.task_left_line import run_task_test
 from task_5.task_5 import run_task_5
-from task_4.pass_bar import pass_bar
 from base_move.turn_degree import turn_degree, turn_degree_v2
 from base_move.center_on_dual_tracks import center_on_dual_tracks
 from base_move.go_to_xy import go_to_x_v2, go_to_y_v2
 
+from base_move.go_straight import go_straight
+from task_4.pass_bar import pass_bar
+
 from utils.log_helper import info
 
 pass_marker = True
@@ -52,6 +54,8 @@ def main():
 
         pass_bar(Ctrl, msg)
 
+        Ctrl.base_msg.stand_up()
+
         # time.sleep(100) # TEST,
         # run_task_1(Ctrl, msg, time_sleep=TIME_SLEEP)
 
@@ -103,7 +107,7 @@ class Robot_Ctrl(object):
         self.cmd_msg = robot_control_cmd_lcmt()
         self.rec_msg = robot_control_response_lcmt()
         self.odo_msg = localization_lcmt()
-        self.image_processor = ImageProcessor()
+        # self.image_processor = ImageProcessor()
         # DEBUG
         # self.marker_runner = MarkerRunner(pass_flag=pass_marker)
         self.send_lock = Lock()
@@ -124,7 +128,7 @@ class Robot_Ctrl(object):
         self.send_thread.start()
         self.rec_thread.start()
         self.odo_thread.start()
-        self.image_processor.run()
+        # self.image_processor.run()
         # self.marker_runner.run()
 
     def msg_handler(self, channel, data):
@@ -210,7 +214,7 @@ class Robot_Ctrl(object):
         self.runing = 0
         self.rec_thread.join()
         self.send_thread.join()
-        self.image_processor.destroy()
+        # self.image_processor.destroy()
         # 销毁 MarkerRunner
         if hasattr(self, 'marker_runner') and self.marker_runner is not None:
             try: