diff --git a/Dockerfile-gym-ros b/Dockerfile-gym-ros index 822c3466..b8106865 100644 --- a/Dockerfile-gym-ros +++ b/Dockerfile-gym-ros @@ -74,10 +74,13 @@ RUN echo "source /opt/ros/foxy/setup.bash" >> ~/.bashrc && \ mv f1tenth_racetracks /sim_ws/src/car_node/car_node/map/ COPY model-gym-ros-env/car_node/setup.py /sim_ws/src/car_node/setup.py -COPY model-gym-ros-env/car_node/car_node/main.py model-gym-ros-env/car_node/car_node/wrapper.py model-gym-ros-env/car_node/car_node/map_utility.py model-gym-ros-env/car_node/car_node/setup.py /sim_ws/src/car_node/car_node/ +COPY model-gym-ros-env/car_node/car_node/main.py model-gym-ros-env/car_node/car_node/setup.py /sim_ws/src/car_node/car_node/ + +COPY model-gym-ros-env/f1tenth_gym_ros/config/sim.yaml /sim_ws/src/f1tenth_gym_ros/config/sim.yaml +COPY model-gym-ros-env/f1tenth_gym_ros/f1tenth_gym_ros/gym_bridge.py /sim_ws/src/f1tenth_gym_ros/f1tenth_gym_ros/gym_bridge.py + COPY train_test /sim_ws/src/car_node/car_node/train_test COPY models/* /sim_ws/src/car_node/car_node/models/ -COPY model-gym-ros-env/f1tenth_gym_ros/config/sim.yaml /sim_ws/src/f1tenth_gym_ros/config/sim.yaml RUN pip install -e /sim_ws/src/car_node/car_node && \ source /opt/ros/foxy/setup.bash && \ diff --git a/model-gym-ros-env/car_node/car_node/main.py b/model-gym-ros-env/car_node/car_node/main.py index 0a868745..7a552631 100644 --- a/model-gym-ros-env/car_node/car_node/main.py +++ b/model-gym-ros-env/car_node/car_node/main.py @@ -8,23 +8,16 @@ from sensor_msgs.msg import LaserScan from ackermann_msgs.msg import AckermannDriveStamped - -import gym from stable_baselines3 import PPO -from car_node.wrapper import F110_Wrapped -from f110_gym.envs.base_classes import Integrator -from car_node.map_utility import get_map, get_formatted_map, map_ext from car_node.wrapper import convert_range -timestep = 0.01 -tensorboard_path = './train_test/' -map = "BrandsHatch" -path = get_map(map) -map_path = get_formatted_map(path) device = 'cpu' -STEER_SCALE = 0.5 -SPEED_SCALE = 0.7 +STEERING_RANGE_MIN = -0.4189 +STEERING_RANGE_MAX = 0.4189 +SPEED_RANGE_MIN = -5 +SPEED_RANGE_MAX = 20 +MODEL_INTERVAL = [-1, 1] class PPOModelEvaluator(Node): @@ -35,12 +28,7 @@ def __init__(self): lidarscan_topic = '/scan' drive_topic = '/drive' - # Model & Env - self.eval_env = gym.make('f110_gym:f110-v0', map=map_path, map_ext=map_ext, num_agents=1, timestep=timestep, integrator=Integrator.RK4) - self.eval_env = F110_Wrapped(self.eval_env, random_map=False) - self.eval_env.set_map_path(path) - self.model = PPO.load("/sim_ws/src/car_node/car_node/train_test/best_global_model", self.eval_env, device=device) - self.vec_env = self.model.get_env() + self.model = PPO.load("/sim_ws/src/car_node/car_node/models/YasMarina_optimize_model.zip", device=device) # ROS self.lidar_sub = self.create_subscription(LaserScan, lidarscan_topic, self.lidar_callback, 10) @@ -48,18 +36,17 @@ def __init__(self): def lidar_callback(self, data): d = np.array(data.ranges, dtype=np.float64) - d = convert_range(d, [data.angle_min, data.angle_max], [-1, 1]) - - action, _states = self.model.predict(d, deterministic=True) - action = self.eval_env.un_normalise_actions(action) + d = convert_range(d, [data.range_min, data.range_max], MODEL_INTERVAL) - self.get_logger().info(f'{action[0], action[1]}') + action = self.model.predict(d, deterministic=True)[0] + steer = convert_range(action[0], MODEL_INTERVAL, [STEERING_RANGE_MIN, STEERING_RANGE_MAX]) + speed = convert_range(action[1], MODEL_INTERVAL, [SPEED_RANGE_MIN, SPEED_RANGE_MAX]) drive_msg = AckermannDriveStamped() drive_msg.header.stamp = self.get_clock().now().to_msg() drive_msg.header.frame_id = "laser" - drive_msg.drive.steering_angle = action[0] * STEER_SCALE - drive_msg.drive.speed = action[1] * SPEED_SCALE + drive_msg.drive.steering_angle = steer + drive_msg.drive.speed = speed self.drive_pub.publish(drive_msg) def main(args=None): diff --git a/model-gym-ros-env/car_node/car_node/map_utility.py b/model-gym-ros-env/car_node/car_node/map_utility.py deleted file mode 100644 index 000b325b..00000000 --- a/model-gym-ros-env/car_node/car_node/map_utility.py +++ /dev/null @@ -1,101 +0,0 @@ -#get all the path of the map in ./map/f1tenth_racetracks -import os -import random -import numpy as np - - -wpt_delim= ';' -wpt_rowskip= 3 -wpt_xind= 1 -wpt_yind= 2 -wpt_thind= 3 -wpt_vind= 5 -map_ext= '.png' - -def get_map_path(): - path = os.path.abspath(os.path.join(os.path.dirname(__file__), os.path.pardir)) - map_path = "/sim_ws/src/car_node/car_node/map/f1tenth_racetracks" - return map_path - -def get_map_list(): - map_path = get_map_path() - map_list = os.listdir(map_path) - filtered_map_list = [] - - for map_file in map_list: - if os.path.isdir(os.path.join(map_path, map_file)): - raceline_file = os.path.join(map_path, map_file, f"{map_file}_raceline.csv") - if os.path.exists(raceline_file): - filtered_map_list.append(map_file) - - return filtered_map_list - -def get_map(map_name): - map_path = get_map_path() - map_file = map_path + "/" + map_name - return map_file - -def get_formatted_map(map_name): - map_file = map_name + "/" + get_map_name(map_name) + "_map" - return map_file - -def get_map_name(map_file): - map_name = map_file.split("/")[-1] - return map_name - -def get_map_list_name(): - map_list = get_map_list() - map_list_name = [] - for map_file in map_list: - map_list_name.append(get_map_name(map_file)) - return map_list_name - -def get_map_list_file(): - map_list = get_map_list() - map_list_file = [] - for map_file in map_list: - map_list_file.append(get_map(map_file)) - return map_list_file - -def get_map_list_name_file(): - map_list = get_map_list() - map_list_name_file = [] - for map_file in map_list: - map_list_name_file.append(get_map_name(map_file)) - map_list_name_file.append(get_map(map_file)) - return map_list_name_file - -def get_map_list_name_file_dict(): - map_list = get_map_list() - map_list_name_file_dict = {} - for map_file in map_list: - map_list_name_file_dict[get_map_name(map_file)] = get_map(map_file) - return map_list_name_file_dict - - - -def get_one_random_map(): - map_list = get_map_list() - map_file = random.choice(map_list) - return get_map(map_file) - - -def get_raceline(map_folder): - map_name = get_map_name(map_folder) - raceline_file = map_folder + "/"+ map_name + "_raceline.csv" - raceline = np.genfromtxt(raceline_file, delimiter=wpt_delim, skip_header=wpt_rowskip) - return raceline - -def get_x_y_theta_from_raceline(raceline): - x = raceline[:, wpt_xind] - y = raceline[:, wpt_yind] - theta = raceline[:, wpt_thind] - return x, y, theta - -def get_start_position(map_folder): - raceline = get_raceline(map_folder) - x, y, theta = get_x_y_theta_from_raceline(raceline) - start_pose = [x[0], y[0], theta[0]] - return start_pose - - diff --git a/model-gym-ros-env/car_node/car_node/wrapper.py b/model-gym-ros-env/car_node/car_node/wrapper.py deleted file mode 100644 index c0f8b958..00000000 --- a/model-gym-ros-env/car_node/car_node/wrapper.py +++ /dev/null @@ -1,253 +0,0 @@ -import gym -import numpy as np -from gym import spaces -from pathlib import Path -from argparse import Namespace -from pyglet.gl import GL_POINTS -import car_node.map_utility as map_utility -from typing import Any, Dict, List, Optional, SupportsFloat, Tuple, Union - -def convert_range(value, input_range, output_range): - # converts value(s) from range to another range - # ranges ---> [min, max] - (in_min, in_max), (out_min, out_max) = input_range, output_range - in_range = in_max - in_min - out_range = out_max - out_min - return (((value - in_min) * out_range) / in_range) + out_min - - -class F110_Wrapped(gym.Wrapper): - """ - This is a wrapper for the F1Tenth Gym environment intended - for only one car, but should be expanded to handle multi-agent scenarios - """ - - def __init__(self, env, random_map=False): - super().__init__(env) - - # normalised action space, steer and speed - self.action_space = spaces.Box(low=np.array([-1.0, -1.0]), high=np.array([1.0, 1.0]), dtype=np.float) - - # normalised observations, just take the lidar scans - self.observation_space = spaces.Box(low=-1.0, high=1.0, shape=(1080,), dtype=np.float) - - # store allowed steering/speed/lidar ranges for normalisation - self.s_min = self.env.params['s_min'] - self.s_max = self.env.params['s_max'] - self.v_min = self.env.params['v_min'] - self.v_max = self.env.params['v_max'] - self.lidar_min = 0 - self.lidar_max = 30 # see ScanSimulator2D max_range - - # store car dimensions and some track info - self.car_length = self.env.params['length'] - self.car_width = self.env.params['width'] - self.track_width = 3.2 # ~= track width, see random_trackgen.py - - # radius of circle where car can start on track, relative to a centerpoint - self.start_radius = (self.track_width / 2) - ((self.car_length + self.car_width) / 2) # just extra wiggle room - - self.step_count = 0 - - # set threshold for maximum angle of car, to prevent spinning - self.max_theta = 100 - self.count = 0 - - self.map_path = None - self.random_map = random_map - - self.race_line_color = [255, 0, 0] - self.race_line_x = [] - self.race_line_y = [] - self.race_line_theta = [] - - self.episode_returns = [] - - self.is_rendering = False - - self.last_position = {'x': None, 'y': None} - - - def get_total_steps(self) -> int: - return self.step_count - - def get_episode_rewards(self) -> List[float]: - """ - Returns the rewards of all the episodes - - :return: - """ - return self.episode_returns - - - - - - def set_raceliens(self): - if self.map_path is not None: - raceline = map_utility.get_raceline(self.map_path) - self.race_line_x, self.race_line_y, self.race_line_theta = map_utility.get_x_y_theta_from_raceline(raceline) - self.race_line_x = self.race_line_x.tolist() - self.race_line_y = self.race_line_y.tolist() - self.race_line_theta = self.race_line_theta.tolist() - - def set_map_path(self, map_path): - self.map_path = map_path - self.set_raceliens() - - - def start_position(self): - if self.map_path is not None: - x, y, t = map_utility.get_start_position(self.map_path) - self.set_raceliens() - return x, y, t - raise Exception("Map path not set") - - - - def step(self, action): - - #add noise to action - #action = action + np.random.normal(0, 0.1, 2) - - action_convert = self.un_normalise_actions(action) - observation, _, done, info = self.env.step(np.array([action_convert])) - - self.step_count += 1 - - - - next_x = self.race_line_x[self.count] - next_y = self.race_line_y[self.count] - - if self.env.renderer: - if done: - self.race_line_color = [np.random.randint(0, 255), np.random.randint(0, 255), np.random.randint(0, 255)] - self.env.renderer.batch.add(1, GL_POINTS, None, ('v3f/stream', [next_x * 50, next_y * 50, 0.]), - ('c3B/stream', self.race_line_color)) - - reward = 0 - - - - - if self.count < len(self.race_line_x) - 1: - X, Y = observation['poses_x'][0], observation['poses_y'][0] - - dist = np.sqrt(np.power((X - next_x), 2) + np.power((Y - next_y), 2)) - if dist < 2: - self.count = self.count + 1 - reward += 0.01 - pass - - if dist < 2.5: - complete = 1#(self.count/len(self.race_line_x)) * 0.5 - reward += complete - else: - print("---- Lap Done ---->", self.map_path) - self.count = 0 - reward += 10 - - - # Check if the car has moved a significant distance from the last position - distance_from_last_position = np.sqrt( - np.power((observation["poses_x"][0] - self.last_position['x']), 2) - + - np.power((observation["poses_y"][0] - self.last_position['y']), 2) - ) - - reward += distance_from_last_position - - self.last_position['x'] = observation['poses_x'][0] - self.last_position['y'] = observation['poses_y'][0] - - - - - if observation['collisions'][0]: - done = True - self.count = 0 - reward = 0 - - if abs(observation['poses_theta'][0]) > self.max_theta: - done = True - self.count = 0 - reward = 0 - #if the car go out of the track the episode is done - if len(self.episode_returns) > 50_000: - print(reward) - self.episode_returns = [] - done = True - self.count = 0 - reward = 0 - print("Episod Done - Too slow") - - - - - - self.episode_returns.append(reward) - - - - - return self.normalise_observations(observation['scans'][0]), reward, bool(done), info - - - def reset(self): - if self.random_map: - path = map_utility.get_one_random_map() - map_path = map_utility.get_formatted_map(path) - map_ext = map_utility.map_ext - self.update_map(map_path, map_ext, update_render=True) - self.set_map_path(path) - - # Select a random point from the race line - race_line_x = self.race_line_x - race_line_y = self.race_line_y - race_line_theta = self.race_line_theta - random_index = np.random.randint(len(race_line_x)) - x = race_line_x[random_index] - y = race_line_y[random_index] - t = race_line_theta[random_index] - - - # Update the race line to start from the selected point - race_line_x = race_line_x[random_index:] + race_line_x[:random_index] - race_line_y = race_line_y[random_index:] + race_line_y[:random_index] - race_line_theta = race_line_theta[random_index:] + race_line_theta[:random_index] - - - self.race_line_x = race_line_x - self.race_line_y = race_line_y - self.race_line_theta = race_line_theta - - - self.episode_returns = [] - - self.last_position = {'x': x, 'y': y} - - - observation, _, _, _ = self.env.reset(np.array([[x, y, t]])) - return self.normalise_observations(observation['scans'][0]) - - def update_map(self, map_name, map_extension, update_render=False): - self.env.map_name = map_name - self.env.map_ext = map_extension - self.env.update_map(f"{map_name}.yaml", map_extension) - - def seed(self, seed): - self.current_seed = seed - np.random.seed(self.current_seed) - print(f"Seed -> {self.current_seed}") - - def un_normalise_actions(self, actions): - # convert actions from range [-1, 1] to normal steering/speed range - steer = convert_range(actions[0], [-1, 1], [self.s_min, self.s_max]) - speed = convert_range(actions[1], [-1, 1], [self.v_min, self.v_max]) - return np.array([steer, speed], dtype=np.float) - - def normalise_observations(self, observations): - # convert observations from normal lidar distances range to range [-1, 1] - return convert_range(observations, [self.lidar_min, self.lidar_max], [-1, 1]) - diff --git a/model-gym-ros-env/f1tenth_gym_ros/f1tenth_gym_ros/gym_bridge.py b/model-gym-ros-env/f1tenth_gym_ros/f1tenth_gym_ros/gym_bridge.py new file mode 100644 index 00000000..8c9d8e11 --- /dev/null +++ b/model-gym-ros-env/f1tenth_gym_ros/f1tenth_gym_ros/gym_bridge.py @@ -0,0 +1,411 @@ +# MIT License + +# Copyright (c) 2020 Hongrui Zheng + +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: + +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. + +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. + +import rclpy +from rclpy.node import Node + +from sensor_msgs.msg import LaserScan +from nav_msgs.msg import Odometry +from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import PoseWithCovarianceStamped +from geometry_msgs.msg import Twist +from geometry_msgs.msg import TransformStamped +from geometry_msgs.msg import Transform +from geometry_msgs.msg import Quaternion +from ackermann_msgs.msg import AckermannDriveStamped +from tf2_ros import TransformBroadcaster + +import gym +import numpy as np +from transforms3d import euler + +class GymBridge(Node): + def __init__(self): + super().__init__('gym_bridge') + + self.declare_parameter('ego_namespace') + self.declare_parameter('ego_odom_topic') + self.declare_parameter('ego_opp_odom_topic') + self.declare_parameter('ego_scan_topic') + self.declare_parameter('ego_drive_topic') + self.declare_parameter('opp_namespace') + self.declare_parameter('opp_odom_topic') + self.declare_parameter('opp_ego_odom_topic') + self.declare_parameter('opp_scan_topic') + self.declare_parameter('opp_drive_topic') + self.declare_parameter('scan_distance_to_base_link') + self.declare_parameter('scan_fov') + self.declare_parameter('scan_beams') + self.declare_parameter('map_path') + self.declare_parameter('map_img_ext') + self.declare_parameter('num_agent') + self.declare_parameter('sx') + self.declare_parameter('sy') + self.declare_parameter('stheta') + self.declare_parameter('sx1') + self.declare_parameter('sy1') + self.declare_parameter('stheta1') + self.declare_parameter('kb_teleop') + + # check num_agents + num_agents = self.get_parameter('num_agent').value + if num_agents < 1 or num_agents > 2: + raise ValueError('num_agents should be either 1 or 2.') + elif type(num_agents) != int: + raise ValueError('num_agents should be an int.') + + # env backend + self.env = gym.make('f110_gym:f110-v0', + map=self.get_parameter('map_path').value, + map_ext=self.get_parameter('map_img_ext').value, + num_agents=num_agents) + + sx = self.get_parameter('sx').value + sy = self.get_parameter('sy').value + stheta = self.get_parameter('stheta').value + self.ego_pose = [sx, sy, stheta] + self.ego_speed = [0.0, 0.0, 0.0] + self.ego_requested_speed = 0.0 + self.ego_steer = 0.0 + self.ego_collision = False + ego_scan_topic = self.get_parameter('ego_scan_topic').value + ego_drive_topic = self.get_parameter('ego_drive_topic').value + scan_fov = self.get_parameter('scan_fov').value + scan_beams = self.get_parameter('scan_beams').value + self.angle_min = -scan_fov / 2. + self.angle_max = scan_fov / 2. + self.angle_inc = scan_fov / scan_beams + self.ego_namespace = self.get_parameter('ego_namespace').value + ego_odom_topic = self.ego_namespace + '/' + self.get_parameter('ego_odom_topic').value + self.scan_distance_to_base_link = self.get_parameter('scan_distance_to_base_link').value + + if num_agents == 2: + self.has_opp = True + self.opp_namespace = self.get_parameter('opp_namespace').value + sx1 = self.get_parameter('sx1').value + sy1 = self.get_parameter('sy1').value + stheta1 = self.get_parameter('stheta1').value + self.opp_pose = [sx1, sy1, stheta1] + self.opp_speed = [0.0, 0.0, 0.0] + self.opp_requested_speed = 0.0 + self.opp_steer = 0.0 + self.opp_collision = False + self.obs, _ , self.done, _ = self.env.reset(np.array([[sx, sy, stheta], [sx1, sy1, stheta1]])) + self.ego_scan = list(self.obs['scans'][0]) + self.opp_scan = list(self.obs['scans'][1]) + + opp_scan_topic = self.get_parameter('opp_scan_topic').value + opp_odom_topic = self.opp_namespace + '/' + self.get_parameter('opp_odom_topic').value + opp_drive_topic = self.get_parameter('opp_drive_topic').value + + ego_opp_odom_topic = self.ego_namespace + '/' + self.get_parameter('ego_opp_odom_topic').value + opp_ego_odom_topic = self.opp_namespace + '/' + self.get_parameter('opp_ego_odom_topic').value + else: + self.has_opp = False + self.obs, _ , self.done, _ = self.env.reset(np.array([[sx, sy, stheta]])) + self.ego_scan = list(self.obs['scans'][0]) + + # sim physical step timer + self.drive_timer = self.create_timer(0.05, self.drive_timer_callback) + # topic publishing timer + self.timer = self.create_timer(0.004, self.timer_callback) + + # transform broadcaster + self.br = TransformBroadcaster(self) + + # publishers + self.ego_scan_pub = self.create_publisher(LaserScan, ego_scan_topic, 10) + self.ego_odom_pub = self.create_publisher(Odometry, ego_odom_topic, 10) + self.ego_drive_published = False + if num_agents == 2: + self.opp_scan_pub = self.create_publisher(LaserScan, opp_scan_topic, 10) + self.ego_opp_odom_pub = self.create_publisher(Odometry, ego_opp_odom_topic, 10) + self.opp_odom_pub = self.create_publisher(Odometry, opp_odom_topic, 10) + self.opp_ego_odom_pub = self.create_publisher(Odometry, opp_ego_odom_topic, 10) + self.opp_drive_published = False + + # subscribers + self.ego_drive_sub = self.create_subscription( + AckermannDriveStamped, + ego_drive_topic, + self.drive_callback, + 10) + self.ego_reset_sub = self.create_subscription( + PoseWithCovarianceStamped, + '/initialpose', + self.ego_reset_callback, + 10) + if num_agents == 2: + self.opp_drive_sub = self.create_subscription( + AckermannDriveStamped, + opp_drive_topic, + self.opp_drive_callback, + 10) + self.opp_reset_sub = self.create_subscription( + PoseStamped, + '/goal_pose', + self.opp_reset_callback, + 10) + + if self.get_parameter('kb_teleop').value: + self.teleop_sub = self.create_subscription( + Twist, + '/cmd_vel', + self.teleop_callback, + 10) + + + def drive_callback(self, drive_msg): + self.ego_requested_speed = drive_msg.drive.speed + self.ego_steer = drive_msg.drive.steering_angle + self.ego_drive_published = True + + def opp_drive_callback(self, drive_msg): + self.opp_requested_speed = drive_msg.drive.speed + self.opp_steer = drive_msg.drive.steering_angle + self.opp_drive_published = True + + def ego_reset_callback(self, pose_msg): + rx = pose_msg.pose.pose.position.x + ry = pose_msg.pose.pose.position.y + rqx = pose_msg.pose.pose.orientation.x + rqy = pose_msg.pose.pose.orientation.y + rqz = pose_msg.pose.pose.orientation.z + rqw = pose_msg.pose.pose.orientation.w + _, _, rtheta = euler.quat2euler([rqw, rqx, rqy, rqz], axes='sxyz') + if self.has_opp: + opp_pose = [self.obs['poses_x'][1], self.obs['poses_y'][1], self.obs['poses_theta'][1]] + self.obs, _ , self.done, _ = self.env.reset(np.array([[rx, ry, rtheta], opp_pose])) + else: + self.obs, _ , self.done, _ = self.env.reset(np.array([[rx, ry, rtheta]])) + + def opp_reset_callback(self, pose_msg): + if self.has_opp: + rx = pose_msg.pose.position.x + ry = pose_msg.pose.position.y + rqx = pose_msg.pose.orientation.x + rqy = pose_msg.pose.orientation.y + rqz = pose_msg.pose.orientation.z + rqw = pose_msg.pose.orientation.w + _, _, rtheta = euler.quat2euler([rqw, rqx, rqy, rqz], axes='sxyz') + self.obs, _ , self.done, _ = self.env.reset(np.array([list(self.ego_pose), [rx, ry, rtheta]])) + def teleop_callback(self, twist_msg): + if not self.ego_drive_published: + self.ego_drive_published = True + + self.ego_requested_speed = twist_msg.linear.x + + if twist_msg.angular.z > 0.0: + self.ego_steer = 0.3 + elif twist_msg.angular.z < 0.0: + self.ego_steer = -0.3 + else: + self.ego_steer = 0.0 + + def drive_timer_callback(self): + if self.ego_drive_published and not self.has_opp: + self.obs, _, self.done, _ = self.env.step(np.array([[self.ego_steer, self.ego_requested_speed]])) + elif self.ego_drive_published and self.has_opp and self.opp_drive_published: + self.obs, _, self.done, _ = self.env.step(np.array([[self.ego_steer, self.ego_requested_speed], [self.opp_steer, self.opp_requested_speed]])) + self._update_sim_state() + + def timer_callback(self): + ts = self.get_clock().now().to_msg() + + # pub scans + scan = LaserScan() + scan.header.stamp = ts + scan.header.frame_id = self.ego_namespace + '/laser' + scan.angle_min = self.angle_min + scan.angle_max = self.angle_max + scan.angle_increment = self.angle_inc + scan.range_min = 0. + scan.range_max = 30. + scan.ranges = self.ego_scan + self.ego_scan_pub.publish(scan) + + if self.has_opp: + opp_scan = LaserScan() + opp_scan.header.stamp = ts + opp_scan.header.frame_id = self.opp_namespace + '/laser' + opp_scan.angle_min = self.angle_min + opp_scan.angle_max = self.angle_max + opp_scan.angle_increment = self.angle_inc + opp_scan.range_min = 0. + opp_scan.range_max = 30. + opp_scan.ranges = self.opp_scan + self.opp_scan_pub.publish(opp_scan) + + # pub tf + self._publish_odom(ts) + self._publish_transforms(ts) + self._publish_laser_transforms(ts) + self._publish_wheel_transforms(ts) + + def _update_sim_state(self): + self.ego_scan = list(self.obs['scans'][0]) + if self.has_opp: + self.opp_scan = list(self.obs['scans'][1]) + self.opp_pose[0] = self.obs['poses_x'][1] + self.opp_pose[1] = self.obs['poses_y'][1] + self.opp_pose[2] = self.obs['poses_theta'][1] + self.opp_speed[0] = self.obs['linear_vels_x'][1] + self.opp_speed[1] = self.obs['linear_vels_y'][1] + self.opp_speed[2] = self.obs['ang_vels_z'][1] + + self.ego_pose[0] = self.obs['poses_x'][0] + self.ego_pose[1] = self.obs['poses_y'][0] + self.ego_pose[2] = self.obs['poses_theta'][0] + self.ego_speed[0] = self.obs['linear_vels_x'][0] + self.ego_speed[1] = self.obs['linear_vels_y'][0] + self.ego_speed[2] = self.obs['ang_vels_z'][0] + + + + def _publish_odom(self, ts): + ego_odom = Odometry() + ego_odom.header.stamp = ts + ego_odom.header.frame_id = 'map' + ego_odom.child_frame_id = self.ego_namespace + '/base_link' + ego_odom.pose.pose.position.x = self.ego_pose[0] + ego_odom.pose.pose.position.y = self.ego_pose[1] + ego_quat = euler.euler2quat(0., 0., self.ego_pose[2], axes='sxyz') + ego_odom.pose.pose.orientation.x = ego_quat[1] + ego_odom.pose.pose.orientation.y = ego_quat[2] + ego_odom.pose.pose.orientation.z = ego_quat[3] + ego_odom.pose.pose.orientation.w = ego_quat[0] + ego_odom.twist.twist.linear.x = self.ego_speed[0] + ego_odom.twist.twist.linear.y = self.ego_speed[1] + ego_odom.twist.twist.angular.z = self.ego_speed[2] + self.ego_odom_pub.publish(ego_odom) + + if self.has_opp: + opp_odom = Odometry() + opp_odom.header.stamp = ts + opp_odom.header.frame_id = 'map' + opp_odom.child_frame_id = self.opp_namespace + '/base_link' + opp_odom.pose.pose.position.x = self.opp_pose[0] + opp_odom.pose.pose.position.y = self.opp_pose[1] + opp_quat = euler.euler2quat(0., 0., self.opp_pose[2], axes='sxyz') + opp_odom.pose.pose.orientation.x = opp_quat[1] + opp_odom.pose.pose.orientation.y = opp_quat[2] + opp_odom.pose.pose.orientation.z = opp_quat[3] + opp_odom.pose.pose.orientation.w = opp_quat[0] + opp_odom.twist.twist.linear.x = self.opp_speed[0] + opp_odom.twist.twist.linear.y = self.opp_speed[1] + opp_odom.twist.twist.angular.z = self.opp_speed[2] + self.opp_odom_pub.publish(opp_odom) + self.opp_ego_odom_pub.publish(ego_odom) + self.ego_opp_odom_pub.publish(opp_odom) + + def _publish_transforms(self, ts): + ego_t = Transform() + ego_t.translation.x = self.ego_pose[0] + ego_t.translation.y = self.ego_pose[1] + ego_t.translation.z = 0.0 + ego_quat = euler.euler2quat(0.0, 0.0, self.ego_pose[2], axes='sxyz') + ego_t.rotation.x = ego_quat[1] + ego_t.rotation.y = ego_quat[2] + ego_t.rotation.z = ego_quat[3] + ego_t.rotation.w = ego_quat[0] + + ego_ts = TransformStamped() + ego_ts.transform = ego_t + ego_ts.header.stamp = ts + ego_ts.header.frame_id = 'map' + ego_ts.child_frame_id = self.ego_namespace + '/base_link' + self.br.sendTransform(ego_ts) + + if self.has_opp: + opp_t = Transform() + opp_t.translation.x = self.opp_pose[0] + opp_t.translation.y = self.opp_pose[1] + opp_t.translation.z = 0.0 + opp_quat = euler.euler2quat(0.0, 0.0, self.opp_pose[2], axes='sxyz') + opp_t.rotation.x = opp_quat[1] + opp_t.rotation.y = opp_quat[2] + opp_t.rotation.z = opp_quat[3] + opp_t.rotation.w = opp_quat[0] + + opp_ts = TransformStamped() + opp_ts.transform = opp_t + opp_ts.header.stamp = ts + opp_ts.header.frame_id = 'map' + opp_ts.child_frame_id = self.opp_namespace + '/base_link' + self.br.sendTransform(opp_ts) + + def _publish_wheel_transforms(self, ts): + ego_wheel_ts = TransformStamped() + ego_wheel_quat = euler.euler2quat(0., 0., self.ego_steer, axes='sxyz') + ego_wheel_ts.transform.rotation.x = ego_wheel_quat[1] + ego_wheel_ts.transform.rotation.y = ego_wheel_quat[2] + ego_wheel_ts.transform.rotation.z = ego_wheel_quat[3] + ego_wheel_ts.transform.rotation.w = ego_wheel_quat[0] + ego_wheel_ts.header.stamp = ts + ego_wheel_ts.header.frame_id = self.ego_namespace + '/front_left_hinge' + ego_wheel_ts.child_frame_id = self.ego_namespace + '/front_left_wheel' + self.br.sendTransform(ego_wheel_ts) + ego_wheel_ts.header.frame_id = self.ego_namespace + '/front_right_hinge' + ego_wheel_ts.child_frame_id = self.ego_namespace + '/front_right_wheel' + self.br.sendTransform(ego_wheel_ts) + + if self.has_opp: + opp_wheel_ts = TransformStamped() + opp_wheel_quat = euler.euler2quat(0., 0., self.opp_steer, axes='sxyz') + opp_wheel_ts.transform.rotation.x = opp_wheel_quat[1] + opp_wheel_ts.transform.rotation.y = opp_wheel_quat[2] + opp_wheel_ts.transform.rotation.z = opp_wheel_quat[3] + opp_wheel_ts.transform.rotation.w = opp_wheel_quat[0] + opp_wheel_ts.header.stamp = ts + opp_wheel_ts.header.frame_id = self.opp_namespace + '/front_left_hinge' + opp_wheel_ts.child_frame_id = self.opp_namespace + '/front_left_wheel' + self.br.sendTransform(opp_wheel_ts) + opp_wheel_ts.header.frame_id = self.opp_namespace + '/front_right_hinge' + opp_wheel_ts.child_frame_id = self.opp_namespace + '/front_right_wheel' + self.br.sendTransform(opp_wheel_ts) + + def _publish_laser_transforms(self, ts): + ego_scan_ts = TransformStamped() + ego_scan_ts.transform.translation.x = self.scan_distance_to_base_link + # ego_scan_ts.transform.translation.z = 0.04+0.1+0.025 + ego_scan_ts.transform.rotation.w = 1. + ego_scan_ts.header.stamp = ts + ego_scan_ts.header.frame_id = self.ego_namespace + '/base_link' + ego_scan_ts.child_frame_id = self.ego_namespace + '/laser' + self.br.sendTransform(ego_scan_ts) + + if self.has_opp: + opp_scan_ts = TransformStamped() + opp_scan_ts.transform.translation.x = self.scan_distance_to_base_link + opp_scan_ts.transform.rotation.w = 1. + opp_scan_ts.header.stamp = ts + opp_scan_ts.header.frame_id = self.opp_namespace + '/base_link' + opp_scan_ts.child_frame_id = self.opp_namespace + '/laser' + self.br.sendTransform(opp_scan_ts) + +def main(args=None): + rclpy.init(args=args) + gym_bridge = GymBridge() + rclpy.spin(gym_bridge) + +if __name__ == '__main__': + main()