From 2c89ff71514d40760cd037e9d2e237e59e7b9cf3 Mon Sep 17 00:00:00 2001 From: jcrm1 <52137472+jcrm1@users.noreply.github.com> Date: Sun, 14 Apr 2024 18:35:51 +0000 Subject: [PATCH 1/4] add node to spin and add qos_profile to create sub/pub --- src/rktl_control/rktl_control/websocket_node.py | 2 +- src/rktl_control/rktl_control/xbox_interface | 2 +- src/rktl_control/test/test_mean_filter_node | 2 +- src/rktl_control/test/test_sync_node | 6 +++--- src/rktl_perception/nodes/pose_to_tf | 2 +- src/rktl_perception/nodes/projector | 2 +- src/rktl_planner/rktl_planner/nodes/bezier_path_server.py | 2 +- 7 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/rktl_control/rktl_control/websocket_node.py b/src/rktl_control/rktl_control/websocket_node.py index cb9d4dff..cb66193e 100644 --- a/src/rktl_control/rktl_control/websocket_node.py +++ b/src/rktl_control/rktl_control/websocket_node.py @@ -45,7 +45,7 @@ def receive_callback(thr, str, data): print("Starting") rclpy.init(args=sys.args) node = rclpy.create_node('car_websocket') - node.create_subscription(ControlEffort, f'/cars/car{car_num}/effort', receive_callback) + node.create_subscription(ControlEffort, f'/cars/car{car_num}/effort', receive_callback, 1) print("Running") loop = asyncio.get_event_loop() task = asyncio.run(main()) diff --git a/src/rktl_control/rktl_control/xbox_interface b/src/rktl_control/rktl_control/xbox_interface index 85190009..fc30d607 100644 --- a/src/rktl_control/rktl_control/xbox_interface +++ b/src/rktl_control/rktl_control/xbox_interface @@ -36,7 +36,7 @@ class XboxInterface(): 'effort', queue_size=1) # Subscribers - self.node.create_subscription(Joy, 'joy', self.callback) + self.node.create_subscription(Joy, 'joy', self.callback, 1) # Services self.reset_srv = self.node.create_client(Empty, '/sim_reset') diff --git a/src/rktl_control/test/test_mean_filter_node b/src/rktl_control/test/test_mean_filter_node index b8e03827..0da400a8 100755 --- a/src/rktl_control/test/test_mean_filter_node +++ b/src/rktl_control/test/test_mean_filter_node @@ -53,7 +53,7 @@ class TestMeanFilter(unittest.TestCase): self.node = rclpy.create_node('test_mean_filter_node') pub = self.node.create_publisher(PoseWithCovarianceStamped, 'pose_sync') - self.node.create_subscription(Odometry, 'odom', self.odom_cb) + self.node.create_subscription(Odometry, 'odom', self.odom_cb, 1) # member variables used for test self.odom = None diff --git a/src/rktl_control/test/test_sync_node b/src/rktl_control/test/test_sync_node index 9f0d1a61..46805405 100755 --- a/src/rktl_control/test/test_sync_node +++ b/src/rktl_control/test/test_sync_node @@ -51,9 +51,9 @@ class TestSync(unittest.TestCase): car1_pub = self.node.create_publisher(PoseWithCovarianceStamped, 'cars/car1/pose') ball_pub = self.node.create_publisher(PoseWithCovarianceStamped, 'ball/pose') - self.node.create_subscription(PoseWithCovarianceStamped, 'cars/car0/pose_sync', self.car0_cb) - self.node.create_subscription(PoseWithCovarianceStamped, 'cars/car1/pose_sync', self.car1_cb) - self.node.create_subscription(PoseWithCovarianceStamped, 'ball/pose_sync', self.ball_cb) + self.node.create_subscription(PoseWithCovarianceStamped, 'cars/car0/pose_sync', self.car0_cb, 1) + self.node.create_subscription(PoseWithCovarianceStamped, 'cars/car1/pose_sync', self.car1_cb, 1) + self.node.create_subscription(PoseWithCovarianceStamped, 'ball/pose_sync', self.ball_cb, 1) # member variables used for test self.last_car0 = None diff --git a/src/rktl_perception/nodes/pose_to_tf b/src/rktl_perception/nodes/pose_to_tf index 2bc9c89b..ebf9f59e 100755 --- a/src/rktl_perception/nodes/pose_to_tf +++ b/src/rktl_perception/nodes/pose_to_tf @@ -24,7 +24,7 @@ class Publisher(rclpy.create_node): self.FRAME = self.get_parameter_or('cam_frame_id', 'cam0').get_parameter_value().string_value #rospy.Subscriber("pose", PoseWithCovarianceStamped, self.pose_cb) - self.subscription = self.create_subscription(PoseWithCovarianceStamped, "pose", self.pose_cb) + self.subscription = self.create_subscription(PoseWithCovarianceStamped, "pose", self.pose_cb, 1) #rospy.spin() -> Moved into main function def pose_cb(self, pose_msg): diff --git a/src/rktl_perception/nodes/projector b/src/rktl_perception/nodes/projector index 8f21e536..7df3b070 100755 --- a/src/rktl_perception/nodes/projector +++ b/src/rktl_perception/nodes/projector @@ -24,7 +24,7 @@ class Projector(rclpy.create_node): #self.depth_pub = rospy.Publisher('depth_rect', Image, queue_size=1) self.depth_pub = self.create_publisher(Image, 'depth_rect', queue_size=1) #rospy.Subscriber('camera_info', CameraInfo, self.camera_info_cb) - self.subscription = self.create_subscription(CameraInfo, 'camera_info',self.camera_info_cb) + self.subscription = self.create_subscription(CameraInfo, 'camera_info', self.camera_info_cb, 1) # constants #self.GROUND_HEIGHT = rospy.get_param('~ground_height', 0.0) diff --git a/src/rktl_planner/rktl_planner/nodes/bezier_path_server.py b/src/rktl_planner/rktl_planner/nodes/bezier_path_server.py index 5a06f3e2..7099711d 100755 --- a/src/rktl_planner/rktl_planner/nodes/bezier_path_server.py +++ b/src/rktl_planner/rktl_planner/nodes/bezier_path_server.py @@ -100,7 +100,7 @@ def main(): rclpy.init() node = rclpy.create_node('bezier_path_server') service = node.create_service(CreateBezierPath, 'create_bezier_path', bezier_path_server) - rclpy.spin() + rclpy.spin(node) if __name__ == '__main__': main() From 95e59e7b7f9ec0ca9dd76571f8b2ce841ed13e9a Mon Sep 17 00:00:00 2001 From: jcrm1 <52137472+jcrm1@users.noreply.github.com> Date: Sun, 14 Apr 2024 18:51:25 +0000 Subject: [PATCH 2/4] rktl_planner fix parameters (maybe) (probably) --- .../rktl_planner/nodes/path_follower.py | 19 ++++-- .../rktl_planner/nodes/path_planner.py | 23 +++---- .../rktl_planner/nodes/patrol_planner.py | 64 ++++++++++++------- 3 files changed, 68 insertions(+), 38 deletions(-) diff --git a/src/rktl_planner/rktl_planner/nodes/path_follower.py b/src/rktl_planner/rktl_planner/nodes/path_follower.py index fa70524d..56528887 100755 --- a/src/rktl_planner/rktl_planner/nodes/path_follower.py +++ b/src/rktl_planner/rktl_planner/nodes/path_follower.py @@ -34,19 +34,28 @@ def __init__(self): self.start_time = None self.max_speed = None - self.frame_id = node.get_parameter_or('frame_id', rclpy.Parameter('map')).get_parameter_value().string_value + node.declare_parameters(namespace='', parameters=[ + ('frame_id', "map"), + ('max_speed', 0.1), + ('lookahead_dist', 0.15), + ('lookahead_gain', 0.055), + ('lookahead_pnts', -1), + ('car_name', "car0"), + ]) + + self.frame_id = node.get_parameter('frame_id').get_parameter_value().string_value # Max speed to travel path - self.max_speed = node.get_parameter_or('max_speed', rclpy.Parameter(0.1)).get_parameter_value().double_value + self.max_speed = node.get_parameter('max_speed').get_parameter_value().double_value # Radius to search for intersections - self.lookahead_dist = node.get_parameter_or('lookahead_dist', rclpy.Parameter(0.15)).get_parameter_value().double_value + self.lookahead_dist = node.get_parameter('lookahead_dist').get_parameter_value().double_value # Coeffient to adjust lookahead distance by speed - self.lookahead_gain = node.get_parameter_or('lookahead_gain', rclpy.Parameter(.055)).get_parameter_value().double_value + self.lookahead_gain = node.get_parameter('lookahead_gain').get_parameter_value().double_value # Number of waypoints to search per pass (-1 is full path) - self.lookahead_pnts = node.get_parameter_or('lookahead_pnts', rclpy.Parameter(-1)).get_parameter_value().integer_value + self.lookahead_pnts = node.get_parameter('lookahead_pnts').get_parameter_value().integer_value # Publishers car_name = node.get_parameter('car_name').get_parameter_value().string_value diff --git a/src/rktl_planner/rktl_planner/nodes/path_planner.py b/src/rktl_planner/rktl_planner/nodes/path_planner.py index d902c407..34b90dc8 100755 --- a/src/rktl_planner/rktl_planner/nodes/path_planner.py +++ b/src/rktl_planner/rktl_planner/nodes/path_planner.py @@ -102,34 +102,35 @@ def __init__(self): rclpy.init() global node node = rclpy.create_node('path_planner') - - planner_type = node.declare_parameter('~planner_type', 'simple') + node.declare_parameter('planner_type', 'simple') + planner_type = node.get_parameter('planner_type').get_parameter_value().string_value if planner_type == 'simple': self.path_req = create_simple_path_req elif planner_type == 'complex': self.path_req = create_complex_path_req else: - raise NotImplementedError(f'unrecognized planner type: {node.declare_parameter("~planner_type")}') + raise NotImplementedError(f'unrecognized planner type: {planner_type}') # Subscribers - car_name = node.declare_parameter('~car_name') - node.create_subscription(Odometry, f'/cars/{car_name}/odom', self.car_odom_cb, rclpy.qos.QoSProfile()) - node.create_subscription(Odometry, '/ball/odom', self.ball_odom_cb) + node.declare_parameter('car_name') + car_name = node.get_parameter('car_name').get_parameter_value().string_value + node.create_subscription(Odometry, f'/cars/{car_name}/odom', self.car_odom_cb, 1) + node.create_subscription(Odometry, '/ball/odom', self.ball_odom_cb, 1) # Services self.reset_server = node.create_service(Empty, 'reset_planner', self.reset) self.path_client = node.create_client(CreateBezierPath, 'create_bezier_path') # Publishers - self.linear_path_pub = node.create_publisher(Path, 'linear_path', 1, latch=True) - self.bezier_path_pub = node.create_publisher(BezierPathList, 'bezier_path', 1, latch=True) + self.linear_path_pub = node.create_publisher(Path, 'linear_path', 1) + self.bezier_path_pub = node.create_publisher(BezierPathList, 'bezier_path', 1) self.car_odom = Odometry() self.ball_odom = Odometry() + node.declare_parameter('/field/length', 1.0) + self.goal_pos = (node.get_parameter('/field/length').get_parameter_value().double_value / 2.0, 0.) - self.goal_pos = (node.declare_paramete('/field/length', 1) / 2, 0.) - - rclpy.spin() + rclpy.spin(node) def car_odom_cb(self, data: Odometry): self.car_odom = data diff --git a/src/rktl_planner/rktl_planner/nodes/patrol_planner.py b/src/rktl_planner/rktl_planner/nodes/patrol_planner.py index 5dac4a8c..8386fa77 100755 --- a/src/rktl_planner/rktl_planner/nodes/patrol_planner.py +++ b/src/rktl_planner/rktl_planner/nodes/patrol_planner.py @@ -22,39 +22,59 @@ def __init__(self): rclpy.init() global node node = rclpy.create_node('path_follower') + node.declare_parameter('/field/width') + node.declare_parameter('/field/height') + node.declare_parameter('/cars/steering/max_throw') + node.declare_parameter('/cars/length', 0.12) + node.declare_parameter('speed') + node.declare_parameter('curvature_gain/kp') + node.declare_parameter('curvature_gain/kd') + node.declare_parameter('curvature_gain/falloff') + node.declare_parameter('patrol_wall_dist') + node.declare_parameter('wall_avoidance/reverse_time') + node.declare_parameter('wall_avoidance/distance_margin') + node.declare_parameter('wall_avoidance/heading_margin') + node.declare_parameter('attempt_timeout') + node.declare_parameter('defensive_line') + node.declare_parameter('reverse_line') + node.declare_parameter('scoring/heading_margin') + node.declare_parameter('scoring/car_lookahead_dist') + node.declare_parameter('scoring/ball_lookahead_time') + node.declare_parameter('scoring/goal_depth_target') + node.declare_parameter('defense/reverse_time_gain') # physical constants (global) - 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 + self.FIELD_WIDTH = node.get_parameter('/field/width').get_parameter_value().double_value + self.FIELD_HEIGHT = node.get_parameter('/field/height').get_parameter_value().double_value + self.MAX_CURVATURE = math.tan(node.get_parameter('/cars/steering/max_throw').get_parameter_value().double_value) / node.get_parameter('/cars/length').get_parameter_value().double_value # constants # general stability parameters - 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 + self.SPEED = node.get_parameter('speed').get_parameter_value().double_value + self.CURVATURE_P_GAIN = node.get_parameter('curvature_gain/kp').get_parameter_value().double_value + self.CURVATURE_D_GAIN = node.get_parameter('curvature_gain/kd').get_parameter_value().double_value + self.CURVATURE_GAIN_FALLOFF_SQ = pow(node.get_parameter('curvature_gain/falloff').get_parameter_value().double_value, 2) + self.PATROL_DIST = node.get_parameter('patrol_wall_dist').get_parameter_value().double_value # wall avoidance parameters - 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 + self.WALL_REVERSE_TIME = node.get_parameter('wall_avoidance/reverse_time').get_parameter_value().double_value + self.WALL_DIST_MARGIN = node.get_parameter('wall_avoidance/distance_margin').get_parameter_value().double_value + self.WALL_HEADING_MARGIN = node.get_parameter('wall_avoidance/heading_margin').get_parameter_value().double_value # offense & defense parameters - 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 + self.ATTEMPT_TIMEOUT = node.get_parameter('attempt_timeout').get_parameter_value().double_value + self.DEFENSIVE_LINE = node.get_parameter('defensive_line').get_parameter_value().double_value + self.REVERSE_LINE = node.get_parameter('reverse_line').get_parameter_value().double_value # offensive related parameters - 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 + self.SCORING_MARGIN = node.get_parameter('scoring/heading_margin').get_parameter_value().double_value + self.LOOKAHEAD_DIST = node.get_parameter('scoring/car_lookahead_dist').get_parameter_value().double_value + self.LOOKAHEAD_TIME = node.get_parameter('scoring/ball_lookahead_time').get_parameter_value().double_value + self.GOAL_DEPTH_TARGET = node.get_parameter('scoring/goal_depth_target').get_parameter_value().double_value # defense related parameters - self.DEFENSE_TIME_GAIN = node.get_parameter_or('defense/reverse_time_gain', rclpy.Parameter(0.5)).get_parameter_value().double_value + self.DEFENSE_TIME_GAIN = node.get_parameter('defense/reverse_time_gain').get_parameter_value().double_value # variables self.ball_position = None @@ -66,10 +86,10 @@ def __init__(self): self.cmd_pub = node.create_publisher(ControlCommand, 'command', 1) # Subscribers - node.create_subscription(Odometry, 'odom', self.car_odom_cb) - node.create_subscription(Odometry, '/ball/odom', self.ball_odom_cb) + node.create_subscription(Odometry, 'odom', self.car_odom_cb, 1) + node.create_subscription(Odometry, '/ball/odom', self.ball_odom_cb, 1) - rclpy.spin() + rclpy.spin(node) def ball_odom_cb(self, odom_msg): """Callback for ball odometry.""" From ed3bddb12e80bb362e95c277e33509dd55c71d71 Mon Sep 17 00:00:00 2001 From: sixfootsix50 Date: Sun, 14 Apr 2024 19:25:46 +0000 Subject: [PATCH 3/4] Partially replaced the get_sim_param method --- src/rktl_sim/rktl_sim/sim_simulator.py | 46 ++++++++++++++++++++++---- 1 file changed, 39 insertions(+), 7 deletions(-) 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() From 76c51f695463238cb3eb21bb7c93481c8a36d2ff Mon Sep 17 00:00:00 2001 From: sixfootsix50 Date: Sat, 20 Apr 2024 19:40:28 +0000 Subject: [PATCH 4/4] Finished get_sim_param conversion --- src/rktl_sim/config/simulation.yaml | 6 +- src/rktl_sim/rktl_sim/sim_simulator.py | 190 ++++++++++++++----------- 2 files changed, 107 insertions(+), 89 deletions(-) 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: