此处自定义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_msgs</build_depend>
<exec_depend>nav_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<nav_core plugin="${prefix}/rgp_plugin.xml" />
</export>
检测,需要先加载环境
rospack plugins --attrib=plugin nav_core
调用
<param name="base_global_planner" value="robot_planner/RobotGlobalPlanner" />
评论(1)
您还未登录,请登录后发表或查看评论