移动机器人——自定义规划算法

此处自定义global_planner，local_planner流程一样但继承nav_core::BaseLocalPlanner

继承nav_core::BaseGlobalPlanner编写插件

• 需重写initialize，makePlan，footprintCost
• 读入参数的命名空间为/move_base/RobotGlobalPlanner/，通过move_base加载参数时默认加入move_base命名空间，只需要在yaml文件中设置RobotGlobalPlanner命名空间即可

namespace robot_planner
{
class RobotGlobalPlanner : public nav_core::BaseGlobalPlanner
{
public:
  RobotGlobalPlanner();
  RobotGlobalPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros);

  /**
   * @brief 初始化地图
   *
   * @param name
   * @param costmap_ros
   */
  void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);

  /**
   * @brief 规划
   *
   * @param start
   * @param goal
   * @param plan
   * @return true
   * @return false
   */
  bool makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
                std::vector<geometry_msgs::PoseStamped>& plan);

private:
  costmap_2d::Costmap2DROS* costmap_ros_;
  double step_size_, min_dist_from_robot_;
  costmap_2d::Costmap2D* costmap_;
  base_local_planner::WorldModel* world_model_;
  bool initialized_;
  std::vector<geometry_msgs::Point> footprint_spec_;

  /**
   * @brief 在特定位姿下，计算机器人模型是否与障碍物碰撞
   *
   * @param x_i
   * @param y_i
   * @param theta_i
   * @return double
   */
  double footprintCost(double x_i, double y_i, double theta_i);
};
};  // namespace robot_planner

注册运动学插件并编译

• CMakeList.txt

cmake_minimum_required(VERSION 3.0.2)
project(robot_planner)

## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++11)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  tf2_ros
  costmap_2d
  base_local_planner
  dynamic_reconfigure
  nav_core
  navfn
  pluginlib
  angles
  tf2_geometry_msgs
  geometry_msgs
  nav_msgs
)

###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
  INCLUDE_DIRS include
  LIBRARIES ${PROJECT_NAME}
  CATKIN_DEPENDS 
    roscpp
    rospy
    tf2_ros
    costmap_2d
    base_local_planner
    dynamic_reconfigure
    nav_core
    navfn
    pluginlib
    angles
    tf2_geometry_msgs
    geometry_msgs
    nav_msgs
#  DEPENDS system_lib
)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
  include
  ${catkin_INCLUDE_DIRS}
)

#############
## Install ##
#############

add_library(${PROJECT_NAME}
src/planner_core.cpp
src/a_star_planner.cpp)
add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(${PROJECT_NAME}
    ${catkin_LIBRARIES})

## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
install(TARGETS ${PROJECT_NAME}
  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
)

## Mark cpp header files for installation
install(DIRECTORY include/${PROJECT_NAME}/
  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
  FILES_MATCHING PATTERN "*.h"
  PATTERN ".svn" EXCLUDE
)

## Mark other files for installation (e.g. launch and bag files, etc.)
install(FILES rgp_plugin.xml
  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

• rgp_plugin.xml

<library path="lib/librobot_planner">
  <class name="robot_planner/RobotGlobalPlanner" type="robot_planner::RobotGlobalPlanner" base_class_type="nav_core::BaseGlobalPlanner">
    <description>
      A modify A*
    description>
  class>
library>

• package.xml

<build_depend>nav_msgsbuild_depend>
<exec_depend>nav_msgsexec_depend>


<export>
    <nav_core plugin="${prefix}/rgp_plugin.xml" />
export>

检测，需要先加载环境

rospack plugins --attrib=plugin nav_core

调用

<param name="base_global_planner" value="robot_planner/RobotGlobalPlanner" />