48 planner_loader_(
"nav_core",
"nav_core::BaseGlobalPlanner")
59 std::shared_ptr<CostmapAdapter> ptr = std::static_pointer_cast<
CostmapAdapter>(costmap);
64 "GlobalPlannerAdapter2 can only be used with the CostmapAdapter, not other Costmaps!");
70 std::string planner_name;
71 planner_nh.
param(
"planner_name", planner_name, std::string(
"global_planner/GlobalPlanner"));
72 ROS_INFO_NAMED(
"GlobalPlannerAdapter2",
"Loading plugin %s", planner_name.c_str());
78 const nav_2d_msgs::Pose2DStamped& goal)
82 std::vector<geometry_msgs::PoseStamped> plan;
83 bool ret =
planner_->makePlan(start3d, goal3d, plan);
#define ROS_INFO_NAMED(name,...)
costmap_2d::Costmap2DROS * costmap_ros_
used for employing a nav_core global planner (such as navfn) as a nav_core2 plugin, like in locomotor.
boost::shared_ptr< nav_core::BaseGlobalPlanner > planner_
pluginlib::ClassLoader< nav_core::BaseGlobalPlanner > planner_loader_
PLUGINLIB_EXPORT_CLASS(BAGReader, nodelet::Nodelet)
void initialize(const ros::NodeHandle &parent, const std::string &name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override
Load the nav_core global planner and initialize it.
nav_2d_msgs::Path2D posesToPath2D(const std::vector< geometry_msgs::PoseStamped > &poses)
geometry_msgs::PoseStamped pose2DToPoseStamped(const nav_2d_msgs::Pose2DStamped &pose2d)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
nav_core2::Costmap::Ptr costmap_
virtual std::string getName(const std::string &lookup_name)
nav_2d_msgs::Path2D makePlan(const nav_2d_msgs::Pose2DStamped &start, const nav_2d_msgs::Pose2DStamped &goal) override
#define ROS_FATAL_NAMED(name,...)
std::shared_ptr< Costmap > Ptr
std::shared_ptr< tf::TransformListener > TFListenerPtr