Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include <nav_core_adapter/global_planner_adapter.h>
00036 #include <nav_core_adapter/costmap_adapter.h>
00037 #include <nav_2d_utils/conversions.h>
00038 #include <nav_2d_utils/tf_help.h>
00039 #include <nav_core2/exceptions.h>
00040 #include <pluginlib/class_list_macros.h>
00041 #include <string>
00042 #include <vector>
00043
00044 namespace nav_core_adapter
00045 {
00046
00047 GlobalPlannerAdapter::GlobalPlannerAdapter() :
00048 planner_loader_("nav_core2", "nav_core2::GlobalPlanner")
00049 {
00050 }
00051
00055 void GlobalPlannerAdapter::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
00056 {
00057 costmap_ros_ = costmap_ros;
00058 costmap_adapter_ = std::make_shared<CostmapAdapter>();
00059 costmap_adapter_->initialize(costmap_ros);
00060
00061 ros::NodeHandle private_nh("~");
00062 ros::NodeHandle adapter_nh("~/" + name);
00063 std::string planner_name;
00064 adapter_nh.param("planner_name", planner_name, std::string("dlux_global_planner::DluxGlobalPlanner"));
00065 ROS_INFO_NAMED("GlobalPlannerAdapter", "Loading plugin %s", planner_name.c_str());
00066 planner_ = planner_loader_.createInstance(planner_name);
00067 planner_->initialize(private_nh, planner_loader_.getName(planner_name), tf_, costmap_adapter_);
00068 path_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
00069 }
00070
00071 bool GlobalPlannerAdapter::makePlan(const geometry_msgs::PoseStamped& start,
00072 const geometry_msgs::PoseStamped& goal,
00073 std::vector<geometry_msgs::PoseStamped>& plan)
00074 {
00075 nav_2d_msgs::Pose2DStamped start2d = nav_2d_utils::poseStampedToPose2D(start),
00076 goal2d = nav_2d_utils::poseStampedToPose2D(goal);
00077 try
00078 {
00079 nav_2d_msgs::Path2D path2d = planner_->makePlan(start2d, goal2d);
00080 nav_msgs::Path path = nav_2d_utils::pathToPath(path2d);
00081 plan = path.poses;
00082 path_pub_.publish(path);
00083 return true;
00084 }
00085 catch (nav_core2::PlannerException& e)
00086 {
00087 ROS_ERROR_NAMED("GlobalPlannerAdapter", "makePlan Exception: %s", e.what());
00088 return false;
00089 }
00090 }
00091 }
00092
00093
00094
00095 PLUGINLIB_EXPORT_CLASS(nav_core_adapter::GlobalPlannerAdapter, nav_core::BaseGlobalPlanner)