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