48 planner_loader_(
"nav_core2",
"nav_core2::GlobalPlanner")
63 std::string planner_name;
64 adapter_nh.
param(
"planner_name", planner_name, std::string(
"dlux_global_planner::DluxGlobalPlanner"));
65 ROS_INFO_NAMED(
"GlobalPlannerAdapter",
"Loading plugin %s", planner_name.c_str());
72 const geometry_msgs::PoseStamped& goal,
73 std::vector<geometry_msgs::PoseStamped>& plan)
79 nav_2d_msgs::Path2D path2d =
planner_->makePlan(start2d, goal2d);
87 ROS_ERROR_NAMED(
"GlobalPlannerAdapter",
"makePlan Exception: %s", e.what());
#define ROS_INFO_NAMED(name,...)
void initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros) override
Load the nav_core2 global planner and initialize it.
void publish(const boost::shared_ptr< M > &message) const
boost::shared_ptr< nav_core2::GlobalPlanner > planner_
std::shared_ptr< CostmapAdapter > costmap_adapter_
PLUGINLIB_EXPORT_CLASS(BAGReader, nodelet::Nodelet)
nav_2d_msgs::Path2D pathToPath(const nav_msgs::Path &path)
pluginlib::ClassLoader< nav_core2::GlobalPlanner > planner_loader_
bool makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, std::vector< geometry_msgs::PoseStamped > &plan) override
nav_2d_msgs::Pose2DStamped poseStampedToPose2D(const geometry_msgs::PoseStamped &pose)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
virtual std::string getName(const std::string &lookup_name)
costmap_2d::Costmap2DROS * costmap_ros_
#define ROS_ERROR_NAMED(name,...)
used for employing a nav_core2 global planner interface as a nav_core plugin, like in move_base...