def TakeAction(self, action):
if not self.injail:
proposal = Point((self.current_state.x + action[0],
self.current_state.y + action[1]))
#print proposal.x, proposal.y
path = LineString([self.current_state, proposal])
if self.obstacles:
for obstacle in self.obstacles:
if obstacle.intersects(path):
self.injail = True
self.current_state = Point((-1, -1))
return
if self.muds:
for mud in self.muds:
if mud.intersects(path):
# print 'we are here'
path_inmud = mud.intersection(path)
coords = [path.coords[0], path.coords[1]]
for loc in path_inmud.coords:
if loc not in coords:
coords.append(loc)
coords.sort(key=lambda tup: tup[1])
p_in_mud = proposal.intersects(mud)
s_in_mud = self.current_state.intersects(mud)
if p_in_mud and not s_in_mud:
# print 'current not in mud'
if coords.index((self.current_state.x,
self.current_state.y)) == 0:
x = coords[1][0] - coords[0][0] + 0.5 * (
coords[-1][0] - coords[1][0])
y = coords[1][1] - coords[0][1] + 0.5 * (
coords[-1][1] - coords[1][1])
proposal = Point(
(coords[0][0] + x, coords[0][1] + y))
else:
x = coords[1][0] - coords[-1][0] + 0.5 * (
coords[0][0] - coords[1][0])
y = coords[1][1] - coords[-1][1] + 0.5 * (
coords[0][1] - coords[1][1])
proposal = Point(
(coords[-1][0] + x, coords[-1][1] + y))
elif s_in_mud and not p_in_mud:
# print 'proposal not in mud'
if coords.index((self.current_state.x,
self.current_state.y)) == 0:
x = 0.5 * (coords[1][0] - coords[0][0]) + (
coords[-1][0] - coords[1][0])
y = 0.5 * (coords[1][1] - coords[0][1]) + (
coords[-1][1] - coords[1][1])
proposal = Point(
(coords[0][0] + x, coords[0][1] + y))
else:
x = 0.5 * (coords[1][0] - coords[-1][0]) + (
coords[0][0] - coords[1][0])
y = 0.5 * (coords[1][1] - coords[-1][1]) + (
coords[0][1] - coords[1][1])
proposal = Point(
(coords[-1][0] + x, coords[-1][1] + y))
else:
proposal = Point(
(self.current_state.x + action[0] * 0.5,
self.current_state.y + action[1] * 0.5))
path = LineString([self.current_state, proposal])
bounds = LinearRing(self.maze.exterior.coords)
if bounds.intersects(path):
onedge = bounds.intersection(path)
if type(onedge) is MultiPoint:
for point in onedge:
if not point.equals(self.current_state):
proposal = point
elif type(onedge) is Point:
if not onedge.equals(self.current_state):
proposal = onedge
elif not self.maze.contains(proposal):
proposal = bounds.interpolate(bounds.project(proposal))
self.current_state = proposal
else:
self.deadend_toleration = self.deadend_toleration - 1
return self.GetCurrentState()
class DeepRacer(object):
def __init__(self):
''' Constructor for the Deep Racer object, will load track and waypoints
'''
# Wait for required services to be available
rospy.wait_for_service('/gazebo/set_model_state')
rospy.wait_for_service('/gazebo/get_model_state')
# We have no guarantees as to when gazebo will load the model, therefore we need
# to wait until the model is loaded prior to resetting it for the first time
get_model_client = rospy.ServiceProxy('/gazebo/get_model_state',
GetModelState)
wait_for_model = True
while wait_for_model:
# If the model is not loaded the get model service will log an error
# Therefore, we add an timer to prevent the log from getting spammed with
# errors
time.sleep(WAIT_TO_PREVENT_SPAM)
model = get_model_client('racecar', '')
wait_for_model = not model.success
try:
bundle_prefix = os.environ.get(BUNDLE_KEY, None)
route_file_name = os.path.join(
bundle_prefix, 'install', 'deepracer_simulation', 'share',
'deepracer_simulation', 'routes',
'{}.npy'.format(rospy.get_param('WORLD_NAME')))
self.waypoints = np.load(route_file_name)
except Exception as ex:
print('[ERROR] Unable to import the waypoints {}'.format(ex))
sys.exit(1)
# Find the centerline
is_loop = np.all(self.waypoints[0, :] == self.waypoints[-1, :])
self.center_line = LinearRing(self.waypoints[:, 0:2]) if is_loop \
else LineString(self.waypoints[:, 0:2])
# Create the service that allows clients to retrieve the waypoints
self.waypoints_service = rospy.Service(WAYPOINT_SRV_NAME,
GetWaypointSrv,
self.handle_get_waypoints)
# Create service that allows clients to reset the car
self.resetcar_service = rospy.Service(RESET_CAR_SRV_NAME, ResetCarSrv,
self.handle_reset_car)
# Gazebo service that allows us to position the car
self.model_state_client = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
# Place the car at the starting point facing the forward direction
self.reset_car(0, 1)
def reset_car(self, ndist, next_index):
''' Reset's the car on the track
ndist - normalized track distance
next_index - index of the next way point
'''
# Compute the starting position and heading
start_point = self.center_line.interpolate(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.model_state_client(model_state)
def handle_get_waypoints(self, req):
'''Request handler for get way point, unconditionally returns the waypoint list
req - Request from the client which is empty for this service
'''
# Unfortunately, there is no clean way to send a numpy array through the ROS system.
# Therefore, we need to unroll the array and let the client recreate and reshape the
# numpy object
waypoint_shape = self.waypoints.shape
return GetWaypointSrvResponse(waypoint_shape[0], waypoint_shape[1],
self.waypoints.ravel().tolist())
def handle_reset_car(self, req):
'''Request handler for resetting the car
req - Request from the client, should contain ndist and next_index
'''
self.reset_car(req.ndist, req.next_index)
return ResetCarSrvResponse(1)
def intersectionsGridFrontiere(P, X, Y):
"""Calcule la trace de la grille cartésienne définie par X, Y sur le Polyligne P
autrement dit les intersections de P avec les droites
- x = X[i] verticales et
- y = Y[j] horizontales
qui représentent la grille.
:param P : un polyligne np.ndarray de shape (n,3) ou (n,2). La 3-eme dimension est ignorée.
:param X, Y : la grille cartésienne.
X et Y sont des np.ndarray de shape (nx,1) et (ny,1) qui représentent
les abscisses et les ordonnées de la grille
- On suppose que X et Y sont croissants (i)
- On suppose également que la grille recouvre entièrement P, et déborde, i.e.
min(X) < xmin(P) <= xmax(P) < max(X)
min(Y) < ymin(P) <= ymax(P) < max(Y)
:return PD: np.ndarray((npd,2)) contenant les points
"""
# nx, ny = len(X), len(Y)
# for x in X : plt.plot(ny*[x], Y, 'y-', linewidth=0.3,)
# for y in Y : plt.plot(X, nx*[y], 'y-', linewidth=0.3)
# plt.plot(P[:,0],P[:,1], 'r.')
# P = LineString(P)
P = LinearRing(P)
# x,y = P.xy
# plt.plot(x,y,'g-')
# plt.axes().set_aspect('equal')
# debug(P)
(minx, miny, maxx, maxy) = P.bounds
# debug(P.bounds)
#Les numeros de droites verticales intersectant P
iX = np.where(np.logical_and(minx < X, X < maxx))[0]
# px = X[iX]#une croix pour marquer les droite concernées (graphique)
# plt.plot(px, len(px)*[min(Y)], 'rx')
#les droites verticales
ms = [((X[i], Y[0]), (X[i], Y[-1])) for i in iX]
#Les numeros de droites horizontales intersectant P
iY = np.where(np.logical_and(miny < Y, Y < maxy))[0]
# px = Y[iY]#une croix pour marquer les droite concernées
# plt.plot(len(px)*[max(X)], px, 'rx')
# plt.legend()
# plt.show()
#Les droites horizontales concernées par P
ms.extend([((X[0], Y[i]), (X[-1], Y[i])) for i in iY])
D = MultiLineString(ms) #La famille des droites de la grille
# debug(len(D))
#convex_hull pour réordonner les points, en faire un polygone
#array_interface pour recuperer les data pures numpy
PD = P.intersection(D) #.array_interface()
# debug(type(PD))
D = [P.project(pd)
for pd in PD] #abscisse curviligne des points d'intersection
D.sort()
PD = [P.interpolate(d).xy for d in D] #Les points dans l'ordre
# PD = PD.array_interface()
# exit()
#.convex_hull.exterior
# shape = PD['shape']
#PD.reshape(...) : si le tableau PD doit être recopié => en silence
#PD = array(PD['data']).reshape(shape)
# PD = array(PD['data'])
PD = array(PD)
#PD.shape=... : si le tableau PD doit être recopié => erreur
# PD.shape = shape
return PD
