diff --git a/src/rktl_sim/config/simulation.yaml b/src/rktl_sim/config/simulation.yaml index 0ddaa2a3..b15c3492 100644 --- a/src/rktl_sim/config/simulation.yaml +++ b/src/rktl_sim/config/simulation.yaml @@ -13,9 +13,9 @@ ball: init_speed: 0.0 - # init_pose: - # pos: [0.0, 0.5, 0.06] - # orient: [0.0, 0.0, 0.0] + init_pose: + pos: [0.0, 0.5, 0.06] + orient: [0.0, 0.0, 0.0] sensor_noise: pos: [0.01, 0.01, 0.0] orient: [0.0, 0.0, 0.0] diff --git a/src/rktl_sim/rktl_sim/sim_simulator.py b/src/rktl_sim/rktl_sim/sim_simulator.py index e1491afa..60ae1771 100755 --- a/src/rktl_sim/rktl_sim/sim_simulator.py +++ b/src/rktl_sim/rktl_sim/sim_simulator.py @@ -72,22 +72,18 @@ def __init__(self): 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 - } - } - + #Sensor Noise Parameters + self.node.declare_parameter('/sensor_noise/car/pos', [0.01, 0.01, 0.00]) + self.node.declare_parameter('/sensor_noise/car/orient', [0.0, 0.0, 0.09]) + self.node.declare_parameter('/sensor_noise/car/dropout', 0.15) + self.node.declare_parameter('/sensor_noise/ball/pos', [0.01, 0.01, 0.0]) + self.node.declare_parameter('/sensor_noise/ball/orient', [0.0, 0.0, 0.0]) + self.node.declare_parameter('/sensor_noise/ball/dropout', 0.1) + #Ball Initial Position Parameters + self.node.declare_parameter('ball/init_pose/pos', [0.0, 0.5, 0.06]) + self.node.declare_parameter('ball/init_pose/orient', [0.0, 0.0, 0.0]) + + #mode = self.get_sim_param('~mode') mode = self.node.get_parameter('mode').get_parameter_value().string_value if mode == 'ideal': @@ -178,6 +174,18 @@ def reset_cb(self, _): self.spawn_bounds, self.field_setup = self.configure_field() #self.sensor_noise = self.get_sim_param('~sensor_noise', secondParam=None) + 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 + } + } #self.sensor_noise = self.node.get_parameter('sensor_noise').get_parameter_value().double_array_value self.car_noise = None @@ -214,8 +222,12 @@ def reset_ball_cb(self, _): 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.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.ball_init_pose = { + 'pos' : self.node.get_parameter('ball/init_pose/pos').get_parameter_value().double_array_value, + 'orient': self.node.get_parameter('ball/init_pose/orient').get_parameter_value().double_array_value + } + #self.ball_init_speed = self.get_sim_param('/ball/init_speed') + self.ball_init_speed = self.node.get_parameter('/ball/init_speed').get_parameter_value().double_value self.sim.reset_ball() return Empty_Response() # might be Empty() @@ -327,89 +339,94 @@ def loop_once(self): # for param in rclpy_param: - def get_sim_param(self, path, returnValue=False, secondParam=None): - """ - @param secondParam: Specify if you want to pass in a second parameter to rclpy. - @param path: A direct path to the variable. - @param returnValue: - True: None is returned if variable does not exist. - False: An error is thrown if variable does not exist. - @return: None or Exception. - """ - #rospy_param = rospy.get_param(path, secondParam) - rclpy_param = self.node.declare_parameter(path, secondParam).value - if not rclpy_param: - if returnValue: - #rospy.logfatal(f'invalid file path: {path}') - - self.node.get_logger().fatal(f'invalid file path: {path}') - return None - else: - if '~' in path: - if secondParam is not None: - #return rospy.get_param(f'{path}', secondParam) - return self.node.declare_parameter(f'{path}', secondParam) - else: + # def get_sim_param(self, path, returnValue=False, secondParam=None): + # """ + # @param secondParam: Specify if you want to pass in a second parameter to rclpy. + # @param path: A direct path to the variable. + # @param returnValue: + # True: None is returned if variable does not exist. + # False: An error is thrown if variable does not exist. + # @return: None or Exception. + # """ + # #rospy_param = rospy.get_param(path, secondParam) + # rclpy_param = self.node.declare_parameter(path, secondParam).value + # if not rclpy_param: + # if returnValue: + # #rospy.logfatal(f'invalid file path: {path}') + + # self.node.get_logger().fatal(f'invalid file path: {path}') + # return None + # else: + # if '~' in path: + # if secondParam is not None: + # #return rospy.get_param(f'{path}', secondParam) + # return self.node.declare_parameter(f'{path}', secondParam) + # else: - #return rospy.get_param(f'{path}') - return self.node.declare_parameter(f'{path}') + # #return rospy.get_param(f'{path}') + # return self.node.declare_parameter(f'{path}') - type_rclpy = type(rclpy_param) + # type_rclpy = type(rclpy_param) - if type_rclpy == dict: - if secondParam is None: + # if type_rclpy == dict: + # if secondParam is None: - # min_param = (float)(rospy.get_param(f'{path}/min')) - # max_param = (float)(rospy.get_param(f'{path}/max')) + # # min_param = (float)(rospy.get_param(f'{path}/min')) + # # max_param = (float)(rospy.get_param(f'{path}/max')) - min_param = (float)(self.node.declare_parameter(f'{path}/min')) - max_param = (float)(self.node.declare_parameter(f'{path}/max')) - else: - # min_param = (float)(rospy.get_param( - # f'{path}/min', secondParam)) - # max_param = (float)(rospy.get_param( - # f'{path}/max', secondParam)) + # min_param = (float)(self.node.declare_parameter(f'{path}/min')) + # max_param = (float)(self.node.declare_parameter(f'{path}/max')) + # else: + # # min_param = (float)(rospy.get_param( + # # f'{path}/min', secondParam)) + # # max_param = (float)(rospy.get_param( + # # f'{path}/max', secondParam)) - min_param = (float)(self.node.declare_parameter(f'{path}/min', secondParam)) - max_param = (float)(self.node.declare_parameter(f'{path}/max', secondParam)) + # min_param = (float)(self.node.declare_parameter(f'{path}/min', secondParam)) + # max_param = (float)(self.node.declare_parameter(f'{path}/max', secondParam)) - if not max_param: - if returnValue: - #rospy.logfatal(f'invalid file path: {path}/min') - - self.node.get_logger().fatal(f'invalid file path: {path}/min') - return None - # accounting for bugs in yaml file - if min_param > max_param: - param_val =(float)(random.uniform(max_param, min_param)) - #rospy.set_param(f'{path}',param_val) - self.node.declare_parameter(f'{path}',param_val) + # if not max_param: + # if returnValue: + # #rospy.logfatal(f'invalid file path: {path}/min') + + # self.node.get_logger().fatal(f'invalid file path: {path}/min') + # return None + # # accounting for bugs in yaml file + # if min_param > max_param: + # param_val =(float)(random.uniform(max_param, min_param)) + # #rospy.set_param(f'{path}',param_val) + # self.node.declare_parameter(f'{path}',param_val) - return param_val + # return param_val - elif type_rclpy == float or type_rclpy == int: - if secondParam is not None: - #return rospy.get_param(path, secondParam) - return self.node.declare_parameter(path, secondParam) - else: - #return rospy.get_param(path) - return self.node.declare_parameter(path) - if returnValue: - #rospy.logfatal(f'invalid file path: {path}') + # elif type_rclpy == float or type_rclpy == int: + # if secondParam is not None: + # #return rospy.get_param(path, secondParam) + # return self.node.declare_parameter(path, secondParam) + # else: + # #return rospy.get_param(path) + # return self.node.declare_parameter(path) + # if returnValue: + # #rospy.logfatal(f'invalid file path: {path}') - self.node.get_logger().fatal(f'invalid file path: {path}') - return None + # self.node.get_logger().fatal(f'invalid file path: {path}') + # return None def configure_field(self): """Configures the field boundries and goals to be used in the simulator.""" - fw = self.get_sim_param('/field/width') - fl = self.get_sim_param('/field/length') - wt = self.get_sim_param('/field/wall_thickness') - gw = self.get_sim_param('/field/goal/width') - spawn_height = self.get_sim_param('~spawn_height', 0.06) + #fw = self.get_sim_param('/field/width') + fw = self.node.get_parameter('/field/width').get_parameter_value().double_value + #fl = self.get_sim_param('/field/length') + fl = self.node.get_parameter('/field/length').get_parameter_value().double_value + #wt = self.get_sim_param('/field/wall_thickness') + wt = self.node.get_parameter('/field/wall_thickness').get_parameter_value().double_value + #gw = self.get_sim_param('/field/goal/width') + gw = self.node.get_parameter('/field/goal/width').get_parameter_value().double_value + #spawn_height = self.get_sim_param('~spawn_height', 0.06) + spawn_height = self.node.get_parameter('spawn_height').get_parameter_value().double_value spawn_bounds = [[-(fl / 2) + (2 * wt), (fl / 2) - (2 * wt)], [-(fw / 2) + (2 * wt), (fw / 2) - (2 * wt)], @@ -432,7 +449,8 @@ def configure_field(self): def update_all_cars(self): """Generates instance-parameters, Subscribers, Publishers for each car.""" - car_configs = self.get_sim_param('~cars', secondParam=[]) + #car_configs = self.get_sim_param('~cars', secondParam=[]) + car_configs = self.node.get_parameter('cars').get_parameter_value().string_value #Find value type for car_config in car_configs: