magazino / move_base_flex

Move Base Flex: a backwards-compatible replacement for move_base
https://uos.github.io/mbf_docs/
BSD 3-Clause "New" or "Revised" License
429 stars 154 forks source link

get_path-Action returns INVALID_PLUGIN-errorcode #226

Closed ML-S closed 3 years ago

ML-S commented 3 years ago

I'm trying to call the get_path-Action inside a SMACH. When calling the action, it returns the error code 59, which is according to the get_path-Actionserver the code for 'INVALID_PLUGIN'. My mbf-launchfile looks as follows:

<?xml version="1.0"?>

<launch>
  <!-- Move Base Flex -->
  <master auto="start"/>
  <node pkg="rosservice" type="rosservice" name="set_move_base_log_level" args="call --wait /move_base_flex/set_logger_level 'ros.mbf_abstract_nav.actionlib' 'debug'" />
  <node pkg="mbf_costmap_nav" type="mbf_costmap_nav" respawn="false" name="move_base_flex" output="screen">
    <param name="tf_timeout" value="1.5"/>
    <param name="planner_max_retries" value="3"/>
    <rosparam file="$(find joggingblinde_2dnav)/config/base_global_planner_params.yaml" command="load" />
    <rosparam file="$(find joggingblinde_2dnav)/config/base_local_planner_params.yaml" command="load" />
    <rosparam file="$(find joggingblinde_2dnav)/config/costmap_common_params.yaml" command="load" ns="global_costmap"/>
    <rosparam file="$(find joggingblinde_2dnav)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find joggingblinde_2dnav)/config/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find joggingblinde_2dnav)/config/local_costmap_params.yaml" command="load" />
  </node>

</launch> 

The costmap_commmon_params.yaml looks as follows:

footprint: [[0.20, 0.24125], [0.20, 0.175], [0.65, 0.175], [0.65, -0.175], [0.20, -0.175], [0.20, -0.24125], [-0.20, -0.24125], [-0.20, -0.175], [-0.65, -0.175], [-0.65, 0.175], [-0.20, 0.175], [-0.20, 0.24125]] # jogging person at regular speed
# robot_radius: 0.5
inflation_radius: 0.1
# cost_scaling_factor:
transform_tolerance: 0.05

The global_costmap_params.yaml looks as follows:

global_costmap:
  plugins: 
    - {name: static_layer, type: "costmap_2d::StaticLayer"}
    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
  track_unknown_space: true
  global_frame: map #according to ros base_local_planner page it should be the same frame as the odometry runs in (/odom atm).
  robot_base_frame: base_link
  update_frequency: 10.0 # data comes in
  publish_frequency: 10.0 # costmap publishes info
  rolling_window: true
  recovery_behavior: false
  static_layer:
      trinary_costmap: false
      map_topic: "path_segmentation_occgrid"

And the base_global_planner_params.yaml looks as follows:

GlobalPlanner:
  #basic parameters
  default_tolerance: 3.0  #default is 0.0
  use_dijkstra: true  #default is true
  old_navfn_behavior: false  #default is false
  use_quadratic: true  #default is true
  use_grid_path: false  #default is false
  allow_unknown: true  #default is true
  lethal_cost: 253  #default is 253
  neutral_cost: 50  #default is 50
  cost_factor: 3  #default is 3

  #debug parameters
  publish_potential: True

All plugins worked with regular move_base. All plugins were loaded successfully without error when calling the mbf-launchfile. Has somebody any idea what could be the issue?

spuetz commented 3 years ago

INVALID_PLUGIN means that you specify a wrong plugin in the action goal message or if that argument is empty, you did not specify any planners to load at all at program start. You need to specify which plugins to load: For exmaple in another yaml config file which needs to be loaded into the namespace of mbf_costmap_nav.

planners:
  - name: 'global_planner'
    type: 'global_planner/GlobalPlanner'

Next you can and should specify the planner in the action goal message planner="global_planner". This allows you to call different loaded planners at runtime. MBF lists the loaded plugins at program start. Note that the name can be arbitrary, but it must be same in the action goal message and the config yaml.

ML-S commented 3 years ago

That solved it, thank you!