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()