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() 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."""