def fig_inner_cross_count(fig) -> int:
count = 0
ext_fig = fig + [fig[0]]
for i in range(len(ext_fig) - 1):
for j in range(i + 1, len(ext_fig) - 1):
edge_i = LineString([ext_fig[i], ext_fig[i+1]])
edge_j = LineString([ext_fig[j], ext_fig[j+1]])
if edge_i.crosses(edge_j):
count += 1
return count
def make_point_link_data(fig):
n = len(fig)
links = dict([(i, {(i + 1) % n, (i - n - 1) % n}) for i in range(n)])
cross_points = []
cross_vertexes = {}
crossed_lines = []
cp_idx = n
# find cross-points
for i in range(len(fig) - 1):
for j in range(i + 1, len(fig)):
edge_vertexes = [i % n, (i + 1) % n, j % n, (j + 1) % n]
i1, i2, j1, j2 = edge_vertexes
edge_i = LineString([fig[i1], fig[i2]])
edge_j = LineString([fig[j1], fig[j2]])
if edge_i.crosses(edge_j):
ip = edge_i.intersection(edge_j)
cross_points.append((ip.x, ip.y))
links[cp_idx] = set()
cr_line1 = {i1, i2}
cr_line2 = {j1, j2}
cross_vertexes[cp_idx] = (cr_line1, cr_line2)
if cr_line1 not in crossed_lines:
crossed_lines.append(cr_line1)
if cr_line2 not in crossed_lines:
crossed_lines.append(cr_line2)
cp_idx += 1
points = fig + cross_points
# add cross-point links
for base_line in crossed_lines:
inner = [] # inline vertexes
for vx, lines in cross_vertexes.items():
if base_line in lines:
inner.append(vx)
if len(inner) > 0:
bs1, bs2 = list(base_line)
if len(inner) >= 2:
inner.sort(key=lambda x: Point(points[x]).distance(Point(points[bs1])))
links[bs1].remove(bs2)
links[bs2].remove(bs1)
vxs = [bs1] + inner + [bs2]
for i in range(1, len(vxs) - 1):
links[vxs[i]].add(vxs[i-1])
links[vxs[i-1]].add(vxs[i])
links[vxs[i]].add(vxs[i+1])
links[vxs[i+1]].add(vxs[i])
return points, links
def findTheFrontFarestCorner(probe, floorMeta, floorPoly, pd):
MAXLEN = -1
wallDiagIndex = -1
for wallJndex in range(floorMeta.shape[0]):
trobe = floorMeta[wallJndex][0:2]
# check if the diagonal lies inside the edges of the polygon.
line = LineString((probe, trobe))
if sk.isLineIntersectsWithEdges(line, floorMeta):
continue
# check if some point on the diagonal is inside the polygon.
mPoint = Point((probe + trobe) / 2)
if not floorPoly.contains(mPoint):
continue
if np.dot(trobe - probe, pd) < 0:
continue
LEN = np.linalg.norm(probe - trobe, ord=2)
if LEN > MAXLEN:
MAXLEN = LEN
wallDiagIndex = wallJndex
return wallDiagIndex
def convert_to_PhysObj(self):
# finds the coordinates of the closest point in every line between waypoints
# and stores them as PysObj to work with the ConeTypeSensor parent class
# clear the objects list
self.objects = []
owner_loc = Point(self.owner.location.local_frame.east,
self.owner.location.local_frame.north)
# Sensor has 4 slices (front, left, ...), it is possible that the closest point in a slice is at the intersection
# of a geoFenceline and the edge of a slice.
sliceAngles = [45, 135, 225, 315]
sliceAngles = [x + self.owner.heading for x in sliceAngles]
sliceLineStrings = []
for a in sliceAngles:
p1_complex = cmath.rect(self.range, math.radians(a))
p1_local = Point(p1_complex.real, p1_complex.imag)
# go from local to global
p1_global = affine_transform(
p1_local, [1, 0, 0, 1, owner_loc.x, owner_loc.y])
sliceLineStrings.append(LineString([p1_global, owner_loc]))
for line in self.lines:
# add the closest point in every line to the object list
p1, p2 = nearest_points(line, owner_loc)
self.objects.append(LocationLocal(north=p1.y, east=p1.x, down=0))
# check if any of the sliceLineStrings intersect with the line
for s in sliceLineStrings:
intersect = s.intersection(line)
if intersect:
# only 1 intersect possible between 2 lines
self.objects.append(
LocationLocal(north=intersect.y,
east=intersect.x,
down=0))
def GetCrossPoint(infc,NeedIndex):
#生成曲线
Curve=LineString(GetCurveLinecoordinates(infc))
#读取文本文件，获得总体最小二乘压缩点坐标数组
TLSTxtpath= infc[0:-4]+"D.txt"
TLSfp= open(TLSTxtpath,"r")
TLSLine=[]
for line in TLSfp.readlines():
x0=float(line.replace("\n", "").split("\t")[0])
y0=float(line.replace("\n", "").split("\t")[1])
TLSLine.append((x0,y0))
TLSfp.close()
#求交点
for i in range(0,len(TLSLine)-1,1):
if i == NeedIndex:
##后交点
#生成直线
LineA=LineString(GetStraightLinecoordinates(TLSLine[i][0],TLSLine[i][1],TLSLine[i+1][0],TLSLine[i+1][1]))
#获得交点
CrossPoint=LineA.intersection(Curve)
#存放交点
CrossPointArrays=[]
#判断交点类型
if str(type(CrossPoint))== "<class 'shapely.geometry.multipoint.MultiPoint'>":
for j in range(0,len(list(CrossPoint)),1):
CrossPointArrays.append([list(CrossPoint)[j].x,list(CrossPoint)[j].y])
elif str(type(CrossPoint))== "<class 'shapely.geometry.point.Point'>":
CrossPointArrays.append([CrossPoint.x,CrossPoint.y])
#前交点
LineB=LineString(GetStraightLinecoordinates(TLSLine[i-1][0],TLSLine[i-1][1],TLSLine[i][0],TLSLine[i][1]))
print "前直线",TLSLine[i-1][0],TLSLine[i-1][1],TLSLine[i][0],TLSLine[i][1]
CrossPoint2=LineB.intersection(Curve)
CrossPointArraysBefore=[]
if str(type(CrossPoint2))== "<class 'shapely.geometry.multipoint.MultiPoint'>":
for k in range(0,len(list(CrossPoint2)),1):
CrossPointArraysBefore.append([list(CrossPoint2)[k].x,list(CrossPoint2)[k].y])
elif str(type(CrossPoint2))== "<class 'shapely.geometry.point.Point'>":
CrossPointArraysBefore.append([CrossPoint2.x,CrossPoint2.y])
return CrossPointArraysBefore,CrossPointArrays
def plot_graph(robots, obstacles):
# split the graph into subplots for plotting robots & obstacles on same graph
fig, ax = plt.subplots()
graph = nx.Graph()
# nodes are labelled 0,1,...,n where n = number of robots
nodes = range(len(robots))
# edges map each node to every other node
edges = list(itertools.product(nodes, nodes))
# add all edges and nodes to graph
graph.add_nodes_from(nodes, pos=robots)
graph.add_edges_from(edges)
nx.draw(graph, pos=robots)
#plot obstacles
for obstacle in obstacles:
ring = LinearRing(obstacle)
#calculate their intersections with any edges
print(ring.intersection(LineString(edges)))
x, y = ring.xy
ax.plot(x, y)
plt.show()
def draw_line_radec(self, ra, dec, **kwargs):
"""Draw a line assuming a Geodetic transform.
Parameters
----------
ra : right ascension (deg)
dec : declination (deg)
kwargs: passed to plot
Returns
-------
feat : cartopy.FeatureArtist
"""
# Color will fill a polygon...
# https://github.com/SciTools/cartopy/issues/856
color = kwargs.pop('c', kwargs.pop('color', 'k'))
defaults = dict(crs=ccrs.Geodetic(), edgecolor=color, facecolor='none')
setdefaults(kwargs, defaults)
line = LineString(list(zip(ra, dec))[::-1])
return self.ax.add_geometries([line], **kwargs)
def findTheLongestDiagonal(wallIndex, floorMeta, floorPoly):
probe = floorMeta[wallIndex][0:2]
MAXLEN = -1
wallDiagIndex = -1
for wallJndex in range(floorMeta.shape[0]):
# a diagonal can not be formed using adjacent vertices or 'wallIndex' itself.
if wallJndex == wallIndex or wallJndex == (
wallIndex + 1) % floorMeta.shape[0] or wallIndex == (
wallJndex + 1) % floorMeta.shape[0]:
continue
trobe = floorMeta[wallJndex][0:2]
# check if the diagonal lies inside the edges of the polygon.
line = LineString((probe, trobe))
if sk.isLineIntersectsWithEdges(line, floorMeta):
continue
# check if some point on the diagonal is inside the polygon.
mPoint = Point((probe + trobe) / 2)
if not floorPoly.contains(mPoint):
continue
LEN = np.linalg.norm(probe - trobe, ord=2)
if LEN > MAXLEN:
MAXLEN = LEN
wallDiagIndex = wallJndex
return wallDiagIndex
def __init__(self):
# Create the observation space
self.observation_space = spaces.Box(low=0,
high=255,
shape=(TRAINING_IMAGE_SIZE[1],
TRAINING_IMAGE_SIZE[0], 3),
dtype=np.uint8)
# Create the action space
self.action_space = spaces.Box(low=np.array([-1, 0]),
high=np.array([+1, +1]),
dtype=np.float32)
if node_type == SIMULATION_WORKER:
# ROS initialization
rospy.init_node('rl_coach', anonymous=True)
# wait for required services
rospy.wait_for_service(
'/deepracer_simulation_environment/get_waypoints')
rospy.wait_for_service(
'/deepracer_simulation_environment/reset_car')
rospy.wait_for_service('/gazebo/get_model_state')
rospy.wait_for_service('/gazebo/get_link_state')
rospy.wait_for_service('/gazebo/clear_joint_forces')
self.get_model_state = rospy.ServiceProxy(
'/gazebo/get_model_state', GetModelState)
self.get_link_state = rospy.ServiceProxy('/gazebo/get_link_state',
GetLinkState)
self.clear_forces_client = rospy.ServiceProxy(
'/gazebo/clear_joint_forces', JointRequest)
self.reset_car_client = rospy.ServiceProxy(
'/deepracer_simulation_environment/reset_car', ResetCarSrv)
get_waypoints_client = rospy.ServiceProxy(
'/deepracer_simulation_environment/get_waypoints',
GetWaypointSrv)
# Create the publishers for sending speed and steering info to the car
self.velocity_pub_dict = OrderedDict()
self.steering_pub_dict = OrderedDict()
for topic in VELOCITY_TOPICS:
self.velocity_pub_dict[topic] = rospy.Publisher(topic,
Float64,
queue_size=1)
for topic in STEERING_TOPICS:
self.steering_pub_dict[topic] = rospy.Publisher(topic,
Float64,
queue_size=1)
# Read in parameters
self.world_name = rospy.get_param('WORLD_NAME')
self.job_type = rospy.get_param('JOB_TYPE')
self.aws_region = rospy.get_param('AWS_REGION')
self.metrics_s3_bucket = rospy.get_param('METRICS_S3_BUCKET')
self.metrics_s3_object_key = rospy.get_param(
'METRICS_S3_OBJECT_KEY')
self.metrics = []
self.simulation_job_arn = 'arn:aws:robomaker:' + self.aws_region + ':' + \
rospy.get_param('ROBOMAKER_SIMULATION_JOB_ACCOUNT_ID') + \
':simulation-job/' + rospy.get_param('AWS_ROBOMAKER_SIMULATION_JOB_ID')
if self.job_type == TRAINING_JOB:
from custom_files.customer_reward_function import reward_function
self.reward_function = reward_function
self.metric_name = rospy.get_param('METRIC_NAME')
self.metric_namespace = rospy.get_param('METRIC_NAMESPACE')
self.training_job_arn = rospy.get_param('TRAINING_JOB_ARN')
self.target_number_of_episodes = rospy.get_param(
'NUMBER_OF_EPISODES')
self.target_reward_score = rospy.get_param(
'TARGET_REWARD_SCORE')
else:
from markov.defaults import reward_function
self.reward_function = reward_function
self.number_of_trials = 0
self.target_number_of_trials = rospy.get_param(
'NUMBER_OF_TRIALS')
# Request the waypoints
waypoints = None
try:
resp = get_waypoints_client()
waypoints = np.array(resp.waypoints).reshape(
resp.row, resp.col)
except Exception as ex:
utils.json_format_logger(
"Unable to retrieve waypoints: {}".format(ex),
**utils.build_system_error_dict(
utils.SIMAPP_ENVIRONMENT_EXCEPTION,
utils.SIMAPP_EVENT_ERROR_CODE_500))
is_loop = np.all(waypoints[0, :] == waypoints[-1, :])
if is_loop:
self.center_line = LinearRing(waypoints[:, 0:2])
self.inner_border = LinearRing(waypoints[:, 2:4])
self.outer_border = LinearRing(waypoints[:, 4:6])
self.road_poly = Polygon(self.outer_border,
[self.inner_border])
else:
self.center_line = LineString(waypoints[:, 0:2])
self.inner_border = LineString(waypoints[:, 2:4])
self.outer_border = LineString(waypoints[:, 4:6])
self.road_poly = Polygon(
np.vstack(
(self.outer_border, np.flipud(self.inner_border))))
self.center_dists = [
self.center_line.project(Point(p), normalized=True)
for p in self.center_line.coords[:-1]
] + [1.0]
self.track_length = self.center_line.length
# Queue used to maintain image consumption synchronicity
self.image_queue = queue.Queue(IMG_QUEUE_BUF_SIZE)
rospy.Subscriber('/camera/zed/rgb/image_rect_color', sensor_image,
self.callback_image)
# Initialize state data
self.episodes = 0
self.start_ndist = 0.0
self.reverse_dir = False
self.change_start = rospy.get_param(
'CHANGE_START_POSITION', (self.job_type == TRAINING_JOB))
self.alternate_dir = rospy.get_param('ALTERNATE_DRIVING_DIRECTION',
False)
self.is_simulation_done = False
self.steering_angle = 0
self.speed = 0
self.action_taken = 0
self.prev_progress = 0
self.prev_point = Point(0, 0)
self.prev_point_2 = Point(0, 0)
self.next_state = None
self.reward = None
self.reward_in_episode = 0
self.done = False
self.steps = 0
self.simulation_start_time = 0
self.allow_servo_step_signals = False
class DeepRacerRacetrackEnv(gym.Env):
def __init__(self):
# Create the observation space
self.observation_space = spaces.Box(low=0,
high=255,
shape=(TRAINING_IMAGE_SIZE[1],
TRAINING_IMAGE_SIZE[0], 3),
dtype=np.uint8)
# Create the action space
self.action_space = spaces.Box(low=np.array([-1, 0]),
high=np.array([+1, +1]),
dtype=np.float32)
if node_type == SIMULATION_WORKER:
# ROS initialization
rospy.init_node('rl_coach', anonymous=True)
# wait for required services
rospy.wait_for_service(
'/deepracer_simulation_environment/get_waypoints')
rospy.wait_for_service(
'/deepracer_simulation_environment/reset_car')
rospy.wait_for_service('/gazebo/get_model_state')
rospy.wait_for_service('/gazebo/get_link_state')
rospy.wait_for_service('/gazebo/clear_joint_forces')
self.get_model_state = rospy.ServiceProxy(
'/gazebo/get_model_state', GetModelState)
self.get_link_state = rospy.ServiceProxy('/gazebo/get_link_state',
GetLinkState)
self.clear_forces_client = rospy.ServiceProxy(
'/gazebo/clear_joint_forces', JointRequest)
self.reset_car_client = rospy.ServiceProxy(
'/deepracer_simulation_environment/reset_car', ResetCarSrv)
get_waypoints_client = rospy.ServiceProxy(
'/deepracer_simulation_environment/get_waypoints',
GetWaypointSrv)
# Create the publishers for sending speed and steering info to the car
self.velocity_pub_dict = OrderedDict()
self.steering_pub_dict = OrderedDict()
for topic in VELOCITY_TOPICS:
self.velocity_pub_dict[topic] = rospy.Publisher(topic,
Float64,
queue_size=1)
for topic in STEERING_TOPICS:
self.steering_pub_dict[topic] = rospy.Publisher(topic,
Float64,
queue_size=1)
# Read in parameters
self.world_name = rospy.get_param('WORLD_NAME')
self.job_type = rospy.get_param('JOB_TYPE')
self.aws_region = rospy.get_param('AWS_REGION')
self.metrics_s3_bucket = rospy.get_param('METRICS_S3_BUCKET')
self.metrics_s3_object_key = rospy.get_param(
'METRICS_S3_OBJECT_KEY')
self.metrics = []
self.simulation_job_arn = 'arn:aws:robomaker:' + self.aws_region + ':' + \
rospy.get_param('ROBOMAKER_SIMULATION_JOB_ACCOUNT_ID') + \
':simulation-job/' + rospy.get_param('AWS_ROBOMAKER_SIMULATION_JOB_ID')
if self.job_type == TRAINING_JOB:
from custom_files.customer_reward_function import reward_function
self.reward_function = reward_function
self.metric_name = rospy.get_param('METRIC_NAME')
self.metric_namespace = rospy.get_param('METRIC_NAMESPACE')
self.training_job_arn = rospy.get_param('TRAINING_JOB_ARN')
self.target_number_of_episodes = rospy.get_param(
'NUMBER_OF_EPISODES')
self.target_reward_score = rospy.get_param(
'TARGET_REWARD_SCORE')
else:
from markov.defaults import reward_function
self.reward_function = reward_function
self.number_of_trials = 0
self.target_number_of_trials = rospy.get_param(
'NUMBER_OF_TRIALS')
# Request the waypoints
waypoints = None
try:
resp = get_waypoints_client()
waypoints = np.array(resp.waypoints).reshape(
resp.row, resp.col)
except Exception as ex:
utils.json_format_logger(
"Unable to retrieve waypoints: {}".format(ex),
**utils.build_system_error_dict(
utils.SIMAPP_ENVIRONMENT_EXCEPTION,
utils.SIMAPP_EVENT_ERROR_CODE_500))
is_loop = np.all(waypoints[0, :] == waypoints[-1, :])
if is_loop:
self.center_line = LinearRing(waypoints[:, 0:2])
self.inner_border = LinearRing(waypoints[:, 2:4])
self.outer_border = LinearRing(waypoints[:, 4:6])
self.road_poly = Polygon(self.outer_border,
[self.inner_border])
else:
self.center_line = LineString(waypoints[:, 0:2])
self.inner_border = LineString(waypoints[:, 2:4])
self.outer_border = LineString(waypoints[:, 4:6])
self.road_poly = Polygon(
np.vstack(
(self.outer_border, np.flipud(self.inner_border))))
self.center_dists = [
self.center_line.project(Point(p), normalized=True)
for p in self.center_line.coords[:-1]
] + [1.0]
self.track_length = self.center_line.length
# Queue used to maintain image consumption synchronicity
self.image_queue = queue.Queue(IMG_QUEUE_BUF_SIZE)
rospy.Subscriber('/camera/zed/rgb/image_rect_color', sensor_image,
self.callback_image)
# Initialize state data
self.episodes = 0
self.start_ndist = 0.0
self.reverse_dir = False
self.change_start = rospy.get_param(
'CHANGE_START_POSITION', (self.job_type == TRAINING_JOB))
self.alternate_dir = rospy.get_param('ALTERNATE_DRIVING_DIRECTION',
False)
self.is_simulation_done = False
self.steering_angle = 0
self.speed = 0
self.action_taken = 0
self.prev_progress = 0
self.prev_point = Point(0, 0)
self.prev_point_2 = Point(0, 0)
self.next_state = None
self.reward = None
self.reward_in_episode = 0
self.done = False
self.steps = 0
self.simulation_start_time = 0
self.allow_servo_step_signals = False
def reset(self):
if node_type == SAGEMAKER_TRAINING_WORKER:
return self.observation_space.sample()
# Simulation is done - so RoboMaker will start to shut down the app.
# Till RoboMaker shuts down the app, do nothing more else metrics may show unexpected data.
if (node_type == SIMULATION_WORKER) and self.is_simulation_done:
while True:
time.sleep(1)
self.steering_angle = 0
self.speed = 0
self.action_taken = 0
self.prev_progress = 0
self.prev_point = Point(0, 0)
self.prev_point_2 = Point(0, 0)
self.next_state = None
self.reward = None
self.reward_in_episode = 0
self.done = False
# Reset the car and record the simulation start time
if self.allow_servo_step_signals:
self.send_action(0, 0)
self.racecar_reset()
self.steps = 0
self.simulation_start_time = time.time()
self.infer_reward_state(0, 0)
return self.next_state
def set_next_state(self):
# Make sure the first image is the starting image
image_data = self.image_queue.get(block=True, timeout=None)
# Read the image and resize to get the state
image = Image.frombytes('RGB', (image_data.width, image_data.height),
image_data.data, 'raw', 'RGB', 0, 1)
image = image.resize(TRAINING_IMAGE_SIZE, resample=2)
self.next_state = np.array(image)
def racecar_reset(self):
try:
for joint in EFFORT_JOINTS:
self.clear_forces_client(joint)
prev_index, next_index = self.find_prev_next_waypoints(
self.start_ndist)
self.reset_car_client(self.start_ndist, next_index)
# First clear the queue so that we set the state to the start image
_ = self.image_queue.get(block=True, timeout=None)
self.set_next_state()
except Exception as ex:
utils.json_format_logger(
"Unable to reset the car: {}".format(ex),
**utils.build_system_error_dict(
utils.SIMAPP_ENVIRONMENT_EXCEPTION,
utils.SIMAPP_EVENT_ERROR_CODE_500))
def set_allow_servo_step_signals(self, allow_servo_step_signals):
self.allow_servo_step_signals = allow_servo_step_signals
def step(self, action):
if node_type == SAGEMAKER_TRAINING_WORKER:
return self.observation_space.sample(), 0, False, {}
# Initialize next state, reward, done flag
self.next_state = None
self.reward = None
self.done = False
# Send this action to Gazebo and increment the step count
self.steering_angle = float(action[0])
self.speed = float(action[1])
if self.allow_servo_step_signals:
self.send_action(self.steering_angle, self.speed)
self.steps += 1
# Compute the next state and reward
self.infer_reward_state(self.steering_angle, self.speed)
return self.next_state, self.reward, self.done, {}
def callback_image(self, data):
try:
self.image_queue.put_nowait(data)
except queue.Full:
pass
except Exception as ex:
utils.json_format_logger(
"Error retrieving frame from gazebo: {}".format(ex),
**utils.build_system_error_dict(
utils.SIMAPP_ENVIRONMENT_EXCEPTION,
utils.SIMAPP_EVENT_ERROR_CODE_500))
def send_action(self, steering_angle, speed):
# Simple v/r to computes the desired rpm
wheel_rpm = speed / WHEEL_RADIUS
for _, pub in self.velocity_pub_dict.items():
pub.publish(wheel_rpm)
for _, pub in self.steering_pub_dict.items():
pub.publish(steering_angle)
def infer_reward_state(self, steering_angle, speed):
try:
self.set_next_state()
except Exception as ex:
utils.json_format_logger(
"Unable to retrieve image from queue: {}".format(ex),
**utils.build_system_error_dict(
utils.SIMAPP_ENVIRONMENT_EXCEPTION,
utils.SIMAPP_EVENT_ERROR_CODE_500))
# Read model state from Gazebo
model_state = self.get_model_state('racecar', '')
model_orientation = Rotation.from_quat([
model_state.pose.orientation.x, model_state.pose.orientation.y,
model_state.pose.orientation.z, model_state.pose.orientation.w
])
model_location = np.array([
model_state.pose.position.x,
model_state.pose.position.y,
model_state.pose.position.z]) + \
model_orientation.apply(RELATIVE_POSITION_OF_FRONT_OF_CAR)
model_point = Point(model_location[0], model_location[1])
model_heading = model_orientation.as_euler('zyx')[0]
# Read the wheel locations from Gazebo
left_rear_wheel_state = self.get_link_state('racecar::left_rear_wheel',
'')
left_front_wheel_state = self.get_link_state(
'racecar::left_front_wheel', '')
right_rear_wheel_state = self.get_link_state(
'racecar::right_rear_wheel', '')
right_front_wheel_state = self.get_link_state(
'racecar::right_front_wheel', '')
wheel_points = [
Point(left_rear_wheel_state.link_state.pose.position.x,
left_rear_wheel_state.link_state.pose.position.y),
Point(left_front_wheel_state.link_state.pose.position.x,
left_front_wheel_state.link_state.pose.position.y),
Point(right_rear_wheel_state.link_state.pose.position.x,
right_rear_wheel_state.link_state.pose.position.y),
Point(right_front_wheel_state.link_state.pose.position.x,
right_front_wheel_state.link_state.pose.position.y)
]
# Project the current location onto the center line and find nearest points
current_ndist = self.center_line.project(model_point, normalized=True)
prev_index, next_index = self.find_prev_next_waypoints(current_ndist)
distance_from_prev = model_point.distance(
Point(self.center_line.coords[prev_index]))
distance_from_next = model_point.distance(
Point(self.center_line.coords[next_index]))
closest_waypoint_index = (
prev_index, next_index)[distance_from_next < distance_from_prev]
# Compute distance from center and road width
nearest_point_center = self.center_line.interpolate(current_ndist,
normalized=True)
nearest_point_inner = self.inner_border.interpolate(
self.inner_border.project(nearest_point_center))
nearest_point_outer = self.outer_border.interpolate(
self.outer_border.project(nearest_point_center))
distance_from_center = nearest_point_center.distance(model_point)
distance_from_inner = nearest_point_inner.distance(model_point)
distance_from_outer = nearest_point_outer.distance(model_point)
track_width = nearest_point_inner.distance(nearest_point_outer)
is_left_of_center = (distance_from_outer < distance_from_inner) if self.reverse_dir \
else (distance_from_inner < distance_from_outer)
# Convert current progress to be [0,100] starting at the initial waypoint
if self.reverse_dir:
current_progress = self.start_ndist - current_ndist
else:
current_progress = current_ndist - self.start_ndist
if current_progress < 0.0: current_progress = current_progress + 1.0
current_progress = 100 * current_progress
if current_progress < self.prev_progress:
# Either: (1) we wrapped around and have finished the track,
delta1 = current_progress + 100 - self.prev_progress
# or (2) for some reason the car went backwards (this should be rare)
delta2 = self.prev_progress - current_progress
current_progress = (self.prev_progress, 100)[delta1 < delta2]
# Car is off track if all wheels are outside the borders
wheel_on_track = [self.road_poly.contains(p) for p in wheel_points]
all_wheels_on_track = all(wheel_on_track)
any_wheels_on_track = any(wheel_on_track)
# Compute the reward
if any_wheels_on_track:
done = False
params = {
'all_wheels_on_track': all_wheels_on_track,
'x': model_point.x,
'y': model_point.y,
'heading': model_heading * 180.0 / math.pi,
'distance_from_center': distance_from_center,
'progress': current_progress,
'steps': self.steps,
'speed': speed,
'steering_angle': steering_angle * 180.0 / math.pi,
'track_width': track_width,
'waypoints': list(self.center_line.coords),
'closest_waypoints': [prev_index, next_index],
'is_left_of_center': is_left_of_center,
'is_reversed': self.reverse_dir
}
try:
reward = float(self.reward_function(params))
except Exception as e:
utils.json_format_logger(
"Exception {} in customer reward function. Job failed!".
format(e),
**utils.build_user_error_dict(
utils.SIMAPP_SIMULATION_WORKER_EXCEPTION,
utils.SIMAPP_EVENT_ERROR_CODE_400))
traceback.print_exc()
sys.exit(1)
else:
done = True
reward = CRASHED
# Reset if the car position hasn't changed in the last 2 steps
prev_pnt_dist = min(model_point.distance(self.prev_point),
model_point.distance(self.prev_point_2))
if prev_pnt_dist <= 0.0001 and self.steps % NUM_STEPS_TO_CHECK_STUCK == 0:
done = True
reward = CRASHED # stuck
# Simulation jobs are done when progress reaches 100
if current_progress >= 100:
done = True
# Keep data from the previous step around
self.prev_point_2 = self.prev_point
self.prev_point = model_point
self.prev_progress = current_progress
# Set the reward and done flag
self.reward = reward
self.reward_in_episode += reward
self.done = done
# Trace logs to help us debug and visualize the training runs
# btown TODO: This should be written to S3, not to CWL.
logger.info(
'SIM_TRACE_LOG:%d,%d,%.4f,%.4f,%.4f,%.2f,%.2f,%d,%.4f,%s,%s,%.4f,%d,%.2f,%s\n'
%
(self.episodes, self.steps, model_location[0], model_location[1],
model_heading, self.steering_angle, self.speed, self.action_taken,
self.reward, self.done, all_wheels_on_track, current_progress,
closest_waypoint_index, self.track_length, time.time()))
metrics = {
"episodes": self.episodes,
"steps": self.steps,
"x": model_location[0],
"y": model_location[1],
"heading": model_heading,
"steering_angle": self.steering_angle,
"speed": self.speed,
"action": self.action_taken,
"reward": self.reward,
"done": self.done,
"all_wheels_on_track": all_wheels_on_track,
"current_progress": current_progress,
"closest_waypoint_index": closest_waypoint_index,
"track_length": self.track_length,
"time": time.time()
}
self.log_to_datadog(rewards)
# Terminate this episode when ready
if done and node_type == SIMULATION_WORKER:
self.finish_episode(current_progress)
def find_prev_next_waypoints(self, ndist):
if self.reverse_dir:
next_index = bisect.bisect_left(self.center_dists, ndist) - 1
prev_index = next_index + 1
if next_index == -1: next_index = len(self.center_dists) - 1
else:
next_index = bisect.bisect_right(self.center_dists, ndist)
prev_index = next_index - 1
if next_index == len(self.center_dists): next_index = 0
return prev_index, next_index
def stop_car(self):
self.steering_angle = 0
self.speed = 0
self.action_taken = 0
self.send_action(0, 0)
self.racecar_reset()
def finish_episode(self, progress):
# Increment episode count, update start position and direction
self.episodes += 1
if self.change_start:
self.start_ndist = (self.start_ndist +
ROUND_ROBIN_ADVANCE_DIST) % 1.0
if self.alternate_dir:
self.reverse_dir = not self.reverse_dir
# Reset the car
self.stop_car()
# Update metrics based on job type
if self.job_type == TRAINING_JOB:
self.send_reward_to_cloudwatch(self.reward_in_episode)
self.update_training_metrics()
self.write_metrics_to_s3()
if self.is_training_done():
self.cancel_simulation_job()
elif self.job_type == EVALUATION_JOB:
self.number_of_trials += 1
self.update_eval_metrics(progress)
self.write_metrics_to_s3()
def update_eval_metrics(self, progress):
eval_metric = {}
eval_metric['completion_percentage'] = int(progress)
eval_metric['metric_time'] = int(round(time.time() * 1000))
eval_metric['start_time'] = int(
round(self.simulation_start_time * 1000))
eval_metric['elapsed_time_in_milliseconds'] = int(
round((time.time() - self.simulation_start_time) * 1000))
eval_metric['trial'] = int(self.number_of_trials)
self.metrics.append(eval_metric)
def update_training_metrics(self):
training_metric = {}
training_metric['reward_score'] = int(round(self.reward_in_episode))
training_metric['metric_time'] = int(round(time.time() * 1000))
training_metric['start_time'] = int(
round(self.simulation_start_time * 1000))
training_metric['elapsed_time_in_milliseconds'] = int(
round((time.time() - self.simulation_start_time) * 1000))
training_metric['episode'] = int(self.episodes)
self.metrics.append(training_metric)
def write_metrics_to_s3(self):
session = boto3.session.Session()
s3_client = session.client('s3', region_name=self.aws_region)
metrics_body = json.dumps({'metrics': self.metrics})
s3_client.put_object(Bucket=self.metrics_s3_bucket,
Key=self.metrics_s3_object_key,
Body=bytes(metrics_body, encoding='utf-8'))
def is_training_done(self):
if ((self.target_number_of_episodes > 0) and (self.target_number_of_episodes == self.episodes)) or \
((isinstance(self.target_reward_score, (int, float))) and (self.target_reward_score <= self.reward_in_episode)):
self.is_simulation_done = True
return self.is_simulation_done
def cancel_simulation_job(self):
session = boto3.session.Session()
robomaker_client = session.client('robomaker',
region_name=self.aws_region)
robomaker_client.cancel_simulation_job(job=self.simulation_job_arn)
def log_to_datadog(self, metrics):
for metric, value in metrics.items():
if isinstance(value, (int, float)):
api.Metric.send(metric=metric,
points=value,
tags=self.job_type + "-" +
SIMULATION_START_TIMESTAMP)
def send_reward_to_cloudwatch(self, reward):
session = boto3.session.Session()
cloudwatch_client = session.client('cloudwatch',
region_name=self.aws_region)
cloudwatch_client.put_metric_data(MetricData=[
{
'MetricName':
self.metric_name,
'Dimensions': [
{
'Name': 'TRAINING_JOB_ARN',
'Value': self.training_job_arn
},
],
'Unit':
'None',
'Value':
reward
},
],
Namespace=self.metric_namespace)
def extendProcessing(boxGeoDataFrame, linesGeoDataFrame, epsgValue,
dirNameVectorObjResults, linesExtendedFileName,
vectorMask, col, row, seedValue):
"""
extendProcessing.
:param boxGeoDataFrame: (Box) box
:param linesGeoDataFrame: (int) lines
:param epsgValue: (int) spatial reference system
:param dirNameVectorObjResults: (String) vector objects folder
:param linesExtendedFileName: (String) lines extended file.
:param seedValue: (int) seed for crop rows orientation.
:returns none: None.
"""
crsEpsgId = {'init': 'epsg:' + str(epsgValue)}
longLinesArray = []
idLongLinesArray = []
cuttedLineArray = []
newidcutedline = []
newobjdistances = []
distanceLinear = []
index = []
flagCounter = 0
externalPoint = Point((0, 0))
#Extrapolate lines
for x in range(0, len(linesGeoDataFrame.geometry)):
linea_bx = (list(linesGeoDataFrame.geometry[x].coords))
extrapoledLine = getExtrapoledLine(*linea_bx[-2:])
idLongLinesArray.append(x)
longLinesArray.append(extrapoledLine)
dataFrameLongLines = pd.DataFrame({'id': idLongLinesArray})
longLinesGeoDataFrame = gpd.GeoDataFrame(dataFrameLongLines,
crs=crsEpsgId,
geometry=longLinesArray)
crutils.printLogMsg(crglobals.DONE_MSG + 'Generated long lines !')
dataFrameLineCuttedByBox = (longLinesGeoDataFrame.intersection(
boxGeoDataFrame.geometry.iloc[0]))
geoDataFrameLinesCuttedByBox = gpd.GeoDataFrame(
crs=crsEpsgId, geometry=dataFrameLineCuttedByBox)
crutils.printLogMsg(crglobals.DONE_MSG + 'Cut long lines by bounds !')
#############################################
### TEST #change 06-06-2018
#Get the convex hull lines
convexHullFromMask = vectorMask.convex_hull.iloc[0]
x, y = convexHullFromMask.exterior.xy
pointsConvexHullFromMaskArray = np.array(list(zip(x, y)))
minBBoxRect = imboundrect.minimum_bounding_rectangle(
pointsConvexHullFromMaskArray)
polygonOMBB = Polygon(
[minBBoxRect[0], minBBoxRect[1], minBBoxRect[2], minBBoxRect[3]])
#cut lines by ombb
#longLinesGeoDataFrame
dataFrameLineCuttedByMask = (
geoDataFrameLinesCuttedByBox.intersection(polygonOMBB))
#############################################
#change 06-06-2018
#dataFrameLineCuttedByMask=(geoDataFrameLinesCuttedByBox.intersection(vectorMask.geometry.iloc[0]))
geoDataFrameLineCuttedByMask = gpd.GeoDataFrame(
crs=crsEpsgId, geometry=dataFrameLineCuttedByMask)
crutils.printLogMsg(crglobals.DONE_MSG + 'Line clipping by mask!')
#################################
#if cutlinedk[0].length > 0:
# angle=crutils.getAzimuth( (cutlinedk[0].coords[0][0]) , (cutlinedk[0].coords[0][1]) , (cutlinedk[0].coords[1][0]) , (cutlinedk[0].coords[1][1]) )
# anglep =(angle+270)
# xp = (np.min(box.geometry.bounds.minx)) + np.sin(np.deg2rad(anglep)) * 10
# yp = (np.max(box.geometry.bounds.maxy)) + np.cos(np.deg2rad(anglep)) * 10
# externalPoint = Point( ( xp,yp ) )
#################################
#print(str(anglep))
#print(cutlinedk[0].centroid.x)
#print(cutlinedk[0].centroid.y)
#print('--------------ANGULO -------------------')
#print(angle)
#print('--------------ANGULO PERPEN-------------------')
#xp = (cutlinedk[0].centroid.x) + np.sin(np.deg2rad(anglep)) * 20
#yp = (cutlinedk[0].centroid.y) + np.cos(np.deg2rad(anglep)) * 20
#print( 'POINT( %s %s )' % ( xp,yp))
#####
#TODO: Order id by spatial criteria
#####
#line1 = LineString([(np.min(box.geometry.bounds.minx), np.min(box.geometry.bounds.miny)),
# (np.max(box.geometry.bounds.maxx), np.min(box.geometry.bounds.miny))])
crutils.printLogMsg(crglobals.DONE_MSG +
'Calculate distance by seed criteria : %s ' %
(str(seedValue)))
projectDistance = 100
if (seedValue == 1):
pnt0Calc = (LineString([(np.min(boxGeoDataFrame.geometry.bounds.minx),
np.max(boxGeoDataFrame.geometry.bounds.maxy)),
(np.max(boxGeoDataFrame.geometry.bounds.maxx),
np.max(boxGeoDataFrame.geometry.bounds.maxy))
])).centroid
elif (seedValue == 2):
pnt0Calc = (LineString([(np.min(boxGeoDataFrame.geometry.bounds.minx),
np.max(boxGeoDataFrame.geometry.bounds.maxy)),
(np.min(boxGeoDataFrame.geometry.bounds.minx),
np.min(boxGeoDataFrame.geometry.bounds.miny))
])).centroid
elif (seedValue == 3):
if dataFrameLineCuttedByMask[0].length > 0:
angle = crutils.getAzimuth(
(dataFrameLineCuttedByMask[0].coords[0][0]),
(dataFrameLineCuttedByMask[0].coords[0][1]),
(dataFrameLineCuttedByMask[0].coords[1][0]),
(dataFrameLineCuttedByMask[0].coords[1][1]))
anglep = (angle + 270)
xp = (np.min(boxGeoDataFrame.geometry.bounds.minx)) + np.sin(
np.deg2rad(anglep)) * projectDistance
yp = (np.max(boxGeoDataFrame.geometry.bounds.maxy)) + np.cos(
np.deg2rad(anglep)) * projectDistance
externalPoint = Point((xp, yp))
pnt0Calc = externalPoint
#Point((np.min(boxGeoDataFrame.geometry.bounds.minx), np.max(boxGeoDataFrame.geometry.bounds.maxy)))
elif (seedValue == 4):
if dataFrameLineCuttedByMask[0].length > 0:
angle = crutils.getAzimuth(
(dataFrameLineCuttedByMask[0].coords[0][0]),
(dataFrameLineCuttedByMask[0].coords[0][1]),
(dataFrameLineCuttedByMask[0].coords[1][0]),
(dataFrameLineCuttedByMask[0].coords[1][1]))
anglep = (angle + 270)
xp = (np.max(boxGeoDataFrame.geometry.bounds.maxx)) + np.sin(
np.deg2rad(anglep)) * projectDistance
yp = (np.max(boxGeoDataFrame.geometry.bounds.maxy)) + np.cos(
np.deg2rad(anglep)) * projectDistance
externalPoint = Point((xp, yp))
pnt0Calc = externalPoint
#pnt0Calc = Point((np.max(boxGeoDataFrame.geometry.bounds.maxx), np.max(boxGeoDataFrame.geometry.bounds.maxy)))
boxminxmaypoint = pnt0Calc
crutils.printLogMsg(crglobals.DONE_MSG +
'%s chosen for distance calculation' %
(boxminxmaypoint))
for x in range(0, len(geoDataFrameLineCuttedByMask.geometry)):
if geoDataFrameLineCuttedByMask.geometry.geom_type[x] == 'LineString':
if (len(list(
geoDataFrameLineCuttedByMask.geometry[x].coords))) == 2:
linea_bx = LineString(
list(geoDataFrameLineCuttedByMask.geometry[x].coords))
if (linea_bx.length > crglobals.MINLINEDISTANCE):
index.append(flagCounter)
flagCounter += 1
#newidcutedline.append(x)
cuttedLineArray.append(linea_bx)
distanceLinear.append(linea_bx.length)
#print('centroid')
#print(linea_bx.centroid)
distanceplin = boxminxmaypoint.distance(linea_bx.centroid)
newobjdistances.append(distanceplin)
sortedDistance = np.argsort(newobjdistances).astype('int')
idByGeo = [x for _, x in sorted(zip(sortedDistance, index))]
#Sort Distances
newObjDistancesSorted = np.sort(newobjdistances)
#Removing Adjacents and lines duplicates
newObjDistancesSorted = crutils.removeAdjacentsInArray(
newObjDistancesSorted)
crutils.printLogMsg(crglobals.DONE_MSG +
'Removed adjacents and duplicate lines !')
#print('distances: %s ' % (newobjdistances) )
#print('---------->distances kk: %s ' % (newObjDistancesSorted) )
##Removing Closing Lines
pairsdistances = zip([0] + newObjDistancesSorted, newObjDistancesSorted)
distancesFiltered = [
pair[1] for pair in pairsdistances
if abs(pair[0] - pair[1]) >= crglobals.MINCROPROWDISTANCE
]
#distancesFiltered = [pair[1] for pair in pairsdistances if abs(pair[0]-pair[1]) >= crglobals.MINCROPROWDISTANCE and abs(pair[0]-pair[1]) <= crglobals.MAXCROPROWDISTANCE ]
#remove
#add 3-9-2018
#pairsdistances2 = zip([0]+distancesFiltered, distancesFiltered)
#distancesFiltered = [pair2[1] for pair2 in pairsdistances2 if abs(pair2[0]-pair2[1]) <= crglobals.MAXCROPROWDISTANCE ]
#distancesFiltered.append(newObjDistancesSorted[len(newObjDistancesSorted)])
crutils.printLogMsg(crglobals.DONE_MSG +
'Removed closing lines by proximity MIN : %s units ' %
(str(crglobals.MINCROPROWDISTANCE)))
#crutils.printLogMsg(crglobals.DONE_MSG+'Removed closing lines by proximity MAX : %s units ' % ( str(crglobals.MAXCROPROWDISTANCE)) )
#print('new x: %s ' % (distancesFiltered) )
getIndexes = lambda x, xs: [
i for (y, i) in zip(xs, range(len(xs))) if x == y
]
#look for
k = []
flagCounter3 = 0
for x in distancesFiltered:
#print(distancesFiltered[i])
#print(getIndexes(distancesFiltered[i],newobjdistances))
k.append(
getIndexes(distancesFiltered[flagCounter3], newobjdistances)[0])
flagCounter3 = flagCounter3 + 1
#Reindex lines filtered
index2 = []
flagCounter2 = 0
m = []
j = 0
for x in k:
m.append(cuttedLineArray[k[j]])
index2.append(flagCounter2)
flagCounter2 += 1
j = j + 1
sortdistance2 = np.argsort(distancesFiltered).astype('int')
idByGeo2 = [x for _, x in sorted(zip(sortdistance2, index2))]
crutils.printLogMsg(crglobals.DONE_MSG + 'Re-indexing candidate lines !')
#Fix distances substracting projectDistance
arrayDistances = np.array(distancesFiltered)
fixdist = arrayDistances - projectDistance
crutils.printLogMsg(crglobals.DONE_MSG + 'Distances fixed !')
dataFrameFixedLines = pd.DataFrame({
'id': k,
'col': str(col),
'row': str(row),
'colrow': str(col) + '_' + str(row)
})
geoDataFrameFixedLines = gpd.GeoDataFrame(dataFrameFixedLines,
crs=crsEpsgId,
geometry=m)
geoDataFrameFixedLines.dropna()
extfile = os.path.join(
dirNameVectorObjResults, linesExtendedFileName
) #dirNameVectorObjResults+'/'+linesExtendedFileName
if (len(geoDataFrameFixedLines.values) > 0):
#ddkfhmm.to_file(driver = 'ESRI Shapefile', filename=str(extfile))
crutils.printLogMsg(crglobals.DONE_MSG +
'Writing file line extended and clipped: %s ' %
(extfile))
geoDataFrameFixedLines.to_file(driver='ESRI Shapefile',
filename=str(extfile))
else:
crutils.printLogMsg(crglobals.FAIL_MSG +
'Invalid geometry skip file writing: %s ' %
(extfile))
return 1
[ 5.53164382, 1.27253724],
[ 5.44249526, 1.31371417],
[ 5.35201168, 1.35311694],
[ 5.26023149, 1.39090822],
[ 5.16720118, 1.42725323],
[ 5.07297767, 1.46231616],
[ 4.97763079, 1.49625767],
[ 4.88124504, 1.52923333],
[ 4.78391895, 1.5613916 ],
[ 4.68575941, 1.59286997],
[ 4.58690814, 1.62381599]])
# Globals
g_last_progress_value = 0.0
g_start_offset_percent = 0.0
g_race_line_string = LineString(CANADA_RACE_LINE)
TARGET_NUMBER_STEPS = 100
#===============================================================================
#
# REWARD
#
#===============================================================================
def reward_function(params):
reward = progress_factor(params) # add rewards shaping later here
return float(max(reward, 1e-3)) # make sure we never return exactly zero
# Using the race-line coords, calculate progress the same way
# that params['progress'] is calculated (without support for reverse direction)
def __init__(self):
# Create the observation space
img_width = TRAINING_IMAGE_SIZE[0]
img_height = TRAINING_IMAGE_SIZE[1]
self.observation_space = spaces.Box(low=0,
high=255,
shape=(img_height, img_width, 3),
dtype=np.uint8)
# Create the action space
self.action_space = spaces.Box(low=np.array([-1, 0]),
high=np.array([+1, +1]),
dtype=np.float32)
if node_type == SIMULATION_WORKER:
# ROS initialization
rospy.init_node('rl_coach', anonymous=True)
rospy.Subscriber('/camera/zed/rgb/image_rect_color', sensor_image,
self.callback_image)
self.ack_publisher = rospy.Publisher(
'/vesc/low_level/ackermann_cmd_mux/output',
AckermannDriveStamped,
queue_size=100)
self.set_model_state = rospy.ServiceProxy(
'/gazebo/set_model_state', SetModelState)
self.get_model_state = rospy.ServiceProxy(
'/gazebo/get_model_state', GetModelState)
self.get_link_state = rospy.ServiceProxy('/gazebo/get_link_state',
GetLinkState)
# Read in parameters
self.world_name = rospy.get_param('WORLD_NAME')
self.job_type = rospy.get_param('JOB_TYPE')
self.aws_region = rospy.get_param('AWS_REGION')
self.metrics_s3_bucket = rospy.get_param('METRICS_S3_BUCKET')
self.metrics_s3_object_key = rospy.get_param(
'METRICS_S3_OBJECT_KEY')
self.metrics = []
self.simulation_job_arn = 'arn:aws:robomaker:' + self.aws_region + ':' + \
rospy.get_param('ROBOMAKER_SIMULATION_JOB_ACCOUNT_ID') + \
':simulation-job/' + rospy.get_param('AWS_ROBOMAKER_SIMULATION_JOB_ID')
if self.job_type == TRAINING_JOB:
from custom_files.customer_reward_function import reward_function
self.reward_function = reward_function
self.metric_name = rospy.get_param('METRIC_NAME')
self.metric_namespace = rospy.get_param('METRIC_NAMESPACE')
self.training_job_arn = rospy.get_param('TRAINING_JOB_ARN')
self.target_number_of_episodes = rospy.get_param(
'NUMBER_OF_EPISODES')
self.target_reward_score = rospy.get_param(
'TARGET_REWARD_SCORE')
else:
from markov.defaults import reward_function
self.reward_function = reward_function
self.number_of_trials = 0
self.target_number_of_trials = rospy.get_param(
'NUMBER_OF_TRIALS')
# Read in the waypoints
BUNDLE_CURRENT_PREFIX = os.environ.get("BUNDLE_CURRENT_PREFIX",
None)
if not BUNDLE_CURRENT_PREFIX:
raise ValueError("Cannot get BUNDLE_CURRENT_PREFIX")
route_file_name = os.path.join(BUNDLE_CURRENT_PREFIX, 'install',
'deepracer_simulation', 'share',
'deepracer_simulation', 'routes',
'{}.npy'.format(self.world_name))
waypoints = np.load(route_file_name)
self.is_loop = np.all(waypoints[0, :] == waypoints[-1, :])
if self.is_loop:
self.center_line = LinearRing(waypoints[:, 0:2])
self.inner_border = LinearRing(waypoints[:, 2:4])
self.outer_border = LinearRing(waypoints[:, 4:6])
self.road_poly = Polygon(self.outer_border,
[self.inner_border])
else:
self.center_line = LineString(waypoints[:, 0:2])
self.inner_border = LineString(waypoints[:, 2:4])
self.outer_border = LineString(waypoints[:, 4:6])
self.road_poly = Polygon(
np.vstack(
(self.outer_border, np.flipud(self.inner_border))))
self.center_dists = [
self.center_line.project(Point(p), normalized=True)
for p in self.center_line.coords[:-1]
] + [1.0]
self.track_length = self.center_line.length
# Initialize state data
self.episodes = 0
self.start_dist = 0.0
self.round_robin = (self.job_type == TRAINING_JOB)
self.is_simulation_done = False
self.image = None
self.steering_angle = 0
self.speed = 0
self.action_taken = 0
self.prev_progress = 0
self.prev_point = Point(0, 0)
self.prev_point_2 = Point(0, 0)
self.next_state = None
self.reward = None
self.reward_in_episode = 0
self.done = False
self.steps = 0
self.simulation_start_time = 0
self.reverse_dir = False
def _build_spline(self):
"""Build spline for lane change
Returns:
tuple: lane change lane, lane point distance,
prepared lane change spline.
"""
# cetner line
center_line = self._track_data.center_line
# start lane
start_lane_line = self._start_lane.lane["track_line"]
start_lane_dists = self._start_lane.lane["dists"]
# end lane
end_lane_line = self._end_lane.lane["track_line"]
end_lane_dists = self._end_lane.lane["dists"]
start_lane_point = Point(
np.array(self._start_lane.eval_spline(
self._lane_change_start_dist))[:, 0])
end_lane_point = Point(
np.array(self._end_lane.eval_spline(
self._lane_change_end_dist))[:, 0])
end_offset = (0.0 if
(self._lane_change_start_dist <
self._lane_change_end_dist) else center_line.length)
# Find prev/next points on each lane
current_prev_index = bisect.bisect_left(start_lane_dists,
self._current_dist) - 1
start_prev_index = bisect.bisect_left(start_lane_dists,
self._lane_change_start_dist) - 1
end_next_index = bisect.bisect_right(end_lane_dists,
self._lane_change_end_dist)
# Define intervals on start/end lanes to build the spline from
num_start_coords = len(start_lane_line.coords)
num_end_coords = len(end_lane_line.coords)
if self._track_data.is_loop:
num_start_coords -= 1
num_end_coords -= 1
start_index_0 = (current_prev_index - 3) % num_start_coords
start_index_1 = start_prev_index
end_index_0 = end_next_index
end_index_1 = (end_next_index + 3) % num_end_coords
# Grab waypoint indices for these intervals (some corner cases here...)
if start_index_0 < start_index_1:
start_indices = list(range(start_index_0, start_index_1 + 1))
start_offsets = [0.0] * len(start_indices)
else:
start_indices_0 = list(range(start_index_0, num_start_coords))
start_indices_1 = list(range(start_index_1 + 1))
start_indices = start_indices_0 + start_indices_1
start_offsets = [-center_line.length] * len(
start_indices_0) + [0.0] * len(start_indices_1)
if end_index_0 < end_index_1:
end_indices = list(range(end_index_0, end_index_1 + 1))
end_offsets = [end_offset] * len(end_indices)
else:
end_indices_0 = list(range(end_index_0, num_end_coords))
end_indices_1 = list(range(end_index_1 + 1))
end_indices = end_indices_0 + end_indices_1
end_offsets = [end_offset] * len(end_indices_0) + [
end_offset + center_line.length
] * len(end_indices_1)
# Logic to avoid start and end point are too close to track waypoints
before_start_lane_point = Point(
np.array(start_lane_line.coords.xy)[:, start_indices[-1]])
after_end_lane_point = Point(
np.array(end_lane_line.coords.xy)[:, end_indices[0]])
if before_start_lane_point.distance(start_lane_point) < DIST_THRESHOLD:
# pop last index of start_indices
start_indices.pop()
start_offsets.pop()
if after_end_lane_point.distance(end_lane_point) < DIST_THRESHOLD:
# pop first index of end_indices
end_indices.pop(0)
end_offsets.pop(0)
# Build the spline
u = np.hstack((
np.array(start_lane_dists)[start_indices] +
np.array(start_offsets),
self._lane_change_start_dist,
self._lane_change_end_dist + end_offset,
np.array(end_lane_dists)[end_indices] + np.array(end_offsets),
))
x = np.hstack((
np.array(start_lane_line.coords.xy)[:, start_indices],
start_lane_point.xy,
end_lane_point.xy,
np.array(end_lane_line.coords.xy)[:, end_indices],
))
u, ui = np.unique(u, return_index=True)
x = x[:, ui]
bot_car_spline, _ = splprep(x, u=u, k=SPLINE_DEGREE, s=0)
return TrackLine(LineString(np.array(
np.transpose(x)))), u, bot_car_spline
def test_race_line_heading(self):
reward.RACE_LINE_WAYPOINTS = reward.OVAL_TRACK_RACE_LINE
reward.g_race_line_dists = [
LineString(reward.RACE_LINE_WAYPOINTS).project(Point(p),
normalized=True)
for p in LineString(reward.RACE_LINE_WAYPOINTS).coords[:-1]
] + [1.0]
params = self.default_params()
# racing line point 10 is close to track centerline on the bottom
params['x'] = reward.OVAL_TRACK_RACE_LINE[9][0]
params['y'] = reward.OVAL_TRACK_RACE_LINE[9][1]
params['heading'] = 0
self.assertTrue(
math.isclose(reward.race_line_heading(params), 1.0, abs_tol=1e-2))
params['heading'] = 45
self.assertTrue(
math.isclose(reward.race_line_heading(params), 0.5, abs_tol=1e-2))
params['heading'] = -45
self.assertTrue(
math.isclose(reward.race_line_heading(params), 0.5, abs_tol=1e-2))
params['heading'] = 90
self.assertTrue(
math.isclose(reward.race_line_heading(params), 0.0, abs_tol=1e-2))
params['heading'] = -90
self.assertTrue(
math.isclose(reward.race_line_heading(params), 0.0, abs_tol=1e-2))
params['heading'] = 30
self.assertTrue(
math.isclose(reward.race_line_heading(params), 0.66, abs_tol=1e-2))
# close to -170
params['x'] = reward.OVAL_TRACK_RACE_LINE[65][0]
params['y'] = reward.OVAL_TRACK_RACE_LINE[65][1]
params['heading'] = 170
self.assertTrue(
math.isclose(reward.race_line_heading(params), 0.81, abs_tol=1e-2))
# close to 170
params['x'] = reward.OVAL_TRACK_RACE_LINE[55][0]
params['y'] = reward.OVAL_TRACK_RACE_LINE[55][1]
params['heading'] = -170
self.assertTrue(
math.isclose(reward.race_line_heading(params), 0.84, abs_tol=1e-2))
# add steering angle
params['x'] = reward.OVAL_TRACK_RACE_LINE[9][0]
params['y'] = reward.OVAL_TRACK_RACE_LINE[9][1]
params['heading'] = 0
params['steering_angle'] = 30
self.assertTrue(
math.isclose(reward.race_line_heading(params), 0.66, abs_tol=1e-2))
# close to -170
params['x'] = reward.OVAL_TRACK_RACE_LINE[65][0]
params['y'] = reward.OVAL_TRACK_RACE_LINE[65][1]
params['heading'] = 170
params['steering_angle'] = 17
self.assertTrue(
math.isclose(reward.race_line_heading(params), 1.0, abs_tol=1e-2))
# close to 170
params['x'] = reward.OVAL_TRACK_RACE_LINE[55][0]
params['y'] = reward.OVAL_TRACK_RACE_LINE[55][1]
params['heading'] = -170
params['steering_angle'] = -20
self.assertTrue(
math.isclose(reward.race_line_heading(params), 0.94, abs_tol=1e-2))
class Polyline(PolylineInterface):
def __init__(self, vertices, closed):
vertices = np.array(vertices) # NOTE this create a copy
if vertices.size > 0:
assert (len(vertices.shape) == 2)
assert (vertices.shape[0] == 3)
# remove duplicates
dup = np.all(np.isclose(vertices[:2, :-1], vertices[:2, 1:]),
axis=0)
dup = np.append(dup, False)
vertices = vertices[:, ~dup]
else:
# ensure shape is correct
vertices = np.empty(shape=(3, 0))
Polyline._init_internal(vertices, closed, self)
def _init_internal(vertices, closed, object=None):
if object is None:
object = Polyline.__new__(Polyline)
object._vertices = vertices
object._closed = closed
object._cavc_up_to_date = False
object._shapely_up_to_date = False
object._lines_up_to_date = False
return object
def _update_cavc(self):
if not self._cavc_up_to_date:
self._cavc_pline = cavc.Polyline(self._vertices, self._closed)
self._cavc_up_to_date = True
def _update_shapely(self):
if not self._shapely_up_to_date:
if self.is_closed():
self._shapely = Polygon(self.to_lines().T)
else:
self._shapely = LineString(self.to_lines().T)
self._shapely_up_to_date = True
def _update_lines(self):
if self._lines_up_to_date:
return
precision = 1e-3
points = []
n = self._vertices.shape[1]
for i in range(n - int(not self._closed)):
a = self._vertices[:2, i]
b = self._vertices[:2, (i + 1) % n]
bulge = self._vertices[2, i]
if points:
points.pop(-1)
if math.isclose(bulge, 0):
points += [a, b]
else:
rot = np.array([[0, -1], [1, 0]])
on_right = bulge >= 0
if not on_right:
rot = -rot
bulge = abs(bulge)
ab = b - a
chord = np.linalg.norm(ab)
radius = chord * (bulge + 1. / bulge) / 4
center_offset = radius - chord * bulge / 2
center = a + ab / 2 + center_offset / chord * rot.dot(ab)
a_dir = a - center
b_dir = b - center
rad_start = math.atan2(a_dir[1], a_dir[0])
rad_end = math.atan2(b_dir[1], b_dir[0])
# TODO handle case where bulge almost 0 or inf (line or circle)
if not math.isclose(rad_start, rad_end):
if on_right != (rad_start < rad_end):
if on_right:
rad_start -= 2 * math.pi
else:
rad_end -= 2 * math.pi
rad_len = abs(rad_end - rad_start)
if radius > precision:
max_angle = 2 * math.acos(1.0 - precision / radius)
else:
max_angle = math.pi
nb_segments = max(2, math.ceil(rad_len / max_angle) + 1)
angles = np.linspace(rad_start, rad_end, nb_segments + 1)
arc_data = (center.reshape(2, 1) + radius * np.vstack(
(np.cos(angles), np.sin(angles))))
points += np.transpose(arc_data).tolist()
self._lines = np.transpose(np.array(points))
self._lines_up_to_date = True
@property
def raw(self):
return self._vertices
@property
def start(self):
return self._vertices[:2, 0]
@property
def end(self):
return self._vertices[:2, -1]
@property
def bounds(self):
self._update_cavc()
min_x, min_y, max_x, max_y = self._cavc_pline.get_extents()
return np.array([[min_x, min_y], [max_x, max_y]])
@property
def centroid(self):
self._update_shapely()
return self._shapely.centroid
def is_closed(self):
return self._closed
def is_ccw(self):
self._update_cavc()
return self._cavc_pline.get_area() >= 0
# TODO
def is_simple(self):
return True
def reverse(self):
polyline = Polyline._init_internal(np.copy(self._vertices),
self._closed)
polyline._vertices = np.flip(polyline._vertices, axis=1)
polyline._vertices[2] = -polyline._vertices[2]
if polyline._closed:
polyline._vertices[:2] = np.roll(polyline._vertices[:2], 1, axis=1)
else:
polyline._vertices[2] = np.roll(polyline._vertices[2], -1)
polyline._vertices[2, -1] = 0.
return polyline
def contains(self, object):
if not self._closed:
return False
if isinstance(object, (list, np.ndarray)):
point = np.array(object)
self._update_cavc()
return self._cavc_pline.get_winding_number(object) != 0
elif isinstance(object, Polyline):
self._update_shapely()
object._update_shapely()
return self._shapely.contains(object._shapely)
else:
raise Exception('Incorrect argument type')
def intersects(self, polyline):
self._update_shapely()
polyline._update_shapely()
return self._shapely.intersects(polyline._shapely)
def affine(self, d, r, s):
polyline = Polyline._init_internal(np.copy(self._vertices),
self._closed)
cos = math.cos(r) * s
sin = math.sin(r)
tr_mat = np.array([[cos, -sin, d[0]], [sin, cos, d[1]], [0, 0, 1]])
vertices = polyline._vertices[:-1]
vertices = np.dot(tr_mat, np.insert(vertices, 2, 1., axis=0))
vertices[-1] = polyline._vertices[-1]
polyline._vertices = vertices
return polyline
def offset(self, offset):
self._update_cavc()
cavc_plines = self._cavc_pline.parallel_offset(offset, 0)
polylines = []
for cavc_pline in cavc_plines:
polyline = Polyline._init_internal(cavc_pline.vertex_data(),
cavc_pline.is_closed())
polyline._cavc_pline = cavc_pline
polyline._cavc_up_to_date = True
polylines.append(polyline)
return polylines
def loop(self, limit_angle, selected_radius, loop_radius):
polyline = Polyline._init_internal(np.copy(self._vertices),
self._closed)
limit_bulge = math.tan((math.pi - limit_angle) / 4)
ids = np.where(np.abs(polyline._vertices[2]) >= limit_bulge)[0]
cur = np.take(polyline._vertices, ids, axis=1)
next_ids = (ids + 1) % polyline._vertices.shape[1]
next = np.take(polyline._vertices[:2], next_ids, axis=1)
# i, x, y, b, h_theta, vx, vy
data = np.vstack(
(ids, cur, 2 * np.arctan(np.abs(cur[2])), next - cur[:2]))
# i, x, y, b, h_theta, vx, vy, d
data = np.vstack((data, np.linalg.norm(data[-2:], axis=0)))
radius = data[7] / (2 * np.sin(data[4]))
ignored = np.where(np.logical_not(np.isclose(radius,
selected_radius)))[0]
data = np.delete(data, ignored, axis=1)
# i, x, y, b, h_theta, vx, vy, d, h
data = np.vstack((data, (data[7] + 2 * loop_radius * np.sin(data[4])) *
np.tan(data[4]) / 2))
a = data[1:3]
ab = data[5:7]
normalized_ab = ab / data[7]
ab_normal = np.array([-normalized_ab[1], normalized_ab[0]])
h = data[8]
top = a + ab / 2 + ab_normal * h
half_top_side = loop_radius * np.sin(data[4])
new_a = top + normalized_ab * half_top_side
new_b = top - normalized_ab * half_top_side
new_b = np.insert(new_b, 2, 0, axis=0)
new_a = np.vstack((new_a, -1 / data[3]))
for i in reversed(range(data.shape[1])):
id = int(data[0, i] + 0.1)
polyline._vertices[2, id] = 0
polyline._vertices = np.insert(polyline._vertices,
id + 1,
new_b[:, i],
axis=1)
polyline._vertices = np.insert(polyline._vertices,
id + 1,
new_a[:, i],
axis=1)
return polyline
def to_lines(self):
self._update_lines()
return self._lines
class DeepRacerRacetrackEnv(gym.Env):
def __init__(self):
# Create the observation space
self.observation_space = spaces.Box(low=0,
high=255,
shape=(TRAINING_IMAGE_SIZE[1],
TRAINING_IMAGE_SIZE[0], 3),
dtype=np.uint8)
# Create the action space
self.action_space = spaces.Box(low=np.array([-1, 0]),
high=np.array([+1, +1]),
dtype=np.float32)
if node_type == SIMULATION_WORKER:
# ROS initialization
rospy.init_node('rl_coach', anonymous=True)
# wait for required services
rospy.wait_for_service(
'/deepracer_simulation_environment/get_waypoints')
rospy.wait_for_service(
'/deepracer_simulation_environment/reset_car')
rospy.wait_for_service('/gazebo/get_model_state')
rospy.wait_for_service('/gazebo/get_link_state')
rospy.wait_for_service('/gazebo/clear_joint_forces')
rospy.wait_for_service('/gazebo/get_physics_properties')
rospy.wait_for_service('/gazebo/set_physics_properties')
self.get_model_state = rospy.ServiceProxy(
'/gazebo/get_model_state', GetModelState)
self.get_link_state = rospy.ServiceProxy('/gazebo/get_link_state',
GetLinkState)
self.clear_forces_client = rospy.ServiceProxy(
'/gazebo/clear_joint_forces', JointRequest)
self.reset_car_client = rospy.ServiceProxy(
'/deepracer_simulation_environment/reset_car', ResetCarSrv)
get_waypoints_client = rospy.ServiceProxy(
'/deepracer_simulation_environment/get_waypoints',
GetWaypointSrv)
get_physics_properties = rospy.ServiceProxy(
'/gazebo/get_physics_properties', GetPhysicsProperties)
set_physics_properties = rospy.ServiceProxy(
'/gazebo/set_physics_properties', SetPhysicsProperties)
# Create the publishers for sending speed and steering info to the car
self.velocity_pub_dict = OrderedDict()
self.steering_pub_dict = OrderedDict()
for topic in VELOCITY_TOPICS:
self.velocity_pub_dict[topic] = rospy.Publisher(topic,
Float64,
queue_size=1)
for topic in STEERING_TOPICS:
self.steering_pub_dict[topic] = rospy.Publisher(topic,
Float64,
queue_size=1)
# Read in parameters
self.world_name = rospy.get_param('WORLD_NAME')
self.job_type = rospy.get_param('JOB_TYPE')
self.aws_region = rospy.get_param('AWS_REGION')
self.metrics_s3_bucket = rospy.get_param('METRICS_S3_BUCKET')
self.metrics_s3_object_key = rospy.get_param(
'METRICS_S3_OBJECT_KEY')
self.aws_endpoint_url = rospy.get_param('AWS_ENDPOINT_URL')
self.metrics = []
self.simulation_job_arn = 'arn:aws:robomaker:' + self.aws_region + ':' + \
rospy.get_param('ROBOMAKER_SIMULATION_JOB_ACCOUNT_ID') + \
':simulation-job/' + rospy.get_param('AWS_ROBOMAKER_SIMULATION_JOB_ID')
self.max_update_rate = rospy.get_param('SIM_UPDATE_RATE')
# Set the simulation rate
try:
resp = utils.gazebo_service_call(get_physics_properties)
# float64 time_step
# bool pause
# float64 max_update_rate
# geometry_msgs/Vector3 gravity
# float64 x
# float64 y
# float64 z
# gazebo_msgs/ODEPhysics ode_config
# bool auto_disable_bodies
# uint32 sor_pgs_precon_iters
# uint32 sor_pgs_iters
# float64 sor_pgs_w
# float64 sor_pgs_rms_error_tol
# float64 contact_surface_layer
# float64 contact_max_correcting_vel
# float64 cfm
# float64 erp
# uint32 max_contacts
# bool success
# string status_message
update_msg = SetPhysicsPropertiesRequest()
update_msg.time_step = resp.time_step
update_msg.max_update_rate = self.max_update_rate
update_msg.gravity = resp.gravity
update_msg.ode_config = resp.ode_config
# float64 time_step
# float64 max_update_rate
# geometry_msgs/Vector3 gravity
# float64 x
# float64 y
# float64 z
# gazebo_msgs/ODEPhysics ode_config
# bool auto_disable_bodies
# uint32 sor_pgs_precon_iters
# uint32 sor_pgs_iters
# float64 sor_pgs_w
# float64 sor_pgs_rms_error_tol
# float64 contact_surface_layer
# float64 contact_max_correcting_vel
# float64 cfm
# float64 erp
# uint32 max_contacts
# ---
# bool success
resp = utils.gazebo_service_call(set_physics_properties,
update_msg)
except Exception as ex:
utils.json_format_logger(
"Unable to set physics update rate: {}".format(ex),
**utils.build_system_error_dict(
utils.SIMAPP_ENVIRONMENT_EXCEPTION,
utils.SIMAPP_EVENT_ERROR_CODE_500))
# Setup SIM_TRACE_LOG data upload to s3
self.setup_simtrace_data_upload_to_s3()
if self.job_type == TRAINING_JOB:
from custom_files.customer_reward_function import reward_function
self.reward_function = reward_function
self.target_number_of_episodes = rospy.get_param(
'NUMBER_OF_EPISODES')
self.target_reward_score = rospy.get_param(
'TARGET_REWARD_SCORE')
else:
from markov.defaults import reward_function
self.reward_function = reward_function
self.number_of_trials = 0
self.target_number_of_trials = rospy.get_param(
'NUMBER_OF_TRIALS')
# Request the waypoints
waypoints = None
try:
resp = utils.gazebo_service_call(get_waypoints_client)
waypoints = np.array(resp.waypoints).reshape(
resp.row, resp.col)
except Exception as ex:
utils.json_format_logger(
"Unable to retrieve waypoints: {}".format(ex),
**utils.build_system_error_dict(
utils.SIMAPP_ENVIRONMENT_EXCEPTION,
utils.SIMAPP_EVENT_ERROR_CODE_500))
is_loop = np.all(waypoints[0, :] == waypoints[-1, :])
if is_loop:
self.center_line = LinearRing(waypoints[:, 0:2])
self.inner_border = LinearRing(waypoints[:, 2:4])
self.outer_border = LinearRing(waypoints[:, 4:6])
self.road_poly = Polygon(self.outer_border,
[self.inner_border])
else:
self.center_line = LineString(waypoints[:, 0:2])
self.inner_border = LineString(waypoints[:, 2:4])
self.outer_border = LineString(waypoints[:, 4:6])
self.road_poly = Polygon(
np.vstack(
(self.outer_border, np.flipud(self.inner_border))))
self.center_dists = [
self.center_line.project(Point(p), normalized=True)
for p in self.center_line.coords[:-1]
] + [1.0]
self.track_length = self.center_line.length
# Queue used to maintain image consumption synchronicity
self.image_queue = queue.Queue(IMG_QUEUE_BUF_SIZE)
rospy.Subscriber('/camera/zed/rgb/image_rect_color', sensor_image,
self.callback_image)
# Initialize state data
self.episodes = 0
self.start_ndist = rospy.get_param('START_POSITION', 0.0)
self.reverse_dir = False
self.change_start = rospy.get_param(
'CHANGE_START_POSITION', (self.job_type == TRAINING_JOB))
self.alternate_dir = rospy.get_param('ALTERNATE_DRIVING_DIRECTION',
False)
self.is_simulation_done = False
self.steering_angle = 0
self.speed = 0
self.action_taken = 0
self.prev_progress = 0
self.prev_point = Point(0, 0)
self.prev_point_2 = Point(0, 0)
self.next_state = None
self.reward = None
self.reward_in_episode = 0
self.done = False
self.steps = 0
self.simulation_start_time = 0
self.allow_servo_step_signals = False
#self.metadata = {'render.modes': ['rgb_array']}
def setup_simtrace_data_upload_to_s3(self):
if node_type == SIMULATION_WORKER:
#start timer to upload SIM_TRACE data to s3 when simapp exits
#There is not enough time to upload data to S3 when robomaker shuts down
#Set up timer to upload data to S3 300 seconds before the robomaker quits
# - 300 seocnds is randomly chosen number based on various jobs launched that
# provides enough time to upload data to S3
global simapp_data_upload_timer
# CT: Use MAX_JOB_DURATION env var instead
#session = boto3.session.Session()
#robomaker_client = session.client('robomaker', region_name=self.aws_region, endpoint_url=self.aws_endpoint_url)
#result = robomaker_client.describe_simulation_job(
# job=self.simulation_job_arn
#)
result = {
"maxJobDurationInSeconds": rospy.get_param('MAX_JOB_DURATION'),
"lastStartedAt": datetime.datetime.now(),
"lastUpdatedAt": datetime.datetime.now()
}
logger.info("robomaker job description: {}".format(result))
self.simapp_upload_duration = result[
'maxJobDurationInSeconds'] - SIMAPP_DATA_UPLOAD_TIME_TO_S3
start = 0
if self.job_type == TRAINING_JOB:
logger.info("simapp training job started_at={}".format(
result['lastStartedAt']))
start = result['lastStartedAt']
self.simtrace_s3_endpoint_url = rospy.get_param(
'AWS_ENDPOINT_URL')
self.simtrace_s3_bucket = rospy.get_param(
'SAGEMAKER_SHARED_S3_BUCKET')
self.simtrace_s3_key = os.path.join(
rospy.get_param('SAGEMAKER_SHARED_S3_PREFIX'),
TRAINING_SIMTRACE_DATA_S3_OBJECT_KEY)
else:
logger.info("simapp evaluation job started_at={}".format(
result['lastUpdatedAt']))
start = result['lastUpdatedAt']
self.simtrace_s3_endpoint_url = rospy.get_param(
'AWS_ENDPOINT_URL')
self.simtrace_s3_bucket = rospy.get_param('MODEL_S3_BUCKET')
self.simtrace_s3_key = os.path.join(
rospy.get_param('MODEL_S3_PREFIX'),
EVALUATION_SIMTRACE_DATA_S3_OBJECT_KEY)
end = start + datetime.timedelta(
seconds=self.simapp_upload_duration)
now = datetime.datetime.now(
tz=end.tzinfo) # use tzinfo as robomaker
self.simapp_data_upload_time = (end - now).total_seconds()
logger.info(
"simapp job started_at={} now={} end={} upload_data_in={} sec".
format(start, now, end, self.simapp_data_upload_time))
simapp_data_upload_timer = threading.Timer(
self.simapp_data_upload_time, simapp_data_upload_timer_expiry)
simapp_data_upload_timer.daemon = True
simapp_data_upload_timer.start()
logger.info("Timer with {} seconds is {}".format(
self.simapp_data_upload_time,
simapp_data_upload_timer.is_alive()))
#setup to upload SIM_TRACE_DATA to S3
self.simtrace_data = DeepRacerRacetrackSimTraceData(
self.simtrace_s3_bucket, self.simtrace_s3_key,
self.simtrace_s3_endpoint_url)
#def render(self, mode='rgb_array'):
# if mode == 'rgb_array':
# if self.next_state is None:
# image = np.empty(shape=(TRAINING_IMAGE_SIZE[1], TRAINING_IMAGE_SIZE[0], 3), dtype=np.uint8)
# else:
# image = self.next_state
# return image
# else:
# return NotImplementedError
def reset(self):
if node_type == SAGEMAKER_TRAINING_WORKER:
return self.observation_space.sample()
# Simulation is done - so RoboMaker will start to shut down the app.
# Till RoboMaker shuts down the app, do nothing more else metrics may show unexpected data.
if (node_type == SIMULATION_WORKER) and self.is_simulation_done:
while True:
time.sleep(1)
self.steering_angle = 0
self.speed = 0
self.action_taken = 0
self.prev_progress = 0
self.prev_point = Point(0, 0)
self.prev_point_2 = Point(0, 0)
self.next_state = None
self.reward = None
self.reward_in_episode = 0
self.done = False
# Reset the car and record the simulation start time
if self.allow_servo_step_signals:
self.send_action(0, 0)
self.racecar_reset()
self.steps = 0
self.simulation_start_time = time.time()
self.infer_reward_state(0, 0)
return self.next_state
def set_next_state(self):
# Make sure the first image is the starting image
image_data = self.image_queue.get(block=True, timeout=None)
# Read the image and resize to get the state
image = Image.frombytes('RGB', (image_data.width, image_data.height),
image_data.data, 'raw', 'RGB', 0, 1)
image = image.resize(TRAINING_IMAGE_SIZE, resample=2)
### IMAGE FILTERING ###
changes = list()
probability = 0.75
# contrast: multiplier in RGB space
if np.random.choice(2, p=(1.0 - probability, probability)):
changes.append('Contrast')
contrast_mult = np.random.exponential()
new_contrast_array = np.array(image, 'float') * contrast_mult
new_contrast_array = np.clip(new_contrast_array, 0, 255)
image = Image.fromarray(new_contrast_array.astype('uint8'))
### BEGIN HSV SPACE ###
# HSV conversion
hsv_image = image.convert(mode='HSV')
h, s, v = hsv_image.split()
# hue: rotation
if np.random.choice(2, p=(1.0 - probability, probability)):
changes.append("Hue")
# PIL uses 0..255 for Hue range
hue_delta = np.random.randint(0, 255)
hue_array = np.array(
h, dtype='float') # convert to flow to allow overflow
new_hue_array = (hue_array + hue_delta) % 256
h = Image.fromarray(new_hue_array.astype('uint8'), 'L')
# saturation: multiplier
if np.random.choice(2, p=(1.0 - probability, probability)):
changes.append("Saturation")
sat_mult = np.random.uniform(0.0, 1.0)
sat_array = np.array(s, dtype='float')
new_sat_array = sat_array * sat_mult
new_sat_array = np.clip(new_sat_array, 0, 255)
s = Image.fromarray(new_sat_array.astype('uint8'), 'L')
# brightness: delta
if np.random.choice(2, p=(1.0 - probability, probability)):
changes.append("Brightness")
bright_delta = np.random.randint(-127, 127)
bright_array = np.array(v, dtype='float')
new_bright_array = bright_array + bright_delta
new_bright_array = np.clip(new_bright_array, 0, 255)
v = Image.fromarray(new_bright_array.astype('uint8'), 'L')
final_image = Image.merge('HSV', (h, s, v))
final_image = final_image.convert(mode='RGB')
### END HSV SPACE ###
if False:
# hide the print statement here so its only invoked when we don't care about performance
print("ObservationColorPerturbation Changes: %s" %
','.join(changes))
fname = 'color_perturb_filter_%f' % time.time()
np.save(os.path.join("/opt/ml/model", fname),
np.array(final_image))
self.next_state = np.array(final_image)
def racecar_reset(self):
try:
for joint in EFFORT_JOINTS:
utils.gazebo_service_call(self.clear_forces_client, joint)
prev_index, next_index = self.find_prev_next_waypoints(
self.start_ndist)
utils.gazebo_service_call(self.reset_car_client, self.start_ndist,
next_index)
# First clear the queue so that we set the state to the start image
_ = self.image_queue.get(block=True, timeout=None)
self.set_next_state()
except Exception as ex:
utils.json_format_logger(
"Unable to reset the car: {}".format(ex),
**utils.build_system_error_dict(
utils.SIMAPP_ENVIRONMENT_EXCEPTION,
utils.SIMAPP_EVENT_ERROR_CODE_500))
def set_allow_servo_step_signals(self, allow_servo_step_signals):
self.allow_servo_step_signals = allow_servo_step_signals
def step(self, action):
if node_type == SAGEMAKER_TRAINING_WORKER:
return self.observation_space.sample(), 0, False, {}
# Initialize next state, reward, done flag
self.next_state = None
self.reward = None
self.done = False
# Send this action to Gazebo and increment the step count
self.steering_angle = float(action[0])
self.speed = max(float(0.0), float(action[1]))
if self.allow_servo_step_signals:
self.send_action(self.steering_angle, self.speed)
self.steps += 1
# Compute the next state and reward
self.infer_reward_state(self.steering_angle, self.speed)
return self.next_state, self.reward, self.done, {}
def callback_image(self, data):
try:
self.image_queue.put_nowait(data)
except queue.Full:
# Only warn if its the middle of an episode, not during training
if self.allow_servo_step_signals:
logger.info("Warning: dropping image due to queue full")
pass
except Exception as ex:
utils.json_format_logger(
"Error retrieving frame from gazebo: {}".format(ex),
**utils.build_system_error_dict(
utils.SIMAPP_ENVIRONMENT_EXCEPTION,
utils.SIMAPP_EVENT_ERROR_CODE_500))
def send_action(self, steering_angle, speed):
# Simple v/r to computes the desired rpm
wheel_rpm = speed / WHEEL_RADIUS
for _, pub in self.velocity_pub_dict.items():
pub.publish(wheel_rpm)
for _, pub in self.steering_pub_dict.items():
pub.publish(steering_angle)
def infer_reward_state(self, steering_angle, speed):
try:
self.set_next_state()
except Exception as ex:
utils.json_format_logger(
"Unable to retrieve image from queue: {}".format(ex),
**utils.build_system_error_dict(
utils.SIMAPP_ENVIRONMENT_EXCEPTION,
utils.SIMAPP_EVENT_ERROR_CODE_500))
# Read model state from Gazebo
model_state = utils.gazebo_service_call(self.get_model_state,
'racecar', '')
model_orientation = Rotation.from_quat([
model_state.pose.orientation.x, model_state.pose.orientation.y,
model_state.pose.orientation.z, model_state.pose.orientation.w
])
model_location = np.array([
model_state.pose.position.x,
model_state.pose.position.y,
model_state.pose.position.z]) + \
model_orientation.apply(RELATIVE_POSITION_OF_FRONT_OF_CAR)
model_point = Point(model_location[0], model_location[1])
model_heading = model_orientation.as_euler('zyx')[0]
# Read the wheel locations from Gazebo
left_rear_wheel_state = utils.gazebo_service_call(
self.get_link_state, 'racecar::left_rear_wheel', '')
left_front_wheel_state = utils.gazebo_service_call(
self.get_link_state, 'racecar::left_front_wheel', '')
right_rear_wheel_state = utils.gazebo_service_call(
self.get_link_state, 'racecar::right_rear_wheel', '')
right_front_wheel_state = utils.gazebo_service_call(
self.get_link_state, 'racecar::right_front_wheel', '')
wheel_points = [
Point(left_rear_wheel_state.link_state.pose.position.x,
left_rear_wheel_state.link_state.pose.position.y),
Point(left_front_wheel_state.link_state.pose.position.x,
left_front_wheel_state.link_state.pose.position.y),
Point(right_rear_wheel_state.link_state.pose.position.x,
right_rear_wheel_state.link_state.pose.position.y),
Point(right_front_wheel_state.link_state.pose.position.x,
right_front_wheel_state.link_state.pose.position.y)
]
# Project the current location onto the center line and find nearest points
current_ndist = self.center_line.project(model_point, normalized=True)
prev_index, next_index = self.find_prev_next_waypoints(current_ndist)
distance_from_prev = model_point.distance(
Point(self.center_line.coords[prev_index]))
distance_from_next = model_point.distance(
Point(self.center_line.coords[next_index]))
closest_waypoint_index = (
prev_index, next_index)[distance_from_next < distance_from_prev]
# Compute distance from center and road width
nearest_point_center = self.center_line.interpolate(current_ndist,
normalized=True)
nearest_point_inner = self.inner_border.interpolate(
self.inner_border.project(nearest_point_center))
nearest_point_outer = self.outer_border.interpolate(
self.outer_border.project(nearest_point_center))
distance_from_center = nearest_point_center.distance(model_point)
distance_from_inner = nearest_point_inner.distance(model_point)
distance_from_outer = nearest_point_outer.distance(model_point)
track_width = nearest_point_inner.distance(nearest_point_outer)
is_left_of_center = (distance_from_outer < distance_from_inner) if self.reverse_dir \
else (distance_from_inner < distance_from_outer)
# Convert current progress to be [0,100] starting at the initial waypoint
if self.reverse_dir:
current_progress = self.start_ndist - current_ndist
else:
current_progress = current_ndist - self.start_ndist
if current_progress < 0.0: current_progress = current_progress + 1.0
current_progress = 100 * current_progress
if current_progress < self.prev_progress:
# Either: (1) we wrapped around and have finished the track,
delta1 = current_progress + 100 - self.prev_progress
# or (2) for some reason the car went backwards (this should be rare)
delta2 = self.prev_progress - current_progress
current_progress = (self.prev_progress, 100)[delta1 < delta2]
# Car is off track if all wheels are outside the borders
wheel_on_track = [self.road_poly.contains(p) for p in wheel_points]
all_wheels_on_track = all(wheel_on_track)
any_wheels_on_track = any(wheel_on_track)
# Simulation elapsed time, which may be faster or slower than wall time
simulation_time = rospy.get_rostime()
# Compute the reward
reward = 0.0
if any_wheels_on_track:
done = False
params = {
'all_wheels_on_track': all_wheels_on_track,
'x': model_point.x,
'y': model_point.y,
'heading': model_heading * 180.0 / math.pi,
'distance_from_center': distance_from_center,
'progress': current_progress,
'steps': self.steps,
'speed': speed,
'steering_angle': steering_angle * 180.0 / math.pi,
'track_width': track_width,
'waypoints': list(self.center_line.coords),
'closest_waypoints': [prev_index, next_index],
'is_left_of_center': is_left_of_center,
'is_reversed': self.reverse_dir,
'action': self.action_taken
}
try:
reward = float(self.reward_function(params))
except Exception as e:
utils.json_format_logger(
"Error in the reward function: {}".format(e),
**utils.build_user_error_dict(
utils.SIMAPP_SIMULATION_WORKER_EXCEPTION,
utils.SIMAPP_EVENT_ERROR_CODE_400))
traceback.print_exc()
utils.simapp_exit_gracefully()
else:
done = True
reward = CRASHED
# Reset if the car position hasn't changed in the last 2 steps
prev_pnt_dist = min(model_point.distance(self.prev_point),
model_point.distance(self.prev_point_2))
if prev_pnt_dist <= 0.0001 and self.steps % NUM_STEPS_TO_CHECK_STUCK == 0:
done = True
reward = CRASHED # stuck
# Simulation jobs are done when progress reaches 100
if current_progress >= 100:
done = True
# Keep data from the previous step around
self.prev_point_2 = self.prev_point
self.prev_point = model_point
self.prev_progress = current_progress
# Set the reward and done flag
self.reward = reward
self.reward_in_episode += reward
self.done = done
# Trace logs to help us debug and visualize the training runs
# btown TODO: This should be written to S3, not to CWL.
logger.info(
'SIM_TRACE_LOG:%d,%d,%.4f,%.4f,%.4f,%.2f,%.2f,%d,%.4f,%s,%s,%.4f,%d,%.2f,%s,%.4f\n'
%
(self.episodes, self.steps, model_location[0], model_location[1],
model_heading, self.steering_angle, self.speed, self.action_taken,
self.reward, self.done, all_wheels_on_track, current_progress,
closest_waypoint_index, self.track_length, time.time(),
float(simulation_time.secs) + float(simulation_time.nsecs) / 1e9))
#build json record of the reward metrics
reward_metrics = OrderedDict()
reward_metrics['episode'] = self.episodes
reward_metrics['steps'] = self.steps
reward_metrics['X'] = model_location[0]
reward_metrics['Y'] = model_location[1]
reward_metrics['yaw'] = model_heading
reward_metrics['steer'] = self.steering_angle
reward_metrics['throttle'] = self.speed
reward_metrics['action'] = self.action_taken
reward_metrics['reward'] = self.reward
reward_metrics['done'] = self.done
reward_metrics['all_wheels_on_track'] = all_wheels_on_track
reward_metrics['progress'] = current_progress
reward_metrics['closest_waypoint'] = closest_waypoint_index
reward_metrics['track_len'] = self.track_length
reward_metrics['tstamp'] = time.time()
self.simtrace_data.write_simtrace_data(reward_metrics)
# Terminate this episode when ready
if done and node_type == SIMULATION_WORKER:
self.finish_episode(current_progress)
def find_prev_next_waypoints(self, ndist):
if self.reverse_dir:
next_index = bisect.bisect_left(self.center_dists, ndist) - 1
prev_index = next_index + 1
if next_index == -1: next_index = len(self.center_dists) - 1
else:
next_index = bisect.bisect_right(self.center_dists, ndist)
prev_index = next_index - 1
if next_index == len(self.center_dists): next_index = 0
return prev_index, next_index
def stop_car(self):
self.steering_angle = 0
self.speed = 0
self.action_taken = 0
self.send_action(0, 0)
self.racecar_reset()
def finish_episode(self, progress):
# Increment episode count, update start position and direction
self.episodes += 1
if self.change_start:
self.start_ndist = (self.start_ndist +
ROUND_ROBIN_ADVANCE_DIST) % 1.0
if self.alternate_dir:
self.reverse_dir = not self.reverse_dir
# Reset the car
self.stop_car()
# upload SIM_TRACE data to S3
self.simtrace_data.upload_to_s3(self.episodes)
# Update metrics based on job type
if self.job_type == TRAINING_JOB:
self.update_training_metrics(progress)
self.write_metrics_to_s3()
if self.is_training_done():
self.cancel_simulation_job()
elif self.job_type == EVALUATION_JOB:
self.number_of_trials += 1
self.update_eval_metrics(progress)
self.write_metrics_to_s3()
def update_eval_metrics(self, progress):
eval_metric = {}
eval_metric['completion_percentage'] = int(progress)
eval_metric['metric_time'] = int(round(time.time() * 1000))
eval_metric['start_time'] = int(
round(self.simulation_start_time * 1000))
eval_metric['elapsed_time_in_milliseconds'] = int(
round((time.time() - self.simulation_start_time) * 1000))
eval_metric['trial'] = int(self.number_of_trials)
self.metrics.append(eval_metric)
def update_training_metrics(self, progress):
training_metric = {}
training_metric['reward_score'] = int(round(self.reward_in_episode))
training_metric['metric_time'] = int(round(time.time() * 1000))
training_metric['start_time'] = int(
round(self.simulation_start_time * 1000))
training_metric['elapsed_time_in_milliseconds'] = int(
round((time.time() - self.simulation_start_time) * 1000))
training_metric['episode'] = int(self.episodes)
training_metric['completion_percentage'] = int(progress)
self.metrics.append(training_metric)
def write_metrics_to_s3(self):
session = boto3.session.Session()
s3_client = session.client('s3',
region_name=self.aws_region,
endpoint_url=self.aws_endpoint_url)
metrics_body = json.dumps({'metrics': self.metrics})
s3_client.put_object(Bucket=self.metrics_s3_bucket,
Key=self.metrics_s3_object_key,
Body=bytes(metrics_body, encoding='utf-8'))
def is_training_done(self):
if ((self.target_number_of_episodes > 0) and (self.target_number_of_episodes == self.episodes)) or \
((isinstance(self.target_reward_score, (int, float))) and (self.target_reward_score <= self.reward_in_episode)):
self.is_simulation_done = True
return self.is_simulation_done
def cancel_simulation_job(self):
logger.info(
"cancel_simulation_job: make sure to shutdown simapp first")
simapp_shutdown()
def test_linearring_from_invalid():
coords = [(0.0, 0.0), (0.0, 0.0), (0.0, 0.0)]
line = LineString(coords)
assert not line.is_valid
with pytest.raises(TopologicalError):
LinearRing(line)
def test_linearring_from_invalid(self):
coords = [(0.0, 0.0), (0.0, 0.0), (0.0, 0.0)]
line = LineString(coords)
self.assertFalse(line.is_valid)
with self.assertRaises(TopologicalError):
ring = LinearRing(line)
def plot_line(ax, ob, color):
x, y = ob.xy
ax.plot(x,
y,
color=color,
alpha=0.7,
linewidth=3,
solid_capstyle='round',
zorder=2)
polygon = [[3, 1], [1, 3], [1, 6], [3, 9], [6, 9], [8, 6], [8, 3], [6, 1]]
line = LineString(polygon)
poly_line_offset = line.parallel_offset(1,
side="right",
resolution=16,
join_style=2)
##########################
# afpoly = shp.Polygon(polygon)
# noffafpoly = afpoly.buffer(-1, join_style=2) # Inward offset
# poly_line_offset = noffafpoly.exterior
# print(poly_line_offset)
###########################
print(request)
api = overpass.API()
data = api.Get(request, responseformat="geojson")
print("request finished")
with open('response.geo.json', 'w') as outfile:
json.dump(data, outfile, indent=4, sort_keys=True)
else:
with open('response.geo.json') as inputfile:
data = json.load(inputfile)
for feature in data["features"]:
if feature["geometry"]["type"] == "LineString":
if len(feature["geometry"]["coordinates"]) == 2:
print("Line")
line = LineString(feature["geometry"]["coordinates"])
labelpoint = list(line.representative_point().coords)[0]
else:
poly = Polygon(feature["geometry"]["coordinates"])
labelpoint = list(polylabel(poly).coords)[0]
feature["geometry"]["coordinates"] = labelpoint
feature["geometry"]["type"] = "Point"
searchKeys = list(set(keys).intersection(feature["properties"]))
i = 0
searchValue = feature["properties"][searchKeys[i]]
while searchValue not in keys[searchKeys[i]]:
i += 1
searchValue = feature["properties"][searchKeys[i]]
own = keys[searchKeys[i]][searchValue]
feature["properties"]["own"] = own
# need three points at a time
for counter in range(0, len(polyx) - 3):
# get first offset intercept
pt = getoffsetcornerpoint(polyx[counter], polyx[counter + 1],
polyx[counter + 2], offset)
# append new point to polyy
polyy.append(pt)
# last three points
pt = getoffsetcornerpoint(polyx[-3], polyx[-2], polyx[-1], offset)
polyy.append(pt)
pt = getoffsetcornerpoint(polyx[-2], polyx[-1], polyx[0], offset)
polyy.append(pt)
pt = getoffsetcornerpoint(polyx[-1], polyx[0], polyx[1], offset)
polyy.append(pt)
return polyy
polygon = [[3, 1], [1, 3], [1, 6], [3, 9], [6, 9], [8, 6], [8, 3], [6, 1]]
res = offsetpolygon(polygon, 1)
print(res)
line = LineString(polygon)
poly_line_offset = LineString(res)
fig = plt.figure()
ax = fig.add_subplot(111)
plot_line(ax, line, "blue")
plot_line(ax, poly_line_offset, "green")
plt.show()
for d in data:
idx.append(idx[-1] + np.size(d,0))
#
# Interface node positions along mesh edges
#
ifp = [] # interface node positions
i_nodes = [] # interior node indices for which the distance is < 0
for i in range(0, np.size(idx)-1):
print("* Shape %d/%d" %(i+1, np.size(idx)-1))
d = dscaled[idx[i]:idx[i+1]]
if_nodes = [(d[j][0], d[j][1]) for j in range(0, len(d))]
# Add the first node to the end to create a closed loop for intersecting:
if_loop = if_nodes
if_loop.append(if_nodes[0])
interface = LineString(if_loop)
# Intersect edges_close with the interface; construct interface nodes.
for j in range(0, len(edges_close)):
n1 = edges[edges_close[j], 0]
n2 = edges[edges_close[j], 1]
o = LineString( [p[n1,:], p[n2,:]] )
x = interface.intersection(o)
if (np.size(x) == 2):
ifp.append( [x.x, x.y] )
if (np.size(x) > 2):
# print("\nWarning: mesh edge crosses multiple interface edges! "
# "Using the centroid for interface node position.")
ifp.append( [x[:].centroid.x, x[:].centroid.y] )
if ((j+1) % 1000 == 0 or (j+1) == len(edges_close)):
print("\r- Processing edge %d / %d" %(j+1,len(edges_close)), end='')
def __init__(self):
self.sampling_rate = 30.0
self.sampling_sleep = (1.0/self.sampling_rate)
self.sampling_rates = [30.0, 30.0]
self.sampling_rate_index = 0
self.latencies = [10.0, 20.0, 40.0, 60.0, 80.0, 100.0, 120.0]
self.latency_index = 0
self.latency_max_num_steps = 500 # for these steps latency will be fixed or change on reset or done after 250.
self.latency_steps = 0
self.latency = 10.0 #10 is the starting latency
self.model_running_time = (2.0/1000.0) #model runtime
screen_height = TRAINING_IMAGE_SIZE[1]
screen_width = TRAINING_IMAGE_SIZE[0]
self.on_track = 0
self.progress = 0
self.yaw = 0
self.x = 0
self.y = 0
self.z = 0
self.distance_from_center = 0
self.distance_from_border_1 = 0
self.distance_from_border_2 = 0
self.steps = 0
self.progress_at_beginning_of_race = 0
self.reverse_dir = False
self.start_ndist = 0.0
# actions -> steering angle, throttle
self.action_space = spaces.Box(low=np.array([-1, 0]), high=np.array([+1, +1]), dtype=np.float32)
# given image from simulator
self.observation_space = spaces.Box(low=0, high=255,
shape=(screen_height, screen_width, 1), dtype=np.uint8)
self.allow_servo_step_signals = True
#stores the time when camera images are received
self.cam_update_time=[]
#stores the time when consequetive actions are send
self.cons_action_send_time=[]
#stores the time when progress updates are received
self.progress_update_time = []
#folder location to store the debug data
self.debug_data_folder = []
self.debug_index = 0
if node_type == SIMULATION_WORKER:
# ROS initialization
rospy.init_node('rl_coach', anonymous=True)
self.ack_publisher = rospy.Publisher('/vesc/low_level/ackermann_cmd_mux/output',
AckermannDriveStamped, queue_size=100)
self.racecar_service = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
self.clear_forces_client = rospy.ServiceProxy('/gazebo/clear_joint_forces',
JointRequest)
# Subscribe to ROS topics and register callbacks
rospy.Subscriber('/progress', Progress, self.callback_progress)
rospy.Subscriber('/camera/zed/rgb/image_rect_color', sensor_image, self.callback_image)
self.world_name = 'hard_track'#rospy.get_param('WORLD_NAME')
self.set_waypoints()
waypoints = self.waypoints
is_loop = np.all(waypoints[0,:] == waypoints[-1,:])
if is_loop:
self.center_line = LinearRing(waypoints[:,0:2])
else:
self.center_line = LineString(waypoints[:,0:2])
self.center_dists = [self.center_line.project(Point(p), normalized=True) for p in self.center_line.coords[:-1]] + [1.0]
self.track_length = self.center_line.length
self.reward_in_episode = 0
self.prev_progress = 0
self.steps = 0
# Create the publishers for sending speed and steering info to the car
self.velocity_pub_dict = OrderedDict()
self.steering_pub_dict = OrderedDict()
for topic in VELOCITY_TOPICS:
self.velocity_pub_dict[topic] = rospy.Publisher(topic, Float64, queue_size=1)
for topic in STEERING_TOPICS:
self.steering_pub_dict[topic] = rospy.Publisher(topic, Float64, queue_size=1)
class DeepRacerRacetrackEnv(gym.Env):
def __init__(self):
# Create the observation space
img_width = TRAINING_IMAGE_SIZE[0]
img_height = TRAINING_IMAGE_SIZE[1]
self.observation_space = spaces.Box(low=0,
high=255,
shape=(img_height, img_width, 3),
dtype=np.uint8)
# Create the action space
self.action_space = spaces.Box(low=np.array([-1, 0]),
high=np.array([+1, +1]),
dtype=np.float32)
if node_type == SIMULATION_WORKER:
# ROS initialization
rospy.init_node('rl_coach', anonymous=True)
rospy.Subscriber('/camera/zed/rgb/image_rect_color', sensor_image,
self.callback_image)
self.ack_publisher = rospy.Publisher(
'/vesc/low_level/ackermann_cmd_mux/output',
AckermannDriveStamped,
queue_size=100)
self.set_model_state = rospy.ServiceProxy(
'/gazebo/set_model_state', SetModelState)
self.get_model_state = rospy.ServiceProxy(
'/gazebo/get_model_state', GetModelState)
self.get_link_state = rospy.ServiceProxy('/gazebo/get_link_state',
GetLinkState)
# Read in parameters
self.world_name = rospy.get_param('WORLD_NAME')
self.job_type = rospy.get_param('JOB_TYPE')
self.aws_region = rospy.get_param('AWS_REGION')
self.metrics_s3_bucket = rospy.get_param('METRICS_S3_BUCKET')
self.metrics_s3_object_key = rospy.get_param(
'METRICS_S3_OBJECT_KEY')
self.metrics = []
self.simulation_job_arn = 'arn:aws:robomaker:' + self.aws_region + ':' + \
rospy.get_param('ROBOMAKER_SIMULATION_JOB_ACCOUNT_ID') + \
':simulation-job/' + rospy.get_param('AWS_ROBOMAKER_SIMULATION_JOB_ID')
if self.job_type == TRAINING_JOB:
from custom_files.customer_reward_function import reward_function
self.reward_function = reward_function
self.metric_name = rospy.get_param('METRIC_NAME')
self.metric_namespace = rospy.get_param('METRIC_NAMESPACE')
self.training_job_arn = rospy.get_param('TRAINING_JOB_ARN')
self.target_number_of_episodes = rospy.get_param(
'NUMBER_OF_EPISODES')
self.target_reward_score = rospy.get_param(
'TARGET_REWARD_SCORE')
else:
from markov.defaults import reward_function
self.reward_function = reward_function
self.number_of_trials = 0
self.target_number_of_trials = rospy.get_param(
'NUMBER_OF_TRIALS')
# Read in the waypoints
BUNDLE_CURRENT_PREFIX = os.environ.get("BUNDLE_CURRENT_PREFIX",
None)
if not BUNDLE_CURRENT_PREFIX:
raise ValueError("Cannot get BUNDLE_CURRENT_PREFIX")
route_file_name = os.path.join(BUNDLE_CURRENT_PREFIX, 'install',
'deepracer_simulation', 'share',
'deepracer_simulation', 'routes',
'{}.npy'.format(self.world_name))
waypoints = np.load(route_file_name)
self.is_loop = np.all(waypoints[0, :] == waypoints[-1, :])
if self.is_loop:
self.center_line = LinearRing(waypoints[:, 0:2])
self.inner_border = LinearRing(waypoints[:, 2:4])
self.outer_border = LinearRing(waypoints[:, 4:6])
self.road_poly = Polygon(self.outer_border,
[self.inner_border])
else:
self.center_line = LineString(waypoints[:, 0:2])
self.inner_border = LineString(waypoints[:, 2:4])
self.outer_border = LineString(waypoints[:, 4:6])
self.road_poly = Polygon(
np.vstack(
(self.outer_border, np.flipud(self.inner_border))))
self.center_dists = [
self.center_line.project(Point(p), normalized=True)
for p in self.center_line.coords[:-1]
] + [1.0]
self.track_length = self.center_line.length
# Initialize state data
self.episodes = 0
self.start_dist = 0.0
self.round_robin = (self.job_type == TRAINING_JOB)
self.is_simulation_done = False
self.image = None
self.steering_angle = 0
self.speed = 0
self.action_taken = 0
self.prev_progress = 0
self.prev_point = Point(0, 0)
self.prev_point_2 = Point(0, 0)
self.next_state = None
self.reward = None
self.reward_in_episode = 0
self.done = False
self.steps = 0
self.simulation_start_time = 0
self.reverse_dir = False
def reset(self):
if node_type == SAGEMAKER_TRAINING_WORKER:
return self.observation_space.sample()
# Simulation is done - so RoboMaker will start to shut down the app.
# Till RoboMaker shuts down the app, do nothing more else metrics may show unexpected data.
if (node_type == SIMULATION_WORKER) and self.is_simulation_done:
while True:
time.sleep(1)
self.image = None
self.steering_angle = 0
self.speed = 0
self.action_taken = 0
self.prev_progress = 0
self.prev_point = Point(0, 0)
self.prev_point_2 = Point(0, 0)
self.next_state = None
self.reward = None
self.reward_in_episode = 0
self.done = False
# Reset the car and record the simulation start time
self.send_action(0, 0)
self.racecar_reset()
time.sleep(SLEEP_AFTER_RESET_TIME_IN_SECOND)
self.steps = 0
self.simulation_start_time = time.time()
# Compute the initial state
self.infer_reward_state(0, 0)
return self.next_state
def racecar_reset(self):
rospy.wait_for_service('/gazebo/set_model_state')
# Compute the starting position and heading
next_point_index = bisect.bisect(self.center_dists, self.start_dist)
start_point = self.center_line.interpolate(self.start_dist,
normalized=True)
start_yaw = math.atan2(
self.center_line.coords[next_point_index][1] - start_point.y,
self.center_line.coords[next_point_index][0] - start_point.x)
start_quaternion = Rotation.from_euler('zyx',
[start_yaw, 0, 0]).as_quat()
# Construct the model state and send to Gazebo
modelState = ModelState()
modelState.model_name = 'racecar'
modelState.pose.position.x = start_point.x
modelState.pose.position.y = start_point.y
modelState.pose.position.z = 0
modelState.pose.orientation.x = start_quaternion[0]
modelState.pose.orientation.y = start_quaternion[1]
modelState.pose.orientation.z = start_quaternion[2]
modelState.pose.orientation.w = start_quaternion[3]
modelState.twist.linear.x = 0
modelState.twist.linear.y = 0
modelState.twist.linear.z = 0
modelState.twist.angular.x = 0
modelState.twist.angular.y = 0
modelState.twist.angular.z = 0
self.set_model_state(modelState)
def step(self, action):
if node_type == SAGEMAKER_TRAINING_WORKER:
return self.observation_space.sample(), 0, False, {}
# Initialize next state, reward, done flag
self.next_state = None
self.reward = None
self.done = False
# Send this action to Gazebo and increment the step count
self.steering_angle = float(action[0])
self.speed = float(action[1])
self.send_action(self.steering_angle, self.speed)
time.sleep(SLEEP_BETWEEN_ACTION_AND_REWARD_CALCULATION_TIME_IN_SECOND)
self.steps += 1
# Compute the next state and reward
self.infer_reward_state(self.steering_angle, self.speed)
return self.next_state, self.reward, self.done, {}
def callback_image(self, data):
self.image = data
def send_action(self, steering_angle, speed):
ack_msg = AckermannDriveStamped()
ack_msg.header.stamp = rospy.Time.now()
ack_msg.drive.steering_angle = steering_angle
ack_msg.drive.speed = speed
self.ack_publisher.publish(ack_msg)
def infer_reward_state(self, steering_angle, speed):
rospy.wait_for_service('/gazebo/get_model_state')
rospy.wait_for_service('/gazebo/get_link_state')
# Wait till we have a image from the camera
# btown TODO: Incorporate feedback from callejae@ here (CR-6434645 rev1)
while not self.image:
time.sleep(SLEEP_WAITING_FOR_IMAGE_TIME_IN_SECOND)
# Read model state from Gazebo
model_state = self.get_model_state('racecar', '')
model_orientation = Rotation.from_quat([
model_state.pose.orientation.x, model_state.pose.orientation.y,
model_state.pose.orientation.z, model_state.pose.orientation.w
])
model_location = np.array([
model_state.pose.position.x,
model_state.pose.position.y,
model_state.pose.position.z]) + \
model_orientation.apply(RELATIVE_POSITION_OF_FRONT_OF_CAR)
model_point = Point(model_location[0], model_location[1])
model_heading = model_orientation.as_euler('zyx')[0]
# Read the wheel locations from Gazebo
left_rear_wheel_state = self.get_link_state('racecar::left_rear_wheel',
'')
left_front_wheel_state = self.get_link_state(
'racecar::left_front_wheel', '')
right_rear_wheel_state = self.get_link_state(
'racecar::right_rear_wheel', '')
right_front_wheel_state = self.get_link_state(
'racecar::right_front_wheel', '')
wheel_points = [
Point(left_rear_wheel_state.link_state.pose.position.x,
left_rear_wheel_state.link_state.pose.position.y),
Point(left_front_wheel_state.link_state.pose.position.x,
left_front_wheel_state.link_state.pose.position.y),
Point(right_rear_wheel_state.link_state.pose.position.x,
right_rear_wheel_state.link_state.pose.position.y),
Point(right_front_wheel_state.link_state.pose.position.x,
right_front_wheel_state.link_state.pose.position.y)
]
# Project the current location onto the center line and find nearest points
current_dist = self.center_line.project(model_point, normalized=True)
next_waypoint_index = max(
0,
min(bisect.bisect(self.center_dists, current_dist),
len(self.center_dists) - 1))
prev_waypoint_index = next_waypoint_index - 1
distance_from_next = model_point.distance(
Point(self.center_line.coords[next_waypoint_index]))
distance_from_prev = model_point.distance(
Point(self.center_line.coords[prev_waypoint_index]))
closest_waypoint_index = (
prev_waypoint_index,
next_waypoint_index)[distance_from_next < distance_from_prev]
# Compute distance from center and road width
nearest_point_center = self.center_line.interpolate(current_dist,
normalized=True)
nearest_point_inner = self.inner_border.interpolate(
self.inner_border.project(nearest_point_center))
nearest_point_outer = self.outer_border.interpolate(
self.outer_border.project(nearest_point_center))
distance_from_center = nearest_point_center.distance(model_point)
distance_from_inner = nearest_point_inner.distance(model_point)
distance_from_outer = nearest_point_outer.distance(model_point)
track_width = nearest_point_inner.distance(nearest_point_outer)
is_left_of_center = (distance_from_outer < distance_from_inner) if self.reverse_dir \
else (distance_from_inner < distance_from_outer)
# Convert current progress to be [0,100] starting at the initial waypoint
current_progress = current_dist - self.start_dist
if current_progress < 0.0: current_progress = current_progress + 1.0
current_progress = 100 * current_progress
if current_progress < self.prev_progress:
# Either: (1) we wrapped around and have finished the track,
delta1 = current_progress + 100 - self.prev_progress
# or (2) for some reason the car went backwards (this should be rare)
delta2 = self.prev_progress - current_progress
current_progress = (self.prev_progress, 100)[delta1 < delta2]
# Car is off track if all wheels are outside the borders
wheel_on_track = [self.road_poly.contains(p) for p in wheel_points]
all_wheels_on_track = all(wheel_on_track)
any_wheels_on_track = any(wheel_on_track)
# Compute the reward
if any_wheels_on_track:
done = False
params = {
'all_wheels_on_track': all_wheels_on_track,
'x': model_point.x,
'y': model_point.y,
'heading': model_heading * 180.0 / math.pi,
'distance_from_center': distance_from_center,
'progress': current_progress,
'steps': self.steps,
'speed': speed,
'steering_angle': steering_angle * 180.0 / math.pi,
'track_width': track_width,
'waypoints': list(self.center_line.coords),
'closest_waypoints':
[prev_waypoint_index, next_waypoint_index],
'is_left_of_center': is_left_of_center,
'is_reversed': self.reverse_dir
}
reward = self.reward_function(params)
else:
done = True
reward = CRASHED
# Reset if the car position hasn't changed in the last 2 steps
if min(model_point.distance(self.prev_point),
model_point.distance(self.prev_point_2)) <= 0.0001:
done = True
reward = CRASHED # stuck
# Simulation jobs are done when progress reaches 100
if current_progress >= 100:
done = True
# Keep data from the previous step around
self.prev_point_2 = self.prev_point
self.prev_point = model_point
self.prev_progress = current_progress
# Read the image and resize to get the state
image = Image.frombytes('RGB', (self.image.width, self.image.height),
self.image.data, 'raw', 'RGB', 0, 1)
image = image.resize(TRAINING_IMAGE_SIZE, resample=2)
state = np.array(image)
# Set the next state, reward, and done flag
self.next_state = state
self.reward = reward
self.reward_in_episode += reward
self.done = done
# Trace logs to help us debug and visualize the training runs
# btown TODO: This should be written to S3, not to CWL.
stdout_ = 'SIM_TRACE_LOG:%d,%d,%.4f,%.4f,%.4f,%.2f,%.2f,%d,%.4f,%s,%s,%.4f,%d,%.2f,%s\n' % (
self.episodes, self.steps, model_location[0], model_location[1],
model_heading, self.steering_angle, self.speed, self.action_taken,
self.reward, self.done, all_wheels_on_track, current_progress,
closest_waypoint_index, self.track_length, time.time())
print(stdout_)
# Terminate this episode when ready
if self.done and node_type == SIMULATION_WORKER:
self.finish_episode(current_progress)
def finish_episode(self, progress):
# Stop the car from moving
self.send_action(0, 0)
# Increment episode count, update start dist for round robin
self.episodes += 1
if self.round_robin:
self.start_dist = (self.start_dist +
ROUND_ROBIN_ADVANCE_DIST) % 1.0
# Update metrics based on job type
if self.job_type == TRAINING_JOB:
self.send_reward_to_cloudwatch(self.reward_in_episode)
self.update_training_metrics()
self.write_metrics_to_s3()
if self.is_training_done():
self.cancel_simulation_job()
elif self.job_type == EVALUATION_JOB:
self.number_of_trials += 1
self.update_eval_metrics(progress)
self.write_metrics_to_s3()
if self.is_evaluation_done():
self.cancel_simulation_job()
def update_eval_metrics(self, progress):
eval_metric = {}
eval_metric['completion_percentage'] = int(progress)
eval_metric['metric_time'] = int(round(time.time() * 1000))
eval_metric['start_time'] = int(
round(self.simulation_start_time * 1000))
eval_metric['elapsed_time_in_milliseconds'] = int(
round((time.time() - self.simulation_start_time) * 1000))
eval_metric['trial'] = int(self.number_of_trials)
self.metrics.append(eval_metric)
def update_training_metrics(self):
training_metric = {}
training_metric['reward_score'] = int(round(self.reward_in_episode))
training_metric['metric_time'] = int(round(time.time() * 1000))
training_metric['start_time'] = int(
round(self.simulation_start_time * 1000))
training_metric['elapsed_time_in_milliseconds'] = int(
round((time.time() - self.simulation_start_time) * 1000))
training_metric['episode'] = int(self.episodes)
self.metrics.append(training_metric)
def write_metrics_to_s3(self):
session = boto3.session.Session()
s3_url = os.environ.get('S3_ENDPOINT_URL')
s3_client = session.client('s3',
region_name=self.aws_region,
endpoint_url=s3_url)
metrics_body = json.dumps({'metrics': self.metrics})
s3_client.put_object(Bucket=self.metrics_s3_bucket,
Key=self.metrics_s3_object_key,
Body=bytes(metrics_body, encoding='utf-8'))
def is_evaluation_done(self):
if ((self.target_number_of_trials > 0)
and (self.target_number_of_trials == self.number_of_trials)):
self.is_simulation_done = True
return self.is_simulation_done
def is_training_done(self):
if ((self.target_number_of_episodes > 0) and (self.target_number_of_episodes == self.episodes)) or \
((self.is_number(self.target_reward_score)) and (self.target_reward_score <= self.reward_in_episode)):
self.is_simulation_done = True
return self.is_simulation_done
def is_number(self, value_to_check):
try:
float(value_to_check)
return True
except ValueError:
return False
def cancel_simulation_job(self):
self.send_action(0, 0)
session = boto3.session.Session()
robomaker_client = session.client('robomaker',
region_name=self.aws_region)
robomaker_client.cancel_simulation_job(job=self.simulation_job_arn)
def send_reward_to_cloudwatch(self, reward):
isLocal = os.environ.get("LOCAL")
if isLocal == None:
session = boto3.session.Session()
cloudwatch_client = session.client('cloudwatch',
region_name=self.aws_region)
cloudwatch_client.put_metric_data(MetricData=[
{
'MetricName':
self.metric_name,
'Dimensions': [
{
'Name': 'TRAINING_JOB_ARN',
'Value': self.training_job_arn
},
],
'Unit':
'None',
'Value':
reward
},
],
Namespace=self.metric_namespace)
else:
print("{}: {}".format(self.metric_name, reward))
STEERING_FACTOR_WEIGHT = 0.0
STEERING_FACTOR_EASING = 'linear'
PROGRESS_FACTOR_WEIGHT = 0.0
PROGRESS_FACTOR_EASING = 'linear'
LANE_FACTOR_WEIGHT = 0.0
LANE_FACTOR_EASING = 'quintic'
RACE_LINE_FACTOR_WEIGHT = 0.0
RACE_LINE_FACTOR_EASING = 'linear'
# Globals
g_last_progress_value = 0.0
g_last_progress_time = 0.0
g_last_speed_value = 0.0
g_last_steering_angle = 0.0
# for getting current race line waypoint distances
g_race_line_dists = [LineString(RACE_LINE_WAYPOINTS).project(Point(p), normalized=True) for p in LineString(RACE_LINE_WAYPOINTS).coords[:-1]] + [1.0]
#===============================================================================
#
# REWARD
#
#===============================================================================
def reward_function(params):
"""Reward function is:
f(s,w,h,t,p) = 1.0 * W(s,Ks) * W(w,Kw) * W(h,Kh) * W(t,Kt) * W(p,Kp) * W(l,Kl)
s: speed factor, linear 0..1 for range of speed from 0 to MAX_SPEED
w: wheel factor, non-linear 0..1 for wheels being off the track and
vehicle in danger of going off the track. We want to use the full
if d["geometry"]["type"] == "Point":
mean = d["geometry"]["coordinates"]
elif d["geometry"]["type"] == "LineString":
mean = get_mean(d["geometry"]["coordinates"])
elif d["geometry"]["type"] == "Polygon":
mean = get_mean(d["geometry"]["coordinates"][0])
d.pop("geometry")
d["GeoCoordinate"] = {"longitude" : mean[0], "latitude" : mean[1]}
listOfStations.append(d)
for d_i, d in enumerate(data):
if "railway" in d and not "station" in d["railway"]:
if "geometry" in d:
poly = np.array(d["geometry"]["coordinates"])
poly = LineString(poly if len(poly.shape) == 2 else poly[0])
geom = ops.transform(tr.transform, poly)
for r in listOfStations:
circle = Point([r["GeoCoordinate"]["longitude"],r["GeoCoordinate"]["latitude"]])
circle = ops.transform(tr.transform, circle)
circle = Point(circle).buffer(100).boundary
i = circle.intersection(geom)
if not i.is_empty:
if "stations" in d:
d["stations"].append(r["@id"])
else:
d["stations"] = [r["@id"]]
if "stations" in d:
print(len(d["stations"]))
def link(self):
for osm_id,road in self.segment_map.items():
road_id = "r{0}".format(osm_id)
segment_link_map = {}
for segment in road:
segment_links = self.link_segment(segment,osm_id)
segment_link_map[segment] = segment_links
for (segment1,segment2) in zip(road[:-1],road[1:]):
assert(segment1[1] == segment2[0])
buildings1 = segment_link_map[segment1]
buildings2 = segment_link_map[segment2]
if segment1[1] in self.int_map:
i_id = "i{0}".format(self.int_map[segment1[1]])
self.add_edge(i_id,road_id, reverse=False)
minDistBuilding1 = self.findMinDistanceBuilding(self.int_map[segment1[1]],segment1[1][0],segment1[1][1],buildings1[0])
b1_id = "b{0}".format(minDistBuilding1)
minDistBuilding1R = self.findMinDistanceBuilding(self.int_map[segment1[1]],segment1[1][0],segment1[1][1],buildings1[1])
b1r_id = "b{0}".format(minDistBuilding1R)
minDistBuilding2 = self.findMinDistanceBuilding(self.int_map[segment1[1]],segment1[1][0],segment1[1][1],buildings2[0])
b2_id = "b{0}".format(minDistBuilding2)
minDistBuilding2R = self.findMinDistanceBuilding(self.int_map[segment1[1]],segment1[1][0],segment1[1][1],buildings2[1])
b2r_id = "b{0}".format(minDistBuilding2R)
#self.add_edge(b1_id,i_id)
#self.add_edge(b2_id,i_id)
#self.add_edge(b1r_id,i_id)
#self.add_edge(b2r_id,i_id)
self.add_edge(b1_id,b2_id, weight=0.5)
self.add_edge(b1r_id,b2r_id, weight=0.5)
nb = self.nodes[minDistBuilding1]
cp = (nb['x'],nb['y'])
#cpLatlon = to_latlon(cp)
nbR = self.nodes[minDistBuilding1R]
cpR = (nbR['x'],nbR['y'])
#cpRLatlon = to_latlon(cpR)
nb2 = self.nodes[minDistBuilding2]
cp2 = (nb2['x'],nb2['y'])
#cp2Latlon = to_latlon(cp2)
nb2R = self.nodes[minDistBuilding2R]
cp2R = (nb2R['x'],nb2R['y'])
#cp2RLatlon = to_latlon(cp2R)
edgeLine = LineString([cp, cp2])
edgeLine2 = LineString([cpR, cp2R])
roads = self.segment_lookup([cp[0],cp[1]],5)
for rd in roads:
rdLine = LineString([rd[0],rd[1]])
if edgeLine.intersects(rdLine):
self.add_edge(b1_id,i_id)
self.add_edge(b2_id,i_id)
if edgeLine2.intersects(rdLine):
self.add_edge(b1r_id,i_id)
self.add_edge(b2r_id,i_id)
else:
minDist = 99999999.9
bldPair = (buildings1[0][0],buildings2[0][-1])
for building in buildings1[0]:
bld = self.nodes[building]
minDistBuilding = self.findMinDistanceBuilding(building,bld['x'],bld['y'],buildings2[0])
minBld = self.nodes[minDistBuilding]
dist = self.getDistance(bld['x'],bld['y'],minBld['x'],minBld['y'])
if dist < minDist:
minDist = dist
bldPair = (building,minDistBuilding)
b1_id = "b{0}".format(bldPair[0])
b2_id = "b{0}".format(bldPair[1])
minDist = 99999999.9
bldPair = (buildings1[1][0],buildings2[1][-1])
for building in buildings1[1]:
bld = self.nodes[building]
minDistBuilding = self.findMinDistanceBuilding(building,bld['x'],bld['y'],buildings2[1])
minBld = self.nodes[minDistBuilding]
dist = self.getDistance(bld['x'],bld['y'],minBld['x'],minBld['y'])
if dist < minDist:
minDist = dist
bldPair = (building,minDistBuilding)
b1r_id = "b{0}".format(bldPair[0])
b2r_id = "b{0}".format(bldPair[1])
self.add_edge(b1_id,b2_id)
self.add_edge(b1r_id,b2r_id)
start = road[0]
end = road[-1]
buildings1 = segment_link_map[start]
buildings2 = segment_link_map[end]
if start[0] in self.int_map:
i_id = "i{0}".format(self.int_map[start[0]])
minDistBuilding = self.findMinDistanceBuilding(self.int_map[start[0]],start[0][0],start[0][1],buildings1[0])
b1_id = "b{0}".format(minDistBuilding)
minDistBuildingR = self.findMinDistanceBuilding(self.int_map[start[0]],start[0][0],start[0][1],buildings1[1])
b1r_id = "b{0}".format(minDistBuildingR)
self.add_edge(b1_id,i_id)
self.add_edge(b1r_id,i_id)
self.add_edge(b1_id,b1r_id, weight=0.5)
if end[1] in self.int_map:
i_id = "i{0}".format(self.int_map[end[1]])
minDistBuilding = self.findMinDistanceBuilding(self.int_map[end[1]],end[1][0],end[1][1],buildings2[0])
b2_id = "b{0}".format(minDistBuilding)
minDistBuildingR = self.findMinDistanceBuilding(self.int_map[end[1]],end[1][0],end[1][1],buildings2[1])
b2r_id = "b{0}".format(minDistBuildingR)
self.add_edge(b2_id,i_id)
self.add_edge(b2r_id,i_id)
self.add_edge(b2_id,b2r_id, weight=0.5)
# -*- coding: utf-8 -*-
"""
Created on Wed Sep 21 23:53:20 2016
@author: ckirst
"""
import matplotlib.pyplot as plt
## test shapel intersection
from shapely.geometry.polygon import LinearRing, LineString
contour = LinearRing([(0, 0), (2, 0), (2,2), (1,2)]);
line = LineString([(0,0), (3,4)]);
plt.figure(100); plt.clf();
x, y = contour.xy
plt.plot(x, y, color='r', alpha=0.7, linewidth=3, solid_capstyle='round', zorder=2)
x,y = line.xy;
plt.plot(x, y, color='b', alpha=0.7, linewidth=3, solid_capstyle='round', zorder=2)
x = line.intersection(contour);
for ob in x:
x, y = ob.xy
if len(x) == 1:
plt.plot(x, y, 'o', color='m', zorder=2)
else:
plt.plot(x, y, color='m', alpha=0.7, linewidth=3, solid_capstyle='round', zorder=2)
# -*- coding: utf-8 -*-
"""
Created on Wed Sep 21 23:53:20 2016
@author: ckirst
"""
import matplotlib.pyplot as plt
## test shapel intersection
from shapely.geometry.polygon import LinearRing, LineString
contour = LinearRing([(0, 0), (2, 0), (2, 2), (1, 2)])
line = LineString([(0, 0), (3, 4)])
plt.figure(100)
plt.clf()
x, y = contour.xy
plt.plot(x,
y,
color='r',
alpha=0.7,
linewidth=3,
solid_capstyle='round',
zorder=2)
x, y = line.xy
plt.plot(x,
y,
color='b',
alpha=0.7,
linewidth=3,
solid_capstyle='round',
class DeepRacerEnv(gym.Env):
def __init__(self):
self.sampling_rate = 30.0
self.sampling_sleep = (1.0/self.sampling_rate)
self.sampling_rates = [30.0, 30.0]
self.sampling_rate_index = 0
self.latencies = [10.0, 20.0, 40.0, 60.0, 80.0, 100.0, 120.0]
self.latency_index = 0
self.latency_max_num_steps = 500 # for these steps latency will be fixed or change on reset or done after 250.
self.latency_steps = 0
self.latency = 10.0 #10 is the starting latency
self.model_running_time = (2.0/1000.0) #model runtime
screen_height = TRAINING_IMAGE_SIZE[1]
screen_width = TRAINING_IMAGE_SIZE[0]
self.on_track = 0
self.progress = 0
self.yaw = 0
self.x = 0
self.y = 0
self.z = 0
self.distance_from_center = 0
self.distance_from_border_1 = 0
self.distance_from_border_2 = 0
self.steps = 0
self.progress_at_beginning_of_race = 0
self.reverse_dir = False
self.start_ndist = 0.0
# actions -> steering angle, throttle
self.action_space = spaces.Box(low=np.array([-1, 0]), high=np.array([+1, +1]), dtype=np.float32)
# given image from simulator
self.observation_space = spaces.Box(low=0, high=255,
shape=(screen_height, screen_width, 1), dtype=np.uint8)
self.allow_servo_step_signals = True
#stores the time when camera images are received
self.cam_update_time=[]
#stores the time when consequetive actions are send
self.cons_action_send_time=[]
#stores the time when progress updates are received
self.progress_update_time = []
#folder location to store the debug data
self.debug_data_folder = []
self.debug_index = 0
if node_type == SIMULATION_WORKER:
# ROS initialization
rospy.init_node('rl_coach', anonymous=True)
self.ack_publisher = rospy.Publisher('/vesc/low_level/ackermann_cmd_mux/output',
AckermannDriveStamped, queue_size=100)
self.racecar_service = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
self.clear_forces_client = rospy.ServiceProxy('/gazebo/clear_joint_forces',
JointRequest)
# Subscribe to ROS topics and register callbacks
rospy.Subscriber('/progress', Progress, self.callback_progress)
rospy.Subscriber('/camera/zed/rgb/image_rect_color', sensor_image, self.callback_image)
self.world_name = 'hard_track'#rospy.get_param('WORLD_NAME')
self.set_waypoints()
waypoints = self.waypoints
is_loop = np.all(waypoints[0,:] == waypoints[-1,:])
if is_loop:
self.center_line = LinearRing(waypoints[:,0:2])
else:
self.center_line = LineString(waypoints[:,0:2])
self.center_dists = [self.center_line.project(Point(p), normalized=True) for p in self.center_line.coords[:-1]] + [1.0]
self.track_length = self.center_line.length
self.reward_in_episode = 0
self.prev_progress = 0
self.steps = 0
# Create the publishers for sending speed and steering info to the car
self.velocity_pub_dict = OrderedDict()
self.steering_pub_dict = OrderedDict()
for topic in VELOCITY_TOPICS:
self.velocity_pub_dict[topic] = rospy.Publisher(topic, Float64, queue_size=1)
for topic in STEERING_TOPICS:
self.steering_pub_dict[topic] = rospy.Publisher(topic, Float64, queue_size=1)
def get_data_debug(self):
print("center_line",self.center_line)
print("track_length",self.track_length)
def reset(self,inp_x=1.75,inp_y=0.6):
if node_type == SAGEMAKER_TRAINING_WORKER:
return self.observation_space.sample()
#print('Total Reward Reward=%.2f' % self.reward_in_episode,
# 'Total Steps=%.2f' % self.steps)
#self.send_reward_to_cloudwatch(self.reward_in_episode)
self.reward_in_episode = 0
self.reward = None
self.done = False
self.next_state = None
self.image = None
self.steps = 0
self.prev_progress = 0
# Reset car in Gazebo
self.send_action(0, 0) # set the throttle to 0
self.racecar_reset(0, 0)
self.infer_reward_state(0, 0)
self.cam_update_time = []
self.cons_action_send_time = []
self.progress_update_time = []
self.debug_index= self.debug_index+1
return self.next_state
def add_latency_to_image(self,observation):
observation = observation.reshape(observation.shape[0],observation.shape[1],1)
#print('Set latency is:',self.latency*self.latency_max)
observation[119, 159, 0] = int(self.latency)
#setting the sampling rate
observation[119, 158, 0] = int(self.sampling_rate)
#print(observation[119, 159, 0],observation[119, 158, 0] )
return observation
def convert_rgb_to_gray(self, observation):
r, g, b = observation[:, :, 0], observation[:, :, 1], observation[:, :, 2]
observation = 0.2989 * r + 0.5870 * g + 0.1140 * b
return observation
def set_next_state(self):
if(self.image!=None):
#t1 = time.time()
image_data = self.image
# Read the image and resize to get the state
#print(image_data.width, image_data.height)
image = Image.frombytes('RGB', (image_data.width, image_data.height), image_data.data, 'raw', 'RGB', 0, 1)
image = image.resize(TRAINING_IMAGE_SIZE, resample=2)
image = np.array(image)
#image = do_randomization(image)
image = self.convert_rgb_to_gray(image)
image = self.add_latency_to_image(image)
self.next_state = image
def racecar_reset(self, ndist, next_index):
rospy.wait_for_service('gazebo/set_model_state')
#random_start = random.random()
prev_index, next_index = self.find_prev_next_waypoints(self.start_ndist)
# Compute the starting position and heading
#start_point = self.center_line.interpolate(ndist, normalized=True)
start_point = self.center_line.interpolate(self.start_ndist, normalized=True)
start_yaw = math.atan2(self.center_line.coords[next_index][1] - start_point.y,
self.center_line.coords[next_index][0] - start_point.x)
start_quaternion = Rotation.from_euler('zyx', [start_yaw, 0, 0]).as_quat()
# Construct the model state and send to Gazebo
model_state = ModelState()
model_state.model_name = 'racecar'
model_state.pose.position.x = start_point.x
model_state.pose.position.y = start_point.y
model_state.pose.position.z = 0
model_state.pose.orientation.x = start_quaternion[0]
model_state.pose.orientation.y = start_quaternion[1]
model_state.pose.orientation.z = start_quaternion[2]
model_state.pose.orientation.w = start_quaternion[3]
model_state.twist.linear.x = 0
model_state.twist.linear.y = 0
model_state.twist.linear.z = 0
model_state.twist.angular.x = 0
model_state.twist.angular.y = 0
model_state.twist.angular.z = 0
self.racecar_service(model_state)
for joint in EFFORT_JOINTS:
self.clear_forces_client(joint)
#keeping track where to start the car
self.reverse_dir = not self.reverse_dir
self.start_ndist = (self.start_ndist + ROUND_ROBIN_ADVANCE_DIST) % 1.0
self.progress_at_beginning_of_race = self.progress
def find_prev_next_waypoints(self, ndist):
if self.reverse_dir:
next_index = bisect.bisect_left(self.center_dists, ndist) - 1
prev_index = next_index + 1
if next_index == -1: next_index = len(self.center_dists) - 1
else:
next_index = bisect.bisect_right(self.center_dists, ndist)
prev_index = next_index - 1
if next_index == len(self.center_dists): next_index = 0
return prev_index, next_index
def step(self, action):
self.latency_steps = self.latency_steps+1
#print('latency set in env:',self.latency)
#bookeeping when the action was send
#self.cons_action_send_time.append([self.steps,time.time()])
latency = (self.latency-2.0)/1000.0
#10ms latency is substracted, because that is the avg default latency observed on the training machine
if latency>0.001:
time.sleep(latency)
else:
latency = 0.0
# Initialize next state, reward, done flag
self.next_state = None
self.reward = None
self.done = False
# Send this action to Gazebo and increment the step count
self.steering_angle = float(action[0])
self.speed = float(action[1])
self.send_action(self.steering_angle, self.speed)
self.steps += 1
#sleep to control sampling rate
to_sleep = (self.sampling_sleep - self.model_running_time - latency)
if to_sleep>0.001:
time.sleep(to_sleep)
# if self.latency_steps == self.latency_max_num_steps:
# #update the latency
# self.latency_index = (self.latency_index+1) % (len(self.latencies))
# self.latency = self.latencies[self.latency_index]
# #update the sampling rate
# self.sampling_rate_index = random.randint(0,1)
# self.sampling_rate = self.sampling_rates[self.sampling_rate_index]
# self.sampling_sleep = (1.0/self.sampling_rate)
# if (self.latency/1000.0)> self.sampling_sleep: # match sampling input to the model and latency
# self.sampling_rate = 1000.0/self.latency
# self.latency_steps = 0
# Compute the next state and reward
self.infer_reward_state(self.steering_angle, self.speed)
return self.next_state, self.reward, self.done, {}
def send_action(self, steering_angle, speed):
# Simple v/r to computes the desired rpm
wheel_rpm = speed/WHEEL_RADIUS
for _, pub in self.velocity_pub_dict.items():
pub.publish(wheel_rpm)
for _, pub in self.steering_pub_dict.items():
pub.publish(steering_angle)
def callback_image(self, data):
self.image = data
#bookeeping when the image was received
#self.cam_update_time.append([self.steps,time.time()])
def callback_progress(self, data):
self.on_track = not (data.off_track)
self.progress = data.progress
self.yaw = data.yaw
self.x = data.x
self.y = data.y
self.z = data.z
self.distance_from_center = data.distance_from_center
self.distance_from_border_1 = data.distance_from_border_1
self.distance_from_border_2 = data.distance_from_border_2
#bookeeping when the progress was received
#self.progress_update_time.append([self.steps,time.time()])
def reward_function (self, on_track, x, y, distance_from_center,
throttle, steering, track_width):
marker_1 = 0.1 * track_width
marker_2 = 0.15 * track_width
marker_3 = 0.20 * track_width
reward = (track_width - distance_from_center) #max reward = 0.44
if distance_from_center >= 0.0 and distance_from_center <= marker_1:
reward = reward * 2.5 #0.90, 0.44 max is scaled to 1.0
elif distance_from_center <= marker_2:
reward = reward * 1.33 #0.85, 0.375 max is scaled to 0.5
elif distance_from_center <= marker_3:
reward = reward * 0.71 #0.80, 0.352 max is scaled to 0.25
else:
reward = 0.001 # may go close to off track
# penalize reward for the car taking slow actions
if throttle < 1.6 and reward>0:
reward *= 0.95
if throttle < 1.4 and reward>0:
reward *= 0.95
return float(reward)
def infer_reward_state(self, steering_angle, throttle):
#state has to be set first, because we need most accurate reward signal
self.set_next_state()
on_track = self.on_track
done = False
if on_track != 1:
reward = CRASHED
done = True
else:
reward = self.reward_function(on_track, self.x, self.y, self.distance_from_center,
throttle, steering_angle, self.road_width)
#after 500 steps in episode we want to restart it
if self.steps==500:
done = True
if reward > 0: #car is not crashed
reward = reward *5.0 #bonus on completing 500 steps
self.reward_in_episode += reward
self.reward = reward
self.done = done
def set_waypoints(self):
if self.world_name.startswith(MEDIUM_TRACK_WORLD):
self.waypoints = vertices = np.zeros((8, 2))
self.road_width = 0.50
vertices[0][0] = -0.99; vertices[0][1] = 2.25;
vertices[1][0] = 0.69; vertices[1][1] = 2.26;
vertices[2][0] = 1.37; vertices[2][1] = 1.67;
vertices[3][0] = 1.48; vertices[3][1] = -1.54;
vertices[4][0] = 0.81; vertices[4][1] = -2.44;
vertices[5][0] = -1.25; vertices[5][1] = -2.30;
vertices[6][0] = -1.67; vertices[6][1] = -1.64;
vertices[7][0] = -1.73; vertices[7][1] = 1.63;
elif self.world_name.startswith(EASY_TRACK_WORLD):
self.waypoints = vertices = np.zeros((2, 2))
self.road_width = 0.90
vertices[0][0] = -1.08; vertices[0][1] = -0.05;
vertices[1][0] = 1.08; vertices[1][1] = -0.05;
else:
self.waypoints = vertices = np.zeros((30, 2))
self.road_width = 0.44
vertices[0][0] = 1.5; vertices[0][1] = 0.58;
vertices[1][0] = 5.5; vertices[1][1] = 0.58;
vertices[2][0] = 5.6; vertices[2][1] = 0.6;
vertices[3][0] = 5.7; vertices[3][1] = 0.65;
vertices[4][0] = 5.8; vertices[4][1] = 0.7;
vertices[5][0] = 5.9; vertices[5][1] = 0.8;
vertices[6][0] = 6.0; vertices[6][1] = 0.9;
vertices[7][0] = 6.08; vertices[7][1] = 1.1;
vertices[8][0] = 6.1; vertices[8][1] = 1.2;
vertices[9][0] = 6.1; vertices[9][1] = 1.3;
vertices[10][0] = 6.1; vertices[10][1] = 1.4;
vertices[11][0] = 6.07; vertices[11][1] = 1.5;
vertices[12][0] = 6.05; vertices[12][1] = 1.6;
vertices[13][0] = 6; vertices[13][1] = 1.7;
vertices[14][0] = 5.9; vertices[14][1] = 1.8;
vertices[15][0] = 5.75; vertices[15][1] = 1.9;
vertices[16][0] = 5.6; vertices[16][1] = 2.0;
vertices[17][0] = 4.2; vertices[17][1] = 2.02;
vertices[18][0] = 4; vertices[18][1] = 2.1;
vertices[19][0] = 2.6; vertices[19][1] = 3.92;
vertices[20][0] = 2.4; vertices[20][1] = 4;
vertices[21][0] = 1.2; vertices[21][1] = 3.95;
vertices[22][0] = 1.1; vertices[22][1] = 3.92;
vertices[23][0] = 1; vertices[23][1] = 3.88;
vertices[24][0] = 0.8; vertices[24][1] = 3.72;
vertices[25][0] = 0.6; vertices[25][1] = 3.4;
vertices[26][0] = 0.58; vertices[26][1] = 3.3;
vertices[27][0] = 0.57; vertices[27][1] = 3.2;
vertices[28][0] = 1; vertices[28][1] = 1;
vertices[29][0] = 1.25; vertices[29][1] = 0.7;
def get_closest_waypoint(self):
res = 0
index = 0
x = self.x
y = self.y
minDistance = float('inf')
for row in self.waypoints:
distance = math.sqrt((row[0] - x) * (row[0] - x) + (row[1] - y) * (row[1] - y))
if distance < minDistance:
minDistance = distance
res = index
index = index + 1
return res
def show_regions(self,
figure_number: int = 1,
title: str = 'Map regions',
block: bool = True,
figure_size: Tuple[float, float] = (7, 7)):
"""Displays the map segments that belong to each region.
Args:
figure_number: Any existing figure with the same value will be overwritten.
title: Plot title.
block: True to stop program execution until the figure window is closed.
figure_size: Figure window dimensions.
"""
x_min, y_min, x_max, y_max = self.bounds()
rows, cols = self._region_segments.shape
major_ticks = np.arange(min(x_min, y_min), max(x_max, y_max) + 0.01, 1)
if rows <= 5 and cols <= 5:
label_size = 8
map_line_width = 2
marker_size = 5
figure, axes = plt.subplots(rows,
cols,
figsize=figure_size,
num=figure_number,
sharex=True,
sharey=True)
else:
label_size = 5
map_line_width = 1.75
marker_size = 1.5
figure, axes = plt.subplots(rows,
cols,
figsize=figure_size,
num=figure_number,
sharex=True,
sharey=True,
gridspec_kw={
'hspace': 0,
'wspace': 0
})
for ax in axes.flat:
ax.set_xlabel('x [m]', fontsize='small')
ax.set_ylabel('y [m]', fontsize='small')
ax.label_outer(
) # Hide x labels and tick labels for top plots and y ticks for right plots.
ax.set_xticks(major_ticks)
ax.set_yticks(major_ticks)
ax.set_xlim(x_min, x_max)
ax.set_ylim(y_min, y_max)
ax.tick_params(axis='x', labelsize=label_size, rotation=90)
ax.tick_params(axis='y', labelsize=label_size)
ax.grid(which='both', alpha=0.33, linestyle='dashed', zorder=1)
figure.suptitle(title)
figure.tight_layout() # Reduce white margins
for y in np.arange(y_max - 0.5, y_min, -1):
for x in np.arange(x_min + 0.5, x_max):
circle = Point(x,
y).buffer(self._sensor_range + 1 / math.sqrt(2))
cx, cy = circle.exterior.xy
r, c = m._xy_to_rc((x, y))
axes[r, c].plot(x, y, 'bo', markersize=marker_size)
axes[r, c].plot(cx,
cy,
color='green',
alpha=1,
linewidth=1,
linestyle='dashed',
zorder=3)
for s in m._region_segments[r][c]:
lx, ly = LineString(s).xy
axes[r, c].plot(lx,
ly,
color='black',
linewidth=map_line_width,
solid_capstyle='round',
zorder=2)
plt.show(block=block)
[1.77997335, 1.3300841 ],
[1.92991192, 1.24882412],
[2.08935204, 1.17997642],
[2.2568989 , 1.12278679],
[2.43128201, 1.07640317],
[2.6112811 , 1.03978189],
[2.79573133, 1.0116745 ],
[2.98364267, 0.99085028]])
TARGET_NUMBER_STEPS = 150
RACE_LINE_WAYPOINTS = CANADA_RACE_LINE
# Globals
g_last_progress_value = 0.0
g_start_offset_percent = 0.0
g_race_line_string = LineString(RACE_LINE_WAYPOINTS)
# for getting current race line waypoint distances
g_race_line_dists = [
LineString(RACE_LINE_WAYPOINTS).project(Point(p), normalized=True)
for p in LineString(RACE_LINE_WAYPOINTS).coords[:-1]
] + [1.0]
#===============================================================================
#
# REWARD
#
#===============================================================================
def reward_function(params):
p = progress_factor(params)
def is_visible(point1, point2, polygons=polygon_buildings):
return int(not (any([
LineString((point1, point2)).crosses(polygon)
or polygon.contains(LineString((point1, point2)))
for polygon in polygons
]))) * LineString((point1, point2)).length
# unew = np.arange(0, 1.01, 0.01)
# out = interpolate.splev(unew, tck)
# Location of tracks folder
absolute_path = "."
# Get waypoints from numpy file
waypoints = np.load("%s/tracks-npy/%s.npy" % (absolute_path, track_name))
waypoints.shape
# Get number of waypoints
print("Number of waypoints[0] = " + str(waypoints.shape[0]))
print("Number of waypoints[1] = " + str(waypoints.shape[1]))
print("waypoints.shape = " + str(waypoints.shape))
l_center_line = LineString(waypoints[:, 0:2])
l_inner_border = LineString(waypoints[:, 2:4])
l_outer_border = LineString(waypoints[:, 4:6])
# rescale waypoints to centimeter scale
center_line = waypoints[:, 0:2] * 100
inner_border = waypoints[:, 2:4] * 100
outer_border = waypoints[:, 4:6] * 100
print('Training waypoints length:', len(waypoints))
# Plot waypoints
for i, point in enumerate(waypoints):
# print("point.shape = " + str(point.shape))
waypoint_center_line = (point[0], point[1]) * 10
waypoint_inner_line = (point[2], point[3]) * 10
def categorizeBuildings(self):
def isLeft(a,b,c):
return ((b[0] - a[0])*(c[1] - a[1]) - (b[1] - a[1])*(c[0] - a[0])) > 0;
def getIntersectionArea(bldPoly,line,perpLine,bldID):
if not line.intersects(bldPoly):
return 0.0
pt1 = list(line.coords)[0]
pt2 = list(line.coords)[1]
perppt1 = list(perpLine.coords)[0]
perppt2 = list(perpLine.coords)[1]
dx = perppt2[0] - perppt1[0]
dy = perppt2[1] - perppt1[1]
pt3 = (pt1[0]-dx,pt1[1]-dy)
pt4 = (pt2[0]-dx,pt2[1]-dy)
linePoly = Polygon([pt1,pt3,pt4,pt2])
try:
intersection_area = linePoly.intersection(bldPoly).area
return intersection_area/bldPoly.area
except:
return -1
def overlapsNeighbor(bldPoly,neighborPoly):
try:
intersection_area = bldPoly.intersection(neighborPoly).area
return intersection_area/bldPoly.area > 0.05
except:
return False
Rd2BldLeft = {}
Rd2BldRight = {}
for bld in self.buildingCenters.keys():
bldID = self.buildingCenters[bld][0]
#if bldID < 3700 or bldID > 3800:
#if bldID != 3751:
#continue
roads = self.segment_lookup([bld[0],bld[1]],10)
p1 = (bld[0], bld[1])
p1LatLon = to_latlon(p1)
res = self.buildingKDTree.query([bld[0],bld[1]], 20)
neighbors = []
for item in res[1][1:]: #start from index 1 because index 0 is always the original building bld
buildingID = self.buildingCenters[self.buildingCenters.keys()[item]][0]
neighbor = self.buildings[buildingID]
#this part removes a bug that some neighbor shapes have extra information, which will result in Polygon error later on.
start = neighbor[0]
for i in range(1,len(neighbor)):
if neighbor[i] == start and i > 2:
break
neighbor = neighbor[:i+1]
neighbors.append(neighbor)
roadDict = {}
bldVertices = self.buildingCenters[bld][1]
bldPolygon = Polygon(bldVertices)
for rd in roads:
rdLine = LineString([rd[0],rd[1]])
intersectsSomething = False
p3Intersect = self.perpIntersectPoint(p1,rd[0],rd[1])
if not p3Intersect:
continue
bldVertices.append(p1)
for vertex in bldVertices:
if intersectsSomething:
break
p3 = self.perpIntersectPoint(vertex,rd[0],rd[1])
vertexLatLon = to_latlon(vertex)
if p3:
p3LatLon = to_latlon(p3)
perpLine = LineString([p1,p3])
for neighbor in neighbors:
neighborPoly = Polygon(neighbor)
if overlapsNeighbor(bldPolygon,neighborPoly):
continue
if perpLine.intersects(neighborPoly):
intersectRd = False
intersectionRdArea = 0
if neighborPoly.intersects(rdLine):
intersectRd = True
intersectionRdArea = getIntersectionArea(neighborPoly,rdLine,perpLine,bldID)
if intersectionRdArea > 0.4 or not intersectRd:
#if bldID == 4287 or bldID == 4288:
#print intersectionRdArea
#road_lines.record(0)
#road_lines.poly(shapeType=shapefile.POLYLINE, parts=[[vertexLatLon,p3LatLon]])
intersectsSomething = True
break
for rd2 in roads:
if rd2 == rd:
continue
roadLine2 = LineString([rd2[0],rd2[1]])
if perpLine.intersects(roadLine2):
intersectsSomething = True
break
if not intersectsSomething:
perpLine = LineString([p1,p3Intersect])
if perpLine.length > (3*bldPolygon.length)/4:
continue
#if rdLine.length < (bldPolygon.length/3):
# continue
p3IntersectLatLon = to_latlon(p3Intersect)
road_lines.record(0)
road_lines.poly(shapeType=shapefile.POLYLINE, parts=[[p3IntersectLatLon,p1LatLon]])
if isLeft(rd[0],rd[1],p1):
if rd not in Rd2BldLeft:
Rd2BldLeft[rd] = [bldID]
else:
Rd2BldLeft[rd].append(bldID)
else:
if rd not in Rd2BldRight:
Rd2BldRight[rd] = [bldID]
else:
Rd2BldRight[rd].append(bldID)
return (Rd2BldLeft, Rd2BldRight)