Patch for PhotoAtWaypoint plugin (ros-navigation#2067)

* nav2_way_point_follower; introduce photo at waypoint arrivals plugin Signed-off-by: jediofgever <[email protected]> * resolve cmake lint errors Signed-off-by: jediofgever <[email protected]> * resolve requested chages of first review Signed-off-by: jediofgever <[email protected]> * minor corrections on photo_at_waypoint header * resolve requested changes of second review Signed-off-by: jediofgever <[email protected]> * update default save_dir Signed-off-by: jediofgever <[email protected]> * move directory checking code to initialize() * add try catch block to catch possible execeptions while creating a directory for photo at waypoint plugin Signed-off-by: jediofgever <[email protected]> * adding logging and disabling * Update nav2_waypoint_follower/plugins/photo_at_waypoint.cpp Co-authored-by: Fetullah Atas <[email protected]> * Update photo_at_waypoint.cpp Co-authored-by: jediofgever <[email protected]>
vinnnyr · Nov 9, 2020 · 18d6137 · 18d6137
1 parent f5ea383
commit 18d6137
Showing 1 changed file with 19 additions and 12 deletions.
diff --git a/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp b/nav2_waypoint_follower/plugins/photo_at_waypoint.cpp
@@ -54,22 +54,29 @@ void PhotoAtWaypoint::initialize(
 
   // 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(
+  try {
+    if (!std::experimental::filesystem::exists(save_dir_)) {
+      RCLCPP_WARN(
         logger_,
-        "Failed to create directory!: %s required by photo at waypoint plugin, "
-        "exiting the plugin with failure!",
+        "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()
       );
-      is_enabled_ = false;
+      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;
+      }
     }
+  } catch (const std::exception & e) {
+    RCLCPP_ERROR(
+      logger_, "Exception (%s) thrown while attempting to create image capture directory."
+      " This task executor is being disabled as it cannot save images.", e.what());
+    is_enabled_ = false;
   }
 
   if (!is_enabled_) {