0
$\begingroup$

based on my previous post MPPI+SmacPlannerHybrid navigation with a large base_footprint problem I've tried so hard tuning parameter for SmacHybridPlanner and I think it's not able to plan the path consider its large footprint for avoiding obstacle beforehand

So I got a solution for Path planning which is the similar algorithm called Hybrid A* that use Reeds-shepp path segment. I tried implement it for using with nav2 , here's my ref code : https://github.com/RajPShinde/Hybrid-A-Star.

Right now, I am able to load map from /map and get footprint--->odom--->map for set a start point.. it does drift a little but the optimal calculated path was published through /plan .

The main question is While running nav2 I am able to see my path that publish from my custom outside planner (not part of nav2), but how can I trigger the controller to follow my given path. (I want to use nav2's Regulated Pure pursuit controller)

P.S. In my beginner pov, it's just too complicated to build the python Hybrid-A-Star source code that I found into nav2 planner plugin or modify behavior tree stuff to use my planner instead of nav2 planner

nav2 planner param:

planner_server:
  ros__parameters:
    expected_planner_frequency: 20.0
    use_sim_time: True
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_smac_planner/SmacPlannerHybrid"
      tolerance: 0.05 
      downsample_costmap: false    
      downsampling_factor: 1          
      allow_unknown: true    
      max_iterations: 1000000       
      max_on_approach_iterations: 1000000 
      max_planning_time: 50.0        
      smooth_path: false
      motion_model_for_search: "REEDS_SHEPP" #REEDS_SHEPP
      angle_quantization_bins: 72    
      minimum_turning_radius: 2.5  #0.6 , 2.5
      reverse_penalty: 1.0  #For Reeds-Shepp model: penalty to apply if motion is reversing, must be => 1
      change_penalty: 0.3      
      non_straight_penalty: 1.0     
      cost_penalty: 6000.0
      lookup_table_size: 20.0    
      cache_obstacle_heuristic: True    
      debug_visualizations: True   

global and local costmap

       local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 20.0
      publish_frequency: 20.0
      global_frame: odom
      robot_base_frame: base_footprint
      use_sim_time: True
      rolling_window: true
      width: 6
      height: 6
      resolution: 0.05
      footprint: "[ [1.90, 0.2],[1.25, 0.2],[1.25, 0.17],[0.0, 0.17],[0.0, 0.2],[-0.50, 0.2],[-0.50, -0.3],[0.0, -0.3],[0.0, -0.17],[1.25, -0.17], [1.25, -0.3], [1.90, -0.3]]" 
      # footprint: "[ [1.0, 0.25], [1.0, -0.25], [-1.0, -0.25], [-1.0, 0.25] ]"   
      origin_x: 0.0  
      origin_y: 0.0
      plugins: ["voxel_layer", "inflation_layer"]
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 5.0
        inflation_radius: 0.01
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 3.5
          obstacle_min_range: 0.2

      always_send_full_costmap: True

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_footprint
      use_sim_time: True
      # robot_radius: 0.22
      footprint: "[ [1.90, 0.2],[1.25, 0.2],[1.25, 0.17],[0.0, 0.17],[0.0, 0.25],[-0.50, 0.25],[-0.50, -0.25],[0.0, -0.25],[0.0, -0.17],[1.25, -0.17], [1.25, -0.2], [1.90, -0.2] ]" 
      resolution: 0.05
      # footprint: "[ [1.0, 0.25], [1.0, -0.25], [-1.0, -0.25], [-1.0, 0.25] ]"   
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 3.5
          obstacle_min_range: 0.8
      static_layer:
        enabled: True
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.05
      always_send_full_costmap: True

      allow_unknown: true    
      max_iterations: 1000000       
      max_on_approach_iterations: 1000000 
      max_planning_time: 50.0        
      smooth_path: false
      motion_model_for_search: "REEDS_SHEPP" #REEDS_SHEPP
      angle_quantization_bins: 72    
      minimum_turning_radius: 2.5  #0.6 , 2.5
      reverse_penalty: 1.0  #For Reeds-Shepp model: penalty to apply if motion is reversing, must be => 1
      change_penalty: 0.3      
      non_straight_penalty: 1.0     
      cost_penalty: 6000.0
      lookup_table_size: 20.0    
      cache_obstacle_heuristic: True    
      debug_visualizations: True  
$\endgroup$

1 Answer 1

0
$\begingroup$

This is what the behavior tree structure is for! If your new planner is a C++ plugin, then you should be able to load it into the planner server and things should just work as is. If you have it in another language or not as a planner plugin type, then you'll need to build a service or action server around it and expose a BT node for calling the planner in the Behavior Tree to replace the existing implementation.

I question the premise of the question on the Smac Planner, but that's not really the question you're asking here :-)

$\endgroup$
2
  • $\begingroup$ Thanks for your answer, Steve. I'll try using ros2 service as you suggested. However, I am still facing issues with the nav2 SmacHybridPlanner which seems to ignore my robot's footprint : youtube.com/watch?v=epyFhfQIlfQ.I really wish I could solve my problem with just pure nav2 I've also included the nav2_param.yaml file in my question if you would like to take a look, or if it's not related to this question you can reply to my previous question : robotics.stackexchange.com/questions/109722/…. Thanks again. $\endgroup$
    – FunkFang
    May 15 at 16:38
  • $\begingroup$ Your inflation looks very suspicious $\endgroup$ May 15 at 19:22

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.