Partially replaced the get_sim_param method

purdue-arc · Apr 14, 2024 · ed3bddb · ed3bddb
1 parent 03ca0b5
commit ed3bddb
Showing 1 changed file with 39 additions and 7 deletions.
diff --git a/src/rktl_sim/rktl_sim/sim_simulator.py b/src/rktl_sim/rktl_sim/sim_simulator.py
@@ -64,9 +64,32 @@ def __init__(self):
         rclpy.init(args=sys.argv)
         #global node
         self.node = rclpy.create_node("simulator")
-
 
-        mode = self.get_sim_param('~mode')
+        self.node.declare_parameter('mode', 'ideal')
+        self.node.declare_parameter('sensor_noise', None)
+        self.node.declare_parameter('/cars/length', 0.12)
+        self.node.declare_parameter('/cars/throttle/max_speed', 2.3)
+        self.node.declare_parameter('/cars/steering/max_throw', 0.1826)
+        self.node.declare_parameter('/cars/throttle/tau', 0.2)
+        self.node.declare_parameter('/cars/steering/rate', 0.9128)
+        #find default value (might be None)
+        self.node.declare_parameter('ball/init_pose', )
+
+        self.sensor_noise = {
+            'car' : {
+                'pos' : self.node.get_parameter('sensor_noise.car.pos').get_parameter_value().double_array_value,
+                'orient': self.node.get_parameter('sensor_noise.car.orient').get_parameter_value().double_array_value,
+                'dropout' : self.node.get_parameter('sensor_noise.car.dropout').get_parameter_value().double_value
+            },
+            'ball' : {
+                'pos' : self.node.get_parameter('sensor_noise.ball.pos').get_parameter_value().double_array_value,
+                'orient' : self.node.get_parameter('sensor_noise.ball.orient').get_parameter_value().double_array_value,
+                'dropout' : self.node.get_parameter('sensor_noise.ball.dropout').get_parameter_value().double_value
+            }
+        }
+
+        #mode = self.get_sim_param('~mode')
+        mode = self.node.get_parameter('mode').get_parameter_value().string_value
         if mode == 'ideal':
             self.mode = SimulatorMode.IDEAL
         elif mode == 'realistic':
@@ -154,21 +177,29 @@ def reset_cb(self, _):
         # setting sim parameters (can be modified by the user)
         self.spawn_bounds, self.field_setup = self.configure_field()
 
-        self.sensor_noise = self.get_sim_param(
-            '~sensor_noise', secondParam=None)
+        #self.sensor_noise = self.get_sim_param('~sensor_noise', secondParam=None)
+        #self.sensor_noise = self.node.get_parameter('sensor_noise').get_parameter_value().double_array_value
 
         self.car_noise = None
         if self.sensor_noise is not None and self.mode == SimulatorMode.REALISTIC:
             self.car_noise = self.sensor_noise.get('car', None)
 
         self.reset_ball_cb(None)
 
-        self.car_properties = {'length': self.get_sim_param('/cars/length'),
+        '''self.car_properties = {'length': self.get_sim_param('/cars/length'),
                                'max_speed': self.get_sim_param("/cars/throttle/max_speed"),
                                'steering_throw': self.get_sim_param("/cars/steering/max_throw"),
                                'throttle_tau': self.get_sim_param("/cars/throttle/tau"),
                                'steering_rate': self.get_sim_param("/cars/steering/rate"),
-                               'simulate_effort': (self.mode == SimulatorMode.REALISTIC)}
+                               'simulate_effort': (self.mode == SimulatorMode.REALISTIC)}'''
+        self.car_properties = {
+            'length' : self.node.get_parameter('/cars/length').get_parameter_value().double_value,
+            'max_speed' : self.node.get_parameter('/cars/throttle/max_speed').get_parameter_value().double_value,
+            'steering_throw' : self.node.get_parameter('/cars/steering/max_throw').get_parameter_value().double_value,
+            'throttle_tau' : self.node.get_parameter('/cars/throttle/tau').get_parameter_value().double_value,
+            'steering_rate' : self.node.get_parameter('/cars/steering/rate').get_parameter_value().double_value,
+            'simulate_effort' : (self.mode == SimulatorMode.REALISTIC)
+        }
         self.sim.reset(self.spawn_bounds, self.car_properties,
                        self.ball_init_pose, self.ball_init_speed)
 
@@ -182,7 +213,8 @@ def reset_ball_cb(self, _):
         if self.sensor_noise is not None and self.mode == SimulatorMode.REALISTIC:
             self.ball_noise = self.sensor_noise.get('ball', None)
 
-        self.ball_init_pose = self.get_sim_param('~ball/init_pose')
+        #self.ball_init_pose = self.get_sim_param('~ball/init_pose')
+        self.ball_init_pose = self.node.get_parameter('ball/init_pose').get_parameter_value(). #find value needed (might have to make new variable)
         self.ball_init_speed = self.get_sim_param('/ball/init_speed')
 
         self.sim.reset_ball()