diff --git a/src/rktl_sim/rktl_sim/sim_simulator.py b/src/rktl_sim/rktl_sim/sim_simulator.py index 330e92e3..e1491afa 100755 --- 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,8 +177,8 @@ 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: @@ -163,12 +186,20 @@ def reset_cb(self, _): 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()