30#include <ament_index_cpp/get_package_share_directory.hpp>
41 auto roslaunchClient =this->createClient<smacc2::client_bases::ClRosLaunch>(
"sm_aws_warehouse_navigation",
"navigation_launch.py");
43 auto movebaseClient = this->createClient<cl_nav2z::ClNav2Z>();
46 movebaseClient->createComponent<
cl_nav2z::Pose>(StandardReferenceFrames::Map);
70 std::string planfilepath;
71 getNode()->declare_parameter(
"waypoints_plan_2", planfilepath);
72 if (
getNode()->get_parameter(
"waypoints_plan_2", planfilepath))
74 std::string package_share_directory =
75 ament_index_cpp::get_package_share_directory(
"sm_aws_warehouse_navigation");
76 boost::replace_all(planfilepath,
"$(pkg_share)", package_share_directory);
79 RCLCPP_INFO(
getLogger(),
"waypoints plan: %s", planfilepath.c_str());
83 RCLCPP_ERROR(
getLogger(),
"waypoints plan file not found: NONE");
void loadWayPointsFromFile2(std::string filepath)
void loadWaypointsFromYaml(WaypointNavigator *waypointsNavigator)
void onInitialize() override
rclcpp::Logger getLogger()
rclcpp::Node::SharedPtr getNode()