def _function_get_data():
camera_img_ = R[left_image][vals][-1]
angles_to_center, angles_surfaces, distances_marker, markers = Angle_Dict_Creator.get_angles_and_distance(
camera_img_, borderColor=None) #borderColor=(255,0,0))
Left = {
'angles_to_center': angles_to_center,
'angles_surfaces': angles_surfaces,
'distances_marker': distances_marker
}
if P[GRAPHICS]:
mci(camera_img_,
color_mode=cv2.COLOR_RGB2BGR,
delay=33,
title='left_image')
camera_img_ = R[right_image][vals][-1]
angles_to_center, angles_surfaces, distances_marker, markers = Angle_Dict_Creator.get_angles_and_distance(
camera_img_, borderColor=None) #borderColor=(0,0,255))
Right = {
'angles_to_center': angles_to_center,
'angles_surfaces': angles_surfaces,
'distances_marker': distances_marker
}
if P[GRAPHICS]:
mci(camera_img_,
color_mode=cv2.COLOR_RGB2BGR,
delay=33,
title='right_image')
return Left, Right
def _function_show(*args): """ Note, the input is in terms of t0 = 0, but the data is time since The Epoch. """ Args = args_to_dictionary(args) if start_time in Args: D[start_time] = D[start_time_init]+Args[start_time] if end_time in Args: D[end_time] = D[start_time] + Args[end_time] if ref_time in Args: ref_time_ = Args[ref_time] else: ref_time_ = (D[start_time]+D[end_time])/2.0 - D[start_time] True D[reference_time] = D[end_time]#find_nearest(np.array(D[topics][left_image][ts][:]),ref_time_+D[start_time]) #D[topics][left_image][ts][0]) img_index_ = D[timestamp_to_left_image][D[reference_time]] D[base_graph][img] *= 0 D[graph_topics]() vertical_line_proportion_ = (D[reference_time]-D[start_time])/(D[end_time]-D[start_time]) D[vertical_line](vertical_line_proportion,vertical_line_proportion_, color,(0,0,255)) D[dt] = (D[start_time]-D[end_time])*0.001 #D[insert_camera_image](camera,D[topics][left_image][vals], img_index, img_index_)#D[timestamp_to_left_image][ts_from_pixel_]) if left_image in D[topics]: camera_img_ = D[topics][left_image][vals][-1].copy() cx_ = (P[Y_PIXEL_SIZE]-P[CAMERA_SCALE]*shape(camera_img_)[0]) cy_ = (P[X_PIXEL_SIZE]-P[CAMERA_SCALE]*shape(camera_img_)[1]) angles_to_center, angles_surfaces, distances_marker, markers = Angle_Dict_Creator.get_angles_and_distance(camera_img_) for i_ in rlen(markers): xy_ = markers[i_].corners_xy[0].mean(axis=0) #xy_ += np.array([cy_-10,cx_-10]) xy_=tuple(xy_.astype(np.int)) if P[SHOW_MARKER_ID]: num_ = markers[i_].marker_id else: num_ = int(np.degrees(angles_to_center[markers[i_].marker_id])/2) cv2.putText( camera_img_, d2n(num_), xy_, cv2.FONT_HERSHEY_SIMPLEX, 1.0,(0,255,0),2) #print(dp(np.array(angles_to_center),1)) #D[insert_camera_image](camera,D[topics][left_image][vals], img_index,-1)#D[timestamp_to_left_image][ts_from_pixel_]) D[base_graph][img][cx_-10:-10,cy_-10:-10,:] = camera_img_ key_ = mci(D[base_graph][img],color_mode=cv2.COLOR_RGB2BGR,delay=1,title='topics') D[process_key_commands](key,key_)
def aruco_thread():
import kzpy3.data_analysis.Angle_Dict_Creator as Angle_Dict_Creator
from kzpy3.Localization_app.Parameters_Module import *
from kzpy3.Localization_app.Project_Aruco_Markers_Module import Aruco_Trajectory
Aruco_trajectory = Aruco_Trajectory()
global robot_steer
print('starting aruco_thread . . .')
error_ctr_ = 0
while not rospy.is_shutdown():
try:
x_avg,y_avg = 0.0,0.0
dx_avg,dy_avg = 0.0,0.0
for camera_list_ in [left_list,right_list]:
camera_img_ = camera_list_[-1]
angles_to_center, angles_surfaces, distances_marker, markers = Angle_Dict_Creator.get_angles_and_distance(camera_img_,borderColor=None)
Q = {'angles_to_center':angles_to_center,'angles_surfaces':angles_surfaces,'distances_marker':distances_marker}
hx_,hy_,x_,y_ = Aruco_trajectory[step](one_frame_aruco_data,Q, p,P)
x_avg += x_
y_avg += y_
dx_avg += hx_-x_
dy_avg += hy_-y_
aruco_position_x_pub.publish(std_msgs.msg.Float32(x_))
aruco_position_y_pub.publish(std_msgs.msg.Float32(y_))
aruco_heading_x_pub.publish(std_msgs.msg.Float32(hx_-x_))
aruco_heading_y_pub.publish(std_msgs.msg.Float32(hy_-y_))
x_avg /= 2.0
dx_avg /= 2.0
y_avg /= 2.0
dy_avg /= 2.0
steer = get_steer(X,x_avg, Y,y_avg, DX,dx_avg, DY,dy_avg)
steer =int((steer-49.0)*rp.robot_steer_gain+49.0)
steer = min(99,steer)
steer = max(0,steer)
robot_steer = steer
print robot_steer
error_ctr_ = 0
except Exception as e:
print("********** Exception ***********************")
print(e.message, e.args)
error_ctr_ += 1
print(d2s("aruco_thread error #",error_ctr_," (may be transient)"))
def aruco_thread():
import kzpy3.data_analysis.Angle_Dict_Creator as Angle_Dict_Creator
from kzpy3.Localization_app.Project_Aruco_Markers_Module import Aruco_Trajectory
Aruco_trajectory = Aruco_Trajectory()
P[past_to_present_proportion] = rp.past_to_present_proportion
global robot_steer,x_avg,y_avg,steer,steer_prev
print('starting aruco_thread . . .')
error_ctr_ = 0
while not rospy.is_shutdown():
try:
x_avg,y_avg = 0.0,0.0
dx_avg,dy_avg = 0.0,0.0
for camera_list_ in [left_list,right_list]:
camera_img_ = camera_list_[-1]
angles_to_center, angles_surfaces, distances_marker, markers = Angle_Dict_Creator.get_angles_and_distance(camera_img_,borderColor=None)
Q = {'angles_to_center':angles_to_center,'angles_surfaces':angles_surfaces,'distances_marker':distances_marker}
hx_,hy_,x_,y_ = Aruco_trajectory[step](one_frame_aruco_data,Q, p,P)
x_avg += x_
y_avg += y_
dx_avg += hx_-x_
dy_avg += hy_-y_
x_avg /= 2.0
dx_avg /= 2.0
y_avg /= 2.0
dy_avg /= 2.0
heading = angle_clockwise((0,1),(dx_avg,dy_avg))
heading_new,heading_floats,x1,y1 = get_best_heading(rp.X_PARAM*x_avg,rp.Y_PARAM*y_avg,heading,rp.radius)
heading_delta = (heading_new - heading)
pose_str = d2n("(",dp(x_avg),',',dp(y_avg),',',dp(dx_avg),',',dp(dy_avg),")")
heading_floats_str = "["
for h in heading_floats:
heading_floats_str += d2n('[',rp.radius*h[0]+x_avg,',',rp.radius*h[1]+y_avg,'],')
heading_floats_str += ']'
xy_str = "["
for xy in zip(x1,y1):
xy_str += d2n('[',xy[0],',',xy[1],'],')
xy_str += ']'
ssh_command_str = d2n("echo 'pose = ",pose_str,"\nxy = ",xy_str,"\nheading_floats = ",heading_floats_str,"' > ~/Desktop/",rp.computer_name,".car.txt ")
try:
ssh.exec_command(ssh_command_str)
except:
print('ssh.exec_command failed')
steer = heading_delta*(-99.0/45.0)
steer =int((steer-49.0)*rp.robot_steer_gain+49.0)
steer = min(99,steer)
steer = max(0,steer)
steer = int((1.0-rp.steer_momentum)*steer+rp.steer_momentum*steer_prev)
steer_prev = steer
print int(heading_new),int(heading),int(heading_delta),int(steer)
robot_steer = steer
if True:
aruco_position_x_pub.publish(std_msgs.msg.Float32(x_avg))
aruco_position_y_pub.publish(std_msgs.msg.Float32(y_avg))
aruco_heading_x_pub.publish(std_msgs.msg.Float32(dx_avg))
aruco_heading_y_pub.publish(std_msgs.msg.Float32(dy_avg))
aruco_freq.freq()
error_ctr_ = 0
except Exception as e:
print("********** Exception ***********************")
print(e.message, e.args)
error_ctr_ += 1
print(d2s("aruco_thread error #",error_ctr_," (may be transient)"))
robot_in_charge_ = 0
robot_in_charge_exit_timer_ = Timer(
2) # gives time to get to normal acc values after car turn signal
while not rospy.is_shutdown():
time.sleep(0.01)
if robot_in_charge_exit_timer_.check():
try:
if reload_timer.check(): # put in thread?
reload(rp)
reload_timer.reset()
if not robot_in_charge_:
if False:
camera_img_ = R[left_image][vals][-1].copy()
angles_to_center, angles_surfaces, distances_marker, markers = Angle_Dict_Creator.get_angles_and_distance(
camera_img_)
if GRAPHICS:
key_ = mci(camera_img_,
color_mode=cv2.COLOR_RGB2BGR,
delay=33,
title='topics')
acc2rd = R[acc_x][vals][-1]**2 + R[acc_z][vals][-1]**2
if True:
acc2rd_list.append(acc2rd)
if len(acc2rd_list) > 100:
acc2rd_list = acc2rd_list[-100:]
if GRAPHICS:
clf()
xylim(0, 100, 0, 20)
plot(acc2rd_list)
#print acc2rd
def preprocess_bagfile(*args):
Args = args_to_dictionary(args)
path_ = Args[path]
visualize_ = Args[visualize]
A = {}
A[left] = {}
A[right] = {}
import kzpy3.data_analysis.Angle_Dict_Creator as Angle_Dict_Creator
import rospy
import rosbag
import cv2
from cv_bridge import CvBridge, CvBridgeError
bridge = CvBridge()
ctr = 0
timer = Timer(0)
cprint('Loading bagfile ' + path_, 'yellow')
assert_disk_locations(path_)
bag = rosbag.Bag(path_)
color_mode = "rgb8"
for s in ['left', 'right']:
for m in bag.read_messages(
topics=['/bair_car/zed/' + s + '/image_rect_color']):
t = round(m.timestamp.to_time(), 3)
A[s][t] = {}
img = bridge.imgmsg_to_cv2(m[1], color_mode)
angles_to_center, angles_surfaces, distances_marker, markers = Angle_Dict_Creator.get_angles_and_distance(
img)
mi(img)
pause(0.0001)
A[s][t]['angles_to_center'] = angles_to_center
A[s][t]['angles_surfaces'] = angles_surfaces
A[s][t]['distances_marker'] = distances_marker
A[s][t]['markers'] = markers
for i_ in rlen(markers):
marker_id_ = markers[i_].marker_id
corners_xy_ = markers[i_].corners_xy.astype(np.int)
xy_ = corners_xy_.mean(axis=1)
#print(shape(xy_))
#print (marker_id_,corners_xy_,'mean:',xy_,xy_[0][1])
x_ = int(xy_[0][0])
y_ = int(xy_[0][1])
try:
cv2.putText(img, str(marker_id_), (x_, y_),
cv2.FONT_HERSHEY_SIMPLEX, 0.35, (255, 0, 0), 1)
except:
print "put text failed"
if visualize_ > 0:
if np.mod(ctr, visualize_) == 0:
#print(d2c(fname(path),s,t,A[s][t]['distances_marker']))
k = mci(img, delay=300, scale=4)
if k == ord('q'):
break
ctr += 1
print(d2s('Done in', timer.time(), 'seconds'))
return A