26#include <ament_index_cpp/get_package_share_directory.hpp>
39using namespace std::chrono_literals;
41using ::cl_nav2z::Pose;
42using ::cl_nav2z::GoalCheckerSwitcher;
43using ::cl_nav2z::odom_tracker::OdomTracker;
44using ::cl_nav2z::CpSlamToolbox;
45using cl_nav2z::CpSquareShapeBoundary;
52 auto movebaseClient = this->createClient<ClNav2Z>();
55 movebaseClient->createComponent<Pose>();
58 movebaseClient->createComponent<PlannerSwitcher>();
61 movebaseClient->createComponent<GoalCheckerSwitcher>();
64 movebaseClient->createComponent<OdomTracker>();
67 movebaseClient->createComponent<CpSlamToolbox>();
70 auto waypointsNavigator = movebaseClient->createComponent<WaypointNavigator>();
74 waypointsNavigator->currentWaypoint_ = 0;
82 std::string planfilepath;
83 getNode()->declare_parameter(
"waypoints_plan", planfilepath);
84 if (
getNode()->get_parameter(
"waypoints_plan", planfilepath))
86 std::string package_share_directory =
87 ament_index_cpp::get_package_share_directory(
"sm_dance_bot_warehouse_3");
88 boost::replace_all(planfilepath,
"$(pkg_share)", package_share_directory);
89 waypointsNavigator->loadWayPointsFromFile(planfilepath);
90 RCLCPP_INFO(
getLogger(),
"waypoints plan: %s", planfilepath.c_str());
94 RCLCPP_ERROR(
getLogger(),
"waypoints plan file not found: NONE");
void loadWaypointsFromYaml(WaypointNavigator *waypointsNavigator)
void onInitialize() override
rclcpp::Logger getLogger()
rclcpp::Node::SharedPtr getNode()