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_adapter2.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 GlobalPlannerAdapter2::GlobalPlannerAdapter2() :
00048 planner_loader_("nav_core", "nav_core::BaseGlobalPlanner")
00049 {
00050 }
00051
00055 void GlobalPlannerAdapter2::initialize(const ros::NodeHandle& parent, const std::string& name,
00056 TFListenerPtr tf, nav_core2::Costmap::Ptr costmap)
00057 {
00058 costmap_ = costmap;
00059 std::shared_ptr<CostmapAdapter> ptr = std::static_pointer_cast<CostmapAdapter>(costmap);
00060
00061 if (!ptr)
00062 {
00063 ROS_FATAL_NAMED("GlobalPlannerAdapter2",
00064 "GlobalPlannerAdapter2 can only be used with the CostmapAdapter, not other Costmaps!");
00065 exit(EXIT_FAILURE);
00066 }
00067 costmap_ros_ = ptr->getCostmap2DROS();
00068
00069 ros::NodeHandle planner_nh(parent, name);
00070 std::string planner_name;
00071 planner_nh.param("planner_name", planner_name, std::string("global_planner/GlobalPlanner"));
00072 ROS_INFO_NAMED("GlobalPlannerAdapter2", "Loading plugin %s", planner_name.c_str());
00073 planner_ = planner_loader_.createInstance(planner_name);
00074 planner_->initialize(planner_loader_.getName(planner_name), costmap_ros_);
00075 }
00076
00077 nav_2d_msgs::Path2D GlobalPlannerAdapter2::makePlan(const nav_2d_msgs::Pose2DStamped& start,
00078 const nav_2d_msgs::Pose2DStamped& goal)
00079 {
00080 geometry_msgs::PoseStamped start3d = nav_2d_utils::pose2DToPoseStamped(start),
00081 goal3d = nav_2d_utils::pose2DToPoseStamped(goal);
00082 std::vector<geometry_msgs::PoseStamped> plan;
00083 bool ret = planner_->makePlan(start3d, goal3d, plan);
00084 if (!ret)
00085 {
00086 throw nav_core2::PlannerException("Generic Global Planner Error");
00087 }
00088 return nav_2d_utils::posesToPath2D(plan);
00089 }
00090 }
00091
00092
00093
00094 PLUGINLIB_EXPORT_CLASS(nav_core_adapter::GlobalPlannerAdapter2, nav_core2::GlobalPlanner)