diff --git a/examples/waypoint_follow.py b/examples/waypoint_follow.py index 0d86f602..855b358a 100644 --- a/examples/waypoint_follow.py +++ b/examples/waypoint_follow.py @@ -5,6 +5,7 @@ import numpy as np from numba import njit +from f1tenth_gym.envs.f110_env import F110Env """ Planner Helpers @@ -184,11 +185,11 @@ def __init__(self, track, wb): self.max_reacquire = 20.0 self.drawn_waypoints = [] - self.lookahead_point = np.array([0, 0, 0], dtype=np.float32) - self.current_index = 0 + self.lookahead_point = None + self.current_index = None - self.local_plan_render = None self.lookahead_point_render = None + self.local_plan_render = None def load_waypoints(self, conf): """ @@ -204,10 +205,11 @@ def render_lookahead_point(self, e): Callback to render the lookahead point. """ if self.lookahead_point is not None: - points = self.lookahead_point[:2][None] # shape (1, 2) if self.lookahead_point_render is None: + points = self.lookahead_point[:2][None] # shape (1, 2) self.lookahead_point_render = e.render_points(points, color=(0, 0, 128), size=2) else: + points = self.lookahead_point[:2][None] # shape (1, 2) self.lookahead_point_render.updateItems(points) def render_local_plan(self, e): @@ -215,10 +217,11 @@ def render_local_plan(self, e): update waypoints being drawn by EnvRenderer """ if self.current_index is not None: - points = self.waypoints[self.current_index : self.current_index + 10, :2] if self.local_plan_render is None: + points = self.waypoints[self.current_index : self.current_index + 10, :2] self.local_plan_render = e.render_lines(points, color=(0, 128, 0), size=1) else: + points = self.waypoints[self.current_index : self.current_index + 10, :2] self.local_plan_render.updateItems(points) def _get_current_waypoint( @@ -296,11 +299,11 @@ def main(): work = { "mass": 3.463388126201571, "lf": 0.15597534362552312, - "tlad": 0.82461887897713965, + "tlad": 0.82461887897713965*10, "vgain": 1, } - num_agents = 3 + num_agents = 1 env = gym.make( "f1tenth_gym:f1tenth-v0", config={ @@ -311,14 +314,15 @@ def main(): "control_input": ["speed", "steering_angle"], "model": "st", "observation_config": {"type": "kinematic_state"}, - "params": {"mu": 1.0}, + "params": F110Env.fullscale_vehicle_params(), "reset_config": {"type": "rl_random_static"}, + "scale": 10.0, }, render_mode="human", ) track = env.unwrapped.track - planner = PurePursuitPlanner(track=track, wb=0.17145 + 0.15875) + planner = PurePursuitPlanner(track=track, wb=(F110Env.fullscale_vehicle_params()["lf"] + F110Env.fullscale_vehicle_params()["lr"])) env.unwrapped.add_render_callback(track.raceline.render_waypoints) env.unwrapped.add_render_callback(planner.render_local_plan) diff --git a/f1tenth_gym/envs/track/track.py b/f1tenth_gym/envs/track/track.py index ba2d9108..f82abbbb 100644 --- a/f1tenth_gym/envs/track/track.py +++ b/f1tenth_gym/envs/track/track.py @@ -117,14 +117,19 @@ def from_track_name(track: str, track_scale: float = 1.0) -> Track: track_spec = Track.load_spec( track=track, filespec=str(track_dir / f"{track_dir.stem}_map.yaml") ) + track_spec.resolution = track_spec.resolution * track_scale + track_spec.origin = ( + track_spec.origin[0] * track_scale, + track_spec.origin[1] * track_scale, + track_spec.origin[2], + ) # load occupancy grid map_filename = pathlib.Path(track_spec.image) image = Image.open(track_dir / str(map_filename)).transpose( Transpose.FLIP_TOP_BOTTOM ) - image = image.resize(np.array(track_scale * np.array(image.size, dtype=np.int32), dtype=np.int32), Image.LANCZOS) - + occupancy_map = np.array(image).astype(np.float32) occupancy_map[occupancy_map <= 128] = 0.0 occupancy_map[occupancy_map > 128] = 255.0