26#include <ament_index_cpp/get_package_share_directory.hpp>
37using namespace std::chrono_literals;
39using ::cl_nav2z::Pose;
40using ::cl_nav2z::GoalCheckerSwitcher;
41using ::cl_nav2z::odom_tracker::OdomTracker;
42using ::cl_nav2z::CpSlamToolbox;
43using ::cl_nav2z::WaypointNavigator;
44using ::cl_nav2z::ClNav2Z;
45using ::cl_nav2z::PlannerSwitcher;
52 auto movebaseClient = this->createClient<ClNav2Z>();
55 movebaseClient->createComponent<
Pose>();
74 waypointsNavigator->currentWaypoint_ = 0;
79 RCLCPP_INFO(
getLogger(),
"OrNavigation loading parameters file");
81 std::string planfilepath;
82 getNode()->declare_parameter(
"waypoints_plan", planfilepath);
83 if (
getNode()->get_parameter(
"waypoints_plan", planfilepath))
85 std::string package_share_directory =
86 ament_index_cpp::get_package_share_directory(
"sm_husky_barrel_search_1");
87 boost::replace_all(planfilepath,
"$(pkg_share)", package_share_directory);
89 RCLCPP_INFO(
getLogger(),
"waypoints plan: %s", planfilepath.c_str());
93 RCLCPP_ERROR(
getLogger(),
"waypoints plan file not found: NONE");
void loadWayPointsFromFile(std::string filepath)
void loadWaypointsFromYaml(WaypointNavigator *waypointsNavigator)
void onInitialize() override
rclcpp::Logger getLogger()
rclcpp::Node::SharedPtr getNode()