def get_steer(*args): Args = args_to_dictionary(args);_=da x = _(Args,X) y = _(Args,Y) dx = _(Args,DX) dy = _(Args,DY) True a = angle_clockwise((0,1),(dx,dy)) if a >= 345 or a < 15: binned_angle = 0; elif a >= 15 and a < 45: binned_angle = 30; elif a >= 45 and a < 75: binned_angle = 60 elif a >= 75 and a < 105: binned_angle = 90 elif a >= 105 and a < 135: binned_angle = 120 elif a >= 135 and a < 165: binned_angle = 150 elif a >= 165 and a < 195: binned_angle = 180 elif a >= 195 and a < 225: binned_angle = 210 elif a >= 225 and a < 255: binned_angle = 240 elif a >= 255 and a < 285: binned_angle = 270 elif a >= 285 and a < 315: binned_angle = 300 elif a >= 315 and a < 345: binned_angle = 330; px = int(x*7.0/wall_length + 4.0) py = int(y*7.0/wall_length + 4.0) steer = int(heading_steering_coordinates[binned_angle][px,py]) return steer
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)"))