Carter navigator file setup2.0

src/navigator/gnc/README.md

@@ -0,0 +1 @@
+I am only here so you can see the directory that I am apart of in GitHub. Once a real file has been placed in this directory, delete me :)

src/navigator/navigator_bringup/CMakeLists.txt

@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.5)
+
+project(navigator_bringup)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# find_package(ament_index_python REQUIRED) find_package(launch REQUIRED)
+# find_package(launch_ros REQUIRED)
+find_package(navigator_description REQUIRED)
+find_package(navigator_gazebo REQUIRED)
+
+# Install project launch files
+install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch)
+
+# Install URDF files install(DIRECTORY urdf DESTINATION
+# share/${PROJECT_NAME}/urdf)
+
+# Install project configuration files install(DIRECTORY config/ DESTINATION
+# share/${PROJECT_NAME}/config)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/navigator/navigator_bringup/launch/gazebo.launch.py

@@ -0,0 +1,93 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    # Configure ROS nodes for launch
+
+    # Setup project paths
+    pkg_project_bringup = get_package_share_directory("navigator_bringup")
+    pkg_project_gazebo = get_package_share_directory("navigator_gazebo")
+    pkg_project_description = get_package_share_directory("navigator_description")
+    # pkg_project_sim_description = get_package_share_directory(
+    #     "navigator_sim_description",
+    # )
+    pkg_ros_gz_sim = get_package_share_directory("ros_gz_sim")
+
+    # !!! Uncomment once navigator_controller is created !!!
+    # pkg_controller = get_package_share_directory("navigator_controller")
+
+    # Setup to launch the simulator and Gazebo world
+    gz_sim_world = DeclareLaunchArgument("world", default_value="robotx2024_1.world")
+    gz_sim = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_ros_gz_sim, "launch", "gz_sim.launch.py"),
+        ),
+        launch_arguments={
+            "gz_args": [
+                PathJoinSubstitution(
+                    [pkg_project_gazebo, "worlds", LaunchConfiguration("world")],
+                ),
+                " --render-engine",
+                " ogre",
+            ],
+        }.items(),
+    )
+
+    # !!! Uncomment once navigator_controller is created !!!
+    # Get controller to use sim values
+    # sim_pid_yaml = os.path.join(pkg_controller, "config", "sim_pid_controller.yaml")
+    # set_sim_params = SetLaunchConfiguration("param_file", sim_pid_yaml)
+
+    # Include the Subjugator_Setup Launch file
+    navigator_setup = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_project_bringup, "launch", "navigator_setup.launch.py"),
+        ),
+        launch_arguments={
+            "use_sim_time": "true",
+            "xacro_file": os.path.join(
+                pkg_project_description,
+                "urdf",
+                "navigator.urdf.xacro",
+            ),
+            "gui": "true",
+        }.items(),
+    )
+
+    # Bridge ROS topics and Gazebo messages for establishing communication
+    bridge = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        parameters=[
+            {
+                "config_file": os.path.join(
+                    pkg_project_bringup,
+                    "config",
+                    "navigator_bridge.yaml",
+                ),
+                "qos_overrides./tf_static.publisher.durability": "transient_local",
+            },
+        ],
+        output="screen",
+    )
+
+    return LaunchDescription(
+        [
+            gz_sim_world,
+            gz_sim,
+            # !!! Uncomment once navigator_controller is created !!!
+            # set_sim_params,
+            navigator_setup,
+            bridge,
+        ],
+    )

src/navigator/navigator_bringup/launch/navigator_setup.launch.py

@@ -0,0 +1,173 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    ExecuteProcess,
+)
+from launch.conditions import IfCondition
+from launch.substitutions import (
+    Command,
+    FindExecutable,
+    LaunchConfiguration,
+    PathJoinSubstitution,
+)
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+
+
+def generate_launch_description():
+
+    # Setup project paths
+    pkg_project_bringup = get_package_share_directory("navigator_bringup")
+    pkg_project_description = get_package_share_directory("navigator_description")
+
+    # !!! Uncomment once navigator_controller is created !!!
+    # pkg_thruster_manager = get_package_share_directory("navigator_thruster_manager")
+    # pkg_localization = get_package_share_directory("navigator_localization")
+    # pkg_controller = get_package_share_directory("navigator_controller")
+
+    # Args
+    gui_cmd = DeclareLaunchArgument(
+        "gui",
+        default_value="true",
+        description="whether to launch the gui",
+    )
+
+    xacro_file_arg = DeclareLaunchArgument(
+        "xacro_file",
+        default_value=PathJoinSubstitution(
+            [pkg_project_description, "urdf", "navigator.urdf.xacro"],
+        ),
+        description="Path to the robot xacro file",
+    )
+
+    use_sim_time_arg = DeclareLaunchArgument(
+        "use_sim_time",
+        default_value="false",
+        description="Use sim clock (true for sim, false for real)",
+    )
+
+    # Expand xacro at runtime
+    robot_desc = ParameterValue(
+        Command([FindExecutable(name="xacro"), " ", LaunchConfiguration("xacro_file")]),
+        value_type=str,
+    )
+
+    # Write an on-disk URDF
+    urdf_out = os.path.join(pkg_project_description, "urdf", "navigator.urdf")
+    generate_urdf = ExecuteProcess(
+        cmd=["xacro", LaunchConfiguration("xacro_file"), "-o", urdf_out],
+        output="screen",
+    )
+
+    # # Convert URDF to SDF using Gazebo's gz tool
+    # sdf_file = os.path.join(pkg_project_description, 'urdf', 'navigator.sdf')
+    # try:
+    #     subprocess.run(['gz', 'sdf', '-p', urdf_file], check=True, stdout=open(sdf_file, 'w'))
+    #     print(f"Successfully converted {urdf_file} to {sdf_file}")
+    # except subprocess.CalledProcessError as e:
+    #     print(f"Error converting URDF to SDF: {e}")
+
+    # Takes the description and joint angles as inputs and publishes the 3D poses of the robot links
+    robot_state_publisher_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher",
+        output="both",
+        parameters=[
+            {"use_sim_time": LaunchConfiguration("use_sim_time")},
+            {"robot_description": robot_desc},
+        ],
+    )
+
+    # Visualize in RViz
+    rviz = Node(
+        package="rviz2",
+        executable="rviz2",
+        arguments=[
+            "-d",
+            os.path.join(pkg_project_bringup, "config", "navigator.rviz"),
+        ],
+        condition=IfCondition(LaunchConfiguration("gui")),
+    )
+
+    # !!! Uncomment once navigator_controller is created !!!
+    # thruster_manager = IncludeLaunchDescription(
+    #     PythonLaunchDescriptionSource(
+    #         os.path.join(pkg_thruster_manager, "launch", "thruster_manager.launch.py"),
+    #     ),
+    # )
+    # !!! Uncomment once navigator_localization is created !!!
+    # localization = IncludeLaunchDescription(
+    #     PythonLaunchDescriptionSource(
+    #         os.path.join(
+    #             pkg_localization,
+    #             "launch",
+    #             "navigator_localization.launch.py",
+    #         ),
+    #     ),
+    #     launch_arguments={
+    #         "params_file": os.path.join(
+    #             pkg_localization,
+    #             "config",
+    #             "localization_parameters.yaml",
+    #         ),
+    #     }.items(),
+    # )
+
+    # depth_to_pose = Node(
+    # package="navigator_localization",
+    # executable="depth_to_pose_node.py",
+    # name="depth_to_pose",
+    # output="screen",
+    # )
+
+    # !!! Uncomment once navigator_localization is created !!!
+    # controller = IncludeLaunchDescription(
+    #     PythonLaunchDescriptionSource(
+    #         os.path.join(pkg_controller, "launch", "pid_controller.launch.py"),
+    #     ),
+    # )
+
+    path_planner = Node(
+        package="navigator_path_planner",
+        executable="navigator_path_planner",
+        name="navigator_path_planner",
+        output="both",
+    )
+
+    trajectory_planner = Node(
+        package="navigator_trajectory_planner",
+        executable="trajectory_planner",
+        name="navigator_trajectory_planner",
+        output="both",
+    )
+
+    # wrench_tuner = IncludeLaunchDescription(
+    # PythonLaunchDescriptionSource(
+    # os.path.join(
+    # get_package_share_directory("navigator_wrench_tuner"),
+    # "launch",
+    # "wrench_tuner_launch.py",
+    # ),
+    # ),
+    # )
+    return LaunchDescription(
+        [
+            gui_cmd,
+            xacro_file_arg,
+            use_sim_time_arg,
+            generate_urdf,
+            robot_state_publisher_node,
+            # joint_state_publisher_node,
+            rviz,
+            # !!! Uncomment once navigator_localization is created !!!
+            # thruster_manager,
+            # localization,
+            # controller,
+            path_planner,
+            trajectory_planner,
+        ],
+    )

src/navigator/navigator_bringup/package.xml

@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>navigator_bringup</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="cckreis0507@gmail.com">Carter Kreis</maintainer>
+  <license>TODO: License declaration</license>
+  <author>Carter Kreis</author>
+
+  <depend>joint_state_publisher_gui</depend>
+  <depend>navigator_description</depend>
+  <depend>navigator_gazebo</depend>
+  <depend>ros_gz</depend>
+  <depend>sdformat_urdf</depend>
+
+  <!-- needed by ros2 launch scripts -->
+  <exec_depend>ament_index_python</exec_depend>
+  <exec_depend>launch</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>ros_gz_bridge</exec_depend>
+  <exec_depend>ros_gz_sim</exec_depend>
+  <exec_depend>rviz2</exec_depend>
+  <exec_depend>xacro</exec_depend>
+
+  <!-- xacro is invoked at launch-time to expand the URDF -->
+
+  <!-- packages referenced by the launch files -->
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/navigator/scripts/README.md

@@ -0,0 +1 @@
+I am only here so you can see the directory that I am apart of in GitHub. Once a real file has been placed in this directory, delete me :)

src/navigator/simulation/navigator_description/CMakeLists.txt

@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 3.5)
+
+project(navigator_description)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+install(DIRECTORY models urdf DESTINATION share/${PROJECT_NAME})
+
+ament_package()

src/navigator/simulation/navigator_description/package.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>navigator_description</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="cckreis0507@gmail.com">ckreis</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>