diff --git a/patch/ros-humble-nav2-waypoint-follower.patch b/patch/ros-humble-nav2-waypoint-follower.patch index b01e8d3d..bf36ad77 100644 --- a/patch/ros-humble-nav2-waypoint-follower.patch +++ b/patch/ros-humble-nav2-waypoint-follower.patch @@ -11,96 +11,3 @@ index a3b46942b..e228086fa 100644 # Try for OpenCV 4.X, but settle for whatever is installed find_package(OpenCV 4 QUIET) if(NOT OpenCV_FOUND) -@@ -25,7 +27,11 @@ find_package(pluginlib REQUIRED) - - nav2_package() - --link_libraries(stdc++fs) -+if(UNIX AND NOT APPLE) -+ link_libraries(stdc++fs) -+else() -+ -+endif() - - include_directories( - include -diff --git a/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp b/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp -index fc4aee5c5..a4fc67b82 100644 ---- a/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp -+++ b/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp -@@ -20,20 +20,22 @@ - * to ignore deprecated declarations - */ - #define _LIBCPP_NO_EXPERIMENTAL_DEPRECATION_WARNING_FILESYSTEM -+#define _SILENCE_EXPERIMENTAL_FILESYSTEM_DEPRECATION_WARNING - - --#include -+#include - #include - #include - #include - -+#include -+#include -+ - #include "rclcpp/rclcpp.hpp" - #include "rclcpp_components/register_node_macro.hpp" - - #include "sensor_msgs/msg/image.hpp" - #include "nav2_core/waypoint_task_executor.hpp" --#include "opencv4/opencv2/core.hpp" --#include "opencv4/opencv2/opencv.hpp" - #include "cv_bridge/cv_bridge.h" - #include "image_transport/image_transport.hpp" - -@@ -97,7 +99,7 @@ protected: - // to ensure safety when accessing global var curr_frame_ - std::mutex global_mutex_; - // the taken photos will be saved under this directory -- std::experimental::filesystem::path save_dir_; -+ std::filesystem::path save_dir_; - // .png ? .jpg ? or some other well known format - std::string image_format_; - // the topic to subscribe in order capture a frame -diff --git a/plugins/photo_at_waypoint.cpp b/plugins/photo_at_waypoint.cpp -index 8a6cb74b9..4e2858f65 100644 ---- a/plugins/photo_at_waypoint.cpp -+++ b/plugins/photo_at_waypoint.cpp -@@ -61,14 +61,14 @@ void PhotoAtWaypoint::initialize( - // get inputted save directory and make sure it exists, if not log and create it - save_dir_ = save_dir_as_string; - try { -- if (!std::experimental::filesystem::exists(save_dir_)) { -+ if (!std::filesystem::exists(save_dir_)) { - RCLCPP_WARN( - logger_, - "Provided save directory for photo at waypoint plugin does not exist," - "provided directory is: %s, the directory will be created automatically.", - save_dir_.c_str() - ); -- if (!std::experimental::filesystem::create_directory(save_dir_)) { -+ if (!std::filesystem::create_directory(save_dir_)) { - RCLCPP_ERROR( - logger_, - "Failed to create directory!: %s required by photo at waypoint plugin, " -@@ -110,16 +110,16 @@ bool PhotoAtWaypoint::processAtWaypoint( - } - try { - // construct the full path to image filename -- std::experimental::filesystem::path file_name = std::to_string( -+ std::filesystem::path file_name = std::to_string( - curr_waypoint_index) + "_" + - std::to_string(curr_pose.header.stamp.sec) + "." + image_format_; -- std::experimental::filesystem::path full_path_image_path = save_dir_ / file_name; -+ std::filesystem::path full_path_image_path = save_dir_ / file_name; - - // save the taken photo at this waypoint to given directory - std::lock_guard guard(global_mutex_); - cv::Mat curr_frame_mat; - deepCopyMsg2Mat(curr_frame_msg_, curr_frame_mat); -- cv::imwrite(full_path_image_path.c_str(), curr_frame_mat); -+ cv::imwrite(full_path_image_path.string().c_str(), curr_frame_mat); - RCLCPP_INFO( - logger_, - "Photo has been taken sucessfully at waypoint %i", curr_waypoint_index);