nav2_way_point_follower; introduce photo at waypoint arrivals plugin

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>`.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
 
 ## recovery_server

nav2_bringup/bringup/params/nav2_params.yaml

@@ -289,8 +289,8 @@ waypoint_follower:
   ros__parameters:
     loop_rate: 20
     stop_on_failure: false
-    waypoint_task_executor_plugin: "waypoint_task_executor"   
-    waypoint_task_executor:
+    waypoint_task_executor_plugin: "wait_at_waypoint"   
+    wait_at_waypoint:
       plugin: "nav2_waypoint_follower::WaitAtWaypoint"
       enabled: True
-      waypoint_pause_duration: 0
+      waypoint_pause_duration: 200

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,18 @@ 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")
+
+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")
+
 
-install(TARGETS ${library_name} simple_waypoint_task_executor
+install(TARGETS ${library_name} simple_wait_at_waypoint simple_photo_at_waypoint
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
@@ -78,7 +96,7 @@ if(BUILD_TESTING)
 endif()
 
 ament_export_include_directories(include)
-ament_export_libraries(simple_waypoint_task_executor)
+ament_export_libraries(simple_wait_at_waypoint simple_photo_at_waypoint)
 
 pluginlib_export_plugin_description_file(nav2_waypoint_follower plugins.xml)
 

nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp

@@ -0,0 +1,125 @@
+// 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 <experimental/filesystem>
+#include <mutex>
+#include <string>
+#include <exception>
+
+#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"
+
+
+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);
+
+  /**
+   * @brief given a shared pointer to sensor::msg::Image type, make a deep copy to inputted cv Mat
+   *
+   * @param msg
+   * @param 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
+   */
+  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_;
+  // 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 image_topic_;
+  // whether plugin is enabled
+  bool is_enabled_;
+  // current 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
+  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr camera_image_subscriber_;
+};
+}  // namespace nav2_waypoint_follower
+
+#endif  // NAV2_WAYPOINT_FOLLOWER__PLUGINS__PHOTO_AT_WAYPOINT_HPP_

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>

nav2_waypoint_follower/plugins/photo_at_waypoint.cpp

@@ -0,0 +1,163 @@
+// 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>
+#include <memory>
+
+
+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 + ".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 + ".image_topic", image_topic_);
+  node->get_parameter(plugin_name + ".save_dir", directory_to_save_images_);
+  node->get_parameter(plugin_name + ".image_format", image_format_);
+
+  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",
+      image_topic_.c_str());
+    camera_image_subscriber_ = node->create_subscription<sensor_msgs::msg::Image>(
+      image_topic_, rclcpp::SystemDefaultsQoS(),
+      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 {
+    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_;
+
+    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);
+  } 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 (...) {
+    RCLCPP_ERROR(
+      logger_,
+      "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);
+  }
+  frame.copyTo(mat);
+}
+
+}      // namespace nav2_waypoint_follower
+PLUGINLIB_EXPORT_CLASS(
+  nav2_waypoint_follower::PhotoAtWaypoint,
+  nav2_core::WaypointTaskExecutor)