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