Adaptions of navigation behavior (#13)

    * using NavFn planner + DWB local planner
    * added custom behavior tree for MiR
    * added robot footprint
    * adapted a few parameters
    * visualization updates in rviz

Author: relffok

mir_description/rviz/mir_visu_full.rviz

@@ -39,7 +39,7 @@ def generate_launch_description():
 
         DeclareLaunchArgument(
             'rviz_config_file',
-            default_value=os.path.join(mir_description_dir, 'rviz', 'mir_visualization.rviz'),
+            default_value=os.path.join(mir_description_dir, 'rviz', 'mir_visu_full.rviz'),
             description='Define rviz config file to be used.'),
 
         DeclareLaunchArgument(

mir_driver/launch/mir_launch.py

@@ -66,7 +66,7 @@ def generate_launch_description():
 
     declare_rviz_config_arg = DeclareLaunchArgument(
         'rviz_config_file',
-        default_value=os.path.join(mir_gazebo_dir, 'rviz', 'mir_visualization.rviz'),
+        default_value=os.path.join(mir_description_dir, 'rviz', 'mir_visu_full.rviz'),
         description='Define rviz config file to be used.')
 
     declare_gui_arg = DeclareLaunchArgument(

mir_gazebo/launch/mir_gazebo_launch.py

@@ -23,7 +23,7 @@ find_package(rviz2)
 
 
 install(DIRECTORY
-  config launch rviz maps
+  config launch rviz maps behavior_trees
   DESTINATION share/${PROJECT_NAME}
 )
 

mir_navigation/CMakeLists.txt

@@ -0,0 +1,42 @@
+
+<!--
+  This Behavior Tree replans the global path periodically at 1 Hz and it also has
+  recovery actions specific to planning / control as well as general system issues.
+  This will be continuous if a kinematically valid planner is selected.
+-->
+<root main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
+      <PipelineSequence name="NavigateWithReplanning">
+        <RateController hz="1.0">
+          <RecoveryNode number_of_retries="1" name="ComputePathToPose">
+            <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
+            <ReactiveFallback name="ComputePathToPoseRecoveryFallback">
+              <GoalUpdated/>
+              <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
+            </ReactiveFallback>
+          </RecoveryNode>
+        </RateController>
+        <RecoveryNode number_of_retries="1" name="FollowPath">
+          <FollowPath path="{path}" controller_id="FollowPath"/>
+          <ReactiveFallback name="FollowPathRecoveryFallback">
+            <GoalUpdated/>
+            <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
+          </ReactiveFallback>
+        </RecoveryNode>
+      </PipelineSequence>
+      <ReactiveFallback name="RecoveryFallback">
+        <GoalUpdated/>
+        <RoundRobin name="RecoveryActions">
+          <Sequence name="ClearingActions">
+            <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
+            <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
+          </Sequence>
+          <Spin spin_dist="0.79"/>
+          <Wait wait_duration="3"/>
+          <BackUp backup_dist="1" backup_speed="0.5"/>
+        </RoundRobin>
+      </ReactiveFallback>
+    </RecoveryNode>
+  </BehaviorTree>
+</root>

mir_navigation/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml

@@ -26,12 +26,12 @@ slam_toolbox:
     debug_logging: true
     throttle_scans: 1
     transform_publish_period: 0.02 #if 0 never publishes odometry
-    map_update_interval: 1.0
+    map_update_interval: 0.01
     resolution: 0.05
     max_laser_range: 20.0 #for rastering images
     minimum_time_interval: 0.5
     transform_timeout: 0.5
-    tf_buffer_duration: 30.
+    tf_buffer_duration: 300.
     stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
     enable_interactive_mode: true
 

mir_navigation/config/mir_mapping_async_sim.yaml

@@ -30,7 +30,7 @@ amcl:
     save_pose_rate: 0.5
     sigma_hit: 0.2
     tf_broadcast: true
-    transform_tolerance: 1.0
+    transform_tolerance: 2.0
     update_min_a: 0.2
     update_min_d: 0.25
     z_hit: 0.5
@@ -47,6 +47,7 @@ amcl_rclcpp_node:
   ros__parameters:
     use_sim_time: True
 
+
 bt_navigator:
   ros__parameters:
     use_sim_time: True
@@ -62,6 +63,9 @@ bt_navigator:
     # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
     # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
     # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
+    #
+    # Note: Not quite correct, params that are not mentioned can not be substituted!
+    plugin_lib_names:
     plugin_lib_names:
     - nav2_compute_path_to_pose_action_bt_node
     - nav2_compute_path_through_poses_action_bt_node
@@ -107,35 +111,33 @@ controller_server:
     min_x_velocity_threshold: 0.001
     min_y_velocity_threshold: 0.5
     min_theta_velocity_threshold: 0.001
+    failure_tolerance: 0.1
+    # speed_limit_topic : "speed_limit"
     progress_checker_plugin: "progress_checker"
-    goal_checker_plugin: "goal_checker"
+    goal_checker_plugins: ["goal_checker"]
     controller_plugins: ["FollowPath"]
 
     # Progress checker parameters
     progress_checker:
       plugin: "nav2_controller::SimpleProgressChecker"
       required_movement_radius: 0.5
       movement_time_allowance: 10.0
-    # Goal checker parameters
     goal_checker:
       plugin: "nav2_controller::SimpleGoalChecker"
       xy_goal_tolerance: 0.25
-      yaw_goal_tolerance: 0.25
+      yaw_goal_tolerance: 0.15
       stateful: True
-    # DWB parameters
     FollowPath:
       plugin: "dwb_core::DWBLocalPlanner"
       debug_trajectory_details: True
       min_vel_x: 0.0
       min_vel_y: 0.0
-      max_vel_x: 0.26
+      max_vel_x: 2.5
       max_vel_y: 0.0
       max_vel_theta: 1.0
       min_speed_xy: 0.0
-      max_speed_xy: 0.26
+      max_speed_xy: 2.5
       min_speed_theta: 0.0
-      # Add high threshold velocity for turtlebot 3 issue.
-      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
       acc_lim_x: 2.5
       acc_lim_y: 0.0
       acc_lim_theta: 3.2
@@ -153,26 +155,27 @@ controller_server:
       trans_stopped_velocity: 0.25
       short_circuit_trajectory_evaluation: True
       stateful: True
-      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
+      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist", "ObstacleFootprint"]
       BaseObstacle.scale: 0.02
-      PathAlign.scale: 32.0
-      PathAlign.forward_point_distance: 0.1
-      GoalAlign.scale: 24.0
       GoalAlign.forward_point_distance: 0.1
+      GoalAlign.scale: 24.0
+      PathAlign.scale: 32.0
+      PathAlign.forward_point_distance: 0.325
       PathDist.scale: 32.0
       GoalDist.scale: 24.0
-      RotateToGoal.scale: 32.0
-      RotateToGoal.slowing_factor: 5.0
+      RotateToGoal.scale: 24.0
+      RotateToGoal.xy_goal_tolerance: 0.25
+      RotateToGoal.slowing_factor: 2.0
       RotateToGoal.lookahead_time: -1.0
-
+      
 controller_server_rclcpp_node:
   ros__parameters:
     use_sim_time: True
 
 local_costmap:
   local_costmap:
     ros__parameters:
-      update_frequency: 10.0
+      update_frequency: 5.0
       publish_frequency: 2.0
       global_frame: odom
       robot_base_frame: base_link
@@ -181,12 +184,15 @@ local_costmap:
       width: 3
       height: 3
       resolution: 0.05
-      robot_radius: 0.5
+      # offset: front / back from base_link
+      # footprint: '[ [front, width/2], [back, width/2], [-back, -width/2], [front, -width/2] ]'
+      footprint: '[ [0.5, 0.25], [-0.4, 0.25], [-0.4, -0.25], [0.5, -0.25] ]'
+      footprint_padding: 0.01
       plugins: ["voxel_layer", "inflation_layer"]
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
+        inflation_radius: 0.55
         cost_scaling_factor: 3.0
-        inflation_radius: 0.8
       voxel_layer:
         plugin: "nav2_costmap_2d::VoxelLayer"
         enabled: True
@@ -203,6 +209,11 @@ local_costmap:
           clearing: True
           marking: True
           data_type: "LaserScan"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+          expected_update_rate: 0.0
       static_layer:
         map_subscribe_transient_local: True
       always_send_full_costmap: True
@@ -221,27 +232,41 @@ global_costmap:
       global_frame: map
       robot_base_frame: base_link
       use_sim_time: True
-      robot_radius: 0.5
-      resolution: 0.05
+      # offset: front / back from base_link
+      # footprint: '[ [front, width/2], [back, width/2], [-back, -width/2], [front, -width/2] ]'
+      footprint: '[ [0.5, 0.25], [-0.4, 0.25], [-0.4, -0.25], [0.5, -0.25] ]'
+      footprint_padding: 0.01
       track_unknown_space: true
       plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
       obstacle_layer:
         plugin: "nav2_costmap_2d::ObstacleLayer"
         enabled: True
         observation_sources: scan
+        footprint_clearing_enabled: true
+        max_obstacle_height: 2.0
+        combination_method: 1
         scan:
           topic: /scan
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
           max_obstacle_height: 2.0
+          min_obstacle_height: 0.0
           clearing: True
           marking: True
           data_type: "LaserScan"
+          inf_is_valid: false
       static_layer:
         plugin: "nav2_costmap_2d::StaticLayer"
         map_subscribe_transient_local: True
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
+        enabled: true
+        inflation_radius: 0.55
         cost_scaling_factor: 3.0
-        inflation_radius: 0.8
+        inflate_unknown: false
+        inflate_around_unknown: true
       always_send_full_costmap: True
   global_costmap_client:
     ros__parameters:
@@ -258,10 +283,10 @@ map_server:
 map_saver:
   ros__parameters:
     use_sim_time: True
-    save_map_timeout: 5000
+    save_map_timeout: 5.0
     free_thresh_default: 0.25
     occupied_thresh_default: 0.65
-    map_subscribe_transient_local: False
+    map_subscribe_transient_local: True
 
 planner_server:
   ros__parameters:
@@ -271,7 +296,7 @@ planner_server:
     GridBased:
       plugin: "nav2_navfn_planner/NavfnPlanner"
       tolerance: 0.5
-      use_astar: false
+      use_astar: true
       allow_unknown: true
 
 planner_server_rclcpp_node:

mir_navigation/config/mir_nav_params.yaml

@@ -32,7 +32,7 @@ def generate_launch_description():
     command_topic = LaunchConfiguration('cmd_vel_topic')
     autostart = LaunchConfiguration('autostart')
     params_file = LaunchConfiguration('params_file')
-    default_bt_xml_filename = LaunchConfiguration('default_bt_xml_filename')
+    default_nav_to_pose_bt_xml = LaunchConfiguration('default_nav_to_pose_bt_xml')
     map_subscribe_transient_local = LaunchConfiguration('map_subscribe_transient_local')
 
     lifecycle_nodes = ['controller_server',
@@ -52,9 +52,11 @@ def generate_launch_description():
                   ('cmd_vel', command_topic)]
 
     # Create our own temporary YAML files that include substitutions
+    # Watch out for parameters that don't exist in yaml - will not be substituted of course (default_nav_to_pose_bt_xml)
+    # TODO: Needs to be addressed in nav2
     param_substitutions = {
         'use_sim_time': use_sim_time,
-        'default_bt_xml_filename': default_bt_xml_filename,
+        'default_nav_to_pose_bt_xml': default_nav_to_pose_bt_xml,
         'autostart': autostart,
         'map_subscribe_transient_local': map_subscribe_transient_local}
 
@@ -90,10 +92,9 @@ def generate_launch_description():
             description='Full path to the ROS2 parameters file to use'),
 
         DeclareLaunchArgument(
-            'default_bt_xml_filename',
+            'default_nav_to_pose_bt_xml',
             default_value=os.path.join(
-                get_package_share_directory('nav2_bt_navigator'),
-                'behavior_trees', 'navigate_w_replanning_and_recovery.xml'),
+                mir_nav_dir, 'behavior_trees', 'navigate_to_pose_w_replanning_and_recovery.xml'),
             description='Full path to the behavior tree xml file to use'),
 
         DeclareLaunchArgument(
@@ -128,7 +129,9 @@ def generate_launch_description():
             executable='bt_navigator',
             name='bt_navigator',
             output='screen',
-            parameters=[configured_params],
+            parameters=[
+                configured_params, 
+                {'default_nav_to_pose_bt_xml' : default_nav_to_pose_bt_xml}],
             remappings=remappings),
 
         Node(