25#include <ament_index_cpp/get_package_share_directory.hpp>
36using namespace std::chrono_literals;
43 auto movebaseClient = this->createClient<ClNav2Z>();
65 waypointsNavigator->currentWaypoint_ = 0;
71 std::string planfilepath;
73 RCLCPP_INFO_STREAM(
getLogger(),
"Reasing parameter file from node: " <<
getNode()->get_name());
74 getNode()->declare_parameter<std::string>(
"waypoints_plan",
"");
75 if (
getNode()->get_parameter(
"waypoints_plan", planfilepath))
77 std::string package_share_directory =
78 ament_index_cpp::get_package_share_directory(
"sm_dance_bot_strikes_back");
79 boost::replace_all(planfilepath,
"$(pkg_share)", package_share_directory);
81 RCLCPP_INFO(
getLogger(),
"waypoints plan: %s", planfilepath.c_str());
85 RCLCPP_ERROR(
getLogger(),
"waypoints plan file not found: NONE");
void loadWayPointsFromFile(std::string filepath)
void onInitialize() override
void loadWaypointsFromYaml(WaypointNavigator *waypointsNavigator)
rclcpp::Logger getLogger()
rclcpp::Node::SharedPtr getNode()