Skip to content

Commit

Permalink
removed a lot of ~
Browse files Browse the repository at this point in the history
  • Loading branch information
T3OTWAWKI committed Apr 13, 2024
1 parent 26034d8 commit 03ca0b5
Show file tree
Hide file tree
Showing 8 changed files with 58 additions and 58 deletions.
2 changes: 1 addition & 1 deletion src/rktl_autonomy/rktl_autonomy/_ros_interface.py
Original file line number Diff line number Diff line change
Expand Up @@ -103,7 +103,7 @@ def __init__(self, node_name='gym_interface', eval=False, launch_file=None, laun
# self.__DELTA_T = rospy.Duration.from_sec(1.0 / rospy.get_param('~rate', 30.0))
# self.__clock_pub = rospy.Publisher('/clock', Clock, queue_size=1, latch=True)

self.__DELTA_T = int(1e9 / self.node.get_parameter_or('~rate', rclpy.Parameter(30.0)).get_parameter_value().double_value) # nanoseconds
self.__DELTA_T = int(1e9 / self.node.get_parameter_or('rate', rclpy.Parameter(30.0)).get_parameter_value().double_value) # nanoseconds
self.__clock_pub = self.node.create_publisher(Clock, '/clock', qos_profile=1)

# initialize sim time
Expand Down
8 changes: 4 additions & 4 deletions src/rktl_autonomy/rktl_autonomy/plotter.py
Original file line number Diff line number Diff line change
Expand Up @@ -25,13 +25,13 @@ def __init__(self):
super().__init__('model_progress_plotter')

# Constants
self.LOG_DIR = normpath(expanduser(self.get_parameter('~log/base_dir').get_parameter_value().string_value))
self.LOG_DIR = normpath(expanduser(self.get_parameter('log/base_dir').get_parameter_value().string_value))
#self.PLOT_FREQ = rospy.get_param('~log/plot_freq', 25)
self.PLOT_FREQ = self.get_parameter_or('~log/plot_freq', Parameter(25)).get_parameter_value().integer_value
self.PLOT_FREQ = self.get_parameter_or('log/plot_freq', Parameter(25)).get_parameter_value().integer_value
#self.BASIC_VARS = rospy.get_param('~log/basic', ["duration"])
self.BASIC_VARS = self.get_parameter_or('~log/basic', Parameter(["duration"])).get_parameter_value().string_value
self.BASIC_VARS = self.get_parameter_or('log/basic', Parameter(["duration"])).get_parameter_value().string_value
#self.ADVANCED_VARS = rospy.get_param('~log/advanced', ["net_reward"])
self.ADVANCED_VARS = self.get_parameter_or('~log/advanced', Parameter(["net_reward"])).get_parameter_value().string_value
self.ADVANCED_VARS = self.get_parameter_or('log/advanced', Parameter(["net_reward"])).get_parameter_value().string_value

self.KEYS = ["episode"]
self.KEYS.append(self.BASIC_VARS)
Expand Down
2 changes: 1 addition & 1 deletion src/rktl_autonomy/rktl_autonomy/rocket_league_agent.py
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@ def main():
env = RocketLeagueInterface(eval=True)

# load the model
weights = expanduser(env.node.get_parameter('~weights').get_parameter_value().string_value)
weights = expanduser(env.node.get_parameter('weights').get_parameter_value().string_value)
model = PPO.load(weights)

# evaluate in real-time
Expand Down
52 changes: 26 additions & 26 deletions src/rktl_autonomy/rktl_autonomy/rocket_league_interface.py
Original file line number Diff line number Diff line change
Expand Up @@ -56,56 +56,56 @@ def __init__(self, eval=False, launch_file=('rktl_autonomy', 'rocket_league_trai

# Action space overrides
if self.node.has_parameter('~action_space/velocity/min'):
min_velocity = self.node.get_parameter('~action_space/velocity/min').get_parameter_value().double_value
min_velocity = self.node.get_parameter('action_space/velocity/min').get_parameter_value().double_value
assert min_velocity > self._MIN_VELOCITY
self._MIN_VELOCITY = min_velocity

if self.node.has_parameter('~action_space/velocity/max'):
max_velocity = self.node.get_parameter('~action_space/velocity/max').get_parameter_value().double_value
max_velocity = self.node.get_parameter('action_space/velocity/max').get_parameter_value().double_value
assert max_velocity < self._MAX_VELOCITY
self._MAX_VELOCITY = max_velocity

if self.node.has_parameter('~action_space/curvature/min'):
min_curvature = self.node.get_parameter('~action_space/curvature/min').get_parameter_value().double_value
min_curvature = self.node.get_parameter('action_space/curvature/min').get_parameter_value().double_value
assert min_curvature > self._MIN_CURVATURE
self._MIN_CURVATURE = min_curvature

if self.node.has_parameter('~action_space/curvature/max'):
max_curvature = self.node.get_parameter('~action_space/curvature/max').get_parameter_value().double_value
max_curvature = self.node.get_parameter('action_space/curvature/max').get_parameter_value().double_value
assert max_curvature < self._MAX_CURVATURE
self._MAX_CURVATURE = max_curvature

# Observations
self._FIELD_WIDTH = self.node.get_parameter('/field/width').get_parameter_value().double_value
self._FIELD_LENGTH = self.node.get_parameter('/field/length').get_parameter_value().double_value
self._GOAL_DEPTH = self.node.get_parameter_or('~observation/goal_depth', Parameter(0.075)).get_parameter_value().double_value
self._MAX_OBS_VEL = self.node.get_parameter_or('~observation/velocity/max_abs', Parameter(3.0)).get_parameter_value().double_value
self._MAX_OBS_ANG_VEL = self.node.get_parameter_or('~observation/angular_velocity/max_abs', Parameter(2*pi)).get_parameter_value().double_value
self._GOAL_DEPTH = self.node.get_parameter_or('observation/goal_depth', Parameter(0.075)).get_parameter_value().double_value
self._MAX_OBS_VEL = self.node.get_parameter_or('observation/velocity/max_abs', Parameter(3.0)).get_parameter_value().double_value
self._MAX_OBS_ANG_VEL = self.node.get_parameter_or('observation/angular_velocity/max_abs', Parameter(2*pi)).get_parameter_value().double_value

# Learning
self._MAX_TIME = self.node.get_parameter_or('~max_episode_time', Parameter(30.0)).get_parameter_value().double_value
self._CONSTANT_REWARD = self.node.get_parameter_or('~reward/constant', Parameter(0.0)).get_parameter_value().double_value
self._BALL_DISTANCE_REWARD = self.node.get_parameter_or('~reward/ball_dist_sq', Parameter(0.0)).get_parameter_value().double_value
self._GOAL_DISTANCE_REWARD = self.node.get_parameter_or('~reward/goal_dist_sq', Parameter(0.0)).get_parameter_value().double_value
self._DIRECTION_CHANGE_REWARD = self.node.get_parameter_or('~reward/direction_change', Parameter(0.0)).get_parameter_value().double_value

if isinstance(self.node.get_parameter_or('~reward/win', Parameter([100.0])).get_parameter_value().double_array_value, int):
self._WIN_REWARD = self.node.get_parameter_or('~reward/win', Parameter([100.0])).get_parameter_value().double_array_value
self._MAX_TIME = self.node.get_parameter_or('max_episode_time', Parameter(30.0)).get_parameter_value().double_value
self._CONSTANT_REWARD = self.node.get_parameter_or('reward/constant', Parameter(0.0)).get_parameter_value().double_value
self._BALL_DISTANCE_REWARD = self.node.get_parameter_or('reward/ball_dist_sq', Parameter(0.0)).get_parameter_value().double_value
self._GOAL_DISTANCE_REWARD = self.node.get_parameter_or('reward/goal_dist_sq', Parameter(0.0)).get_parameter_value().double_value
self._DIRECTION_CHANGE_REWARD = self.node.get_parameter_or('reward/direction_change', Parameter(0.0)).get_parameter_value().double_value

if isinstance(self.node.get_parameter_or('reward/win', Parameter([100.0])).get_parameter_value().double_array_value, int):
self._WIN_REWARD = self.node.get_parameter_or('reward/win', Parameter([100.0])).get_parameter_value().double_array_value
else:
if len(self.node.get_parameter_or('~reward/win', Parameter([100.0])).get_parameter_value().double_array_value) >= self.env_number:
self._WIN_REWARD = self.node.get_parameter_or('~reward/win', Parameter([100.0])).get_parameter_value().double_array_value[0]
if len(self.node.get_parameter_or('reward/win', Parameter([100.0])).get_parameter_value().double_array_value) >= self.env_number:
self._WIN_REWARD = self.node.get_parameter_or('reward/win', Parameter([100.0])).get_parameter_value().double_array_value[0]
else:
self._WIN_REWARD = self.node.get_parameter_or('~reward/win', Parameter([100.0])).get_parameter_value().double_array_value[self.env_number]
if isinstance(self.node.get_parameter_or('~reward/loss', Parameter([100.0])).get_parameter_value().double_array_value, int):
self._LOSS_REWARD = self.node.get_parameter_or('~reward/loss', Parameter([100.0])).get_parameter_value().double_array_value
self._WIN_REWARD = self.node.get_parameter_or('reward/win', Parameter([100.0])).get_parameter_value().double_array_value[self.env_number]
if isinstance(self.node.get_parameter_or('reward/loss', Parameter([100.0])).get_parameter_value().double_array_value, int):
self._LOSS_REWARD = self.node.get_parameter_or('reward/loss', Parameter([100.0])).get_parameter_value().double_array_value
else:
if len(self.node.get_parameter_or('~reward/loss', Parameter([100.0])).get_parameter_value().double_array_value) >= self.env_number:
self._LOSS_REWARD = self.node.get_parameter_or('~reward/loss', Parameter([100.0], type_=8)).get_parameter_value().double_array_value[0]
if len(self.node.get_parameter_or('reward/loss', Parameter([100.0])).get_parameter_value().double_array_value) >= self.env_number:
self._LOSS_REWARD = self.node.get_parameter_or('reward/loss', Parameter([100.0], type_=8)).get_parameter_value().double_array_value[0]
else:
self._LOSS_REWARD = self.node.get_parameter_or('~reward/loss', Parameter([100.0])).get_parameter_value().double_array_value[self.env_number]
self._REVERSE_REWARD = self.node.get_parameter_or('~reward/reverse', Parameter(0.0)).get_parameter_value().double_value
self._WALL_REWARD = self.node.get_parameter_or('~reward/walls/value', Parameter(0.0)).get_parameter_value().double_value
self._WALL_THRESHOLD = self.node.get_parameter_or('~reward/walls/threshold', Parameter(0.0)).get_parameter_value().double_value
self._LOSS_REWARD = self.node.get_parameter_or('reward/loss', Parameter([100.0])).get_parameter_value().double_array_value[self.env_number]
self._REVERSE_REWARD = self.node.get_parameter_or('reward/reverse', Parameter(0.0)).get_parameter_value().double_value
self._WALL_REWARD = self.node.get_parameter_or('reward/walls/value', Parameter(0.0)).get_parameter_value().double_value
self._WALL_THRESHOLD = self.node.get_parameter_or('reward/walls/threshold', Parameter(0.0)).get_parameter_value().double_value

# Publishers
self._command_pub = self.node.create_publisher(ControlCommand, 'cars/car0/command', 1)
Expand Down
2 changes: 1 addition & 1 deletion src/rktl_perception/nodes/pose_to_tf
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@ class Publisher(rclpy.create_node):

self.broadcaster = TransformBroadcaster()
#self.FRAME = rospy.get_param('~cam_frame_id', 'cam0')
self.FRAME = self.get_parameter_or('~cam_frame_id', 'cam0').get_parameter_value().string_value
self.FRAME = self.get_parameter_or('cam_frame_id', 'cam0').get_parameter_value().string_value

#rospy.Subscriber("pose", PoseWithCovarianceStamped, self.pose_cb)
self.subscription = self.create_subscription(PoseWithCovarianceStamped, "pose", self.pose_cb)
Expand Down
6 changes: 3 additions & 3 deletions src/rktl_perception/nodes/projector
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@ class Projector(rclpy.create_node):

# constants
#self.GROUND_HEIGHT = rospy.get_param('~ground_height', 0.0)
self.GROUND_HEIGHT = self.get_parameter_or('~ground_height', 0.0).get_parameter_value().float_value
self.GROUND_HEIGHT = self.get_parameter_or('ground_height', 0.0).get_parameter_value().float_value

# used to access TF tree
buffer = Buffer()
Expand All @@ -46,8 +46,8 @@ class Projector(rclpy.create_node):

# rate at which to re-compute depth map
#rate = rospy.Rate(1.0/rospy.get_param('~update_period', 1))
#rate = rclpy.Rate(1.0/rclpy.get_parameter_or('~update_period', 1).get_parameter_value().integer_value)
self._loop_rate = self.create_rate(1.0/self.get_parameter_or('~update_period', 1).get_parameter_value().integer_value, self.get_clock())
#rate = rclpy.Rate(1.0/rclpy.get_parameter_or('update_period', 1).get_parameter_value().integer_value)
self._loop_rate = self.create_rate(1.0/self.get_parameter_or('update_period', 1).get_parameter_value().integer_value, self.get_clock())

# compute depth map in main loop and cache since it is slow changing
# publish latest in callback so that the stamps match (for rviz)
Expand Down
12 changes: 6 additions & 6 deletions src/rktl_planner/rktl_planner/nodes/path_follower.py
Original file line number Diff line number Diff line change
Expand Up @@ -34,22 +34,22 @@ def __init__(self):
self.start_time = None
self.max_speed = None

self.frame_id = node.get_parameter_or('~frame_id', rclpy.Parameter('map')).get_parameter_value().string_value
self.frame_id = node.get_parameter_or('frame_id', rclpy.Parameter('map')).get_parameter_value().string_value

# Max speed to travel path
self.max_speed = node.get_parameter_or('~max_speed', rclpy.Parameter(0.1)).get_parameter_value().double_value
self.max_speed = node.get_parameter_or('max_speed', rclpy.Parameter(0.1)).get_parameter_value().double_value

# Radius to search for intersections
self.lookahead_dist = node.get_parameter_or('~lookahead_dist', rclpy.Parameter(0.15)).get_parameter_value().double_value
self.lookahead_dist = node.get_parameter_or('lookahead_dist', rclpy.Parameter(0.15)).get_parameter_value().double_value

# Coeffient to adjust lookahead distance by speed
self.lookahead_gain = node.get_parameter_or('~lookahead_gain', rclpy.Parameter(.055)).get_parameter_value().double_value
self.lookahead_gain = node.get_parameter_or('lookahead_gain', rclpy.Parameter(.055)).get_parameter_value().double_value

# Number of waypoints to search per pass (-1 is full path)
self.lookahead_pnts = node.get_parameter_or('~lookahead_pnts', rclpy.Parameter(-1)).get_parameter_value().integer_value
self.lookahead_pnts = node.get_parameter_or('lookahead_pnts', rclpy.Parameter(-1)).get_parameter_value().integer_value

# Publishers
car_name = node.get_parameter('~car_name').get_parameter_value().string_value
car_name = node.get_parameter('car_name').get_parameter_value().string_value

# Subscribers
node.create_subscription(Odometry, f'/cars/{car_name}/odom', self.odom_cb, qos_profile=1)
Expand Down
32 changes: 16 additions & 16 deletions src/rktl_planner/rktl_planner/nodes/patrol_planner.py
Original file line number Diff line number Diff line change
Expand Up @@ -30,31 +30,31 @@ def __init__(self):

# constants
# general stability parameters
self.SPEED = node.get_parameter_or('~speed', rclpy.Parameter(1.0)).get_parameter_value().double_value
self.CURVATURE_P_GAIN = node.get_parameter_or('~curvature_gain/kp', rclpy.Parameter(1.0)).get_parameter_value().double_value
self.CURVATURE_D_GAIN = node.get_parameter_or('~curvature_gain/kd', rclpy.Parameter(0.0)).get_parameter_value().double_value
self.CURVATURE_GAIN_FALLOFF_SQ = pow(node.get_parameter_or('~curvature_gain/falloff', rclpy.Parameter(1e-9)).get_parameter_value().double_value, 2)
self.PATROL_DIST = node.get_parameter_or('~patrol_wall_dist', rclpy.Parameter(0.5)).get_parameter_value().double_value
self.SPEED = node.get_parameter_or('speed', rclpy.Parameter(1.0)).get_parameter_value().double_value
self.CURVATURE_P_GAIN = node.get_parameter_or('curvature_gain/kp', rclpy.Parameter(1.0)).get_parameter_value().double_value
self.CURVATURE_D_GAIN = node.get_parameter_or('curvature_gain/kd', rclpy.Parameter(0.0)).get_parameter_value().double_value
self.CURVATURE_GAIN_FALLOFF_SQ = pow(node.get_parameter_or('curvature_gain/falloff', rclpy.Parameter(1e-9)).get_parameter_value().double_value, 2)
self.PATROL_DIST = node.get_parameter_or('patrol_wall_dist', rclpy.Parameter(0.5)).get_parameter_value().double_value

# wall avoidance parameters
self.WALL_REVERSE_TIME = node.get_parameter_or('~wall_avoidance/reverse_time', rclpy.Parameter(0.25)).get_parameter_value().double_value
self.WALL_DIST_MARGIN = node.get_parameter_or('~wall_avoidance/distance_margin', rclpy.Parameter(0.5)).get_parameter_value().double_value
self.WALL_HEADING_MARGIN = node.get_parameter_or('~wall_avoidance/heading_margin', rclpy.Parameter(math.pi/4)).get_parameter_value().double_value
self.WALL_REVERSE_TIME = node.get_parameter_or('wall_avoidance/reverse_time', rclpy.Parameter(0.25)).get_parameter_value().double_value
self.WALL_DIST_MARGIN = node.get_parameter_or('wall_avoidance/distance_margin', rclpy.Parameter(0.5)).get_parameter_value().double_value
self.WALL_HEADING_MARGIN = node.get_parameter_or('wall_avoidance/heading_margin', rclpy.Parameter(math.pi/4)).get_parameter_value().double_value

# offense & defense parameters
self.ATTEMPT_TIMEOUT = node.get_parameter_or('~attempt_timeout', rclpy.Parameter(5.0)).get_parameter_value().double_value
self.DEFENSIVE_LINE = node.get_parameter_or('~defensive_line', rclpy.Parameter(0.0)).get_parameter_value().double_value
self.REVERSE_LINE = node.get_parameter_or('~reverse_line', rclpy.Parameter(0.0)).get_parameter_value().double_value
self.ATTEMPT_TIMEOUT = node.get_parameter_or('attempt_timeout', rclpy.Parameter(5.0)).get_parameter_value().double_value
self.DEFENSIVE_LINE = node.get_parameter_or('defensive_line', rclpy.Parameter(0.0)).get_parameter_value().double_value
self.REVERSE_LINE = node.get_parameter_or('reverse_line', rclpy.Parameter(0.0)).get_parameter_value().double_value

# offensive related parameters

self.SCORING_MARGIN = node.get_parameter_or('~scoring/heading_margin', rclpy.Parameter(math.pi/8.0)).get_parameter_value().double_value
self.LOOKAHEAD_DIST = node.get_parameter_or('~scoring/car_lookahead_dist', rclpy.Parameter(1.0)).get_parameter_value().double_value
self.LOOKAHEAD_TIME = node.get_parameter_or('~scoring/ball_lookahead_time', rclpy.Parameter(0.0)).get_parameter_value().double_value
self.GOAL_DEPTH_TARGET = node.get_parameter_or('~scoring/goal_depth_target', rclpy.Parameter(0.0)).get_parameter_value().double_value
self.SCORING_MARGIN = node.get_parameter_or('scoring/heading_margin', rclpy.Parameter(math.pi/8.0)).get_parameter_value().double_value
self.LOOKAHEAD_DIST = node.get_parameter_or('scoring/car_lookahead_dist', rclpy.Parameter(1.0)).get_parameter_value().double_value
self.LOOKAHEAD_TIME = node.get_parameter_or('scoring/ball_lookahead_time', rclpy.Parameter(0.0)).get_parameter_value().double_value
self.GOAL_DEPTH_TARGET = node.get_parameter_or('scoring/goal_depth_target', rclpy.Parameter(0.0)).get_parameter_value().double_value

# defense related parameters
self.DEFENSE_TIME_GAIN = node.get_parameter_or('~defense/reverse_time_gain', rclpy.Parameter(0.5)).get_parameter_value().double_value
self.DEFENSE_TIME_GAIN = node.get_parameter_or('defense/reverse_time_gain', rclpy.Parameter(0.5)).get_parameter_value().double_value

# variables
self.ball_position = None
Expand Down

0 comments on commit 03ca0b5

Please sign in to comment.