WIP: Add integration with nav2

README.md

@@ -30,7 +30,7 @@ git clone [email protected]:ardupilot/ardupilot_ros.git
 Install dependencies using rosdep:
 ```bash
 cd ~/ros2_ws
-rosdep install --from-paths src --ignore-src -r
+rosdep install --from-paths src --ignore-src -r --skip-keys gazebo-ros-pkgs
 ```
 
 ## Build
@@ -96,6 +96,47 @@ Then run the controller using,
 
 Now, using the keyboard keys you can control the drone.
 
+### 2. Obstacle avoidance using Cartographer and Nav2
+
+Using the same simulation as before, the nav2 node can be launched to control the copter once it is in the air.
+
+Launch the simulation:
+
+```bash
+cd ~/ros2_ws
+source install/setup.sh
+ros2 launch ardupilot_gz_bringup iris_maze.launch.py rviz:=false
+```
+Launch cartographer:
+
+```bash
+cd ~/ros2_ws
+source install/setup.sh
+ros2 launch ardupilot_ros cartographer.launch.py rviz:=false
+```
+
+Launch nav2:
+
+```bash
+cd ~/ros2_ws
+source install/setup.sh
+ros2 launch ardupilot_ros navigation.launch.py
+```
+
+Takeoff the Copter using `mavproxy` to an altitude of 2.5m:
+
+```bash
+mavproxy.py --console --map --aircraft test --master=:14550
+
+mode guided
+
+arm throttle
+
+takeoff 2.5
+```
+
+You may now navigate while mapping using the `Nav2 Goal` tool in RVIZ!
+
 ## Contribution Guideline
 
 * Ensure the [pre-commit](https://github.com/pre-commit/pre-commit) hooks pass locally before creating your pull request by installing the hooks before committing.

config/navigation.yaml

@@ -0,0 +1,180 @@
+bt_navigator:
+  ros__parameters:
+    use_sim_time: &default_use_sim_time True
+    global_frame: map
+    robot_base_frame: base_link
+    plugin_lib_names:
+      - nav2_compute_path_to_pose_action_bt_node
+      - nav2_compute_path_through_poses_action_bt_node
+      - nav2_smooth_path_action_bt_node
+      - nav2_follow_path_action_bt_node
+      - nav2_spin_action_bt_node
+      - nav2_wait_action_bt_node
+      - nav2_back_up_action_bt_node
+      - nav2_drive_on_heading_bt_node
+      - nav2_clear_costmap_service_bt_node
+      - nav2_is_stuck_condition_bt_node
+      - nav2_goal_reached_condition_bt_node
+      - nav2_goal_updated_condition_bt_node
+      - nav2_globally_updated_goal_condition_bt_node
+      - nav2_is_path_valid_condition_bt_node
+      - nav2_initial_pose_received_condition_bt_node
+      - nav2_reinitialize_global_localization_service_bt_node
+      - nav2_rate_controller_bt_node
+      - nav2_distance_controller_bt_node
+      - nav2_speed_controller_bt_node
+      - nav2_truncate_path_action_bt_node
+      - nav2_truncate_path_local_action_bt_node
+      - nav2_goal_updater_node_bt_node
+      - nav2_recovery_node_bt_node
+      - nav2_pipeline_sequence_bt_node
+      - nav2_round_robin_node_bt_node
+      - nav2_transform_available_condition_bt_node
+      - nav2_time_expired_condition_bt_node
+      - nav2_path_expiring_timer_condition
+      - nav2_distance_traveled_condition_bt_node
+      - nav2_single_trigger_bt_node
+      - nav2_goal_updated_controller_bt_node
+      - nav2_is_battery_low_condition_bt_node
+      - nav2_navigate_through_poses_action_bt_node
+      - nav2_navigate_to_pose_action_bt_node
+      - nav2_remove_passed_goals_action_bt_node
+      - nav2_planner_selector_bt_node
+      - nav2_controller_selector_bt_node
+      - nav2_goal_checker_selector_bt_node
+      - nav2_controller_cancel_bt_node
+      - nav2_path_longer_on_approach_bt_node
+      - nav2_wait_cancel_bt_node
+      - nav2_spin_cancel_bt_node
+      - nav2_back_up_cancel_bt_node
+      - nav2_drive_on_heading_cancel_bt_node
+
+bt_navigator_navigate_through_poses_rclcpp_node:
+  ros__parameters:
+    use_sim_time: *default_use_sim_time
+
+bt_navigator_navigate_to_pose_rclcpp_node:
+  ros__parameters:
+    use_sim_time: *default_use_sim_time
+
+controller_server:
+  ros__parameters:
+    use_sim_time: *default_use_sim_time
+    failure_tolerance: 0.3
+    progress_checker_plugin: "progress_checker"
+    goal_checker_plugins: ["general_goal_checker"]
+    controller_plugins: ["FollowPath"]
+    progress_checker:
+      plugin: "nav2_controller::SimpleProgressChecker"
+      required_movement_radius: 0.5
+      movement_time_allowance: 10.0
+    general_goal_checker:
+      stateful: True
+      plugin: "nav2_controller::SimpleGoalChecker"
+      xy_goal_tolerance: 0.25
+      yaw_goal_tolerance: 0.25
+    FollowPath:
+      plugin: "dwb_core::DWBLocalPlanner"
+      max_vel_x: 0.5
+      max_vel_y: 0.5
+      max_vel_theta: 1.0
+      max_speed_xy: 0.707
+      acc_lim_x: 2.5
+      acc_lim_y: 0.0
+      acc_lim_theta: 3.2
+      decel_lim_x: -2.5
+      decel_lim_y: 0.0
+      decel_lim_theta: -3.2
+      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      global_frame: map
+      robot_base_frame: base_link
+      use_sim_time: *default_use_sim_time
+      robot_radius: 0.25
+      resolution: 0.05
+      lethal_cost_threshold: 50
+      always_send_full_costmap: True
+      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: True
+      obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
+        enabled: True
+        observation_sources: scan
+        scan:
+          topic: /scan
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      global_frame: odom
+      robot_base_frame: base_link
+      use_sim_time: *default_use_sim_time
+      rolling_window: true
+      width: 5
+      height: 5
+      resolution: 0.05
+      robot_radius: 0.25
+      always_send_full_costmap: True
+      plugins: ["voxel_layer", "inflation_layer"]
+      voxel_layer:
+        plugin: "nav2_costmap_2d::VoxelLayer"
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+
+planner_server:
+  ros__parameters:
+    expected_planner_frequency: 20.0
+    use_sim_time: *default_use_sim_time
+    planner_plugins: ["GridBased"]
+    GridBased:
+      plugin: "nav2_navfn_planner/NavfnPlanner"
+
+smoother_server:
+  ros__parameters:
+    use_sim_time: *default_use_sim_time
+    smoother_plugins: ["simple_smoother"]
+    enable_stamped_cmd_vel: True
+    simple_smoother:
+      plugin: "nav2_smoother::SimpleSmoother"
+
+behavior_server:
+  ros__parameters:
+    local_frame: odom
+    global_frame: map
+    robot_base_frame: base_link
+    use_sim_time: *default_use_sim_time
+    enable_stamped_cmd_vel: True
+    behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"]
+    spin:
+      plugin: "nav2_behaviors/Spin"
+    backup:
+      plugin: "nav2_behaviors/BackUp"
+    drive_on_heading:
+      plugin: "nav2_behaviors/DriveOnHeading"
+    wait:
+      plugin: "nav2_behaviors/Wait"
+
+velocity_smoother:
+  ros__parameters:
+    use_sim_time: *default_use_sim_time
+    feedback: "OPEN_LOOP"
+    max_velocity: [0.5, 0.5, 1.0]
+    min_velocity: [-0.5, -0.5, -1.0]
+    velocity_timeout: 1.0

launch/navigation.launch.py

@@ -0,0 +1,66 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction
+from launch.substitutions import LaunchConfiguration
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.conditions import IfCondition
+from launch_ros.actions import SetRemap
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from pathlib import Path
+
+"""Generate a launch description for the navigation example."""
+
+
+def generate_launch_description():
+    # Navigation
+    navigation = GroupAction(
+        actions=[
+            SetRemap(src="/cmd_vel", dst="/ap/cmd_vel"),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    str(
+                        Path(
+                            FindPackageShare("nav2_bringup").find("nav2_bringup"),
+                            "launch",
+                            "navigation_launch.py",
+                        )
+                    )
+                ),
+                launch_arguments={
+                    "use_sim_time": "true",
+                    "params_file": FindPackageShare("ardupilot_ros").find(
+                        "ardupilot_ros"
+                    )
+                    + "/config"
+                    + "/navigation.yaml",
+                }.items(),
+            ),
+        ]
+    )
+
+    # RViz.
+    rviz = Node(
+        package="rviz2",
+        executable="rviz2",
+        arguments=[
+            "-d",
+            str(
+                Path(
+                    FindPackageShare("ardupilot_ros").find("ardupilot_ros"),
+                    "rviz",
+                    "navigation.rviz",
+                )
+            ),
+        ],
+        condition=IfCondition(LaunchConfiguration("rviz")),
+    )
+
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument(
+                "rviz", default_value="true", description="Open RViz."
+            ),
+            rviz,
+            navigation,
+        ]
+    )

package.xml

@@ -17,6 +17,7 @@
   <exec_depend>ros_gz_sim</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
   <exec_depend>topic_tools</exec_depend>
+  <exec_depend>navigation2</exec_depend>
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
   <test_depend>ament_pep257</test_depend>