def __init__(self):
# get params
self.ego_scan_topic = rospy.get_param('ego_scan_topic')
self.ego_odom_topic = rospy.get_param('ego_odom_topic')
self.opp_odom_topic = rospy.get_param('opp_odom_topic')
self.ego_drive_topic = rospy.get_param('ego_drive_topic')
self.race_info_topic = rospy.get_param('race_info_topic')
self.ego_reset_topic = rospy.get_param('ego_reset_topic')
self.scan_distance_to_base_link = rospy.get_param(
'scan_distance_to_base_link')
self.map_path = rospy.get_param('map_path')
self.map_img_ext = rospy.get_param('map_img_ext')
print(self.map_path, self.map_img_ext)
exec_dir = rospy.get_param('executable_dir')
scan_fov = rospy.get_param('scan_fov')
scan_beams = rospy.get_param('scan_beams')
self.angle_min = -scan_fov / 2.
self.angle_max = scan_fov / 2.
self.angle_inc = scan_fov / scan_beams
csv_path = rospy.get_param('waypoints_path')
wheelbase = 0.3302
mass = 3.47
l_r = 0.17145
I_z = 0.04712
mu = 0.523
h_cg = 0.074
cs_f = 4.718
cs_r = 5.4562
# init gym backend
self.racecar_env = gym.make('f110_gym:f110-v0')
self.racecar_env.init_map(self.map_path, self.map_img_ext, False,
False)
self.racecar_env.update_params(mu,
h_cg,
l_r,
cs_f,
cs_r,
I_z,
mass,
exec_dir,
double_finish=True)
# init opponent agent
# TODO: init by params.yaml
self.opp_agent = PurePursuitAgent(csv_path, wheelbase)
initial_state = {
'x': [0.0, 200.0],
'y': [0.0, 200.0],
'theta': [0.0, 0.0]
}
self.obs, _, self.done, _ = self.racecar_env.reset(initial_state)
self.ego_pose = [0., 0., 0.]
self.ego_speed = [0., 0., 0.]
self.ego_steer = 0.0
self.opp_pose = [200., 200., 0.]
self.opp_speed = [0., 0., 0.]
self.opp_steer = 0.0
# keep track of latest sim state
self.ego_scan = list(self.obs['scans'][0])
# keep track of collision
self.ego_collision = False
self.opp_collision = False
# transform broadcaster
self.br = transform_broadcaster.TransformBroadcaster()
# pubs
self.ego_scan_pub = rospy.Publisher(self.ego_scan_topic,
LaserScan,
queue_size=1)
self.ego_odom_pub = rospy.Publisher(self.ego_odom_topic,
Odometry,
queue_size=1)
self.opp_odom_pub = rospy.Publisher(self.opp_odom_topic,
Odometry,
queue_size=1)
self.info_pub = rospy.Publisher(self.race_info_topic,
RaceInfo,
queue_size=1)
# subs
self.drive_sub = rospy.Subscriber(self.ego_drive_topic,
AckermannDriveStamped,
self.drive_callback,
queue_size=1)
# reset sub
self.reset_sub = rospy.Subscriber(self.ego_reset_topic,
Bool,
self.reset_callback,
queue_size=1)
# Timer
self.timer = rospy.Timer(rospy.Duration(0.004), self.timer_callback)
def __init__(self):
# get env vars
self.race_scenario = os.environ.get('RACE_SCENARIO')
if self.race_scenario is None:
print(
'Race scenario not set! Using single car timed trial as default.'
)
self.race_scenario = 0
else:
self.race_scenario = int(self.race_scenario)
self.ego_id = os.environ.get('EGO_ID')
self.opp_id = os.environ.get('OPP_ID')
if self.opp_id is None:
print('Opponent id not set! Using opp_id as default.')
self.opp_id = 'opp_id'
# initialize parameters
# Topic Names
self.race_info_topic = rospy.get_param('race_info_topic')
self.ego_scan_topic = '/' + self.ego_id + '/scan'
self.ego_odom_topic = '/' + self.ego_id + '/odom'
self.opp_odom_topic = '/' + self.opp_id + '/odom'
self.ego_drive_topic = '/' + self.ego_id + '/drive'
if self.race_scenario:
# 2 car grand prix, need extra topics
self.opp_scan_topic = '/' + self.opp_id + '/scan'
self.ego_opp_odom_topic = '/' + self.ego_id + '/opp_odom'
self.opp_ego_odom_topic = '/' + self.opp_id + '/opp_odom'
self.opp_drive_topic = '/' + self.opp_id + '/drive'
else:
# single car timed trial (qualifying)
pass
self.scan_distance_to_base_link = rospy.get_param(
'scan_distance_to_base_link')
# Map
self.map_path = os.environ.get('RACE_MAP_PATH')
self.map_img_ext = os.environ.get('RACE_MAP_IMG_EXT')
# self.map_path = rospy.get_param('map_path')
# self.map_img_ext = rospy.get_param('map_img_ext')
# C++ backend
exec_dir = os.environ.get('F1TENTH_EXEC_DIR')
# exec_dir = rospy.get_param('executable_dir')
# Scan simulation params
scan_fov = rospy.get_param('scan_fov')
scan_beams = rospy.get_param('scan_beams')
self.angle_min = -scan_fov / 2.
self.angle_max = scan_fov / 2.
self.angle_inc = scan_fov / scan_beams
# Track centerline, legacy
# csv_path = rospy.get_param('waypoints_path')
# Vehicle parameters
wheelbase = 0.3302
mass = 3.74
l_r = 0.17145
I_z = 0.04712
mu = 0.523
h_cg = 0.074
cs_f = 4.718
cs_r = 5.4562
# init gym backend
self.racecar_env = gym.make('f110_gym:f110-v0')
self.racecar_env.init_map(self.map_path, self.map_img_ext, False,
False)
self.racecar_env.update_params(mu,
h_cg,
l_r,
cs_f,
cs_r,
I_z,
mass,
exec_dir,
double_finish=True)
# init agent poses
if self.race_scenario:
# two car
initial_state = {
'x': [
float(os.environ.get('EGO_STARTING_X')),
float(os.environ.get('OPP_STARTING_X'))
],
'y': [
float(os.environ.get('EGO_STARTING_Y')),
float(os.environ.get('OPP_STARTING_Y'))
],
'theta': [
float(os.environ.get('EGO_STARTING_THETA')),
float(os.environ.get('OPP_STARTING_THETA'))
]
}
self.ego_pose = [
float(os.environ.get('EGO_STARTING_X')),
float(os.environ.get('EGO_STARTING_Y')),
float(os.environ.get('EGO_STARTING_THETA'))
]
self.opp_pose = [
float(os.environ.get('OPP_STARTING_X')),
float(os.environ.get('OPP_STARTING_Y')),
float(os.environ.get('OPP_STARTING_THETA'))
]
else:
# single car
initial_state = {
'x': [float(os.environ.get('EGO_STARTING_X')), 200.0],
'y': [float(os.environ.get('EGO_STARTING_Y')), 200.0],
'theta': [float(os.environ.get('EGO_STARTING_THETA')), 0.0]
}
self.ego_pose = [
float(os.environ.get('EGO_STARTING_X')),
float(os.environ.get('EGO_STARTING_Y')),
float(os.environ.get('EGO_STARTING_THETA'))
]
self.opp_pose = [200., 200., 0.]
self.obs, _, self.done, _ = self.racecar_env.reset(initial_state)
self.ego_speed = [0., 0., 0.]
self.ego_steer = 0.0
self.ego_requested_speed = 0.0
self.ego_drive_published = False
self.opp_speed = [0., 0., 0.]
self.opp_steer = 0.0
self.opp_requested_speed = 0.0
self.opp_drive_published = False
# keep track of latest sim state
self.ego_scan = list(self.obs['scans'][0])
self.opp_scan = list(self.obs['scans'][1])
# keep track of collision
self.ego_collision = False
self.opp_collision = False
# transform broadcaster
self.br = transform_broadcaster.TransformBroadcaster()
# publishers
if self.race_scenario:
self.ego_scan_pub = rospy.Publisher(self.ego_scan_topic,
LaserScan,
queue_size=1)
self.opp_scan_pub = rospy.Publisher(self.opp_scan_topic,
LaserScan,
queue_size=1)
self.ego_odom_pub = rospy.Publisher(self.ego_odom_topic,
Odometry,
queue_size=1)
self.ego_opp_odom_pub = rospy.Publisher(self.ego_opp_odom_topic,
Odometry,
queue_size=1)
self.opp_odom_pub = rospy.Publisher(self.opp_odom_topic,
Odometry,
queue_size=1)
self.opp_ego_odom_pub = rospy.Publisher(self.opp_ego_odom_topic,
Odometry,
queue_size=1)
self.info_pub = rospy.Publisher(self.race_info_topic,
RaceInfo,
queue_size=1)
else:
self.ego_scan_pub = rospy.Publisher(self.ego_scan_topic,
LaserScan,
queue_size=1)
self.ego_odom_pub = rospy.Publisher(self.ego_odom_topic,
Odometry,
queue_size=1)
self.opp_odom_pub = rospy.Publisher(self.opp_odom_topic,
Odometry,
queue_size=1)
self.info_pub = rospy.Publisher(self.race_info_topic,
RaceInfo,
queue_size=1)
# subs
self.drive_sub = rospy.Subscriber(self.ego_drive_topic,
AckermannDriveStamped,
self.drive_callback,
queue_size=1)
if self.race_scenario:
self.opp_drive_sub = rospy.Subscriber(self.opp_drive_topic,
AckermannDriveStamped,
self.opp_drive_callback,
queue_size=1)
# Timer
self.timer = rospy.Timer(rospy.Duration(0.004), self.timer_callback)
self.drive_timer = rospy.Timer(rospy.Duration(0.01),
self.drive_timer_callback)