Merge branch 'ros-2-complete' of https://github.com/purdue-arc/rocket…

…_league into ros-2-complete

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

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')

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

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

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):

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)

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

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

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

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

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]