Add navigation stack

README.md

@@ -52,6 +52,12 @@ Summary of of the packages in the repository.
    - Spawn Noah
    - Run rviz2 (optional)
 
+### noah_navigation:
+ - Holds navigation configuration for Slam or to navigate `small_house.world` world.
+ - Launch file for:
+   - Navigation on 'small_house.world'
+   - Slam
+
 
 ## Using docker
 
@@ -143,3 +149,26 @@ Once simulation is running, in other terminal run:
 ```sh
 ros2 run  teleop_twist_keyboard teleop_twist_keyboard cmd_vel:=noah/cmd_vel
 ```
+
+Try Navigation!
+
+Launch simulation in the 'small house' world:
+```sh
+ros2 launch noah_gazebo noah_gazebo.launch.py world:=small_house.world
+```
+Once simulation is running, in other terminal run:
+```sh
+ros2 launch noah_navigation noah_navigation.launch.py
+```
+
+Try Slam!
+
+Run:
+```sh
+ros2 launch noah_navigation noah_navigation.launch.py slam:=True
+```
+
+To save map:
+```sh
+ros2 run nav2_map_server map_saver_cli -f ~/map
+```

docker/Dockerfile

@@ -5,6 +5,7 @@ ARG COLCON_WS=/colcon_ws
 COPY noah_description ${COLCON_WS}/src/noah_description
 COPY noah_gazebo ${COLCON_WS}/src/noah_gazebo
 COPY noah_integration_tests ${COLCON_WS}/src/noah_integration_tests
+COPY noah_navigation ${COLCON_WS}/src/noah_navigation
 RUN apt-get update && \
     apt-get -y dist-upgrade && \
     . /opt/ros/humble/setup.sh && \

noah_navigation/CMakeLists.txt

@@ -0,0 +1,13 @@
+cmake_minimum_required(VERSION 3.5)
+project(noah_navigation)
+
+find_package(ament_cmake REQUIRED)
+
+install(DIRECTORY
+  launch
+  params
+  maps
+  rviz
+  DESTINATION share/${PROJECT_NAME})
+
+ament_package()

noah_navigation/launch/noah_navigation.launch.py

@@ -0,0 +1,61 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node, SetRemap
+
+
+def generate_launch_description():
+
+    use_sim_time = LaunchConfiguration("use_sim_time", default="True")
+
+    use_slam = LaunchConfiguration("slam", default="False")
+
+    map_dir = LaunchConfiguration(
+        "map",
+        default=os.path.join(
+            get_package_share_directory("noah_navigation"), "maps", "small_house.yaml"
+        ),
+    )
+
+    param_dir = LaunchConfiguration(
+        "params_file",
+        default=os.path.join(
+            get_package_share_directory("noah_navigation"), "params", "noah_navigation_params.yaml"
+        ),
+    )
+
+    nav2_bringup_launch_dir = os.path.join(get_package_share_directory("nav2_bringup"), "launch")
+
+    rviz_config_dir = os.path.join(get_package_share_directory("noah_navigation"), "rviz", "noah_navigation.rviz")
+
+
+    declare_slam_cmd = DeclareLaunchArgument(
+        'slam',
+        default_value='False',
+        description='Whether run a SLAM')
+
+    return LaunchDescription(
+        [
+            SetRemap(src="/cmd_vel", dst="/noah/cmd_vel"),
+            DeclareLaunchArgument("slam", default_value=use_slam, description="Whether run a SLAM"),
+            DeclareLaunchArgument("map", default_value=map_dir, description="Full path to map file to load"),
+            DeclareLaunchArgument(
+                "params_file", default_value=param_dir, description="Full path to param file to load"
+            ),
+            DeclareLaunchArgument(
+                "use_sim_time", default_value="true", description="Use simulation clock if true"
+            ),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(os.path.join(nav2_bringup_launch_dir, "rviz_launch.py")),
+                launch_arguments={"namespace": "", "use_namespace": "False", "rviz_config": rviz_config_dir, "use_sim_time": use_sim_time}.items(),
+            ),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource([nav2_bringup_launch_dir, "/bringup_launch.py"]),
+                launch_arguments={"map": map_dir, "use_sim_time": use_sim_time, "params_file": param_dir, "slam": use_slam}.items(),
+            ),
+        ]
+    )

noah_navigation/maps/small_house.yaml

@@ -0,0 +1,7 @@
+image: small_house.pgm
+mode: trinary
+resolution: 0.05
+origin: [-9.42, -5.63, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25

noah_navigation/package.xml

@@ -0,0 +1,42 @@
+<?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>noah_navigation</name>
+  <version>0.1.0</version>
+  <description>
+    The noah_navigation package
+  </description>
+
+  <maintainer email="[email protected]">Gonzalo de Pedro</maintainer>
+
+  <license>TBD</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <exec_depend>launch</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
+  <exec_depend>rviz2</exec_depend>
+
+  <exec_depend>navigation2</exec_depend>
+  <exec_depend>nav2_amcl</exec_depend>
+  <exec_depend>nav2_bringup</exec_depend>
+  <exec_depend>nav2_bt_navigator</exec_depend>
+  <exec_depend>nav2_costmap_2d</exec_depend>
+  <exec_depend>nav2_core</exec_depend>
+  <exec_depend>nav2_dwb_controller</exec_depend>
+  <exec_depend>nav2_lifecycle_manager</exec_depend>
+  <exec_depend>nav2_map_server</exec_depend>
+  <exec_depend>nav2_behaviors</exec_depend>
+  <exec_depend>nav2_planner</exec_depend>
+  <exec_depend>nav2_msgs</exec_depend>
+  <exec_depend>nav2_navfn_planner</exec_depend>
+  <exec_depend>nav2_rviz_plugins</exec_depend>
+  <exec_depend>nav2_behavior_tree</exec_depend>
+  <exec_depend>nav2_util</exec_depend>
+  <exec_depend>nav2_controller</exec_depend>
+  <exec_depend>nav2_waypoint_follower</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>