diff --git a/include/dock_planner/dock_planner.h b/include/dock_planner/dock_planner.h index 45fab50..1dc87be 100644 --- a/include/dock_planner/dock_planner.h +++ b/include/dock_planner/dock_planner.h @@ -19,6 +19,8 @@ namespace dock_planner { const robot_geometry_msgs::PoseStamped& goal, std::vector& plan); + static robot_nav_core::BaseGlobalPlanner::Ptr create(); + private: // Core components robot_costmap_2d::Costmap2DROBOT* costmap_robot_; @@ -33,9 +35,6 @@ namespace dock_planner { bool check_free_space_; DockCalc calc_plan_to_dock_; - - - void publishPlan(const std::vector& plan); }; } // namespace dock_planner diff --git a/src/dock_planner.cpp b/src/dock_planner.cpp index 56131dd..cb441e8 100644 --- a/src/dock_planner.cpp +++ b/src/dock_planner.cpp @@ -80,6 +80,7 @@ namespace dock_planner initialized_ = true; // ROS_WARN("[dock_planner] check_free_space_(%d)", check_free_space_); } + return true; } bool DockPlanner::makePlan(const robot_geometry_msgs::PoseStamped& start, @@ -109,7 +110,6 @@ namespace dock_planner pose.pose.orientation = planMoveToDock[i].orientation; plan.push_back(pose); } - publishPlan(plan); return true; } @@ -123,12 +123,12 @@ namespace dock_planner // } // Export factory function - static boost::shared_ptr dock_planner_plugin() { - return boost::make_shared(); + robot_nav_core::BaseGlobalPlanner::Ptr DockPlanner::create() { + return std::make_shared(); } } // namespace dock_planner // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias) -BOOST_DLL_ALIAS(dock_planner::dock_planner_plugin, DockPlanner) +BOOST_DLL_ALIAS(dock_planner::DockPlanner::create, DockPlanner)