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

…_league into ros-2-complete

src/rktl_autonomy/launch/rocket_league_train.launch.py

@@ -7,6 +7,8 @@
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import PathJoinSubstitution
 from launch_ros.substitutions import FindPackageShare
+from ament_index_python.packages import get_package_share_directory
+import os
 
 def generate_launch_description():
     plot_log_launch_arg = DeclareLaunchArgument(
@@ -65,7 +67,8 @@ def generate_launch_description():
         namespace=LaunchConfiguration('agent_name')
         )
 
-    sim_path = PathJoinSubstitution([FindPackageShare('rktl_launch'), 'launch', 'rocket_league_sim.launch.py'])
+    # sim_path = PathJoinSubstitution([FindPackageShare('rktl_launch'), 'launch', 'rocket_league_sim.launch.py'])
+    sim_path = os.path.join(get_package_share_directory('rktl_launch'), 'launch', 'rocket_league_sim.launch.py')
     sim_launch = IncludeLaunchDescription(
                 PythonLaunchDescriptionSource([sim_path]),  # TODO: Replace with the path to the launch file
                 launch_arguments={

src/rktl_control/launch/ball.launch.py

@@ -13,7 +13,7 @@ def generate_launch_description():
             output='screen',
             parameters=[
                 {
-                    PathJoinSubstitution([FindPackageShare('rktl_control'), '/config/mean_odom_filter.yaml'])
+                    PathJoinSubstitution([FindPackageShare('rktl_control'), 'config', 'mean_odom_filter.yaml'])
                 }
             ]
         )

src/rktl_control/launch/hardware_interface.launch.py

@@ -11,7 +11,7 @@ def generate_launch_description():
             name='hardware_interface',
             parameters=[
                 {
-                    launch.substitutions.PathJoinSubstitution([launch_ros.substitutions.FindPackageShare('rktl_control'), '/config/hardware_interface.yaml'])
+                    launch.substitutions.PathJoinSubstitution([launch_ros.substitutions.FindPackageShare('rktl_control'), 'config', 'hardware_interface.yaml'])
                 }
             ]
         )

src/rktl_control/launch/keyboard_control.launch.py

@@ -9,7 +9,7 @@ def generate_launch_description():
             default_value='car0'
         ),
         launch_ros.actions.Node(
-            namespace=launch.substitutions.PathJoinSubstitution(['cars/', launch_ros.substitutions.FindPackageShare('car_name')]),
+            namespace=launch.substitutions.PathJoinSubstitution(['cars', launch_ros.substitutions.FindPackageShare('car_name')]),
             package='rktl_control',
             executable='keyboard_interface',
             name='keyboard_interface',

src/rktl_control/launch/xbox_control.launch.py

@@ -1,6 +1,7 @@
 import launch
 import launch_ros.actions
-
+from launch.substitutions import PathJoinSubstitution
+from ament_index_python.packages import get_package_share_directory
 
 def generate_launch_description():
     ld = launch.LaunchDescription([
@@ -18,7 +19,7 @@ def generate_launch_description():
         ),
         launch.actions.GroupAction(
             actions=[
-                launch_ros.actions.PushRosNamespace("cars/" + launch.substitutions.LaunchConfiguration("car_name")),
+                launch_ros.actions.PushRosNamespace("cars" + get_package_share_directory('car_name')),
 
                 launch_ros.actions.Node(
                     package='joy',

src/rktl_planner/launch/patrol_agent.launch.py

@@ -13,7 +13,7 @@ def generate_launch_description():
         ),
         launch_ros.actions.Node(
             package='rktl_planner',
-            namespace=PathJoinSubstitution(["cars/", launch.substitutions.LaunchConfiguration("car_name")]),
+            namespace=PathJoinSubstitution(["cars", launch.substitutions.LaunchConfiguration("car_name")]),
             executable='patrol_planner',
             name='patrol_planner',
             output='screen',

src/rktl_planner/rktl_planner/nodes/path_follower.py

@@ -26,7 +26,7 @@ class PathFollower(object):
     def __init__(self):
         rclpy.init()
         global node
-        node = rclpy.create_node('path_follower')
+        node = rclpy.Node('path_follower')
 
         self.path_start_time = None
         self.path = None
@@ -53,10 +53,10 @@ def __init__(self):
         self.bot_velocity_cmd = node.create_publisher(ControlCommand, f'/cars/{car_name}/command', 1)
 
         # Subscribers
-        node.create_subscription(Odometry, f'/cars/{car_name}/odom', self.odom_cb, rclpy.qos.QoSProfile())
-        node.create_subscription(Path, 'linear_path', self.path_cb)
+        node.create_subscription(Odometry, f'/cars/{car_name}/odom', self.odom_cb, qos_profile=1)
+        node.create_subscription(Path, 'linear_path', self.path_cb, qos_profile=1)
 
-        rclpy.spin()
+        rclpy.spin(node)
 
     def path_cb(self, path_msg: Path):
         """Creates path using waypoints in Path message."""
@@ -78,15 +78,14 @@ def odom_cb(self, odom_msg: Odometry):
                 odom_msg.twist)
 
             # Set lookahead dist by lookahead gain and current speed
-            lookahead_boost = np.linalg.norm(bot_linear) \
-                * self.lookahead_gain
-            lookahead_dist = self.lookahead_dist + lookahead_boost
-
+            lookahead_boost = np.linalg.norm(bot_linear) * self.lookahead_gain.get_parameter_value().double_value
+            lookahead_dist = rclpy.Parameter('~lookahead_dist', rclpy.Parameter.Type.DOUBLE, self.lookahead_dist.get_parameter_value().double_value + lookahead_boost)
+
             # Set number of waypoints to check
-            if self.lookahead_pnts == -1:
+            if self.lookahead_pnts.get_parameter_value().integer_value == -1:
                 lookahead_pnts = len(self.path)
             else:
-                lookahead_pnts = self.lookahead_pnts
+                lookahead_pnts = self.lookahead_pnts.get_parameter_value().integer_value
 
             # Find next valid intersection along path
             intersect = None