Skip to content

Commit

Permalink
Finished get_sim_param conversion
Browse files Browse the repository at this point in the history
  • Loading branch information
@sixfootsix50
sixfootsix50 committed Apr 20, 2024
1 parent 35e4e36 commit 76c51f6
Show file tree
Hide file tree
Showing 2 changed files with 107 additions and 89 deletions.
6 changes: 3 additions & 3 deletions src/rktl_sim/config/simulation.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand Down
190 changes: 104 additions & 86 deletions src/rktl_sim/rktl_sim/sim_simulator.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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':
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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()
Expand Down Expand Up @@ -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)],
Expand All @@ -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:
Expand Down

0 comments on commit 76c51f6

Please sign in to comment.