From 9451abe9b016c225fa1ac6e1cb986d91a2914c0c Mon Sep 17 00:00:00 2001
From: jediofgever <fetulahatas1@gmail.com>
Date: Wed, 14 Oct 2020 09:16:35 +0800
Subject: [PATCH 1/7] nav2_way_point_follower; introduce photo at waypoint
 arrivals plugin

Signed-off-by: jediofgever <fetulahatas1@gmail.com>
---
 doc/parameters/param_list.md                  |  12 ++
 nav2_bringup/bringup/params/nav2_params.yaml  |  15 ++-
 nav2_waypoint_follower/CMakeLists.txt         |  33 ++++-
 .../nav2_waypoint_follower/plugins/common.hpp |  45 +++++++
 .../plugins/photo_at_waypoint.hpp             | 101 +++++++++++++++
 nav2_waypoint_follower/plugins.xml            |   8 +-
 nav2_waypoint_follower/plugins/common.cpp     |  54 ++++++++
 .../plugins/photo_at_waypoint.cpp             | 117 ++++++++++++++++++
 .../src/waypoint_follower.cpp                 |   2 +-
 9 files changed, 376 insertions(+), 11 deletions(-)
 create mode 100644 nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/common.hpp
 create mode 100644 nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp
 create mode 100644 nav2_waypoint_follower/plugins/common.cpp
 create mode 100644 nav2_waypoint_follower/plugins/photo_at_waypoint.cpp

diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md
index bf5e650a202..67a3ddebbab 100644
--- a/doc/parameters/param_list.md
+++ b/doc/parameters/param_list.md
@@ -541,6 +541,18 @@ When `planner_plugins` parameter is not overridden, the following default plugin
 | `<waypoint task executor>`.enabled | true | Whether it is enabled |
 | `<waypoint task executor>`.waypoint_pause_duration | 0 | Amount of time in milliseconds, for robot to sleep/wait after each waypoint is reached. If zero, robot will directly continue to next waypoint. |
 
+## PhotoAtWaypoint plugin
+
+* `<waypoint task executor>`: Name corresponding to the `nav2_waypoint_follower::PhotoAtWaypoint` plugin. 
+
+| Parameter | Default | Description |
+| ----------| --------| ------------|
+| `<waypoint task executor>`.enabled | true | Whether it is enabled |
+| `<waypoint task executor>`.camera_image_topic_name | "/camera/color/image_raw" | Camera image topic name to susbcribe |
+| `<waypoint task executor>`.save_images_dir | "/home/username/" | Path to directory to save taken photos |
+| `<waypoint task executor>`.image_format | ".png" | Desired image format |
+
+
 # recoveries
 
 ## recovery_server
diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml
index b6925e9f691..ec8fc822ad6 100644
--- a/nav2_bringup/bringup/params/nav2_params.yaml
+++ b/nav2_bringup/bringup/params/nav2_params.yaml
@@ -289,8 +289,17 @@ waypoint_follower:
   ros__parameters:
     loop_rate: 20
     stop_on_failure: false
-    waypoint_task_executor_plugin: "waypoint_task_executor"   
-    waypoint_task_executor:
+    # Choose a plugin from provided ones; 
+    # photo_at_waypoint or photo_at_waypoint ?
+    waypoint_task_executor_plugin: "photo_at_waypoint"   
+    wait_at_waypoint:
       plugin: "nav2_waypoint_follower::WaitAtWaypoint"
       enabled: True
-      waypoint_pause_duration: 0
+      waypoint_pause_duration: 200
+    photo_at_waypoint:
+      plugin: "nav2_waypoint_follower::PhotoAtWaypoint"
+      enabled: True
+      camera_image_topic_name: "/camera/color/image_raw"
+      save_images_dir: "/home/username/"
+      image_format: ".png"
+
diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt
index e9b5b5f280b..c8cb1b1d61e 100644
--- a/nav2_waypoint_follower/CMakeLists.txt
+++ b/nav2_waypoint_follower/CMakeLists.txt
@@ -1,6 +1,15 @@
 cmake_minimum_required(VERSION 3.5)
 project(nav2_waypoint_follower)
 
+# Try for OpenCV 4.X, but settle for whatever is installed
+find_package(OpenCV 4 QUIET)
+if (NOT OpenCV_FOUND)
+  find_package(OpenCV REQUIRED)
+endif ()
+message(STATUS "Found OpenCV version ${OpenCV_VERSION}")
+
+find_package(image_transport REQUIRED)
+find_package(cv_bridge REQUIRED)
 find_package(ament_cmake REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(rclcpp REQUIRED)
@@ -41,6 +50,9 @@ set(dependencies
   tf2_ros
   nav2_core
   pluginlib
+  image_transport
+  cv_bridge
+  OpenCV
 )
 
 ament_target_dependencies(${executable_name}
@@ -53,12 +65,20 @@ ament_target_dependencies(${library_name}
   ${dependencies}
 )
 
-add_library(simple_waypoint_task_executor SHARED plugins/wait_at_waypoint.cpp)
-ament_target_dependencies(simple_waypoint_task_executor ${dependencies})
-# prevent pluginlib from using boost
-target_compile_definitions(simple_waypoint_task_executor PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
+add_library(simple_wait_at_waypoint SHARED plugins/wait_at_waypoint.cpp)
+ament_target_dependencies(simple_wait_at_waypoint ${dependencies})
+#prevent pluginlib from using boost
+target_compile_definitions(simple_wait_at_waypoint PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
 
-install(TARGETS ${library_name} simple_waypoint_task_executor
+
+add_library(simple_photo_at_waypoint SHARED plugins/common.cpp 
+                                            plugins/photo_at_waypoint.cpp)
+ament_target_dependencies(simple_photo_at_waypoint ${dependencies})
+#prevent pluginlib from using boost
+target_compile_definitions(simple_photo_at_waypoint PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
+
+
+install(TARGETS ${library_name} simple_wait_at_waypoint simple_photo_at_waypoint 
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
@@ -78,7 +98,8 @@ if(BUILD_TESTING)
 endif()
 
 ament_export_include_directories(include)
-ament_export_libraries(simple_waypoint_task_executor)
+ament_export_libraries(simple_wait_at_waypoint)
+ament_export_libraries(simple_photo_at_waypoint)
 
 pluginlib_export_plugin_description_file(nav2_waypoint_follower plugins.xml)
 
diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/common.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/common.hpp
new file mode 100644
index 00000000000..a7baf7cfa95
--- /dev/null
+++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/common.hpp
@@ -0,0 +1,45 @@
+// Copyright (c) 2020 Fetullah Atas
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_WAYPOINT_FOLLOWER__PLUGINS__COMMON_HPP_
+#define NAV2_WAYPOINT_FOLLOWER__PLUGINS__COMMON_HPP_
+
+#include <string>
+
+#include "opencv2/opencv.hpp"
+#include "builtin_interfaces/msg/time.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+
+namespace nav2_waypoint_follower
+{
+
+/**
+ * @brief given encoding string , determine corresponding CV format
+ *
+ * @param encoding
+ * @return int
+ */
+int encoding2mat_type(const std::string & encoding);
+
+/**
+ * @brief given CV type encoding return corresponding sensor_msgs::msg::Image::Encoding string
+ *
+ * @param mat_type
+ * @return std::string
+ */
+std::string mat_type2encoding(int mat_type);
+
+}  // namespace nav2_waypoint_follower
+#endif  // NAV2_WAYPOINT_FOLLOWER__PLUGINS__COMMON_HPP_
diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp
new file mode 100644
index 00000000000..5b3997e7956
--- /dev/null
+++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp
@@ -0,0 +1,101 @@
+// Copyright (c) 2020 Fetullah Atas
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_WAYPOINT_FOLLOWER__PLUGINS__PHOTO_AT_WAYPOINT_HPP_
+#define NAV2_WAYPOINT_FOLLOWER__PLUGINS__PHOTO_AT_WAYPOINT_HPP_
+
+#include <mutex>
+#include <string>
+#include <exception>
+
+#include "nav2_waypoint_follower/plugins/common.hpp"
+
+#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 "cv_bridge/cv_bridge.h"
+#include "image_transport/image_transport.hpp"
+
+
+namespace nav2_waypoint_follower
+{
+
+class PhotoAtWaypoint : public nav2_core::WaypointTaskExecutor
+{
+public:
+  /**
+  * @brief Construct a new Photo At Waypoint object
+  *
+  */
+  PhotoAtWaypoint();
+
+  /**
+   * @brief Destroy the Photo At Waypoint object
+   *
+   */
+  ~PhotoAtWaypoint();
+
+  /**
+   * @brief declares and loads parameters used
+   *
+   * @param parent parent node that plugin will be created within
+   * @param plugin_name should be provided in nav2_params.yaml==> waypoint_follower
+   */
+  void initialize(
+    const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+    const std::string & plugin_name);
+
+
+  /**
+   * @brief Override this to define the body of your task that you would like to execute once the robot arrived to waypoint
+   *
+   * @param curr_pose current pose of the robot
+   * @param curr_waypoint_index current waypoint, that robot just arrived
+   * @return true if task execution was successful
+   * @return false if task execution failed
+   */
+  bool processAtWaypoint(
+    const geometry_msgs::msg::PoseStamped & curr_pose, const int & curr_waypoint_index);
+
+  /**
+   * @brief
+   *
+   * @param msg
+   */
+  void imageCallback(const sensor_msgs::msg::Image::SharedPtr msg);
+
+protected:
+  // to ensure safety when accessing global var curr_frame_
+  std::mutex global_mutex_;
+  // the taken photos will be saved under this directory
+  std::string directory_to_save_images_;
+  // .png ? .jpg ? or some other well known format
+  std::string image_format_;
+  // the topic to subscribe in order capture a frame
+  std::string camera_image_topic_name_;
+  // whether plugin is enabled
+  bool is_enabled_;
+  // current frame;
+  cv::Mat curr_frame_;
+  // global logger
+  rclcpp::Logger logger_{rclcpp::get_logger("nav2_waypoint_follower")};
+  // ros susbcriber to get camera image
+  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr camera_image_subscriber_;
+};
+}  // namespace nav2_waypoint_follower
+
+#endif  // NAV2_WAYPOINT_FOLLOWER__PLUGINS__PHOTO_AT_WAYPOINT_HPP_
diff --git a/nav2_waypoint_follower/plugins.xml b/nav2_waypoint_follower/plugins.xml
index aaf7cd191c6..db4e73de606 100644
--- a/nav2_waypoint_follower/plugins.xml
+++ b/nav2_waypoint_follower/plugins.xml
@@ -1,7 +1,13 @@
 <class_libraries>
-  <library path="simple_waypoint_task_executor">
+  <library path="simple_wait_at_waypoint">
     <class type="nav2_waypoint_follower::WaitAtWaypoint" base_class_type="nav2_core::WaypointTaskExecutor">
       <description>Lets robot sleep for a specified amount of time at waypoint arrival</description>
     </class>
   </library>
+  <library path="simple_photo_at_waypoint">
+    <class type="nav2_waypoint_follower::PhotoAtWaypoint" base_class_type="nav2_core::WaypointTaskExecutor">
+      <description>Run-time plugin that takes photos at waypoint arrivals when using waypoint follower node. 
+        Saves the taken photos to specified directory.</description>
+    </class>
+  </library>
 </class_libraries>
\ No newline at end of file
diff --git a/nav2_waypoint_follower/plugins/common.cpp b/nav2_waypoint_follower/plugins/common.cpp
new file mode 100644
index 00000000000..7b88d55be01
--- /dev/null
+++ b/nav2_waypoint_follower/plugins/common.cpp
@@ -0,0 +1,54 @@
+// Copyright (c) 2020 Fetullah Atas
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <string>
+#include "nav2_waypoint_follower/plugins/common.hpp"
+
+namespace nav2_waypoint_follower
+{
+
+int encoding2mat_type(const std::string & encoding)
+{
+  if (encoding == "mono8") {
+    return CV_8UC1;
+  } else if (encoding == "bgr8") {
+    return CV_8UC3;
+  } else if (encoding == "mono16") {
+    return CV_16SC1;
+  } else if (encoding == "rgba8") {
+    return CV_8UC4;
+  } else if (encoding == "rgb8") {
+    return CV_8UC3;
+  } else {
+    throw std::runtime_error("Unsupported mat type");
+  }
+}
+
+std::string mat_type2encoding(int mat_type)
+{
+  switch (mat_type) {
+    case CV_8UC1:
+      return "mono8";
+    case CV_8UC3:
+      return "bgr8";
+    case CV_16SC1:
+      return "mono16";
+    case CV_8UC4:
+      return "rgba8";
+    default:
+      throw std::runtime_error("Unsupported encoding type");
+  }
+}
+
+}  // namespace nav2_waypoint_follower
diff --git a/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp b/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp
new file mode 100644
index 00000000000..76c4cdbc924
--- /dev/null
+++ b/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp
@@ -0,0 +1,117 @@
+// Copyright (c) 2020 Fetullah Atas
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_waypoint_follower/plugins/photo_at_waypoint.hpp"
+#include <pluginlib/class_list_macros.hpp>
+
+#include <string>
+
+
+namespace nav2_waypoint_follower
+{
+PhotoAtWaypoint::PhotoAtWaypoint()
+{
+}
+
+PhotoAtWaypoint::~PhotoAtWaypoint()
+{
+}
+
+void PhotoAtWaypoint::initialize(
+  const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+  const std::string & plugin_name)
+{
+  auto node = parent.lock();
+
+  node->declare_parameter(plugin_name + ".enabled", rclcpp::ParameterValue(true));
+  node->declare_parameter(
+    plugin_name + ".camera_image_topic_name",
+    rclcpp::ParameterValue("/fake_realsense_camera/image_raw"));
+  node->declare_parameter(plugin_name + ".save_images_dir", rclcpp::ParameterValue("/home/atas/"));
+  node->declare_parameter(plugin_name + ".image_format", rclcpp::ParameterValue(".png"));
+
+  node->get_parameter(plugin_name + ".enabled", is_enabled_);
+  node->get_parameter(plugin_name + ".camera_image_topic_name", camera_image_topic_name_);
+  node->get_parameter(plugin_name + ".save_images_dir", directory_to_save_images_);
+  node->get_parameter(plugin_name + ".image_format", image_format_);
+
+  auto qos = rclcpp::QoS(
+    rclcpp::QoSInitialization(
+      rmw_qos_profile_default.history,
+      rmw_qos_profile_default.depth
+  ));
+  qos.reliability(rmw_qos_profile_default.reliability);
+
+  if (!is_enabled_) {
+    RCLCPP_INFO(
+      logger_, "Waypoint task executor plugin is disabled.");
+  } else {
+    RCLCPP_INFO(
+      logger_, "Initializing photo at waypoint plugin, subscribing to camera topic named; %s",
+      camera_image_topic_name_.c_str());
+    camera_image_subscriber_ = node->create_subscription<sensor_msgs::msg::Image>(
+      camera_image_topic_name_, qos,
+      std::bind(&PhotoAtWaypoint::imageCallback, this, std::placeholders::_1));
+  }
+}
+
+bool PhotoAtWaypoint::processAtWaypoint(
+  const geometry_msgs::msg::PoseStamped & curr_pose, const int & curr_waypoint_index)
+{
+  if (!is_enabled_) {
+    RCLCPP_WARN(
+      logger_,
+      "Photo at waypoint plugin is disabled. Not performing anything"
+    );
+    return true;
+  }
+  try {
+    global_mutex_.lock();
+    std::string stamped_name_for_curr_frame = directory_to_save_images_ + std::to_string(
+      curr_waypoint_index) + "_" +
+      std::to_string(curr_pose.header.stamp.sec) + image_format_;
+    cv::imwrite(stamped_name_for_curr_frame, curr_frame_);
+    RCLCPP_INFO(
+      logger_,
+      "Photo has been taken sucessfully at waypoint %i", curr_waypoint_index);
+    global_mutex_.unlock();
+  } catch (...) {
+    RCLCPP_ERROR(
+      logger_,
+      "Photo at waypoint plugin caught an exception while trying taking a photo at waypoint %i.",
+      curr_waypoint_index);
+    RCLCPP_ERROR(
+      logger_,
+      "Not going to take a photo!, Make sure that the image topic named: %s is valid and active!",
+      camera_image_topic_name_.c_str());
+    return false;
+  }
+  return true;
+}
+
+void PhotoAtWaypoint::imageCallback(const sensor_msgs::msg::Image::SharedPtr msg)
+{
+  cv_bridge::CvImageConstPtr cv_bridge_ptr = cv_bridge::toCvShare(msg, msg->encoding);
+  cv::Mat frame = cv_bridge_ptr->image;
+  if (msg->encoding == "rgb8") {
+    cv::cvtColor(frame, frame, cv::COLOR_RGB2BGR);
+  }
+  global_mutex_.lock();
+  frame.copyTo(curr_frame_);
+  global_mutex_.unlock();
+}
+}      // namespace nav2_waypoint_follower
+PLUGINLIB_EXPORT_CLASS(
+  nav2_waypoint_follower::PhotoAtWaypoint,
+  nav2_core::WaypointTaskExecutor)
diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp
index 5c0c8955b02..355ae77df37 100644
--- a/nav2_waypoint_follower/src/waypoint_follower.cpp
+++ b/nav2_waypoint_follower/src/waypoint_follower.cpp
@@ -35,7 +35,7 @@ WaypointFollower::WaypointFollower()
   declare_parameter("loop_rate", 20);
   nav2_util::declare_parameter_if_not_declared(
     this, std::string("waypoint_task_executor_plugin"),
-    rclcpp::ParameterValue(std::string("waypoint_task_executor")));
+    rclcpp::ParameterValue(std::string("photo_at_waypoint")));
   nav2_util::declare_parameter_if_not_declared(
     this, std::string("waypoint_task_executor_plugin.plugin"),
     rclcpp::ParameterValue(std::string("nav2_waypoint_follower::WaitAtWaypoint")));

From 43c0e86aa50a388f9840e2fa5af4c984870fbf99 Mon Sep 17 00:00:00 2001
From: jediofgever <fetulahatas1@gmail.com>
Date: Wed, 14 Oct 2020 11:31:19 +0800
Subject: [PATCH 2/7] resolve cmake lint errors

Signed-off-by: jediofgever <fetulahatas1@gmail.com>
---
 nav2_waypoint_follower/CMakeLists.txt | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt
index c8cb1b1d61e..97857321a09 100644
--- a/nav2_waypoint_follower/CMakeLists.txt
+++ b/nav2_waypoint_follower/CMakeLists.txt
@@ -3,9 +3,9 @@ project(nav2_waypoint_follower)
 
 # Try for OpenCV 4.X, but settle for whatever is installed
 find_package(OpenCV 4 QUIET)
-if (NOT OpenCV_FOUND)
+if(NOT OpenCV_FOUND)
   find_package(OpenCV REQUIRED)
-endif ()
+endif()
 message(STATUS "Found OpenCV version ${OpenCV_VERSION}")
 
 find_package(image_transport REQUIRED)
@@ -71,14 +71,14 @@ ament_target_dependencies(simple_wait_at_waypoint ${dependencies})
 target_compile_definitions(simple_wait_at_waypoint PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
 
 
-add_library(simple_photo_at_waypoint SHARED plugins/common.cpp 
+add_library(simple_photo_at_waypoint SHARED plugins/common.cpp
                                             plugins/photo_at_waypoint.cpp)
 ament_target_dependencies(simple_photo_at_waypoint ${dependencies})
 #prevent pluginlib from using boost
 target_compile_definitions(simple_photo_at_waypoint PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
 
 
-install(TARGETS ${library_name} simple_wait_at_waypoint simple_photo_at_waypoint 
+install(TARGETS ${library_name} simple_wait_at_waypoint simple_photo_at_waypoint
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin

From 57da3ae64b3da9add239b8539366d5b5e1609ff4 Mon Sep 17 00:00:00 2001
From: jediofgever <fetulahatas1@gmail.com>
Date: Thu, 15 Oct 2020 11:13:39 +0800
Subject: [PATCH 3/7] resolve requested chages of first review

Signed-off-by: jediofgever <fetulahatas1@gmail.com>
---
 doc/parameters/param_list.md                  |   6 +-
 nav2_bringup/bringup/params/nav2_params.yaml  |  11 +-
 nav2_waypoint_follower/CMakeLists.txt         |   7 +-
 .../nav2_waypoint_follower/plugins/common.hpp |  45 --------
 .../plugins/photo_at_waypoint.hpp             |  33 +++++-
 nav2_waypoint_follower/plugins/common.cpp     |  54 ----------
 .../plugins/photo_at_waypoint.cpp             | 100 +++++++++++++-----
 .../src/waypoint_follower.cpp                 |   2 +-
 8 files changed, 109 insertions(+), 149 deletions(-)
 delete mode 100644 nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/common.hpp
 delete mode 100644 nav2_waypoint_follower/plugins/common.cpp

diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md
index 67a3ddebbab..c587724f593 100644
--- a/doc/parameters/param_list.md
+++ b/doc/parameters/param_list.md
@@ -548,9 +548,9 @@ When `planner_plugins` parameter is not overridden, the following default plugin
 | Parameter | Default | Description |
 | ----------| --------| ------------|
 | `<waypoint task executor>`.enabled | true | Whether it is enabled |
-| `<waypoint task executor>`.camera_image_topic_name | "/camera/color/image_raw" | Camera image topic name to susbcribe |
-| `<waypoint task executor>`.save_images_dir | "/home/username/" | Path to directory to save taken photos |
-| `<waypoint task executor>`.image_format | ".png" | Desired image format |
+| `<waypoint task executor>`.image_topic | "/camera/color/image_raw" | Camera image topic name to susbcribe |
+| `<waypoint task executor>`.save_dir | "/home/username/" | Path to directory to save taken photos |
+| `<waypoint task executor>`.image_format | ".png" | Desired image format A few other options; ".jpeg" , ".jpg", ".pgm", ".tiff" |
 
 
 # recoveries
diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml
index ec8fc822ad6..3c60d0ae4fb 100644
--- a/nav2_bringup/bringup/params/nav2_params.yaml
+++ b/nav2_bringup/bringup/params/nav2_params.yaml
@@ -289,17 +289,8 @@ waypoint_follower:
   ros__parameters:
     loop_rate: 20
     stop_on_failure: false
-    # Choose a plugin from provided ones; 
-    # photo_at_waypoint or photo_at_waypoint ?
-    waypoint_task_executor_plugin: "photo_at_waypoint"   
+    waypoint_task_executor_plugin: "wait_at_waypoint"   
     wait_at_waypoint:
       plugin: "nav2_waypoint_follower::WaitAtWaypoint"
       enabled: True
       waypoint_pause_duration: 200
-    photo_at_waypoint:
-      plugin: "nav2_waypoint_follower::PhotoAtWaypoint"
-      enabled: True
-      camera_image_topic_name: "/camera/color/image_raw"
-      save_images_dir: "/home/username/"
-      image_format: ".png"
-
diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt
index 97857321a09..a4cb96073f6 100644
--- a/nav2_waypoint_follower/CMakeLists.txt
+++ b/nav2_waypoint_follower/CMakeLists.txt
@@ -70,9 +70,7 @@ ament_target_dependencies(simple_wait_at_waypoint ${dependencies})
 #prevent pluginlib from using boost
 target_compile_definitions(simple_wait_at_waypoint PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
 
-
-add_library(simple_photo_at_waypoint SHARED plugins/common.cpp
-                                            plugins/photo_at_waypoint.cpp)
+add_library(simple_photo_at_waypoint SHARED plugins/photo_at_waypoint.cpp)
 ament_target_dependencies(simple_photo_at_waypoint ${dependencies})
 #prevent pluginlib from using boost
 target_compile_definitions(simple_photo_at_waypoint PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
@@ -98,8 +96,7 @@ if(BUILD_TESTING)
 endif()
 
 ament_export_include_directories(include)
-ament_export_libraries(simple_wait_at_waypoint)
-ament_export_libraries(simple_photo_at_waypoint)
+ament_export_libraries(simple_wait_at_waypoint simple_photo_at_waypoint)
 
 pluginlib_export_plugin_description_file(nav2_waypoint_follower plugins.xml)
 
diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/common.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/common.hpp
deleted file mode 100644
index a7baf7cfa95..00000000000
--- a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/common.hpp
+++ /dev/null
@@ -1,45 +0,0 @@
-// Copyright (c) 2020 Fetullah Atas
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#ifndef NAV2_WAYPOINT_FOLLOWER__PLUGINS__COMMON_HPP_
-#define NAV2_WAYPOINT_FOLLOWER__PLUGINS__COMMON_HPP_
-
-#include <string>
-
-#include "opencv2/opencv.hpp"
-#include "builtin_interfaces/msg/time.hpp"
-#include "rclcpp/rclcpp.hpp"
-
-
-namespace nav2_waypoint_follower
-{
-
-/**
- * @brief given encoding string , determine corresponding CV format
- *
- * @param encoding
- * @return int
- */
-int encoding2mat_type(const std::string & encoding);
-
-/**
- * @brief given CV type encoding return corresponding sensor_msgs::msg::Image::Encoding string
- *
- * @param mat_type
- * @return std::string
- */
-std::string mat_type2encoding(int mat_type);
-
-}  // namespace nav2_waypoint_follower
-#endif  // NAV2_WAYPOINT_FOLLOWER__PLUGINS__COMMON_HPP_
diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp
index 5b3997e7956..761074bcb41 100644
--- a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp
+++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp
@@ -15,18 +15,18 @@
 #ifndef NAV2_WAYPOINT_FOLLOWER__PLUGINS__PHOTO_AT_WAYPOINT_HPP_
 #define NAV2_WAYPOINT_FOLLOWER__PLUGINS__PHOTO_AT_WAYPOINT_HPP_
 
+#include <experimental/filesystem>
 #include <mutex>
 #include <string>
 #include <exception>
 
-#include "nav2_waypoint_follower/plugins/common.hpp"
-
 #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"
 
@@ -78,6 +78,31 @@ class PhotoAtWaypoint : public nav2_core::WaypointTaskExecutor
    */
   void imageCallback(const sensor_msgs::msg::Image::SharedPtr msg);
 
+
+  /**
+   * @brief given a shared pointer to sensor::msg::Image type, make a deep copy to inputted cv Mat
+   *
+   * @param msg
+   * @param mat
+   */
+  void deepCopyMsg2Mat(const sensor_msgs::msg::Image::SharedPtr & msg, cv::Mat & mat);
+
+  /**
+ * @brief given encoding string , determine corresponding CV format
+ *
+ * @param encoding
+ * @return int
+ */
+  static int encoding2mat_type(const std::string & encoding);
+
+/**
+ * @brief given CV type encoding return corresponding sensor_msgs::msg::Image::Encoding string
+ *
+ * @param mat_type
+ * @return std::string
+ */
+  static std::string mat_type2encoding(int mat_type);
+
 protected:
   // to ensure safety when accessing global var curr_frame_
   std::mutex global_mutex_;
@@ -86,11 +111,11 @@ class PhotoAtWaypoint : public nav2_core::WaypointTaskExecutor
   // .png ? .jpg ? or some other well known format
   std::string image_format_;
   // the topic to subscribe in order capture a frame
-  std::string camera_image_topic_name_;
+  std::string image_topic_;
   // whether plugin is enabled
   bool is_enabled_;
   // current frame;
-  cv::Mat curr_frame_;
+  sensor_msgs::msg::Image curr_frame_msg_;
   // global logger
   rclcpp::Logger logger_{rclcpp::get_logger("nav2_waypoint_follower")};
   // ros susbcriber to get camera image
diff --git a/nav2_waypoint_follower/plugins/common.cpp b/nav2_waypoint_follower/plugins/common.cpp
deleted file mode 100644
index 7b88d55be01..00000000000
--- a/nav2_waypoint_follower/plugins/common.cpp
+++ /dev/null
@@ -1,54 +0,0 @@
-// Copyright (c) 2020 Fetullah Atas
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-#include <string>
-#include "nav2_waypoint_follower/plugins/common.hpp"
-
-namespace nav2_waypoint_follower
-{
-
-int encoding2mat_type(const std::string & encoding)
-{
-  if (encoding == "mono8") {
-    return CV_8UC1;
-  } else if (encoding == "bgr8") {
-    return CV_8UC3;
-  } else if (encoding == "mono16") {
-    return CV_16SC1;
-  } else if (encoding == "rgba8") {
-    return CV_8UC4;
-  } else if (encoding == "rgb8") {
-    return CV_8UC3;
-  } else {
-    throw std::runtime_error("Unsupported mat type");
-  }
-}
-
-std::string mat_type2encoding(int mat_type)
-{
-  switch (mat_type) {
-    case CV_8UC1:
-      return "mono8";
-    case CV_8UC3:
-      return "bgr8";
-    case CV_16SC1:
-      return "mono16";
-    case CV_8UC4:
-      return "rgba8";
-    default:
-      throw std::runtime_error("Unsupported encoding type");
-  }
-}
-
-}  // namespace nav2_waypoint_follower
diff --git a/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp b/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp
index 76c4cdbc924..fbc8b852607 100644
--- a/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp
+++ b/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp
@@ -16,6 +16,7 @@
 #include <pluginlib/class_list_macros.hpp>
 
 #include <string>
+#include <memory>
 
 
 namespace nav2_waypoint_follower
@@ -36,32 +37,25 @@ void PhotoAtWaypoint::initialize(
 
   node->declare_parameter(plugin_name + ".enabled", rclcpp::ParameterValue(true));
   node->declare_parameter(
-    plugin_name + ".camera_image_topic_name",
-    rclcpp::ParameterValue("/fake_realsense_camera/image_raw"));
-  node->declare_parameter(plugin_name + ".save_images_dir", rclcpp::ParameterValue("/home/atas/"));
+    plugin_name + ".image_topic",
+    rclcpp::ParameterValue("/camera/color/image_raw"));
+  node->declare_parameter(plugin_name + ".save_dir", rclcpp::ParameterValue("/home/username/"));
   node->declare_parameter(plugin_name + ".image_format", rclcpp::ParameterValue(".png"));
 
   node->get_parameter(plugin_name + ".enabled", is_enabled_);
-  node->get_parameter(plugin_name + ".camera_image_topic_name", camera_image_topic_name_);
-  node->get_parameter(plugin_name + ".save_images_dir", directory_to_save_images_);
+  node->get_parameter(plugin_name + ".image_topic", image_topic_);
+  node->get_parameter(plugin_name + ".save_dir", directory_to_save_images_);
   node->get_parameter(plugin_name + ".image_format", image_format_);
 
-  auto qos = rclcpp::QoS(
-    rclcpp::QoSInitialization(
-      rmw_qos_profile_default.history,
-      rmw_qos_profile_default.depth
-  ));
-  qos.reliability(rmw_qos_profile_default.reliability);
-
   if (!is_enabled_) {
     RCLCPP_INFO(
       logger_, "Waypoint task executor plugin is disabled.");
   } else {
     RCLCPP_INFO(
       logger_, "Initializing photo at waypoint plugin, subscribing to camera topic named; %s",
-      camera_image_topic_name_.c_str());
+      image_topic_.c_str());
     camera_image_subscriber_ = node->create_subscription<sensor_msgs::msg::Image>(
-      camera_image_topic_name_, qos,
+      image_topic_, rclcpp::SystemDefaultsQoS(),
       std::bind(&PhotoAtWaypoint::imageCallback, this, std::placeholders::_1));
   }
 }
@@ -77,40 +71,92 @@ bool PhotoAtWaypoint::processAtWaypoint(
     return true;
   }
   try {
-    global_mutex_.lock();
-    std::string stamped_name_for_curr_frame = directory_to_save_images_ + std::to_string(
+    std::experimental::filesystem::path save_dir = directory_to_save_images_;
+    std::experimental::filesystem::path file_name = std::to_string(
       curr_waypoint_index) + "_" +
       std::to_string(curr_pose.header.stamp.sec) + image_format_;
-    cv::imwrite(stamped_name_for_curr_frame, curr_frame_);
+
+    std::experimental::filesystem::path full_path_image_path = save_dir / file_name;
+
+
+    global_mutex_.lock();
+    cv::Mat curr_frame_mat;
+    auto curr_frame_msg_as_shared_ptr = std::make_shared<sensor_msgs::msg::Image>(curr_frame_msg_);
+    deepCopyMsg2Mat(curr_frame_msg_as_shared_ptr, curr_frame_mat);
+    cv::imwrite(full_path_image_path.c_str(), curr_frame_mat);
+    global_mutex_.unlock();
     RCLCPP_INFO(
       logger_,
       "Photo has been taken sucessfully at waypoint %i", curr_waypoint_index);
-    global_mutex_.unlock();
-  } catch (...) {
+  } catch (const std::exception & e) {
     RCLCPP_ERROR(
-      logger_,
-      "Photo at waypoint plugin caught an exception while trying taking a photo at waypoint %i.",
-      curr_waypoint_index);
+      logger_, "Couldn't take photo at waypoint %i! Caught exception: %s",
+      curr_waypoint_index, e.what());
+    return false;
+  } catch (...) {
     RCLCPP_ERROR(
       logger_,
-      "Not going to take a photo!, Make sure that the image topic named: %s is valid and active!",
-      camera_image_topic_name_.c_str());
+      "An unknown execption caught while taking photo at waypoint %i!"
+      "Make sure that the image topic named: %s is valid and active!",
+      curr_waypoint_index,
+      image_topic_.c_str());
     return false;
   }
   return true;
 }
 
 void PhotoAtWaypoint::imageCallback(const sensor_msgs::msg::Image::SharedPtr msg)
+{
+  global_mutex_.lock();
+  curr_frame_msg_ = *msg;
+  global_mutex_.unlock();
+}
+
+int PhotoAtWaypoint::encoding2mat_type(const std::string & encoding)
+{
+  if (encoding == "mono8") {
+    return CV_8UC1;
+  } else if (encoding == "bgr8") {
+    return CV_8UC3;
+  } else if (encoding == "mono16") {
+    return CV_16SC1;
+  } else if (encoding == "rgba8") {
+    return CV_8UC4;
+  } else if (encoding == "rgb8") {
+    return CV_8UC3;
+  } else {
+    throw std::runtime_error("Unsupported mat type");
+  }
+}
+
+std::string PhotoAtWaypoint::mat_type2encoding(int mat_type)
+{
+  switch (mat_type) {
+    case CV_8UC1:
+      return "mono8";
+    case CV_8UC3:
+      return "bgr8";
+    case CV_16SC1:
+      return "mono16";
+    case CV_8UC4:
+      return "rgba8";
+    default:
+      throw std::runtime_error("Unsupported encoding type");
+  }
+}
+
+void PhotoAtWaypoint::deepCopyMsg2Mat(
+  const sensor_msgs::msg::Image::SharedPtr & msg,
+  cv::Mat & mat)
 {
   cv_bridge::CvImageConstPtr cv_bridge_ptr = cv_bridge::toCvShare(msg, msg->encoding);
   cv::Mat frame = cv_bridge_ptr->image;
   if (msg->encoding == "rgb8") {
     cv::cvtColor(frame, frame, cv::COLOR_RGB2BGR);
   }
-  global_mutex_.lock();
-  frame.copyTo(curr_frame_);
-  global_mutex_.unlock();
+  frame.copyTo(mat);
 }
+
 }      // namespace nav2_waypoint_follower
 PLUGINLIB_EXPORT_CLASS(
   nav2_waypoint_follower::PhotoAtWaypoint,
diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp
index 355ae77df37..ff81e7666dd 100644
--- a/nav2_waypoint_follower/src/waypoint_follower.cpp
+++ b/nav2_waypoint_follower/src/waypoint_follower.cpp
@@ -35,7 +35,7 @@ WaypointFollower::WaypointFollower()
   declare_parameter("loop_rate", 20);
   nav2_util::declare_parameter_if_not_declared(
     this, std::string("waypoint_task_executor_plugin"),
-    rclcpp::ParameterValue(std::string("photo_at_waypoint")));
+    rclcpp::ParameterValue(std::string("wait_at_waypoint")));
   nav2_util::declare_parameter_if_not_declared(
     this, std::string("waypoint_task_executor_plugin.plugin"),
     rclcpp::ParameterValue(std::string("nav2_waypoint_follower::WaitAtWaypoint")));

From c4c92f7a0ea73beba249e3fc601903fa5ff8a335 Mon Sep 17 00:00:00 2001
From: jediofgever <fetulahatas1@gmail.com>
Date: Thu, 15 Oct 2020 13:09:40 +0800
Subject: [PATCH 4/7] minor corrections on photo_at_waypoint header

---
 .../plugins/photo_at_waypoint.hpp             | 25 +++++++++----------
 1 file changed, 12 insertions(+), 13 deletions(-)

diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp
index 761074bcb41..26beaa13168 100644
--- a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp
+++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp
@@ -78,29 +78,28 @@ class PhotoAtWaypoint : public nav2_core::WaypointTaskExecutor
    */
   void imageCallback(const sensor_msgs::msg::Image::SharedPtr msg);
 
-
   /**
    * @brief given a shared pointer to sensor::msg::Image type, make a deep copy to inputted cv Mat
    *
    * @param msg
    * @param mat
    */
-  void deepCopyMsg2Mat(const sensor_msgs::msg::Image::SharedPtr & msg, cv::Mat & mat);
+  static void deepCopyMsg2Mat(const sensor_msgs::msg::Image::SharedPtr & msg, cv::Mat & mat);
 
   /**
- * @brief given encoding string , determine corresponding CV format
- *
- * @param encoding
- * @return int
- */
+   * @brief given encoding string , determine corresponding CV format
+   *
+   * @param encoding
+   * @return int
+   */
   static int encoding2mat_type(const std::string & encoding);
 
-/**
- * @brief given CV type encoding return corresponding sensor_msgs::msg::Image::Encoding string
- *
- * @param mat_type
- * @return std::string
- */
+  /**
+   * @brief given CV type encoding return corresponding sensor_msgs::msg::Image::Encoding string
+   *
+   * @param mat_type
+   * @return std::string
+   */
   static std::string mat_type2encoding(int mat_type);
 
 protected:

From 8992cbdb9e27deba5d80e97b8ebb8e9922ba61ed Mon Sep 17 00:00:00 2001
From: jediofgever <fetulahatas1@gmail.com>
Date: Mon, 19 Oct 2020 10:15:11 +0800
Subject: [PATCH 5/7] resolve requested changes of second review

Signed-off-by: jediofgever <fetulahatas1@gmail.com>
---
 doc/parameters/param_list.md                  |  2 +-
 nav2_waypoint_follower/CMakeLists.txt         | 16 ++---
 .../plugins/photo_at_waypoint.hpp             | 10 +--
 nav2_waypoint_follower/plugins.xml            |  4 +-
 .../plugins/photo_at_waypoint.cpp             | 63 +++++++++----------
 5 files changed, 40 insertions(+), 55 deletions(-)

diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md
index c587724f593..b52dc2ec9fa 100644
--- a/doc/parameters/param_list.md
+++ b/doc/parameters/param_list.md
@@ -550,7 +550,7 @@ When `planner_plugins` parameter is not overridden, the following default plugin
 | `<waypoint task executor>`.enabled | true | Whether it is enabled |
 | `<waypoint task executor>`.image_topic | "/camera/color/image_raw" | Camera image topic name to susbcribe |
 | `<waypoint task executor>`.save_dir | "/home/username/" | Path to directory to save taken photos |
-| `<waypoint task executor>`.image_format | ".png" | Desired image format A few other options; ".jpeg" , ".jpg", ".pgm", ".tiff" |
+| `<waypoint task executor>`.image_format | "png" | Desired image format A few other options; "jpeg" , "jpg", "pgm", "tiff" |
 
 
 # recoveries
diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt
index a4cb96073f6..7d796848fc4 100644
--- a/nav2_waypoint_follower/CMakeLists.txt
+++ b/nav2_waypoint_follower/CMakeLists.txt
@@ -65,18 +65,18 @@ ament_target_dependencies(${library_name}
   ${dependencies}
 )
 
-add_library(simple_wait_at_waypoint SHARED plugins/wait_at_waypoint.cpp)
-ament_target_dependencies(simple_wait_at_waypoint ${dependencies})
+add_library(wait_at_waypoint SHARED plugins/wait_at_waypoint.cpp)
+ament_target_dependencies(wait_at_waypoint ${dependencies})
 #prevent pluginlib from using boost
-target_compile_definitions(simple_wait_at_waypoint PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
+target_compile_definitions(wait_at_waypoint PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
 
-add_library(simple_photo_at_waypoint SHARED plugins/photo_at_waypoint.cpp)
-ament_target_dependencies(simple_photo_at_waypoint ${dependencies})
+add_library(photo_at_waypoint SHARED plugins/photo_at_waypoint.cpp)
+ament_target_dependencies(photo_at_waypoint ${dependencies})
 #prevent pluginlib from using boost
-target_compile_definitions(simple_photo_at_waypoint PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
+target_compile_definitions(photo_at_waypoint PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
 
 
-install(TARGETS ${library_name} simple_wait_at_waypoint simple_photo_at_waypoint
+install(TARGETS ${library_name} wait_at_waypoint photo_at_waypoint
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
@@ -96,7 +96,7 @@ if(BUILD_TESTING)
 endif()
 
 ament_export_include_directories(include)
-ament_export_libraries(simple_wait_at_waypoint simple_photo_at_waypoint)
+ament_export_libraries(wait_at_waypoint photo_at_waypoint)
 
 pluginlib_export_plugin_description_file(nav2_waypoint_follower plugins.xml)
 
diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp
index 26beaa13168..653ee4a6b18 100644
--- a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp
+++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp
@@ -94,14 +94,6 @@ class PhotoAtWaypoint : public nav2_core::WaypointTaskExecutor
    */
   static int encoding2mat_type(const std::string & encoding);
 
-  /**
-   * @brief given CV type encoding return corresponding sensor_msgs::msg::Image::Encoding string
-   *
-   * @param mat_type
-   * @return std::string
-   */
-  static std::string mat_type2encoding(int mat_type);
-
 protected:
   // to ensure safety when accessing global var curr_frame_
   std::mutex global_mutex_;
@@ -114,7 +106,7 @@ class PhotoAtWaypoint : public nav2_core::WaypointTaskExecutor
   // whether plugin is enabled
   bool is_enabled_;
   // current frame;
-  sensor_msgs::msg::Image curr_frame_msg_;
+  sensor_msgs::msg::Image::SharedPtr curr_frame_msg_;
   // global logger
   rclcpp::Logger logger_{rclcpp::get_logger("nav2_waypoint_follower")};
   // ros susbcriber to get camera image
diff --git a/nav2_waypoint_follower/plugins.xml b/nav2_waypoint_follower/plugins.xml
index db4e73de606..ba5b9da6ae0 100644
--- a/nav2_waypoint_follower/plugins.xml
+++ b/nav2_waypoint_follower/plugins.xml
@@ -1,10 +1,10 @@
 <class_libraries>
-  <library path="simple_wait_at_waypoint">
+  <library path="wait_at_waypoint">
     <class type="nav2_waypoint_follower::WaitAtWaypoint" base_class_type="nav2_core::WaypointTaskExecutor">
       <description>Lets robot sleep for a specified amount of time at waypoint arrival</description>
     </class>
   </library>
-  <library path="simple_photo_at_waypoint">
+  <library path="photo_at_waypoint">
     <class type="nav2_waypoint_follower::PhotoAtWaypoint" base_class_type="nav2_core::WaypointTaskExecutor">
       <description>Run-time plugin that takes photos at waypoint arrivals when using waypoint follower node. 
         Saves the taken photos to specified directory.</description>
diff --git a/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp b/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp
index fbc8b852607..ff0b95aa6ae 100644
--- a/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp
+++ b/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp
@@ -40,7 +40,7 @@ void PhotoAtWaypoint::initialize(
     plugin_name + ".image_topic",
     rclcpp::ParameterValue("/camera/color/image_raw"));
   node->declare_parameter(plugin_name + ".save_dir", rclcpp::ParameterValue("/home/username/"));
-  node->declare_parameter(plugin_name + ".image_format", rclcpp::ParameterValue(".png"));
+  node->declare_parameter(plugin_name + ".image_format", rclcpp::ParameterValue("png"));
 
   node->get_parameter(plugin_name + ".enabled", is_enabled_);
   node->get_parameter(plugin_name + ".image_topic", image_topic_);
@@ -71,35 +71,45 @@ bool PhotoAtWaypoint::processAtWaypoint(
     return true;
   }
   try {
+    // get inputted save directory and make sure it exists, if not log and create  it
     std::experimental::filesystem::path save_dir = directory_to_save_images_;
+    if (!std::experimental::filesystem::exists(save_dir)) {
+      RCLCPP_WARN(
+        logger_,
+        "Provided save directory for photo at waypoint plugin does not exist,"
+        "providied directory is: %s directory will be created automatically.",
+        save_dir.c_str()
+      );
+      if (!std::experimental::filesystem::create_directory(save_dir)) {
+        RCLCPP_ERROR(
+          logger_,
+          "Failed to create directory!: %s required by photo at waypoint plugin, "
+          "exiting the plugin with failure!",
+          save_dir.c_str()
+        );
+      }
+    }
+    // construct the full path to image filename
     std::experimental::filesystem::path file_name = std::to_string(
       curr_waypoint_index) + "_" +
-      std::to_string(curr_pose.header.stamp.sec) + image_format_;
-
+      std::to_string(curr_pose.header.stamp.sec) + "." + image_format_;
     std::experimental::filesystem::path full_path_image_path = save_dir / file_name;
 
-
-    global_mutex_.lock();
+    // save the taken photo at this waypoint to given directory
+    std::lock_guard<std::mutex> guard(global_mutex_);
     cv::Mat curr_frame_mat;
-    auto curr_frame_msg_as_shared_ptr = std::make_shared<sensor_msgs::msg::Image>(curr_frame_msg_);
-    deepCopyMsg2Mat(curr_frame_msg_as_shared_ptr, curr_frame_mat);
+    deepCopyMsg2Mat(curr_frame_msg_, curr_frame_mat);
     cv::imwrite(full_path_image_path.c_str(), curr_frame_mat);
-    global_mutex_.unlock();
     RCLCPP_INFO(
       logger_,
       "Photo has been taken sucessfully at waypoint %i", curr_waypoint_index);
-  } catch (const std::exception & e) {
-    RCLCPP_ERROR(
-      logger_, "Couldn't take photo at waypoint %i! Caught exception: %s",
-      curr_waypoint_index, e.what());
-    return false;
-  } catch (...) {
+  } catch (const cv::Exception & e) {
     RCLCPP_ERROR(
       logger_,
-      "An unknown execption caught while taking photo at waypoint %i!"
+      "Couldn't take photo at waypoint %i! Caught exception: %s \n"
       "Make sure that the image topic named: %s is valid and active!",
       curr_waypoint_index,
-      image_topic_.c_str());
+      e.what(), image_topic_.c_str());
     return false;
   }
   return true;
@@ -107,9 +117,8 @@ bool PhotoAtWaypoint::processAtWaypoint(
 
 void PhotoAtWaypoint::imageCallback(const sensor_msgs::msg::Image::SharedPtr msg)
 {
-  global_mutex_.lock();
-  curr_frame_msg_ = *msg;
-  global_mutex_.unlock();
+  std::lock_guard<std::mutex> guard(global_mutex_);
+  curr_frame_msg_ = msg;
 }
 
 int PhotoAtWaypoint::encoding2mat_type(const std::string & encoding)
@@ -129,22 +138,6 @@ int PhotoAtWaypoint::encoding2mat_type(const std::string & encoding)
   }
 }
 
-std::string PhotoAtWaypoint::mat_type2encoding(int mat_type)
-{
-  switch (mat_type) {
-    case CV_8UC1:
-      return "mono8";
-    case CV_8UC3:
-      return "bgr8";
-    case CV_16SC1:
-      return "mono16";
-    case CV_8UC4:
-      return "rgba8";
-    default:
-      throw std::runtime_error("Unsupported encoding type");
-  }
-}
-
 void PhotoAtWaypoint::deepCopyMsg2Mat(
   const sensor_msgs::msg::Image::SharedPtr & msg,
   cv::Mat & mat)

From db8ccd28d40e1f4f1130355ba0c2c3f3d44e930b Mon Sep 17 00:00:00 2001
From: jediofgever <fetulahatas1@gmail.com>
Date: Mon, 19 Oct 2020 14:07:45 +0800
Subject: [PATCH 6/7] update default save_dir

Signed-off-by: jediofgever <fetulahatas1@gmail.com>
---
 doc/parameters/param_list.md                         | 2 +-
 nav2_waypoint_follower/plugins/photo_at_waypoint.cpp | 4 +++-
 2 files changed, 4 insertions(+), 2 deletions(-)

diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md
index b52dc2ec9fa..e637b489fa3 100644
--- a/doc/parameters/param_list.md
+++ b/doc/parameters/param_list.md
@@ -549,7 +549,7 @@ When `planner_plugins` parameter is not overridden, the following default plugin
 | ----------| --------| ------------|
 | `<waypoint task executor>`.enabled | true | Whether it is enabled |
 | `<waypoint task executor>`.image_topic | "/camera/color/image_raw" | Camera image topic name to susbcribe |
-| `<waypoint task executor>`.save_dir | "/home/username/" | Path to directory to save taken photos |
+| `<waypoint task executor>`.save_dir | "/tmp/waypoint_images" | Path to directory to save taken photos |
 | `<waypoint task executor>`.image_format | "png" | Desired image format A few other options; "jpeg" , "jpg", "pgm", "tiff" |
 
 
diff --git a/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp b/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp
index ff0b95aa6ae..14182a1bd1f 100644
--- a/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp
+++ b/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp
@@ -39,7 +39,9 @@ void PhotoAtWaypoint::initialize(
   node->declare_parameter(
     plugin_name + ".image_topic",
     rclcpp::ParameterValue("/camera/color/image_raw"));
-  node->declare_parameter(plugin_name + ".save_dir", rclcpp::ParameterValue("/home/username/"));
+  node->declare_parameter(
+    plugin_name + ".save_dir",
+    rclcpp::ParameterValue("/tmp/waypoint_images"));
   node->declare_parameter(plugin_name + ".image_format", rclcpp::ParameterValue("png"));
 
   node->get_parameter(plugin_name + ".enabled", is_enabled_);

From 4cadb904629b713dc393bd8a08dfdc057aa4fac2 Mon Sep 17 00:00:00 2001
From: jediofgever <fetulahatas1@gmail.com>
Date: Thu, 29 Oct 2020 09:58:37 +0800
Subject: [PATCH 7/7] move directory checking code to initialize()

---
 .../plugins/photo_at_waypoint.hpp             | 10 +--
 .../plugins/photo_at_waypoint.cpp             | 66 ++++++++-----------
 2 files changed, 28 insertions(+), 48 deletions(-)

diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp
index 653ee4a6b18..7d264b7181e 100644
--- a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp
+++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp
@@ -86,19 +86,11 @@ class PhotoAtWaypoint : public nav2_core::WaypointTaskExecutor
    */
   static void deepCopyMsg2Mat(const sensor_msgs::msg::Image::SharedPtr & msg, cv::Mat & mat);
 
-  /**
-   * @brief given encoding string , determine corresponding CV format
-   *
-   * @param encoding
-   * @return int
-   */
-  static int encoding2mat_type(const std::string & encoding);
-
 protected:
   // to ensure safety when accessing global var curr_frame_
   std::mutex global_mutex_;
   // the taken photos will be saved under this directory
-  std::string directory_to_save_images_;
+  std::experimental::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/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp b/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp
index 14182a1bd1f..9939d60b680 100644
--- a/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp
+++ b/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp
@@ -35,6 +35,8 @@ void PhotoAtWaypoint::initialize(
 {
   auto node = parent.lock();
 
+  curr_frame_msg_ = std::make_shared<sensor_msgs::msg::Image>();
+
   node->declare_parameter(plugin_name + ".enabled", rclcpp::ParameterValue(true));
   node->declare_parameter(
     plugin_name + ".image_topic",
@@ -44,14 +46,35 @@ void PhotoAtWaypoint::initialize(
     rclcpp::ParameterValue("/tmp/waypoint_images"));
   node->declare_parameter(plugin_name + ".image_format", rclcpp::ParameterValue("png"));
 
+  std::string save_dir_as_string;
   node->get_parameter(plugin_name + ".enabled", is_enabled_);
   node->get_parameter(plugin_name + ".image_topic", image_topic_);
-  node->get_parameter(plugin_name + ".save_dir", directory_to_save_images_);
+  node->get_parameter(plugin_name + ".save_dir", save_dir_as_string);
   node->get_parameter(plugin_name + ".image_format", image_format_);
 
+  // get inputted save directory and make sure it exists, if not log and create  it
+  save_dir_ = save_dir_as_string;
+  if (!std::experimental::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_)) {
+      RCLCPP_ERROR(
+        logger_,
+        "Failed to create directory!: %s required by photo at waypoint plugin, "
+        "exiting the plugin with failure!",
+        save_dir_.c_str()
+      );
+      is_enabled_ = false;
+    }
+  }
+
   if (!is_enabled_) {
     RCLCPP_INFO(
-      logger_, "Waypoint task executor plugin is disabled.");
+      logger_, "Photo at waypoint plugin is disabled.");
   } else {
     RCLCPP_INFO(
       logger_, "Initializing photo at waypoint plugin, subscribing to camera topic named; %s",
@@ -73,29 +96,11 @@ bool PhotoAtWaypoint::processAtWaypoint(
     return true;
   }
   try {
-    // get inputted save directory and make sure it exists, if not log and create  it
-    std::experimental::filesystem::path save_dir = directory_to_save_images_;
-    if (!std::experimental::filesystem::exists(save_dir)) {
-      RCLCPP_WARN(
-        logger_,
-        "Provided save directory for photo at waypoint plugin does not exist,"
-        "providied directory is: %s directory will be created automatically.",
-        save_dir.c_str()
-      );
-      if (!std::experimental::filesystem::create_directory(save_dir)) {
-        RCLCPP_ERROR(
-          logger_,
-          "Failed to create directory!: %s required by photo at waypoint plugin, "
-          "exiting the plugin with failure!",
-          save_dir.c_str()
-        );
-      }
-    }
     // construct the full path to image filename
     std::experimental::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::experimental::filesystem::path full_path_image_path = save_dir_ / file_name;
 
     // save the taken photo at this waypoint to given directory
     std::lock_guard<std::mutex> guard(global_mutex_);
@@ -105,7 +110,7 @@ bool PhotoAtWaypoint::processAtWaypoint(
     RCLCPP_INFO(
       logger_,
       "Photo has been taken sucessfully at waypoint %i", curr_waypoint_index);
-  } catch (const cv::Exception & e) {
+  } catch (const std::exception & e) {
     RCLCPP_ERROR(
       logger_,
       "Couldn't take photo at waypoint %i! Caught exception: %s \n"
@@ -123,23 +128,6 @@ void PhotoAtWaypoint::imageCallback(const sensor_msgs::msg::Image::SharedPtr msg
   curr_frame_msg_ = msg;
 }
 
-int PhotoAtWaypoint::encoding2mat_type(const std::string & encoding)
-{
-  if (encoding == "mono8") {
-    return CV_8UC1;
-  } else if (encoding == "bgr8") {
-    return CV_8UC3;
-  } else if (encoding == "mono16") {
-    return CV_16SC1;
-  } else if (encoding == "rgba8") {
-    return CV_8UC4;
-  } else if (encoding == "rgb8") {
-    return CV_8UC3;
-  } else {
-    throw std::runtime_error("Unsupported mat type");
-  }
-}
-
 void PhotoAtWaypoint::deepCopyMsg2Mat(
   const sensor_msgs::msg::Image::SharedPtr & msg,
   cv::Mat & mat)