Autonomous Navigation State Machine

urc_bringup/launch/autonomy.launch.py

@@ -0,0 +1,60 @@
+import os
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+
+
+def generate_launch_description():
+    pkg_urc_perception = get_package_share_directory("urc_perception")
+    pkg_trajectory_following = get_package_share_directory("trajectory_following")
+
+    traversability_config = os.path.join(pkg_urc_perception, "config", "traversability_params.yaml")
+    trajectory_config = os.path.join(pkg_trajectory_following, "config", "pure_pursuit.yaml")
+
+    state_machine_node = Node(
+        package="nav_testing",
+        executable="nav_testing_NavCoordinator",
+        name="nav_coordinator",
+        output="screen",
+    )
+
+    path_planning_node = Node(
+        package="path_planning",
+        executable="path_planning_PlannerServer",
+        name="planner_server",
+        output="screen",
+    )
+
+    trajectory_following_node = Node(
+        package="trajectory_following",
+        executable="trajectory_following_FollowerActionServer",
+        name="follower_action_server",
+        parameters=[trajectory_config],
+        output="screen",
+    )
+
+    traversability_mapping_node = Node(
+        package="urc_perception",
+        executable="urc_perception_TraversabilityMapping",
+        name="traversability_mapping",
+        parameters=[traversability_config],
+        output="screen",
+    )
+
+    grid_map_visualization_node = Node(
+        package="grid_map_visualization",
+        executable="grid_map_visualization",
+        name="grid_map_visualization",
+        output="screen",
+        parameters=[traversability_config],
+    )
+
+    return LaunchDescription(
+        [
+            state_machine_node,
+            path_planning_node,
+            trajectory_following_node,
+            traversability_mapping_node,
+            grid_map_visualization_node
+        ]
+    )

urc_bringup/launch/bringup.launch.py

@@ -3,8 +3,9 @@
 from xacro import process_file
 import yaml
 from launch import LaunchDescription
-from launch.actions import GroupAction, IncludeLaunchDescription
+from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription
 from launch.substitutions import LaunchConfiguration
+from launch.conditions import IfCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node, SetRemap
 from launch_ros.substitutions import FindPackageShare
@@ -44,6 +45,12 @@ def generate_launch_description():
     )
     robot_desc = robot_description_config.toxml()
 
+    autonomy_arg = DeclareLaunchArgument(
+        "autonomy",
+        default_value="false",
+        description="Launch autonomy nodes",
+    )
+
     heartbeat_node = Node(
         package="urc_bringup",
         executable="urc_bringup_HeartbeatPublisher",
@@ -169,8 +176,16 @@ def generate_launch_description():
         parameters=[{"port": 9090}],
     )
 
+    launch_autonomy = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_urc_bringup, "launch", "autonomy.launch.py")
+        ),
+        condition=IfCondition(LaunchConfiguration("autonomy")),
+    )
+
     return LaunchDescription(
         [
+            autonomy_arg,
             control_node,
             load_robot_state_publisher,
             load_joint_state_broadcaster,
@@ -186,5 +201,6 @@ def generate_launch_description():
             vectornav_sensor_msg_node,
             heartbeat_node,
             sick_node,
+            launch_autonomy,
         ]
     )

urc_bringup/launch/sim.launch.py

@@ -47,6 +47,12 @@ def generate_launch_description():
         description="Spawn rocker effort controller (Option B)",
     )
 
+    autonomy_arg = DeclareLaunchArgument(
+        "autonomy",
+        default_value="false",
+        description="Launch autonomy nodes",
+    )
+
     world_filename = LaunchConfiguration("world")
     walli_xacro_config = LaunchConfiguration("walli_xacro")
 
@@ -128,6 +134,13 @@ def generate_launch_description():
         )
     )
 
+    launch_autonomy = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(path_urc_bringup, "launch", "autonomy.launch.py")
+        ),
+        condition=IfCondition(LaunchConfiguration("autonomy")),
+    )
+
 
 
 
@@ -229,12 +242,14 @@ def generate_launch_description():
             walli_xacro,
             bridge_yaml,
             spawn_rocker_effort,
+            autonomy_arg,
             gz_sim,
             bridge,
             robot_state_publisher_node,
             covariances_on_imu,
             covariances_on_gps,
             launch_ekf,
+            launch_autonomy,
             rocker_tf_broadcaster,
             rocker_effort_pid_node,
             spawn,

urc_hw_description/urdf/simplified_swerve/simplified_swerve_sensors.xacro

@@ -50,21 +50,21 @@
         <lidar>
           <scan>
             <horizontal>
-              <samples>720</samples>
+              <samples>2880</samples>
               <resolution>1</resolution>
               <min_angle>-3.14</min_angle>
               <max_angle>3.14</max_angle>
             </horizontal>
             <vertical>
-              <samples>720</samples>
+              <samples>144</samples>
               <resolution>1</resolution>
-              <min_angle>-3.14</min_angle>
-              <max_angle>3.14</max_angle>
+              <min_angle>-0.366519</min_angle>
+              <max_angle>0.366519</max_angle>
             </vertical>
           </scan>
           <range>
             <min>0.05</min>
-            <max>10.0</max>
+            <max>65.0</max>
             <resolution>0.01</resolution>
           </range>
           <noise>

urc_localization/config/ekf_redemption.yaml

@@ -76,6 +76,7 @@ navsat_transform:
     yaw_offset: 0.0
     zero_altitude: true
     broadcast_cartesian_transform: false
+    broadcast_utm_transform: true
     publish_filtered_gps: true
     use_odometry_yaw: false
     wait_for_datum: false