Esempio n. 1
0
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()
Esempio n. 2
0
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()
Esempio n. 3
0
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()
Esempio n. 4
0
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()