def py_expand_to_indent(string, start,
end):
line = utils.get_line(string, start, end)
indent = get_indent(string, line)
# we don't expand to indent 0 (whole document)
if indent == 0:
return None
# expand to indent
result = _expand_to_indent(string, start, end)
if result is None:
return None
# get the intent of the first lin
# if the expansion changed return the result increased
if not(result["start"] == start and result["end"] == end):
return result
pos = result["start"] - 1
while True:
if pos < 0:
return None
# get the indent of the line before
before_line = utils.get_line(string, pos, pos)
before_indent = get_indent(string, before_line)
if not empty_line(string, before_line) and before_indent < indent:
start = before_line["start"]
end = result["end"]
return utils.create_return_obj(start, end, string, "py_indent")
# goto the line before the line befor
pos = before_line["start"] - 1
def py_expand_to_indent(string, start,
end):
line = utils.get_line(string, start, end)
indent = get_indent(string, line)
# we don't expand to indent 0 (whole document)
if indent == 0:
return None
# expand to indent
result = _expand_to_indent(string, start, end)
if result is None:
return None
# get the intent of the first lin
# if the expansion changed return the result increased
if not(result["start"] == start and result["end"] == end):
return result
pos = result["start"] - 1
while True:
if pos < 0:
return None
# get the indent of the line before
before_line = utils.get_line(string, pos, pos)
before_indent = get_indent(string, before_line)
if not empty_line(string, before_line) and before_indent < indent:
start = before_line["start"]
end = result["end"]
return utils.create_return_obj(start, end, string, "py_indent")
# goto the line before the line befor
pos = before_line["start"] - 1
def expand_over_line_continuation(string, start, end):
if not string[end-1:end] == "\\":
return None
line = utils.get_line(string, start, start)
next_line = utils.get_line(string, end + 1, end + 1)
start = line["start"]
end = next_line["end"]
next_result = expand_over_line_continuation(string, start, end)
# recursive check if there is an other continuation
if next_result:
start = next_result["start"]
end = next_result["end"]
return utils.create_return_obj(start, end, string, "line_continuation")
def expand_over_line_continuation(string, start, end):
if not string[end - 1:end] == "\\":
return None
line = utils.get_line(string, start, start)
next_line = utils.get_line(string, end + 1, end + 1)
start = line["start"]
end = next_line["end"]
next_result = expand_over_line_continuation(string, start, end)
# recursive check if there is an other continuation
if next_result:
start = next_result["start"]
end = next_result["end"]
return utils.create_return_obj(start, end, string, "line_continuation")
def expand_line_without_indent(string, start, end):
line = utils.get_line(string, start, end)
indent = expand_to_indent.get_indent(string, line)
lstart = min(start, line["start"] + indent)
lend = max(end, line["end"])
if lstart != start or lend != end:
return utils.create_return_obj(lstart, lend, string, "line_no_indent")
def _get_closest_env_border(string, start_pos, end_pos, reverse=False):
open_command = "begin"
close_command = "end"
if _ST3:
iterator = _BEGIN_END_REG.finditer(string, pos=start_pos,
endpos=end_pos)
offset = 0
else:
s = string[start_pos:end_pos]
iterator = _BEGIN_END_REG.finditer(s)
offset = start_pos
if reverse:
iterator = reversed(list(iterator))
open_command, close_command = close_command, open_command
count = 0
for before in iterator:
line = utils.get_line(string, before.start(), before.end())
# ignore comment lines
if string[line["start"]:line["end"]].strip()[0] == "%":
continue
command = before.group("command")
if command == open_command:
count += 1
elif command == close_command and count > 0:
count -= 1
elif command == close_command:
# found begin before
return {
"start": offset + before.start(),
"end": offset + before.end(),
"name": before.group("name")
}
def _get_closest_env_border(string, start_pos, end_pos, reverse=False):
open_command = "begin"
close_command = "end"
if _ST3:
iterator = _BEGIN_END_REG.finditer(string,
pos=start_pos,
endpos=end_pos)
offset = 0
else:
s = string[start_pos:end_pos]
iterator = _BEGIN_END_REG.finditer(s)
offset = start_pos
if reverse:
iterator = reversed(list(iterator))
open_command, close_command = close_command, open_command
count = 0
for before in iterator:
line = utils.get_line(string, before.start(), before.end())
# ignore comment lines
if string[line["start"]:line["end"]].strip()[0] == "%":
continue
command = before.group("command")
if command == open_command:
count += 1
elif command == close_command and count > 0:
count -= 1
elif command == close_command:
# found begin before
return {
"start": offset + before.start(),
"end": offset + before.end(),
"name": before.group("name")
}
def add_visible(obj, walls):
line_cells = get_line(obj.pos, self.pos)
for cell in line_cells[1:-1]:
if cell in walls:
return False
return True
def expand_line_without_indent(string, start, end):
line = utils.get_line(string, start, end)
indent = expand_to_indent.get_indent(string, line)
lstart = min(start, line["start"] + indent)
lend = max(end, line["end"])
if lstart != start or lend != end:
return utils.create_return_obj(lstart, lend, string, "line_no_indent")
def __init__(self):
self.lines = []
for line in self.structure:
output = []
random_line = get_line(line)
for bit in random_line:
output.append(' '.join(bit) if isinstance(bit, list) else bit)
self.lines.append(' '.join(output))
def expand_python_block_from_start(string, start, end):
if string[end - 1:end] != ":":
return None
result = expand_to_indent.expand_to_indent(string, end + 1, end + 1)
if result:
# line = utils.get_line(string, start, start)
line = utils.get_line(string, start, start)
start = line["start"]
end = result["end"]
return utils.create_return_obj(start, end, string, "py_block_start")
def line_loss(self, output, target):
def convert2uint8(img):
img = (img + 1) * 127.5
return tf.cast(img, tf.uint8)
def convert2f32(img):
img = tf.cast(img, tf.float32)
return (img / 127.5) - 1.0
output = convert2uint8(output)
target = convert2uint8(target)
preadLine = utils.get_line(output.numpy())
realLine = utils.get_line(target.numpy())
preadLine = convert2f32(preadLine)
realLine = convert2f32(realLine)
loss = tf.reduce_mean(tf.abs(realLine - preadLine))
return loss
def expand_python_block_from_start(string, start, end):
if string[end-1:end] != ":":
return None
result = expand_to_indent.expand_to_indent(string, end + 1,
end + 1)
if result:
# line = utils.get_line(string, start, start)
line = utils.get_line(string, start, start)
start = line["start"]
end = result["end"]
return utils.create_return_obj(start, end, string, "py_block_start")
def expand(string, start, end):
selection_is_in_string = expand_to_quotes.expand_to_quotes(
string, start, end)
if selection_is_in_string:
string_result = expand_agains_string(
selection_is_in_string["string"],
start - selection_is_in_string["start"],
end - selection_is_in_string["start"])
if string_result:
string_result["start"] = string_result[
"start"] + selection_is_in_string["start"]
string_result[
"end"] = string_result["end"] + selection_is_in_string["start"]
string_result[string] = string[string_result["start"]:
string_result["end"]]
return string_result
if utils.selection_contain_linebreaks(string, start, end) == False:
line = utils.get_line(string, start, end)
line_string = string[line["start"]:line["end"]]
line_result = expand_agains_line(line_string, start - line["start"],
end - line["start"])
if line_result:
line_result["start"] = line_result["start"] + line["start"]
line_result["end"] = line_result["end"] + line["start"]
line_result[string] = string[line_result["start"]:
line_result["end"]]
return line_result
expand_stack = ["semantic_unit"]
result = expand_to_semantic_unit.expand_to_semantic_unit(
string, start, end)
if result:
result["expand_stack"] = expand_stack
return result
expand_stack.append("symbols")
result = expand_to_symbols.expand_to_symbols(string, start, end)
if result:
result["expand_stack"] = expand_stack
return result
print(None)
def expand(string, start, end):
selection_is_in_string = expand_to_quotes.expand_to_quotes(
string, start, end)
if selection_is_in_string:
string_result = expand_agains_string(
selection_is_in_string["string"],
start - selection_is_in_string["start"],
end - selection_is_in_string["start"])
if string_result:
string_result["start"] = string_result[
"start"] + selection_is_in_string["start"]
string_result[
"end"] = string_result["end"] + selection_is_in_string["start"]
string_result[string] = string[
string_result["start"]:string_result["end"]]
return string_result
if utils.selection_contain_linebreaks(string, start, end) == False:
line = utils.get_line(string, start, end)
line_string = string[line["start"]:line["end"]]
line_result = expand_agains_line(line_string, start - line["start"],
end - line["start"])
if line_result:
line_result["start"] = line_result["start"] + line["start"]
line_result["end"] = line_result["end"] + line["start"]
line_result[string] = string[
line_result["start"]:line_result["end"]]
return line_result
expand_stack = ["semantic_unit"]
result = expand_to_semantic_unit.expand_to_semantic_unit(
string, start, end)
if result:
result["expand_stack"] = expand_stack
return result
expand_stack.append("symbols")
result = expand_to_symbols.expand_to_symbols(string, start, end)
if result:
result["expand_stack"] = expand_stack
return result
print(None)
def _expand_to_indent(string, start, end):
line = utils.get_line(string, start, end)
indent = get_indent(string, line)
start = line["start"]
end = line["end"]
before_line = line
while True:
# get the line before
pos = before_line["start"] - 1
if pos <= 0:
break
before_line = utils.get_line(string, pos, pos)
before_indent = get_indent(string, before_line)
# done if the line has a lower indent
if not indent <= before_indent and not empty_line(string, before_line):
break
# if the indent equals the lines indent than update the start
if not empty_line(string, before_line) and indent == before_indent:
start = before_line["start"]
after_line = line
while True:
# get the line after
pos = after_line["end"] + 1
if pos >= len(string):
break
after_line = utils.get_line(string, pos, pos)
after_indent = get_indent(string, after_line)
# done if the line has a lower indent
if not indent <= after_indent and not empty_line(string, after_line):
break
# move the end
if not empty_line(string, after_line):
end = after_line["end"]
return utils.create_return_obj(start, end, string, "indent")
def _expand_to_indent(string, start, end):
line = utils.get_line(string, start, end)
indent = get_indent(string, line)
start = line["start"]
end = line["end"]
before_line = line
while True:
# get the line before
pos = before_line["start"] - 1
if pos <= 0:
break
before_line = utils.get_line(string, pos, pos)
before_indent = get_indent(string, before_line)
# done if the line has a lower indent
if not indent <= before_indent and not empty_line(string, before_line):
break
# if the indent equals the lines indent than update the start
if not empty_line(string, before_line) and indent == before_indent:
start = before_line["start"]
after_line = line
while True:
# get the line after
pos = after_line["end"] + 1
if pos >= len(string):
break
after_line = utils.get_line(string, pos, pos)
after_indent = get_indent(string, after_line)
# done if the line has a lower indent
if not indent <= after_indent and not empty_line(string, after_line):
break
# move the end
if not empty_line(string, after_line):
end = after_line["end"]
return utils.create_return_obj(start, end, string, "indent")
def expand_to_inline_math(string, start, end):
# don't expand if a dollar sign is inside the string
if re.search(r"(?:[^\\]|^)\$", string[start:end]):
return
line = utils.get_line(string, start, end)
escape = inside = False
open_pos = close_pos = None
# we only need to consider one position, because we have checked it does
# not contain any $-signs
pos = start - line["start"]
for i, char in enumerate(string[line["start"]:line["end"]]):
# break if we are behind
behind = pos < i
if not inside and behind:
return
if escape:
escape = False
elif char == "\\":
escape = True
continue
elif char == "$":
if not inside:
# the inner end of the $-sign
open_pos = i + 1
elif behind:
close_pos = i
break
inside = not inside
if open_pos is not None and close_pos is not None:
open_pos = line["start"] + open_pos
close_pos = line["start"] + close_pos
# expand to the outer end
if open_pos == start and close_pos == end:
open_pos -= 1
close_pos += 1
return utils.create_return_obj(open_pos, close_pos, string,
"latex_inline_math")
def expand_to_inline_math(string, start, end):
# don't expand if a dollar sign is inside the string
if re.search(r"(?:[^\\]|^)\$", string[start:end]):
return
line = utils.get_line(string, start, end)
escape = inside = False
open_pos = close_pos = None
# we only need to consider one position, because we have checked it does
# not contain any $-signs
pos = start - line["start"]
for i, char in enumerate(string[line["start"]:line["end"]]):
# break if we are behind
behind = pos < i
if not inside and behind:
return
if escape:
escape = False
elif char == "\\":
escape = True
continue
elif char == "$":
if not inside:
# the inner end of the $-sign
open_pos = i + 1
elif behind:
close_pos = i
break
inside = not inside
if open_pos is not None and close_pos is not None:
open_pos = line["start"] + open_pos
close_pos = line["start"] + close_pos
# expand to the outer end
if open_pos == start and close_pos == end:
open_pos -= 1
close_pos += 1
return utils.create_return_obj(
open_pos, close_pos, string, "latex_inline_math")
def quadrilateral_slice(points, arr):
x = np.linspace(0, arr.shape[1] - 1, arr.shape[1])
y = np.linspace(0, arr.shape[0] - 1, arr.shape[0])
xx, yy = np.meshgrid(x, y)
lines = [utils.get_line(points[i], points[(i + 1) % 4]) for i in range(4)]
if lines[0][0] is None:
eq0 = "xx > lines[0][1]"
elif lines[0][0] > 0:
eq0 = "yy < lines[0][0] * xx + lines[0][1]"
else:
eq0 = "yy > lines[0][0] * xx + lines[0][1]"
if lines[2][0] is None:
eq2 = "xx < lines[2][1]"
elif lines[2][0] > 0:
eq2 = "yy > lines[2][0] * xx + lines[2][1]"
else:
eq2 = "yy < lines[2][0] * xx + lines[2][1]"
indices = np.all([
eval(eq0), yy < lines[1][0] * xx + lines[1][1],
eval(eq2), yy > lines[3][0] * xx + lines[3][1]
],
axis=0)
return arr * np.repeat(indices[..., np.newaxis], 3, axis=2)
#
import MySQLdb
import os
import sys
from subprocess import Popen, PIPE
import utils
cur_mysql = utils.mysql_conn()
#从命令行获取业务线id
line_id = sys.argv[1]
#获取指定业务线的表单ID项
form_id = utils.get_line(cur_mysql, line_id)
#从表单主表中获取昨天的已完成的表单记录
total = cur_mysql.execute(
'select id from col_summary where to_days(now())-to_days(finish_date)<=1 and state=3 and %s'
% form_id)
#print total
#得到当前月份
m = utils.get_month(cur_mysql)
#获取本月业绩
mm = mt = 0
cnt = cur_mysql.execute('select m from year_total where id=%s and line_id=%s' %
(m, line_id))
if cnt > 0:
def stream(tracker, camera=0, server=0):
"""
@brief Captures video and runs tracking and moves robot accordingly
@param tracker The BallTracker object to be used
@param camera The camera number (0 is default) for getting frame data
camera=1 is generally the first webcam plugged in
"""
######## GENERAL PARAMETER SETUP ########
MOVE_DIST_THRESH = 20 # distance at which robot will stop moving
SOL_DIST_THRESH = 150 # distance at which solenoid fires
PACKET_DELAY = 1 # number of frames between sending data packets to pi
OBJECT_RADIUS = 13 # opencv radius for circle detection
AXIS_SAFETY_PERCENT = 0.05 # robot stops if within this % dist of axis edges
packet_cnt = 0
tracker.radius = OBJECT_RADIUS
######## SERVER SETUP ########
motorcontroller_setup = False
if server:
# Create a TCP/IP socket
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
# Obtain server address by going to network settings and getting eth ip
server_address = ('169.254.171.10',10000) # CHANGE THIS
#server_address = ('localhost', 10000) # for local testing
print 'starting up on %s port %s' % server_address
sock.bind(server_address)
sock.listen(1)
connection, client_address = None, None
while True:
# Wait for a connection
print 'waiting for a connection'
connection, client_address = sock.accept()
break
######## CV SETUP ########
# create video capture object for
#cap = cv2.VideoCapture(camera)
cap = WebcamVideoStream(camera).start() # WEBCAM
#cap = cv2.VideoCapture('../media/goalie-test.mov')
#cap = cv2.VideoCapture('../media/bounce.mp4')
cv2.namedWindow(tracker.window_name)
# create trajectory planner object
# value of bounce determines # of bounces. 0 is default (no bounces)
# bounce not currently working correctly
planner = TrajectoryPlanner(frames=4, bounce=0)
# create FPS object for frame rate tracking
fps_timer = FPS(num_frames=20)
while(True):
# start fps timer
fps_timer.start_iteration()
######## CAPTURE AND PROCESS FRAME ########
ret, frame = True, cap.read() # WEBCAM
#ret, frame = cap.read() # for non-webcam testing
if ret is False:
print 'Frame not read'
exit()
# resize to 640x480, flip and blur
frame,img_hsv = tracker.setup_frame(frame=frame, w=640,h=480,
scale=1, blur_window=15)
######## TRACK OBJECTS ########
# use the HSV image to get Circle objects for robot and objects.
# object_list is list of Circle objects found.
# robot is single Circle for the robot position
# robot_markers is 2-elem list of Circle objects for robot markers
object_list = tracker.find_circles(img_hsv.copy(), tracker.track_colors,
tracker.num_objects)
robot, robot_markers = tracker.find_robot_system(img_hsv)
walls = tracker.get_rails(img_hsv, robot_markers, colors.Yellow)
planner.walls = walls
# Get the line/distances between the robot markers
# robot_axis is Line object between the robot axis markers
# points is list of Point objects of closest intersection w/ robot axis
# distanes is a list of distances of each point to the robot axis
robot_axis = utils.line_between_circles(robot_markers)
points, distances = utils.distance_from_line(object_list, robot_axis)
planner.robot_axis = robot_axis
######## TRAJECTORY PLANNING ########
# get closest object and associated point, generate trajectory
closest_obj_index = utils.min_index(distances) # index of min value
closest_line = None
closest_pt = None
if closest_obj_index is not None:
closest_obj = object_list[closest_obj_index]
closest_pt = points[closest_obj_index]
closest_line = utils.get_line(closest_obj, closest_pt) # only for viewing
planner.add_point(closest_obj)
# Get trajectory - list of elements for bounces, and final line traj
# Last line intersects with robot axis
traj_list = planner.get_trajectory_list(colors.Cyan)
traj = planner.traj
######## SEND DATA TO CLIENT ########
if packet_cnt != PACKET_DELAY:
packet_cnt = packet_cnt + 1
else:
packet_cnt = 0 # reset packet counter
# error checking to ensure will run properly
if len(robot_markers) is not 2 or robot is None or closest_pt is None:
pass
elif server:
try:
if motorcontroller_setup is False:
# send S packet for motorcontroller setup
motorcontroller_setup = True
######## SETUP MOTORCONTROLLER ########
axis_pt1 = robot_markers[0].to_pt_string()
axis_pt2 = robot_markers[1].to_pt_string()
data = 'SM '+axis_pt1+' '+axis_pt2+' '+robot.to_pt_string()
print data
connection.sendall(data)
# setup is done, send packet with movement data
else:
obj_robot_dist = utils.get_pt2pt_dist(robot, closest_obj)
print 'dist: ' + str(obj_robot_dist) # USE FOR CALIBRATION
######## SOLENOID ACTIVATION CODE ########
# check if solenoid should fire
if obj_robot_dist <= SOL_DIST_THRESH: # fire solenoid, dont move
print 'activate solenoid!'
# TODO SEND SOLENOID ACTIVATE
######## MOTOR CONTROL ########
# get safety parameters
rob_ax1_dist = utils.get_pt2pt_dist(robot_markers[0],robot)
rob_ax2_dist = utils.get_pt2pt_dist(robot_markers[1],robot)
axis_length = utils.get_pt2pt_dist(robot_markers[0],
robot_markers[1])
# ensure within safe bounds of motion relative to axis
# if rob_ax1_dist/axis_length <= AXIS_SAFETY_PERCENT or \
# rob_ax2_dist/axis_length <= AXIS_SAFETY_PERCENT:
# # in danger zone, kill motor movement
# print 'INVALID ROBOT LOCATION: stopping motor'
# data = 'KM'
# connection.sendall(data)
# if in danger zone near axis edge, move towards other edge
if rob_ax1_dist/axis_length <= AXIS_SAFETY_PERCENT:
print 'INVALID ROBOT LOCATION'
data = 'MM '+robot.to_pt_string()+' '+robot_markers[1].to_pt_string()
elif rob_ax2_dist/axis_length <= AXIS_SAFETY_PERCENT:
print 'INVALID ROBOT LOCATION'
data = 'MM '+robot.to_pt_string()+' '+robot_markers[0].to_pt_string()
# check if robot should stop moving
elif obj_robot_dist <= MOVE_DIST_THRESH: # obj close to robot
# Send stop command, obj is close enough to motor to hit
data = 'KM'
print data
connection.sendall(data)
pass
# Movement code
else: # far enough so robot should move
#### FOR TRAJECTORY ESTIMATION
# if planner.traj is not None:
# axis_intersect=shapes.Point(planner.traj.x2,planner.traj.y2)
# # Clamp the point to send to the robot axis
# traj_axis_pt = utils.clamp_point_to_line(
# axis_intersect, robot_axis)
# data = 'D '+robot.to_pt_string()+' '+traj_axis_pt.to_string()
# connection.sendall(data)
#### FOR CLOSEST POINT ON AXIS ####
if closest_pt is not None and robot is not None:
# if try to move more than length of axis, stop instead
if utils.get_pt2pt_dist(robot,closest_pt) > axis_length:
print 'TRYING TO MOVE OUT OF RANGE'
data = 'KM'
print data
connection.sendall(data)
else:
data = 'MM ' + robot.to_pt_string() + ' ' + \
closest_pt.to_string()
print data
connection.sendall(data)
except IOError:
pass # don't send anything
######## ANNOTATE FRAME FOR VISUALIZATION ########
frame = gfx.draw_lines(img=frame, line_list=walls)
frame = gfx.draw_robot_axis(img=frame, line=robot_axis) # draw axis line
frame = gfx.draw_robot(frame, robot) # draw robot
frame = gfx.draw_robot_markers(frame, robot_markers) # draw markers
frame = gfx.draw_circles(frame, object_list) # draw objects
# eventually won't need to print this one
frame = gfx.draw_line(img=frame, line=closest_line) # closest obj>axis
# draw full set of trajectories, including bounces
#frame = gfx.draw_lines(img=frame, line_list=traj_list)
#frame = gfx.draw_line(img=frame, line=traj) # for no bounces
frame = gfx.draw_point(img=frame, pt=closest_pt)
#frame=gfx.draw_line(frame,planner.debug_line) # for debug
######## FPS COUNTER ########
fps_timer.get_fps()
fps_timer.display(frame)
######## DISPLAY FRAME ON SCREEN ########
cv2.imshow(tracker.window_name,frame)
# quit by pressing q
if cv2.waitKey(1) & 0xFF == ord('q'):
break
# release capture
cap.stop() # WEBCAM
#cap.release() # for testing w/o webcam
cv2.destroyAllWindows()
def bulid_OGM():
# configure
grid_size = .2
x_size = 90
y_size = 105
false_alarm_rate = 0.15
maps = Map(int(x_size // grid_size), int(y_size // grid_size), grid_size)
x_init = -17.5
y_init = -72.5
angle_init = np.deg2rad(90)
# threshold of occupied and free
occupied_threshold = 0.8
free_threshold = 0.2
# get current time for naming figures
currentime = datetime.datetime.now().strftime('%d_%I_%M_')
# load radar data and parse
# root = "/home/aaron-ran/Documents/SLAM_internship/data/Test1_1/"
root = r"C:\Users\aaron.ran\Documents\projects\SLAM_internship\data\Test1_1"
data_file = r'\test1_1.csv'
figure_file = r'\figures_1_1/'
data = pd.read_csv(root + data_file)
# clean the data
data = data[data['Range_m'] *
np.sin(np.rad2deg(data['Elevation_deg'])) <= (2.5 - 0.64)]
# get all frames pose
pose = get_pose_cart(data, x_init, y_init, angle_init)
# initial plot figure
# plt.figure(1)
# plt.ion()
for i in range(len(pose['x'])):
# for i in range(2, 102):
# get current frame data and pose
data_frame = data[data['FrameCnt'] == i]
pose_frame = [pose['x'][i], pose['y'][i], pose['angle'][i]]
# get measures positions
# meas_vehicle = get_meas_vehicle_cart(data_frame)
# meas_global = meas_v2g(pose_frame, meas_vehicle)
# # meas_grid = global2grid((meas_global // grid_size).astype(int))
# with open(root+'meas.txt', 'ab') as file:
# np.savetxt(file, meas_global.T, '%0.4f', delimiter=',')
# radar position
radar_global = get_radar_cart(pose_frame)
radar_grid = global2grid((radar_global // grid_size).astype(int))
min_x_radar_grid, max_x_radar_grid = radar_grid[0].min(
), radar_grid[0].max()
min_y_radar_grid, max_y_radar_grid = radar_grid[1].min(
), radar_grid[1].max()
vehicle_grid = np.array([[min_x_radar_grid, max_x_radar_grid],
[min_y_radar_grid, max_y_radar_grid]])
vehicle_log_odds = float(np.log(0.1 / 0.9))
maps.update_map(vehicle_grid, vehicle_log_odds)
radar_list = data_frame['ID'].drop_duplicates(keep='first').tolist()
for rad_index in radar_list:
print(f"frames: {i}\n\tradars: {rad_index}")
# # sub dataset according to each radar
sub_data = data_frame[data_frame['ID'] == rad_index]
num_pcloud = sub_data.shape[0]
for pc in range(num_pcloud):
# get triangle grid
radar_pos = radar_global[:, rad_index - 1][:, None]
radar_angle = pose_frame[2]
r_meas = sub_data.iloc[pc, 4]
azimuth = sub_data.iloc[pc, 6]
vertexes = global2grid(
get_vertexes(r_meas, azimuth, radar_pos, radar_angle,
grid_size))
if vertexes.shape[1] > 1:
vertexes = np.hstack(
[vertexes, radar_grid[:, rad_index - 1][:, None]])
tri_grid = get_triangle(vertexes)
else:
tri_grid = np.array(
get_line(vertexes[:, 0], radar_grid[:,
rad_index - 1])).T
# calculate each grid's r-range and angle-range.
r_range, angle_range = get_r_and_angle_range(
tri_grid, radar_pos, radar_angle, grid_size)
# probability of detection
SNR = sub_data.iloc[pc, 3]
det_prob = false_alarm_rate**(1.0 / (1.0 + SNR))
# detection data and standard deviation.
meas = [r_meas, azimuth]
std = [0.017, 0.33]
# update maps.
log_odds = get_log_odds(r_range, angle_range, meas, std,
det_prob)
maps.update_map(tri_grid, log_odds)
# if (i % 2) == 0:
# plt.clf()
# plt.xlabel("x")
# plt.ylabel('y')
# plt.imshow(1.0 - 1./(1.+np.exp(maps.log_prob_map)), 'Greys') ## the first arg is prob
# plt.pause(0.001)
img = 1.0 - 1. / (1. + np.exp(maps.log_prob_map))
img[img <= free_threshold] = 0.0
img[img >= occupied_threshold] = 1.0
img[np.logical_and(free_threshold < img, img < occupied_threshold)] = 0.5
filename = root + figure_file + currentime + 'OGM.csv'
np.savetxt(filename, img, delimiter=',', fmt='%.2f')
#
import MySQLdb
import os
import sys
from subprocess import Popen,PIPE
import utils
cur_mysql = utils.mysql_conn()
# 从命令行获取业务线id
line_id = sys.argv[1]
# 获取属于该业务线的表单ID条件列表
form_id = utils.get_line(cur_mysql,line_id)
# 从申请单总表中获取当天的记录
# ed_cnt：当天完成的申请
# ing_cnt：当天受理的申请
ed_cnt = cur_mysql.execute('select id from col_summary where to_days(finish_date)=to_days(now()) and state=3 and line_id=%s and %s' % (line_id,form_id))
ing_cnt = cur_mysql.execute('select id from col_summary where to_days(start_date)=to_days(now()) and state=0 and line_id=%s and %s' % (line_id,form_id))
# 存入当天业绩
#print(">>> %s,%s <<<" % (ing_cnt,ed_cnt))
cur_mysql.execute('update year_total set m=%s where id=0 and line_id=%s' % (ing_cnt,line_id))
cur_mysql.close()
#
# Eof
def on_query_completions(self, view, prefix, locations):
if utils.get_language()!="java":return
line=utils.get_line()
line=line.lstrip()
if line.startswith("@"):line=line[1:]
if len(line)==1 and line.isupper() or line.endswith("new "):return utils.get_completion_list(utils.load_json(PATH_INDEX_CLASSES), "(Class)")
def get_trajectory_list(self, color=colors.Cyan):
"""
@brief Gets best fit traj from n-previous points and predicts bounces
Uses np.polyfit with dimension 1. Creates line from two points. Points
are generated using closest point to most recent frame (x1,y1) and
a point (x1 + 1.0, y2). This works as a line can be represented by any
arbitrary two points along it. The actual points do not matter here or
for the goalie, unless specified/calculated otherwise.
self.traj is always the farthest-predicted line between the last impact
point or object location and the robot axis
self.traj_list contains, from oldest to farthest predicted, a list of
Lines representing each bounce. The lines go obj->wall, wall->wall, ...,
wall->robot_axis.
@param color The color to be used in the trajectory lines
@return list of Line objects representing trajectory path
"""
# Not enough frames collected
if None in self.pt_list:
return []
# reset and get best fit line
self.traj_list = []
ln = self.get_best_fit_line()
# get trajectory towards robot axis, line from obj to axis
# if trajectory not moving towards robot axis, no prediction
if not self.traj_dir_toward_line(self.robot_axis):
ln = None
start_pt = self.pt_list[self.curr_index]
traj_int_pt = utils.line_intersect(ln, self.robot_axis) # Point object
traj = utils.get_line(start_pt, traj_int_pt, color=colors.Blue)
self.traj = traj
# return straight-line trajectory (as a 1-elem list for consistency) if
# no bounces to be predicted
if self.bounce is 0: # no bounce prediction
self.traj_list.append(self.traj)
return self.traj_list
#### BOUNCE (broken) ####
for wall in self.walls:
# Determine where bounce occurs and add line up to bounce
bounce_pt = utils.line_segment_intersect(self.traj, wall)
if bounce_pt is None:
continue
bounce_ln = utils.get_line(self.pt_list[self.curr_index],
bounce_pt, color=colors.Blue)
self.traj_list.append(bounce_ln)
self.debug_pt = bounce_pt
### THIS IS BROKEN
# Reflect line across wall and project to next wall or axis
# incoming d, normal n: outgoing r = d - 2(d dot n)*n
normal_dx = -wall.dy*1.0
normal_dy = wall.dx*1.0
normal_len = math.sqrt(normal_dx*normal_dx + normal_dy*normal_dy)
d = shapes.Point(bounce_ln.dx, bounce_ln.dy)
n = shapes.Point(normal_dx/normal_len, normal_dy/normal_len)
self.debug_line = shapes.Line(x1=bounce_pt.x, y1=bounce_pt.y,
x2=bounce_pt.x+n.x*20,y2=bounce_pt.y+n.y*20,color=colors.Red)
reflect_dx = (d.x - 2 * utils.dot(d, n) * n.x)
reflect_dy = (d.y - 2 * utils.dot(d, n) * n.y)
#### ONCE NEW DX AND DY ARE FOND, IT WORKS. GETTING DX/DY FAILS ####
new_x = bounce_pt.x + reflect_dx * 0.001
new_y = bounce_pt.y + reflect_dy * 0.001
new_pt = shapes.Point(new_x, new_y)
new_ln = utils.get_line(bounce_pt, new_pt) # small line
final_int = utils.line_intersect(new_ln, self.robot_axis)
final_ln = shapes.Line(
x1=bounce_pt.x, y1=bounce_pt.y,
x2=final_int.x, y2=final_int.y, color=colors.Cyan)
self.traj = final_ln
self.traj_list.append(final_ln)
# only one bounce
break
return self.traj_list
