Fix patrol_planner declare_parameter usage

purdue-arc · Apr 7, 2024 · 44917d7 · 44917d7
1 parent 4c7d2e5
commit 44917d7
Showing 1 changed file with 20 additions and 19 deletions.
diff --git a/src/rktl_planner/rktl_planner/nodes/patrol_planner.py b/src/rktl_planner/rktl_planner/nodes/patrol_planner.py
@@ -24,36 +24,37 @@ def __init__(self):
         node = rclpy.create_node('path_follower')
 
         # physical constants (global)
-        self.FIELD_WIDTH = node.declare_parameter('/field/width')
-        self.FIELD_HEIGHT = node.declare_parameter('/field/length')
-        self.MAX_CURVATURE = math.tan(node.declare_parameter('/cars/steering/max_throw')) / node.declare_parameter('/cars/length')
+        self.FIELD_WIDTH = node.get_parameter_or('/field/width', rclpy.Parameter(None)).get_parameter_value().double_value
+        self.FIELD_HEIGHT = node.get_parameter_or('/field/height', rclpy.Parameter(None)).get_parameter_value().double_value
+        self.MAX_CURVATURE = math.tan(node.get_parameter_or('/cars/steering/max_throw', rclpy.Parameter(None)).get_parameter_value().double_value) / node.get_parameter_or('/cars/length', rclpy.Parameter(None)).get_parameter_value().double_value
 
         # constants
         # general stability parameters
-        self.SPEED = node.declare_parameter('~speed', 1.0)
-        self.CURVATURE_P_GAIN = node.declare_parameter('~curvature_gain/kp', 1.0)
-        self.CURVATURE_D_GAIN = node.declare_parameter('~curvature_gain/kd', 0.0)
-        self.CURVATURE_GAIN_FALLOFF_SQ = pow(node.declare_parameter('~curvature_gain/falloff', 1e-9), 2)
-        self.PATROL_DIST = node.declare_parameter('~patrol_wall_dist', 0.5)
+        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.declare_parameter('~wall_avoidance/reverse_time', 0.25)
-        self.WALL_DIST_MARGIN = node.declare_parameter('~wall_avoidance/distance_margin', 0.5)
-        self.WALL_HEADING_MARGIN = node.declare_parameter('~wall_avoidance/heading_margin', math.pi/4)
+        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.declare_parameter('~attempt_timeout', 5.0)
-        self.DEFENSIVE_LINE = node.declare_parameter('~defensive_line', 0.0)
-        self.REVERSE_LINE = node.declare_parameter('~reverse_line', 0.0)
+        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.declare_parameter('~scoring/heading_margin', math.pi/8.0)
-        self.LOOKAHEAD_DIST = node.declare_parameter('~scoring/car_lookahead_dist', 1.0)
-        self.LOOKAHEAD_TIME = node.declare_parameter('~scoring/ball_lookahead_time', 0.0)
-        self.GOAL_DEPTH_TARGET = node.declare_parameter('~scoring/goal_depth_target', 0.0)
+
+        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.declare_parameter('~defense/reverse_time_gain', 0.5)
+        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