''' This demo show the communication interface of MR813 motion control board based on Lcm. Dependency: - robot_control_cmd_lcmt.py - robot_control_response_lcmt.py 雷达参数已经能够读取 替换basic_motion_test_comb中的main.py 运行代码之前需要执行: cd /home/cyberdog_sim source /opt/ros/galactic/setup.bash 修改代码内容： 删去全局变量 odo_rec 使用self.odo_msg代替 删去全局变量 motion_rec 使用self.rec_msg代替 err_handler函数里面的 motion_rec['switch_status'] 改为 self.rec_msg.switch_status walk_forward函数中的 left_dist 名称改为 exp_dist odo_changeback 更名为 odo_verticalturn 删去了无用的读锁 self.handle_lock selaction 更名为 selfdef_action side_equal 更名为 bothside_align while ... return ... 改 if 的函数 ： odo_verticalturn walk_forward rightside_align 删去 turnleft_corner 函数（我当时写了一半没接着写了 speedump_and_circle 更名为 speedbump_and_circle （打错了 我写的大多数函数前面都加了些简单注释 路径/home/mi/Downloads/123/ ''' import lcm import sys import os import time from threading import Thread, Lock from robot_control_cmd_lcmt import robot_control_cmd_lcmt from robot_control_response_lcmt import robot_control_response_lcmt from localization_lcmt import localization_lcmt import toml import copy import math from file_send_lcmt import file_send_lcmt import rclpy from rclpy.executors import MultiThreadedExecutor import numpy from rclpy.node import Node from sensor_msgs.msg import LaserScan,Image from rclpy.qos import QoSProfile, qos_profile_sensor_data from pprint import pprint import cv2 from cv_bridge import CvBridge # 自定义步态发送数据的结构 robot_cmd = { 'mode': 0, 'gait_id': 0, 'contact': 0, 'life_count': 0, 'vel_des': [0.0, 0.0, 0.0], 'rpy_des': [0.0, 0.0, 0.0], 'pos_des': [0.0, 0.0, 0.0], 'acc_des': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 'ctrl_point': [0.0, 0.0, 0.0], 'foot_pose': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 'step_height': [0.0, 0.0], 'value': 0, 'duration': 0 } PI = 3.1416 # R = 0.5 # T = 10 # 雷达数据 laser_rec = { 'data': [0] * 500 } # 激光雷达订阅 class LaserScanSubscriber(Node): def __init__(self): super().__init__('laser_scan_subscriber') # 使用SensorDataQoS作为QoS配置文件 qos_profile = qos_profile_sensor_data # 这就是SensorDataQoS self.subscription = self.create_subscription( LaserScan, '/mi_desktop_48_b0_2d_7a_fb_86/scan', # 替换为你的激光雷达数据发布的话题名 self.listener_callback, qos_profile) def listener_callback(self, msg): global laser_rec laser_rec['data'] = msg.ranges.tolist() time.sleep(3) class RGB_Subscriber(Node): def __init__(self): super().__init__('RGB_subscriber') qos_profile = qos_profile_sensor_data self.subscription = self.create_subscription( Image, '/image_rgb', self.listener_callback, qos_profile) self.bridge = CvBridge() #虚拟环境red_range = [20, 35], green_range = [50, 80] def find_green_location(self, image, red_range = [130, 140], green_range = [180, 185]): #调参部分1################################### #8.12下午13：30测试：红布:red_range = [180, 185], green_range = [120, 130] #8.12下午13：30测试：绿布:red_range = [130, 140], green_range = [180, 185] height, width = image.shape[:2] central_part = [2 * width // 8, 4 * width // 8, 0, 4 * height // 8] #调参部分2 central_image = image[central_part[2]:central_part[3], central_part[0]:central_part[1]] # cv2.imshow("image",central_image) # cv2.waitKey(0) _,g,r = cv2.split(central_image) mean_green = numpy.mean(g) mean_red = numpy.mean(r) if green_range[0] <= mean_green <= green_range[1] and red_range[0] <= mean_red <= red_range[1]: # 如果x坐标的平均值大于图像宽度的一半 return 1 else: return -1 def mid_before_bridge(): return def listener_callback(self, msg): # Convert the ROS Image message to an OpenCV image cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8') # cv2.imwrite('image_red.jpg',cv_image) self.RGB_res = self.find_green_location(cv2.imread("./image_green.jpg")) # cv2.imwrite("image.jpg",cv_image) time.sleep(3) class Robot_Ctrl(object): def __init__(self): # 反馈线程初始化 self.rec_thread = Thread(target=self.rec_responce) self.send_thread = Thread(target=self.send_publish) self.odo_thread = Thread(target=self.rec_responce_o) self.lc_r = lcm.LCM("udpm://239.255.76.67:7670?ttl=255") self.lc_s = lcm.LCM("udpm://239.255.76.67:7671?ttl=255") self.cmd_msg = robot_control_cmd_lcmt() self.rec_msg = robot_control_response_lcmt() self.odo_msg = localization_lcmt() self.send_lock = Lock() self.delay_cnt = 0 self.mode_ok = 0 self.gait_ok = 0 self.runing = 1 self.RGB_res = 0 self.odo_bias = 0 ####有bias之后，原先0度位置顺时针旋转bias(即变换到原bias位置) self.lc_o = lcm.LCM("udpm://239.255.76.67:7667?ttl=255") ###里程计 self.lcm_cmd = lcm.LCM("udpm://239.255.76.67:7671?ttl=255") self.lcm_usergait = lcm.LCM("udpm://239.255.76.67:7671?ttl=255") self.usergait_msg = file_send_lcmt() rclpy.init(args=None) self.laser_scan_subscriber = LaserScanSubscriber() self.RGB_subscriber = RGB_Subscriber() self.executor = MultiThreadedExecutor() self.laser_thread = Thread(target=self.spin_func, args=(self.laser_scan_subscriber,)) self.RGB_thread = Thread(target=self.spin_func, args=(self.RGB_subscriber,)) self.camera_sh = Thread(target = os.system, args=('sh /home/mi/Downloads/motion/ros2.sh',)) def spin_func(self, subscriber): self.executor.add_node(subscriber) self.executor.spin() time.sleep(0.02) def run_rgb(self): os.system('sh ./rgb_start.sh') self.RGB_thread.start() def run(self): self.lc_r.subscribe("robot_control_response", self.msg_handler) self.lc_o.subscribe("global_to_robot", self.msg_handler_o) ###里程计订阅话题 self.send_thread.start() self.rec_thread.start() self.odo_thread.start() ###启动里程计 self.laser_thread.start() self.camera_sh.start() def msg_handler(self, channel, data): self.rec_msg = robot_control_response_lcmt().decode(data) if (self.rec_msg.order_process_bar >= 95): self.mode_ok = self.rec_msg.mode else: self.mode_ok = 0 def msg_handler_o(self, channel, data): ###里程计解码函数，被按照频率调用 self.odo_msg = localization_lcmt().decode(data) def rec_responce(self): while self.runing: self.lc_r.handle() time.sleep(0.002) def rec_responce_o(self): ###里程计 while self.runing: self.lc_o.handle() time.sleep(0.002) def selfdef_action(self, mode): # 自定义步态选择参数 try: self.send_lock.acquire() if mode == 0: # 石子路+上下坡 steps = toml.load("/home/mi/Downloads/motion/Gait_Params_walk.toml") elif mode == 1: # 待定 steps = toml.load( "/home/mi/Downloads/motion/Gait_Params_highwalk.toml") full_steps = {'step': [robot_cmd]} k = 0 for i in steps['step']: cmd = copy.deepcopy(robot_cmd) cmd['duration'] = i['duration'] if i['type'] == 'usergait': cmd['mode'] = 11 # LOCOMOTION cmd['gait_id'] = 110 # USERGAIT cmd['vel_des'] = i['body_vel_des'] cmd['rpy_des'] = i['body_pos_des'][0:3] cmd['pos_des'] = i['body_pos_des'][3:6] cmd['foot_pose'][0:2] = i['landing_pos_des'][0:2] cmd['foot_pose'][2:4] = i['landing_pos_des'][3:5] cmd['foot_pose'][4:6] = i['landing_pos_des'][6:8] cmd['ctrl_point'][0:2] = i['landing_pos_des'][9:11] cmd['step_height'][0] = math.ceil(i['step_height'][0] * 1e3) + math.ceil( i['step_height'][1] * 1e3) * 1e3 cmd['step_height'][1] = math.ceil(i['step_height'][2] * 1e3) + math.ceil( i['step_height'][3] * 1e3) * 1e3 cmd['acc_des'] = i['weight'] cmd['value'] = i['use_mpc_traj'] cmd['contact'] = math.floor(i['landing_gain'] * 1e1) cmd['ctrl_point'][2] = i['mu'] if k == 0: full_steps['step'] = [cmd] else: full_steps['step'].append(cmd) k = k + 1 if mode == 0: # 石子路+上下坡 f = open("/home/mi/Downloads/motion/Gait_Params_walk_full.toml", 'w') elif mode == 1: # 待定 f = open("/home/mi/Downloads/motion/Gait_Params_highwalk_full.toml", 'w') f.write("# Gait Params\n") f.writelines(toml.dumps(full_steps)) f.close() if mode == 0: # 石子路+上下坡 file_obj_gait_def = open( "/home/mi/Downloads/motion/Gait_Def_walk.toml", 'r') file_obj_gait_params = open( "/home/mi/Downloads/motion/Gait_Params_walk_full.toml", 'r') elif mode == 1: # 待定 file_obj_gait_def = open( "/home/mi/Downloads/motion/Gait_Def_highwalk.toml", 'r') file_obj_gait_params = open( "/home/mi/Downloads/motion/Gait_Params_highwalk_full.toml", 'r') self.usergait_msg.data = file_obj_gait_def.read() self.lcm_usergait.publish("user_gait_file", self.usergait_msg.encode()) time.sleep(0.5) self.usergait_msg.data = file_obj_gait_params.read() self.lcm_usergait.publish("user_gait_file", self.usergait_msg.encode()) time.sleep(0.1) file_obj_gait_def.close() file_obj_gait_params.close() if mode == 0: # 石子路+上下坡 user_gait_list = open( "/home/mi/Downloads/motion/Usergait_List.toml", 'r') if mode == 1: # 石子路+上下坡 user_gait_list = open( "/home/mi/Downloads/motion/Usergait_List1.toml", 'r') steps = toml.load(user_gait_list) for step in steps['step']: self.cmd_msg.mode = step['mode'] self.cmd_msg.value = step['value'] self.cmd_msg.contact = step['contact'] self.cmd_msg.gait_id = step['gait_id'] self.cmd_msg.duration = step['duration'] self.cmd_msg.life_count += 1 for i in range(3): self.cmd_msg.vel_des[i] = step['vel_des'][i] self.cmd_msg.rpy_des[i] = step['rpy_des'][i] self.cmd_msg.pos_des[i] = step['pos_des'][i] self.cmd_msg.acc_des[i] = step['acc_des'][i] self.cmd_msg.acc_des[i + 3] = step['acc_des'][i + 3] self.cmd_msg.foot_pose[i] = step['foot_pose'][i] self.cmd_msg.ctrl_point[i] = step['ctrl_point'][i] for i in range(2): self.cmd_msg.step_height[i] = step['step_height'][i] self.lcm_cmd.publish("robot_control_cmd", self.cmd_msg.encode()) time.sleep(0.1) if mode == 0: # 石子路+上下坡 for i in range( 550): # 10s Heat beat It is used to maintain the heartbeat when life count is not updated self.lcm_cmd.publish("robot_control_cmd", self.cmd_msg.encode()) # self.err_handler(msg = robot_control_cmd_lcmt()) time.sleep(0.2) if mode == 1: # 台阶 for i in range(50): # 10s Heat beat It is used to maintain the heartbeat when life count is not updated self.lcm_cmd.publish("robot_control_cmd", self.cmd_msg.encode()) # self.err_handler(msg = robot_control_cmd_lcmt()) time.sleep(0.2) self.send_lock.release() except KeyboardInterrupt: self.cmd_msg.mode = 7 # PureDamper before KeyboardInterrupt: self.cmd_msg.gait_id = 0 self.cmd_msg.duration = 0 self.cmd_msg.life_count += 1 self.lcm_cmd.publish("robot_control_cmd", self.cmd_msg.encode()) self.send_lock.release() pass def Wait_finish(self, mode, gait_id, timeout=2000): # timeout // 0.005 = 多少秒超时 count = 0 while self.runing and count < timeout: # 10s if self.mode_ok == mode and self.gait_ok == gait_id: return True else: time.sleep(0.005) count += 1 def send_publish(self): while self.runing: self.send_lock.acquire() if self.delay_cnt > 20: # Heartbeat signal 10HZ, It is used to maintain the heartbeat when life count is not updated self.lc_s.publish("robot_control_cmd", self.cmd_msg.encode()) self.delay_cnt = 0 self.delay_cnt += 1 self.send_lock.release() time.sleep(0.005) def Send_cmd(self, msg): self.send_lock.acquire() self.delay_cnt = 50 self.cmd_msg = msg self.send_lock.release() def quit(self): self.runing = 0 self.rec_thread.join() self.send_thread.join() self.odo_thread.join() self.laser_thread.join() self.camera_sh.join() def quit_rgb(self): os.system('sh ./rgb_end.sh') self.RGB_thread.join() def jump(self, msg, duration, type): # 跳跃 msg.mode = 16 # Locomotion msg.gait_id = type # 0左跳 1前跳 3右跳 6原地跳 msg.duration = duration # Zero duration means continuous motion until a new command is used. # Continuous motion can interrupt non-zero duration interpolation motion msg.life_count += 1 self.Send_cmd(msg) self.Wait_finish(16, 1) time.sleep(0.1) def Recovery_stand(self, msg):#恢复站立 msg.mode = 12 # Recovery stand msg.gait_id = 0 msg.life_count += 1 # Command will take effect when life_count update self.Send_cmd(msg) self.Wait_finish(12, 0) def Pure_Damp(self, msg, mode): msg.mode = 7 # PureDamp msg.gait_id = mode # 0为自然倒下，1为受控倒下 msg.life_count += 1 # Command will take effect when life_count update self.Send_cmd(msg) self.Wait_finish(12, 0) def circle(self, msg, R=0.6, T=12): # 半径，时间 msg.mode = 11 # Locomotion msg.gait_id = 10 # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26 msg.vel_des = [PI * R * 2 / T, 0, 2 * PI / T] # forward left/rightmove rotate msg.duration = T * 550 # Zero duration means continuous motion until a new command is used. # Continuous motion can interrupt non-zero duration interpolation motion msg.step_height = [0.06, 0.06] # ground clearness of swing leg msg.life_count += 1 msg.rpy_des = [0, 0.0, 0] self.Send_cmd(msg) self.Wait_finish(11, 10) def stone(self, msg): self.Recovery_stand(msg) self.selfdef_action(0) time.sleep(0.1) # def locomotion(s) def slope(self, msg): self.Recovery_stand(msg) self.selfdef_action(1) def err_handler(self, msg): if self.rec_msg.switch_status == 3: # 如果状态为高阻尼保护模式 msg.mode = 12 # Recovery stand msg.gait_id = 0 msg.life_count += 1 # Command will take effect when life_count update self.Send_cmd(msg) self.Wait_finish(12, 0) # 雷达以及里程计 # 探测直线的时候需要的二分查找 def bio_select(self, k, b, start, end, dist_limit, data_with_order_x, data_with_order_y): left, right = start, end - 1 # 二分查找 while left < right: mid = (left + right) // 2 x_mid, y_mid = data_with_order_x[mid], data_with_order_y[mid] # 计算点到直线的距离 distance = abs(k * x_mid - y_mid + b) / (k ** 2 + 1) ** 0.5 if distance <= dist_limit: # 点在直线附近，继续向右搜索 left = mid + 1 else: # 点不在直线附近，缩小搜索范围 right = mid # 返回直线附近点的范围 return left def detect_lines(self,max_limit = 12): # data 实际雷达扫描的数组 #### 此探测直线函数只在实体环境中使用！！！ 此函数只在实体环境中使用！！！ 此函数只在实体环境中使用！！！ try: dist_limit = 0.16 backward_limit = 0.04 init_points = 40 data_with_order_x = [] data_with_order_y = [] data = laser_rec['data'] len_data = len(data) # print("data#####################################",data) # 过滤无效值 for i in range(len_data): if data[i] < 0.08 or data[i] > max_limit: continue # data_with_order.append([math.radians(i-90) - degree,data[i]]) #新坐标系里面的偏转角（弧度）+ 距离 data_with_order_x.append(data[i] * math.cos(math.radians(i * 180 / len_data - 90))) data_with_order_y.append(data[i] * math.sin(math.radians(i * 180 / len_data - 90))) # x y 轴 len_filtered = len(data_with_order_x) # print(len(data_with_order_x)) # for i in range(0, len(data_with_order_x)): # print('(', end='') # print(data_with_order_x[i], data_with_order_y[i], sep=' ,', end='') # print('),', end='\n') # 储存直线方程 lines = [] # 拐动位置 corners = [0] while corners[-1] < len_filtered - init_points - 1: start = corners[-1] k1_init, b_init = numpy.polyfit(data_with_order_x[start:start + init_points], data_with_order_y[start:start + init_points], 1) # print(k1_init, b_init) # 二分查找 end 是范围内最后一个点 end = self.bio_select(k1_init, b_init, start, len_filtered, dist_limit, data_with_order_x, data_with_order_y) # 初次允许偏差稍大，可以防止有需要的点没能落在直线上，即升温 k1_fixed, b1_fixed = numpy.polyfit(data_with_order_x[start:end], data_with_order_y[start:end], 1) dist_bias = (abs(k1_fixed * data_with_order_x[end] - data_with_order_y[end] + b1_fixed) / (k1_fixed ** 2 + 1) ** 0.5) ######################################复活赛贴上来： if dist_bias < dist_limit: end_new = self.bio_select(k1_fixed, b1_fixed, end, len_filtered, dist_limit, data_with_order_x, data_with_order_y) k1_fixed, b1_fixed = numpy.polyfit(data_with_order_x[start:end_new], data_with_order_y[start:end_new], 1) else: end_new = end ######################################复活赛结束 # ###进行退火，降低限制的范围，找到更加精确的点 while dist_bias >= backward_limit: end_new -= 2 dist_bias = (abs(k1_fixed * data_with_order_x[end_new] - data_with_order_y[end_new] + b1_fixed) / (k1_fixed ** 2 + 1) ** 0.5) # 重新拟合直线 k1_fixed, b1_fixed = numpy.polyfit(data_with_order_x[start:end_new], data_with_order_y[start:end_new], 1) corners.append(end_new) # print(end) lines.append([k1_fixed, b1_fixed]) if len(lines) >= 3: # print(corners) print(lines) break for i in lines: # print(f'{i[0]}x - y + {i[1]} = 0') print(f'[{i[0]}, {i[1]}],') if len(lines)>=2: return lines,corners # else: # time.sleep(0.04) # return self.detect_lines() except: time.sleep(0.04) return self.detect_lines(max_limit = max_limit) # def detect_lines(self): # data 180个的数组 # try: # dist_limit = 0.12 # backward_limit = 0.04 # init_points = 5 # data_with_order_x = [] # data_with_order_y = [] # data = laser_rec['data'] # # 过滤无效值 # for i in range(180): # if data[i] < 0.12 or data[i] > 12: # continue # # data_with_order.append([math.radians(i-90) - degree,data[i]]) #新坐标系里面的偏转角（弧度）+ 距离 # data_with_order_x.append(data[i] * math.cos(math.radians(i - 90))) # data_with_order_y.append(data[i] * math.sin(math.radians(i - 90))) # x y 轴 # len_filtered = len(data_with_order_x) # # print(len(data_with_order_x)) # # for i in range(0, len(data_with_order_x)): # # print('(', end='') # # print(data_with_order_x[i], data_with_order_y[i], sep=' ,', end='') # # print('),', end='\n') # # 储存直线方程 # lines = [] # # 拐动位置 # corners = [0] # while corners[-1] < len_filtered - init_points - 1: # start = corners[-1] # k1_init, b_init = numpy.polyfit(data_with_order_x[start:start + init_points], # data_with_order_y[start:start + init_points], 1) # # print(k1_init, b_init) # # 二分查找 end 是范围内最后一个点 # end = self.bio_select(k1_init, b_init, start, len_filtered, dist_limit, data_with_order_x, # data_with_order_y) # # 初次允许偏差稍大，可以防止有需要的点没能落在直线上，即升温 # k1_fixed, b1_fixed = numpy.polyfit(data_with_order_x[start:end], data_with_order_y[start:end], 1) # dist_bias = (abs(k1_fixed * data_with_order_x[end] - data_with_order_y[end] + b1_fixed) / # (k1_fixed ** 2 + 1) ** 0.5) # end_new = end # # ###进行退火，降低限制的范围，找到更加精确的点 # while dist_bias >= backward_limit: # end_new -= 1 # dist_bias = (abs(k1_fixed * data_with_order_x[end_new] - data_with_order_y[end_new] + b1_fixed) / # (k1_fixed ** 2 + 1) ** 0.5) # # 重新拟合直线 # k1_fixed, b1_fixed = numpy.polyfit(data_with_order_x[start:end_new], data_with_order_y[start:end_new], 1) # corners.append(end_new) # # print(end) # lines.append([k1_fixed, b1_fixed]) # if len(lines) >= 3: # print(corners) # break # # for i in lines: # # # print(f'{i[0]}x - y + {i[1]} = 0') # # print(f'[{i[0]}, {i[1]}],') # return lines # ###try后面是原来的代码 # except: # time.sleep(0.04) # return self.detect_lines() ########该函数用右侧直线来作为基准，重新设置前面的正方向为里程计的 exp_angel 角度，默认设置为0度 ########在使用此函数前，调用odo_verticalturn()先大致方向对正，再使用此函数微调 ########在使用此函数后，调用odo_verticalturn(target = exp_angel)可以使机械狗正对前方 def odo_parallel_line(self, exp_angel=0,manu_bias = 0): lines = self.detect_lines()[0] k = lines[0][0] k_angel = math.atan(k) ###反正切算角度 now_angel = self.odo_msg.rpy[2] ###现在的里程计角度 # now_bias = self.odo_bias ###现在的偏置角度 self.odo_bias = now_angel + k_angel - exp_angel + manu_bias # now_angel + k_angel 是以里程计0位 为基准的直线方向角度 # - exp_angel之后得到的就是 里程计0位 和 期望0位 夹角 # ####对于直角而言，修改后的里程计转动函数更加精确，一次性可以转到对应位置 def circle_dist(self,target,location): ###3.1416 -3.1416 是同一个值 value1 = abs(target - location) value2 = 6.2832 - value1 direction1 = 1 if target > location else 0 # ##按value1转动，1为逆时针 0 为顺时针 if value1 < value2: return direction1,value1 else: return 1-direction1,value2 def odo_verticalturn(self, target, msg, limit=0.04, timesleep_s=5): ##允许误差0.04弧度，大概是2.29度 target = target + self.odo_bias if target < -3.14159: target += 3.14159 if target > 3.14159: target -= 3.14159 # ###############坐标系转换回原来的值 const_int = 2470 # 转 1.57 弧度 大概要 3875 duration 每弧度大概这个值 持续时间大概6.5秒 loc = self.odo_msg.rpy[2] direction, dist = self.circle_dist(target=target, location=loc) print('odo_verticalturn', self.odo_msg.rpy[2], target) if abs(dist) > limit: msg.mode = 11 # Locomotion msg.gait_id = 26 # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26 msg.vel_des = [0, 0, 0.5 if direction > 0 else -0.5] # 转向 msg.duration = int(const_int * abs(dist)) # Continuous motion can interrupt non-zero duration interpolation motion msg.step_height = [0.06, 0.06] # ground clearness of swing leg msg.life_count += 1 self.Send_cmd(msg) time.sleep(7 * abs(dist) / 1.57) # print('1 times finish angle',self.odo_msg.rpy[2]) # return dist = target - self.odo_msg.rpy[2] print('odo_verticalturn', self.odo_msg.rpy[2], target) #####左右两侧的直线都可以扫描到时，可以使用这个函数来在路中对齐 #####offset以左为正向，偏离中线的距离 def bothside_align(self, msg, limit=0.08, off_set=0.0, timeout=600): ## 允许最大误差0.08米，即离最中线左右偏向4厘米 ###先里程计回正再算距离 const_int = 6666 # 对 差的距离 进行倍数放大 控制左右移动时间 lines = self.detect_lines()[0] while len(lines) < 3: time.sleep(0.04) lines = self.detect_lines()[0] dist1 = round(abs(lines[0][1] / ((lines[0][0] * lines[0][0] + 1)) ** 0.5), 3) dist2 = round(abs(lines[2][1] / ((lines[2][0] * lines[2][0] + 1)) ** 0.5), 3) # print(f'before move to equal: r :{dist1} l: {dist2}') dist_diff = dist1 - dist2 - 2 * off_set while abs(dist_diff) > limit: msg.mode = 11 # Locomotion msg.gait_id = 27 msg.vel_des = [0, -0.15, 0] if dist_diff > 0 else [0, 0.15, 0] msg.duration = int(const_int * abs(dist_diff) / 2) msg.step_height = [0.06, 0.06] # ground clearness of swing leg msg.life_count += 1 msg.rpy_des = [0, 0.3, 0] self.Send_cmd(msg) time.sleep(msg.duration / 1000 + 0.5) time.sleep(3) lines = self.detect_lines()[0] while len(lines) < 3: time.sleep(0.04) lines = self.detect_lines()[0] dist1 = round(abs(lines[0][1] / ((lines[0][0] * lines[0][0] + 1)) ** 0.5), 3) dist2 = round(abs(lines[2][1] / ((lines[2][0] * lines[2][0] + 1)) ** 0.5), 3) dist_diff = dist1 - dist2 ####前向运动函数需要在里程计回正之后才能使用 def walk_forward(self, msg, exp_dist, limit=0.04, timeout=1200, k_bound=5): ##在对正之后才能调用，exp_dist指定前面预留多长的距离 const_int = 1667 # 对 差的距离 进行倍数放大 控制前进时间 lines = self.detect_lines()[0] dist = round(abs(lines[1][1] / ((lines[1][0] * lines[1][0] + 1)) ** 0.5), 3) print(dist) # for i in lines: # print(i) while (len(lines) <= 1 or (lines[1][0] < k_bound and lines[1][0] > -k_bound)): time.sleep(0.04) lines = self.detect_lines()[0] dist = round(abs(lines[1][1] / ((lines[1][0] * lines[1][0] + 1)) ** 0.5), 3) dist_need_walk = dist - exp_dist if abs(dist_need_walk) > limit: msg.mode = 11 # Locomotion msg.gait_id = 27 msg.vel_des = [0.6 if dist_need_walk > 0 else -0.6, 0, 0] msg.duration = int(const_int * abs(dist_need_walk)) msg.step_height = [0.06, 0.06] # ground clearness of swing leg msg.life_count += 1 self.Send_cmd(msg) self.Wait_finish(11, 27, timeout=timeout) lines = self.detect_lines()[0] dist = round(abs(lines[1][1] / ((lines[1][0] * lines[1][0] + 1)) ** 0.5), 3) # while (len(lines) <= 1 or (lines[1][0] -k_bound)): # time.sleep(0.04) # lines = self.detect_lines() # dist = round(abs(lines[1][1] / ((lines[1][0] * lines[1][0] + 1)) ** 0.5),3) # dist_need_walk = dist - exp_dist print('walk_forward dis_after', dist) ####对部分地形，只使用扫描到的右侧直线即对正赛道，expect_dist为指定离右边直线的距离 def rightside_align(self, msg, expect_dist=0.65, limit=0.04, timeout=800): const_int = 5000 lines = self.detect_lines()[0] dist = round(abs(lines[0][1] / ((lines[0][0] * lines[0][0] + 1)) ** 0.5), 3) dist_need_walk = dist - expect_dist if abs(dist_need_walk) > limit: msg.mode = 11 # Locomotion msg.gait_id = 27 msg.vel_des = [0, -0.2 if dist_need_walk > 0 else 0.2, 0] msg.duration = int(const_int * abs(dist_need_walk)) msg.step_height = [0.06, 0.06] # ground clearness of swing leg msg.life_count += 1 self.Send_cmd(msg) self.Wait_finish(11, 27, timeout=timeout) lines = self.detect_lines()[0] dist = round(abs(lines[0][1] / ((lines[0][0] * lines[0][0] + 1)) ** 0.5), 3) # dist_need_walk = dist - expect_dist # print('right align dis_after', dist) ###此函数用于小角度的纠正，精确程度更加高 ###timesleep_s单位是秒，指定一次回正之后需要间隔多久，建议在verticval_turn之后使用，更加精确 def odo_tinyturn(self, target, msg, limit=0.04, timesleep_s=5): const_int = 1200 dist = target - self.odo_msg.rpy[2] print('odo_tinyturn', self.odo_msg.rpy[2], target) while abs(dist) > limit: msg.mode = 11 # Locomotion msg.gait_id = 26 # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26 msg.vel_des = [0, 0, 0.5 if dist > 0 else -0.5] # 转向 msg.duration = int(const_int * abs(dist)) # Continuous motion can interrupt non-zero duration interpolation motion msg.step_height = [0.06, 0.06] # ground clearness of swing leg msg.life_count += 1 self.Send_cmd(msg) time.sleep(timesleep_s) # print('1 times finish angle',self.odo_msg.rpy[2]) # return dist = target - self.odo_msg.rpy[2] print('odo_tinyturn', self.odo_msg.rpy[2], target) ###在第二个拐角对齐之后，可以调用这个函数。直接通过减速带和圆柱 # def speedbump_and_circle(self, msg): # const_int1 = 5000 #######在走向圆柱的情况下，使用的duration常数 # const_int2 = 5000 #######在侧向靠近圆柱，进入赛道位置的情况下，使用的duration常数 # y_sharp_dist = -0.08 #######给最后的对齐使用的参数 绝对值是对正之后拐点和狗y轴的差距 正常要负数 # x_sharp_dist = 0.30 #######给最后的对齐使用的参数 是对正之后拐点和狗x轴的差距 # #####开环走过大部分减速带 # msg.mode = 11 # Locomotion # msg.gait_id = 26 # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26 # msg.vel_des = [0.2, 0, 0] # 直走 # msg.rpy_des = [0, 0, 0] # # Zero duration means continuous motion until a new command is used. # msg.duration = 14000 # # Continuous motion can interrupt non-zero duration interpolation motion # msg.step_height = [0.06, 0.06] # ground clearness of swing leg # msg.life_count += 1 # self.Send_cmd(msg) # time.sleep(24) # ###找到方向处理剩余的不确定距离，主要是向半径方向靠近圆柱 # time.sleep(0.04) # laser_keep = laser_rec['data'] # idx_min = laser_keep[70:110].index(min(laser_keep[70:110])) + 70 # 最小值点 # dist_min = laser_keep[idx_min] # angel_diff = idx_min / 90 * 1.57 # # print(idx_min, dist_min, angel_diff) # if dist_min > 0.15: # msg.mode = 11 # Locomotion # msg.gait_id = 26 # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26 # msg.vel_des = [0.1 * math.sin(angel_diff), 0.1 * math.cos(angel_diff), 0] # 直走 # msg.rpy_des = [0, 0, 0] # # Zero duration means continuous motion until a new command is used. # msg.duration = int((dist_min - 0.15) * const_int1) # # Continuous motion can interrupt non-zero duration interpolation motion # msg.step_height = [0.06, 0.06] # ground clearness of swing leg # msg.life_count += 1 # self.Send_cmd(msg) # # print('tiny walk') # ####转向正右边，回正 # time.sleep(0.5) # self.odo_verticalturn(1.5708, msg=msg) # self.odo_tinyturn(target=1.5708, msg=msg) # time.sleep(3) # ###########################找右边直线和圆环赛道的交界点，以这个点为基准去进行对齐 # ###########################多次扫描去除错误点 # laser_valid = [0] * 180 # for i in range(10): # time.sleep(0.04) # laser = laser_rec['data'] # for j in range(0, 180): # if laser_valid[j] == 0 and laser[j] > 0.15: # laser_valid[j] = laser[j] # ##########################根据X轴的值去找到拐角点，序号idx # data_with_order_x = [] # data_with_order_y = [] # for i in range(180): # if laser_valid[i] < 0.12 or laser_valid[i] > 12: # continue # # data_with_order.append([math.radians(i-90) - degree,data[i]]) #新坐标系里面的偏转角（弧度）+ 距离 # data_with_order_x.append(laser_valid[i] * math.cos(math.radians(i - 90))) # data_with_order_y.append(laser_valid[i] * math.sin(math.radians(i - 90))) # x y 轴 # len_filtered = len(data_with_order_x) # # print('len_filtered',len_filtered) # # print(data_with_order_x) # idx = -1 # i = 60 # while i < len_filtered - 10: # cnt = 0 # for k in range(i + 1, i + 9): # if data_with_order_x[k] > data_with_order_x[k - 1]: # cnt += 1 # else: # break # if cnt > 6: # idx = i # break # i += 1 # ###################读出拐角点的值 # point_sharp_x = data_with_order_x[idx] # point_sharp_y = data_with_order_y[idx] # # print('idx pt_sharp_x', idx, point_sharp_x) # # print('idx pt_sharp_y', idx, point_sharp_y) # time.sleep(1) # #####根据拐角点矫正方位，先在y轴上矫正 # msg.mode = 11 # Locomotion # msg.gait_id = 26 # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26 # msg.vel_des = [0, 0.2 if (point_sharp_y - y_sharp_dist) > 0 else -0.2, 0] # msg.rpy_des = [0, 0, 0] # # Zero duration means continuous motion until a new command is used. # msg.duration = int(const_int2 * abs(y_sharp_dist - point_sharp_y)) # # Continuous motion can interrupt non-zero duration interpolation motion # msg.step_height = [0.06, 0.06] # ground clearness of swing leg # msg.life_count += 1 # self.Send_cmd(msg) # time.sleep(5) # ####再矫正x轴 # msg.mode = 11 # Locomotion # msg.gait_id = 26 # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26 # msg.vel_des = [-0.2 if (point_sharp_x - x_sharp_dist) < 0 else 0.2, 0, 0] # msg.rpy_des = [0, 0, 0] # # Zero duration means continuous motion until a new command is used. # msg.duration = int(const_int2 * abs(point_sharp_x - x_sharp_dist)) # # Continuous motion can interrupt non-zero duration interpolation motion # msg.step_height = [0.06, 0.06] # ground clearness of swing leg # msg.life_count += 1 # self.Send_cmd(msg) # time.sleep(5) # #####绕圈出去 # msg.mode = 11 # Locomotion # msg.gait_id = 26 # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26 # msg.vel_des = [0.165, 0, 0.3] # 转向 # msg.duration = 12000 # # Continuous motion can interrupt non-zero duration interpolation motion # msg.step_height = [0.06, 0.06] # ground clearness of swing leg # msg.life_count += 1 # self.Send_cmd(msg) # time.sleep(20) def speedbump(self, msg): expect_wall_angel = -1.57 ####面相墙的方向的角度，即绕环开始前方向 const_int1 = 5000 #######在走向圆柱的情况下，使用的duration常数 const_int2 = 5000 #######在侧向靠近圆柱，进入赛道位置的情况下，使用的duration常数 #####开环走过大部分减速带 msg.mode = 11 # Locomotion msg.gait_id = 26 # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26 msg.vel_des = [0.2, 0, 0] # 直走 msg.rpy_des = [0, 0, 0] # Zero duration means continuous motion until a new command is used. msg.duration = 12000 # Continuous motion can interrupt non-zero duration interpolation motion msg.step_height = [0.06, 0.06] # ground clearness of swing leg msg.life_count += 1 self.Send_cmd(msg) time.sleep(18) ###找到方向处理剩余的不确定距离，主要是向半径方向靠近圆柱 time.sleep(0.04) laser_keep = laser_rec['data'] len_data = len(laser_keep) idx_min = laser_keep[210:330].index(min(laser_keep[210:330])) + 210 # 最小值点 dist_min = laser_keep[idx_min] angel_diff = idx_min / 270 * 1.57 ###算出最短距离上的弧度制 # print(idx_min, dist_min, angel_diff) if dist_min > 0.15: msg.mode = 11 # Locomotion msg.gait_id = 26 # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26 msg.vel_des = [0.1 * math.sin(angel_diff), 0.1 * math.cos(angel_diff), 0] # 直走 msg.rpy_des = [0, 0, 0] # Zero duration means continuous motion until a new command is used. msg.duration = int((dist_min - 0.15) * const_int1) # Continuous motion can interrupt non-zero duration interpolation motion msg.step_height = [0.06, 0.06] # ground clearness of swing leg msg.life_count += 1 self.Send_cmd(msg) # print('tiny walk') ####转向正右边，回正 time.sleep(0.5) def pass_circle(self,msg): expect_wall_angel = 1.57 ####面相墙的方向的角度，即绕环开始前方向 y_sharp_dist = 0.02 #######给最后的对齐使用的参数 绝对值是对正之后拐点和狗y轴的差距 正常要负数 x_sharp_dist = 0.45 #######给最后的对齐使用的参数 是对正之后拐点和狗x轴的差距 dist_limit = 0.20 #######计算点到直线的阈值，用来找到拐角点 acc_points = 6 #######累计这么多点的时候判断到了弯道 self.odo_verticalturn(expect_wall_angel, msg=msg) # self.odo_tinyturn(target=1.5708, msg=msg) time.sleep(3) ###########################找右边直线和圆环赛道的交界点，以这个点为基准去进行对齐 ###########################多次扫描去除错误点 # laser_valid = [0] * 450 # for i in range(10): # time.sleep(0.04) # laser = laser_rec['data'] # for j in range(0, 450): # if laser_valid[j] == 0 and laser[j] > 0.10: # laser_valid[j] = laser[j] laser_valid = laser_rec['data'] ##########################根据X轴的值去找到拐角点，序号idx data_with_order_x = [] data_with_order_y = [] for i in range(450): if laser_valid[i] < 0.06 or laser_valid[i] > 12: continue # data_with_order.append([math.radians(i-90) - degree,data[i]]) #新坐标系里面的偏转角（弧度）+ 距离 data_with_order_x.append(laser_valid[i] * math.cos(math.radians(i * 0.36 - 90))) data_with_order_y.append(laser_valid[i] * math.sin(math.radians(i * 0.36 - 90))) # x y 轴 len_filtered = len(data_with_order_x) # -----------------------------------------------------新的获取idx的直线扫描方法 lines_corners = self.detect_lines(max_limit = 6) while abs(lines_corners[0][0][0]) < 7: ###直线 第一条 k time.sleep(0.04) lines_corners = self.detect_lines(max_limit = 6) lines = lines_corners[0] corners = lines_corners[1] dists = [0] * len_filtered # cnt = 0 # idx = 250 # for _ in range(250, min(len_filtered, 450)): # #############abs(kx + b - y) / k ** 2 + 1 # dist = abs(lines[0][0] * data_with_order_x[_] + lines[0][1] - data_with_order_y[_]) / ((lines[0][0] ** 2 + 1) ** 0.5) # if dist > dist_limit: ####距离太大的时候计数 # dists[_] = dist # cnt += 1 # else: # cnt = 0 # if cnt >= acc_points: # idx = _ - int(1.5 * acc_points) # break idx = corners[1] point_sharp_x = data_with_order_x[idx] point_sharp_y = data_with_order_y[idx] print('idx pt_sharp_x', idx, point_sharp_x) print('idx pt_sharp_y', idx, point_sharp_y) for i in range(0, len(data_with_order_x)): print('(', end='') print(data_with_order_x[i], data_with_order_y[i], sep=' ,', end='') print('),', end='\n') print('lines',lines[0]) print('len_filtered',len_filtered) print('绕障前侧墙距离',dists[250:]) # -----------------------------------------------------新的获取idx的直线扫描方法 time.sleep(1) #####根据拐角点矫正方位，先在y轴上矫正 msg.mode = 11 # Locomotion msg.gait_id = 26 # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26 msg.vel_des = [0, 0.2 if (point_sharp_y - y_sharp_dist) > 0 else -0.2, 0] msg.rpy_des = [0, 0, 0] # Zero duration means continuous motion until a new command is used. msg.duration = int(const_int2 * abs(y_sharp_dist - point_sharp_y)) # Continuous motion can interrupt non-zero duration interpolation motion msg.step_height = [0.06, 0.06] # ground clearness of swing leg msg.life_count += 1 self.Send_cmd(msg) time.sleep(5) ####再矫正x轴 msg.mode = 11 # Locomotion msg.gait_id = 26 # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26 msg.vel_des = [-0.2 if (point_sharp_x - x_sharp_dist) < 0 else 0.2, 0, 0] msg.rpy_des = [0, 0, 0] # Zero duration means continuous motion until a new command is used. msg.duration = int(const_int2 * abs(point_sharp_x - x_sharp_dist)) # Continuous motion can interrupt non-zero duration interpolation motion msg.step_height = [0.06, 0.06] # ground clearness of swing leg msg.life_count += 1 self.Send_cmd(msg) time.sleep(5) # ----------------------------------------------------- return # ----------------------------------------------------- #####绕圈出去 msg.mode = 11 # Locomotion msg.gait_id = 26 # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26 msg.vel_des = [0.165, 0, 0.3] # 转向 msg.duration = 12000 # Continuous motion can interrupt non-zero duration interpolation motion msg.step_height = [0.06, 0.06] # ground clearness of swing leg msg.life_count += 1 self.Send_cmd(msg) time.sleep(20) self.odo_verticalturn(expect_wall_angel + 1.5708, msg=msg) # Main function def pass_cloth(Ctrl ,msg ,pos = -1): # Ctrl.Recovery_stand(msg) msg.mode = 11 # Locomotion msg.gait_id = 27 msg.vel_des = [0.3, 0, 0] msg.duration = 1000 #3000 msg.step_height = [0.06, 0.06] # ground clearness of swing leg msg.life_count += 1 Ctrl.Send_cmd(msg) time.sleep(10) Ctrl.rightside_align(msg=msg,expect_dist=0.9) Ctrl.run_rgb() if pos < 0: print(Ctrl.RGB_res) if Ctrl.RGB_res > 0: Ctrl.rightside_align(msg=msg,expect_dist=0.9) else: Ctrl.rightside_align(msg=msg,expect_dist=0.4) pos = 1 else: if Ctrl.RGB_res > 0: Ctrl.rightside_align(msg=msg,expect_dist=0.4) else: Ctrl.rightside_align(msg=msg,expect_dist=0.9) pos = -1 msg.mode = 11 # Locomotion msg.gait_id = 27 msg.vel_des = [0.3, 0, 0] msg.duration = 3333 msg.step_height = [0.06, 0.06] # ground clearness of swing leg msg.life_count += 1 Ctrl.Send_cmd(msg) time.sleep(10) if pos < 0: print(Ctrl.RGB_res) if Ctrl.RGB_res > 0: Ctrl.rightside_align(msg=msg,expect_dist=0.9) else: Ctrl.rightside_align(msg=msg,expect_dist=0.4) pos = 1 else: if Ctrl.RGB_res > 0: Ctrl.rightside_align(msg=msg,expect_dist=0.4) else: Ctrl.rightside_align(msg=msg,expect_dist=0.9) pos = -1 msg.mode = 11 # Locomotion msg.gait_id = 27 msg.vel_des = [0.3, 0, 0] msg.duration = 3333 msg.step_height = [0.06, 0.06] # ground clearness of swing leg msg.life_count += 1 Ctrl.Send_cmd(msg) time.sleep(10) if pos < 0: print(Ctrl.RGB_res) if Ctrl.RGB_res > 0: Ctrl.rightside_align(msg=msg,expect_dist=0.9) else: Ctrl.rightside_align(msg=msg,expect_dist=0.4) pos = 1 else: if Ctrl.RGB_res > 0: Ctrl.rightside_align(msg=msg,expect_dist=0.4) else: Ctrl.rightside_align(msg=msg,expect_dist=0.9) pos = -1 msg.mode = 11 # Locomotion msg.gait_id = 27 msg.vel_des = [0.3, 0, 0] msg.duration = 3333 msg.step_height = [0.06, 0.06] # ground clearness of swing leg msg.life_count += 1 Ctrl.Send_cmd(msg) time.sleep(10) if pos < 0: print(Ctrl.RGB_res) if Ctrl.RGB_res > 0: Ctrl.rightside_align(msg=msg,expect_dist=0.9) else: Ctrl.rightside_align(msg=msg,expect_dist=0.4) pos = 1 else: if Ctrl.RGB_res > 0: Ctrl.rightside_align(msg=msg,expect_dist=0.4) else: Ctrl.rightside_align(msg=msg,expect_dist=0.9) pos = -1 msg.mode = 11 # Locomotion msg.gait_id = 27 msg.vel_des = [0.3, 0, 0] msg.duration = 3333 msg.step_height = [0.06, 0.06] # ground clearness of swing leg msg.life_count += 1 Ctrl.Send_cmd(msg) time.sleep(10) if pos < 0: print(Ctrl.RGB_res) if Ctrl.RGB_res > 0: Ctrl.rightside_align(msg=msg,expect_dist=0.9) else: Ctrl.rightside_align(msg=msg,expect_dist=0.4) pos = 1 else: if Ctrl.RGB_res > 0: Ctrl.rightside_align(msg=msg,expect_dist=0.4) else: Ctrl.rightside_align(msg=msg,expect_dist=0.9) pos = -1 msg.mode = 11 # Locomotion msg.gait_id = 27 msg.vel_des = [0.3, 0, 0] msg.duration = 3333 msg.step_height = [0.06, 0.06] # ground clearness of swing leg msg.life_count += 1 Ctrl.Send_cmd(msg) Ctrl.quit_rgb() time.sleep(10) def main(): Ctrl = Robot_Ctrl() Ctrl.run() msg = robot_control_cmd_lcmt() try: ############################################################################################################################# Ctrl.Recovery_stand(msg) # msg.mode = 11 # Locomotion # msg.gait_id = 26 # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26 # msg.vel_des = [0.2, 0, 0] # msg.rpy_des = [0, 0, 0] # msg.pos_des = [0, 0, 0.1] # # Zero duration means continuous motion until a new command is used. # msg.duration = 10000 # # Continuous motion can interrupt non-zero duration interpolation motion # msg.step_height = [0.15, 0.15] # ground clearness of swing leg # msg.life_count += 1 # Ctrl.Send_cmd(msg) # time.sleep(5) # time.sleep(3) # pass_cloth(Ctrl, msg) ############################################################################################################################# # Ctrl.RGB_res = Ctrl.RGB_subscriber.find_green_location(image = cv2.imread("./image_red.jpg")) # print(Ctrl.RGB_res) # Ctrl.selfdef_action(1) # Ctrl.circle(msg) # 上石子路 # Ctrl.odo_verticalturn(0, msg=msg) # time.sleep(1) # Ctrl.walk_forward(msg=msg, exp_dist=0.60) # time.sleep(0.2) # Ctrl.odo_verticalturn(target=1.5707, msg=msg) # msg.mode = 11 # Locomotion # msg.gait_id = 10 # TROT_FAST:10 TROT_MEDIUM:3 TROT_SLOW:27 自变频:26 # msg.vel_des = [0.5, 0, 0] # forward left/rightmove rotate # msg.duration = 3600 # Zero duration means continuous motion until a new command is used. # # Continuous motion can interrupt non-zero duration interpolation motion # msg.step_height = [0.06, 0.06] # ground clearness of swing leg # msg.life_count += 1 # msg.rpy_des = [0, 0.0, 0] # Ctrl.Send_cmd(msg) # Ctrl.Wait_finish(11, 10) # Ctrl.odo_verticalturn(target=1.5708, msg=msg) # time.sleep(0.1) # Ctrl.stone(msg) # Ctrl.Recovery_stand(msg) # time.sleep(3) # Ctrl.odo_verticalturn(target=1.5708, msg=msg) # time.sleep(3) # Ctrl.walk_forward(msg, exp_dist=0.4) # time.sleep(3) # Ctrl.rightside_align(msg=msg, expect_dist=1.15) # time.sleep(4) # Ctrl.odo_verticalturn(target=3.1415, msg=msg) # time.sleep(1) # Ctrl.speedbump_and_circle(msg) # Ctrl.jump(msg, 1200, 1) # Ctrl.odo_verticalturn(target=3.1415, msg=msg) # Ctrl.bothside_align(msg) # while True: # Ctrl.Recovery_stand(msg) # Ctrl.jump(msg,1200,1) # time.sleep(0.1) except KeyboardInterrupt: pass Ctrl.quit() sys.exit() if __name__ == '__main__': main()