Adding near-complete voxel grid test coverage and more to controller server

nav2_controller/include/nav2_controller/nav2_controller.hpp

@@ -189,6 +189,12 @@ class ControllerServer : public nav2_util::LifecycleNode
     return twist_thresh;
   }
 
+  void pluginFailed(const std::string & name, const pluginlib::PluginlibException & ex)
+  {
+    RCLCPP_FATAL(get_logger(), "Failed to create %s. Exception: %s", name.c_str(), ex.what());
+    exit(-1);
+  }
+
   // The controller needs a costmap node
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
   std::unique_ptr<nav2_util::NodeThread> costmap_thread_;

nav2_controller/src/nav2_controller.cpp

@@ -113,8 +113,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
       progress_checker_id_.c_str(), progress_checker_type_.c_str());
     progress_checker_->initialize(node, progress_checker_id_);
   } catch (const pluginlib::PluginlibException & ex) {
-    RCLCPP_FATAL(get_logger(), "Failed to create controller. Exception: %s", ex.what());
-    exit(-1);
+    pluginFailed("progress_checker", ex);
   }
   try {
     goal_checker_type_ = nav2_util::get_plugin_type_param(node, goal_checker_id_);
@@ -124,8 +123,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
       goal_checker_id_.c_str(), goal_checker_type_.c_str());
     goal_checker_->initialize(node, goal_checker_id_);
   } catch (const pluginlib::PluginlibException & ex) {
-    RCLCPP_FATAL(get_logger(), "Failed to create controller. Exception: %s", ex.what());
-    exit(-1);
+    pluginFailed("goal_checker", ex);
   }
 
   for (size_t i = 0; i != controller_ids_.size(); i++) {
@@ -141,8 +139,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
         costmap_ros_->getTfBuffer(), costmap_ros_);
       controllers_.insert({controller_ids_[i], controller});
     } catch (const pluginlib::PluginlibException & ex) {
-      RCLCPP_FATAL(get_logger(), "Failed to create controller. Exception: %s", ex.what());
-      exit(-1);
+      pluginFailed("controller", ex);
     }
   }
 

nav2_planner/src/planner_server.cpp

@@ -299,15 +299,6 @@ PlannerServer::computePlan()
     //               for example: couldn't get costmap update
     action_server_->terminate_current();
     return;
-  } catch (...) {
-    RCLCPP_WARN(
-      get_logger(), "Plan calculation failed, "
-      "An unexpected error has occurred. The planner server"
-      " may not be able to continue operating correctly.");
-    // TODO(orduno): provide information about fail error to parent task,
-    //               for example: couldn't get costmap update
-    action_server_->terminate_current();
-    return;
   }
 }
 

nav2_voxel_grid/test/voxel_grid_tests.cpp

@@ -147,6 +147,46 @@ TEST(voxel_grid, InvalidSize) {
   EXPECT_TRUE(vg.getVoxelColumn(50, 11, 0, 0) == nav2_voxel_grid::VoxelStatus::UNKNOWN);
 }
 
+TEST(voxel_grid, MarkAndClear) {
+  int size_x = 10, size_y = 10, size_z = 10;
+  nav2_voxel_grid::VoxelGrid vg(size_x, size_y, size_z);
+  vg.markVoxelInMap(5, 5, 5, 0);
+  EXPECT_EQ(vg.getVoxel(5, 5, 5), nav2_voxel_grid::MARKED);
+  vg.clearVoxelColumn(55);
+  EXPECT_EQ(vg.getVoxel(5, 5, 5), nav2_voxel_grid::FREE);
+}
+
+TEST(voxel_grid, clearVoxelLineInMap) {
+  int size_x = 10, size_y = 10, size_z = 10;
+  nav2_voxel_grid::VoxelGrid vg(size_x, size_y, size_z);
+  vg.markVoxelInMap(0, 0, 5, 0);
+  EXPECT_EQ(vg.getVoxel(0, 0, 5), nav2_voxel_grid::MARKED);
+
+  unsigned char * map_2d = new unsigned char[100];
+  map_2d[0] = 254;
+
+  vg.clearVoxelLineInMap(0, 0, 0, 0, 0, 9, map_2d, 16, 0);
+
+  EXPECT_EQ(map_2d[0], 0);
+
+  vg.markVoxelInMap(0, 0, 5, 0);
+  vg.clearVoxelLineInMap(0, 0, 0, 0, 0, 9, nullptr, 16, 0);
+  EXPECT_EQ(vg.getVoxel(0, 0, 5), nav2_voxel_grid::FREE);
+  delete[] map_2d;
+}
+
+TEST(voxel_grid, GetVoxelData) {
+  uint32_t * data = new uint32_t[9];
+  data[4] = 255;
+  data[0] = 0;
+  EXPECT_EQ(
+    nav2_voxel_grid::VoxelGrid::getVoxel(1, 1, 1, 3, 3, 3, data), nav2_voxel_grid::UNKNOWN);
+
+  EXPECT_EQ(
+    nav2_voxel_grid::VoxelGrid::getVoxel(0, 0, 0, 3, 3, 3, data), nav2_voxel_grid::FREE);
+  delete[] data;
+}
+
 int main(int argc, char ** argv)
 {
   testing::InitGoogleTest(&argc, argv);