def main(args=None): # main Funktion rclpy.init(args=args) # initialisieren init() soll = yf_node.YF_SollSpeed(nodeName["SollSpeed"], "SollSpeed") goal = yf_node.YF_Goal(nodeName['Goal'], 'Goal') flag = yf_node.YF_ObjectFlag(nodeName['ObjectFlag'], 'ObjectFlag') flag.publishMsg(101) rclpy.spin_once(flag.node, timeout_sec=0.05) # track_subscriber = trackSubscriber() # der Name von dem zu subscribten Node # rclpy.spin(track_subscriber) t = time.time() while 1: rclpy.spin_once(flag.node, timeout_sec=0.01) signal = flag.subMsg.data if signal == 101: print("Waiting for a Target") continue if 100 > signal.tolist() > 0: rclpy.spin_once(goal.node, timeout_sec=0.01) ziel = goal.subMsg if ziel is None: print("Waiting for a Target") continue target = get_target(ziel) print(target) v_l, v_r = controlalgorithm(target[0], target[1]) # print("vl vr: ", v_l, v_r) wheel_speed = wheel_speed_caculator(v_l, v_r) print("speed: ", wheel_speed) soll.publishMsg(wheel_speed) rclpy.spin_once(goal.node, timeout_sec=0.01) # print fps print("fps: ", int(1 / (time.time() - t))) t = time.time() time.sleep(0.05) elif signal.tolist() == 100: # TODO: target lost wheel_speed = [0., 0., 0., 0.] print("speed: ", wheel_speed) soll.publishMsg(wheel_speed) print("Target lost, wait for new target") time.sleep(0.1) else: print("Waiting User select target") continue goal.destroy_node() # eben vom Oben soll.destroy_node() # eben vom Oben rclpy.shutdown()
def main(): # 初始化 ros2 python - rclpy & 外置参数引入 init() # rclpy.init() # 构建相关节点 poc = yf_node.YF_PointCloud(nodeName["PointCloud"], "PointCloud") cost = yf_node.YF_CostMap(nodeName["CostMap"], "CostMap") showMap = yf_node.YF_CompressedImage(nodeName["ShowMap"], "ShowMap") goal = yf_node.YF_Goal(nodeName["Goal"], "GoalCostMap") # 广播节点的首次初始化 rclpy.spin_once(poc.node, timeout_sec=0.01) rclpy.spin_once(cost.node, timeout_sec=0.01) rclpy.spin_once(showMap.node, timeout_sec=0.01) rclpy.spin_once(goal.node, timeout_sec=0.01) # init a time point for fps t = time.time() ziel = [0, 0] while True: # 中断守护 if cv2.waitKey(25) & 0xFF == ord('q'): cv2.destroyAllWindows() break # 刷新订阅的节点 rclpy.spin_once(cost.node, timeout_sec=0.001) rclpy.spin_once(showMap.node, timeout_sec=0.001) # print fps print("CostMap Fps: ", int(1 / (time.time() - t))) t = time.time() # 等待捕获所有信息 pcl = poc.poc_array ziel = goal.subMsg if pcl is not None: print("Waiting for new point cloud") continue if ziel is not None: print("Waiting for new target") continue # 获得地图 target = get_target(ziel) obMap, sendMap = get_obMap(pcl, poc, target) obMap = obMap.flatten().astype(np.uint8) # 广播地图 cost.publishMsg(obMap) showMap.publishMsg(sendMap.astype(np.uint8)) # 杀死无用节点 cost.node.destroy_node() showMap.node.destroy_node() poc.node.destroy_node() goal.node.destroy_node() # 结束rclpy rclpy.shutdown()
def main(args=None): # 初始化 ros2 python - rclpy & 外置参数引入 init() rclpy.init(args=args) # 构建相关节点 cam = yf_node.YF_Image(nodeName['Image'], 'Image') video = yf_node.YF_Image_PY(nodeName['Video'], 'Video') obj = yf_node.YF_ObjectsArray(nodeName['ObjectsArray'], 'ObjectsArray') goal = yf_node.YF_Goal(nodeName['Goal'], 'Goal') # 广播节点的首次初始化 rclpy.spin_once(goal.node, timeout_sec=0.001) rclpy.spin_once(video.node, timeout_sec=0.001) # init a time point for fps t = time.time() while 1: # 接受 图像,物体,点云信息 rclpy.spin_once(goal.node, timeout_sec=0.001) rclpy.spin_once(cam.node, timeout_sec=0.001) rclpy.spin_once(obj.node, timeout_sec=0.001) # 运行数据捕捉: become target only for debug res = runCam(cam, obj, goal) if res is None: continue else: target, liveVideo = res video.publishMsg(liveVideo.astype(np.uint8)) rclpy.spin_once(video.node, timeout_sec=0.001) # # print fps # print("fps: ", int(1/(time.time()-t))) # t = time.time() # 中断守护 if cv2.waitKey(25) & 0xFF == ord('q'): cv2.destroyAllWindows() break # 杀死所有订阅节点 cam.node.destroy_node() obj.node.destroy_node() goal.node.destroy_node() video.node.destroy_node() # 结束rclpy rclpy.shutdown()
def main(): # ??? ros2 python - rclpy & ?????? rclpy.init() # ?????? init() maps = yf_node.YF_Image_PY(nodeName["CostMap"], "CostMap", "sub") ziel = yf_node.YF_Goal(nodeName['Goal'], 'Goal', 'sub') real = yf_node.YF_RealSpeed(nodeName["RealSpeed"], "RealSpeed", "sub") soll = yf_node.YF_SollSpeed(nodeName["SollSpeed"], "SollSpeed", "pub") rclpy.spin_once(soll.node, timeout_sec=0.001) # original 0.001 rclpy.spin_once(real.node, timeout_sec=0.001) rclpy.spin_once(maps.node, timeout_sec=0.001) # original 0.001 rclpy.spin_once(ziel.node, timeout_sec=0.001) # ??DWA??? test_own_map = False save_first_frame = True car_state, flag_state = dwa_module.dwa_init() pf_state = potential_field_planning.pf_planner_init() dwa = dwa_module.DWA_Controller(car_state, flag_state) pf_planner = potential_field_planning.Potential_Field_Planner(pf_state) old_target = np.array([-1., -1.]) # init a time point for fps start = time.time() u_soll = np.array([0., 0., 0., 0.]) car_traj = [] # obmap_list = np.zeros((1, 30, 30)) i_ = 0 if test_own_map: target = np.array([2.5, 4.]) oblist = np.load("/home/yf/dev_ws/test_2m2m_map.npy") target_traj = pf_planner.potential_field_planning(target, oblist) i_ = 0 goal_to_reach = target_traj[0] car_traj = [] try: while True: t = time.time() # time.sleep(0.1) rclpy.spin_once(soll.node, timeout_sec=0.001) # original 0.001 rclpy.spin_once(real.node, timeout_sec=0.001) rclpy.spin_once(maps.node, timeout_sec=0.001) # original 0.001 rclpy.spin_once(ziel.node, timeout_sec=0.001) # print('spin time :',time.time()-t) # print fps # print("fps: ", int(1 / (time.time() - t))) # target, oblist = dwa.inverse_transforamtion(target, oblist) if real.subMsg is None: print("Waiting for realspeed message") continue elif maps.subMsg is None: print("Waiting for map message") continue elif ziel.subMsg is None: print("Waiting for ziel message") continue elif np.sum(maps.subMsg) == 0: print("Map is None but given") continue else: # get target target = get_target(ziel.subMsg) # get wheel speed real_wheel = real.subMsg left_front = real_wheel[0] right_front = real_wheel[2] left_back = real_wheel[4] right_back = real_wheel[6] u_ist = np.array([(left_front + left_back) / 2, (right_front + right_back) / 2]) # maps transformation obmap = maps.subMsg[:, :, 0] obmap[49, 49] = 51 # potential_map = cv2.resize(maps.subMsg, (20, 20), interpolation=cv2.INTER_CUBIC) obmap = np.swapaxes(obmap, 0, 1) obmap = np.flip(obmap, axis=1) # obmap = cv2.resize(obmap, (50, 50), interpolation=cv2.INTER_CUBIC) # if i_ < 500: # obmap_list = np.concatenate((obmap_list, obmap[None, :]), axis=0) # np.save('/home/yf/dev_ws/obmap.npy', obmap_list) # i_ += 1 obmap[obmap > 50] = 1 oblist = dwa.obmap2coordinaten(obmap, 5. / 50.) + np.array( [-0.06, 0.]) # potential_map[potential_map > 0] = 1 # ob_loc = pf_planner.obmap2coordinaten(potential_map, 5 / 20) # target_traj = pf_planner.potential_field_planning(target, ob_loc) ########################################################change # goal_to_reach = target_traj[1] u_soll, traj_soll, all_traj = dwa.dwa_control( u_ist, dwa.x, target, oblist) # car_traj.append(dwa.x[:2]) # if i_ >= len(target_traj): # goal_to_reach = target # else: # goal_to_reach = target_traj[i_] # # if dwa.TEMPORARY_GOAL_ARRIVED: # i_ += 1 # dwa.TEMPORARY_GOAL_ARRIVED = False # goal_to_reach, oblist = dwa.inverse_transforamtion(goal_to_reach, oblist, tr_map=True) # target = dwa.inverse_transforamtion(target, oblist, tr_map=False) if dwa.SHOW_ANIMATION: plt.cla() figManager = plt.get_current_fig_manager() figManager.window.showMaximized() # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect( 'key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) # plt.plot(dwa.x[0], dwa.x[1], "xr") plt.plot(oblist[:, 0], oblist[:, 1], "sk") for i in range(len(all_traj)): plt.plot(all_traj[i][:, 0], all_traj[i][:, 1], "-c") # print(trajectory_soll) plt.plot(traj_soll[:, 0], traj_soll[:, 1], "-g") dwa_module.plot_robot(dwa.x[0], dwa.x[1], dwa.x[2], dwa) dwa_module.plot_arrow(dwa) # if dwa.RESET_STATE: # plt.plot(goal_traj[:, 0], goal_traj[:, 1], color='purple') # plt.plot(traj_ist[:, 0], traj_ist[:, 1], color='orange') # plt.plot(target_traj[:, 0], target_traj[:, 1], color='orange') # plt.plot(target[0], target[1], 'r') # else: # plt.plot(traj_ist[:, 0], traj_ist[:, 1], "-r") plt.plot(target[0], target[1], "^r") plt.axis("equal") plt.xlim(0, 5) plt.grid(True) plt.pause(0.1) if np.linalg.norm(target - dwa.x[:2]) < 1.: dwa.GOAL_ARRIVAED = True # speed infomation transforamtion u_soll[0] = float(int(u_soll[0])) u_soll[1] = float(int(u_soll[1])) wheel_speed = [u_soll[0], u_soll[1], u_soll[0], u_soll[1]] # if time.time() - start > 10: # np.save('/home/yf/yifan/car_traj.npy', np.array(car_traj)) # dwa.GOAL_ARRIVAED = True if dwa.GOAL_ARRIVAED: print("goal arrived") wheel_speed = [0.0, 0.0, 0.0, 0.0] time.sleep(0.15) # soll.publishMsg(wheel_speed) # rclpy.spin_once(soll.node, timeout_sec=0.01) # dwa.GOAL_ARRIVAED = False print('fps = ', 1 / (time.time() - t)) soll.publishMsg(wheel_speed) if dwa.GOAL_ARRIVAED: dwa.GOAL_ARRIVAED = False # time.sleep(0.3) # ?????? finally: wheel_speed = [0.0, 0.0, 0.0, 0.0] soll.publishMsg(wheel_speed) rclpy.spin_once(soll.node, timeout_sec=0.001) soll.node.destroy_node() real.node.destroy_node()
def main(): # 初始化 ros2 python - rclpy & 外置参数引入 zed = YF_Zed2() init() rclpy.init() # 构建相关节点 flag = yf_node.YF_ObjectFlag(nodeName['ObjectFlag'],'ObjectFlag') video = yf_node.YF_Image_PY(nodeName['Video'],'Video') goal = yf_node.YF_Goal(nodeName['Goal'],'Goal') cost = yf_node.YF_CostMap(nodeName["CostMap"],"CostMap") showMap = yf_node.YF_CompressedImage(nodeName["ShowMap"],"ShowMap") # YF_CompressedImage YF_Image_PY # set init flag = 101 flag.publishMsg(101) # 广播节点的首次初始化 rclpy.spin_once(flag.node,timeout_sec=0.001) rclpy.spin_once(goal.node,timeout_sec=0.001) rclpy.spin_once(video.node,timeout_sec=0.001) rclpy.spin_once(cost.node,timeout_sec=0.001) rclpy.spin_once(showMap.node,timeout_sec=0.001) # init a time point for fps t = time.time() liveVideo = None target = None old_target = [0,0] objectCache = None videoCache = None frozenFrame = False trackTarget = False while 1: # 运行数据捕捉: become target only for debug zed.getData() rclpy.spin_once(flag.node,timeout_sec=0.001) signal = flag.subMsg.data.tolist() liveImage = zed.liveImage objectArray = zed.objectArray poc = zed.pointCloud pcl = PointCloud2_Img2Array(poc) # 等待捕获所有信息 if liveImage is None: print("Waiting for new Image") time.sleep(0.2) continue if objectArray is None: print("Waiting for new Object") time.sleep(0.2) continue print(signal) if signal == 101: liveVideo,_ = runObjectDetection(liveImage, objectArray) elif signal == 0 and not frozenFrame: videoCache = liveImage liveVideo,objectCache = runObjectDetection(liveImage, objectArray) if len(objectCache) == 0: flag.publishMsg(101) continue flag.publishMsg(-1*len(objectCache)) frozenFrame = True trackTarget = False elif signal == 0 and frozenFrame: continue elif 100 >= signal > 0 and not trackTarget: frozenFrame = False trackTarget = True objIdx = flag.subMsg.tolist()-1 obj = objectCache[objIdx] runTargetSet(obj,videoCache,goal) elif 100 >= signal > 0 and trackTarget: target,liveVideo = runTrack(liveImage, objectArray, goal,flag) else: print("Waiting User select target") continue video.publishMsg(liveVideo.astype(np.uint8)) # 获得地图 if target is None: target = old_target else: old_target = target obMap,sendMap = get_obMap(pcl,poc,target,CostMap,showImg) # 广播地图 cost.publishMsg(obMap.flatten().astype(np.uint8)) showMap.publishMsg(sendMap.astype(np.uint8)) # print fps framePerSecond = int(1/(time.time()-t)) # print("LiveVideo Fps: ", framePerSecond) t = time.time() rclpy.spin_once(flag.node,timeout_sec=0.001) rclpy.spin_once(video.node,timeout_sec=0.001) rclpy.spin_once(cost.node,timeout_sec=0.001) rclpy.spin_once(showMap.node,timeout_sec=0.001) # print live video if FPS: cv2.putText(liveVideo, "FPS:{}".format(framePerSecond),org=(5,25), fontFace=cv2.FONT_HERSHEY_SIMPLEX,fontScale=0.8, color=(0,255,0),thickness=2) if showImg[2]: cv2.imshow("LiveVideo",liveVideo) # 中断守护 if cv2.waitKey(25) & 0xFF == ord('q'): cv2.destroyAllWindows() break # 杀死所有订阅节点 video.node.destroy_node() cost.node.destroy_node() showMap.node.destroy_node() goal.node.destroy_node() flag.node.destroy_node() zed.zed2.close() # 结束rclpy rclpy.shutdown()