Remove all exit(-1) crash conditions (#3846)

* Update transform_available_condition.cpp * wrapping all examples of get_plugin_type_param in exceptions and making that throw instead of crash * some linting * fix typo * Update controller.cpp * Update nav2_params.yaml * Update nav2_params.yaml

Author: SteveMacenski

nav2_behavior_tree/plugins/condition/transform_available_condition.cpp

@@ -39,7 +39,7 @@ TransformAvailableCondition::TransformAvailableCondition(
  RCLCPP_FATAL(
  node_->get_logger(), "Child frame (%s) or parent frame (%s) were empty.",
  child_frame_.c_str(), parent_frame_.c_str());
- exit(-1);
+ throw std::runtime_error("TransformAvailableCondition: Child or parent frames not provided!");
  }
 
  RCLCPP_DEBUG(node_->get_logger(), "Initialized an TransformAvailableCondition BT node");

nav2_behaviors/src/behavior_server.cpp

@@ -105,13 +105,13 @@ BehaviorServer::loadBehaviorPlugins()
  auto node = shared_from_this();
 
  for (size_t i = 0; i != behavior_ids_.size(); i++) {
- behavior_types_[i] = nav2_util::get_plugin_type_param(node, behavior_ids_[i]);
  try {
+ behavior_types_[i] = nav2_util::get_plugin_type_param(node, behavior_ids_[i]);
  RCLCPP_INFO(
  get_logger(), "Creating behavior plugin %s of type %s",
  behavior_ids_[i].c_str(), behavior_types_[i].c_str());
  behaviors_.push_back(plugin_loader_.createUniqueInstance(behavior_types_[i]));
- } catch (const pluginlib::PluginlibException & ex) {
+ } catch (const std::exception & ex) {
  RCLCPP_FATAL(
  get_logger(), "Failed to create behavior %s of type %s."
  " Exception: %s", behavior_ids_[i].c_str(), behavior_types_[i].c_str(),

nav2_bt_navigator/src/bt_navigator.cpp

@@ -161,8 +161,8 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
 
  // Load navigator plugins
  for (size_t i = 0; i != navigator_ids.size(); i++) {
- std::string navigator_type = nav2_util::get_plugin_type_param(node, navigator_ids[i]);
  try {
+ std::string navigator_type = nav2_util::get_plugin_type_param(node, navigator_ids[i]);
  RCLCPP_INFO(
  get_logger(), "Creating navigator id %s of type %s",
  navigator_ids[i].c_str(), navigator_type.c_str());
@@ -173,11 +173,10 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
  {
  return nav2_util::CallbackReturn::FAILURE;
  }
- } catch (const pluginlib::PluginlibException & ex) {
+ } catch (const std::exception & ex) {
  RCLCPP_FATAL(
- get_logger(), "Failed to create navigator id %s of type %s."
- " Exception: %s", navigator_ids[i].c_str(), navigator_type.c_str(),
- ex.what());
+ get_logger(), "Failed to create navigator id %s."
+ " Exception: %s", navigator_ids[i].c_str(), ex.what());
  return nav2_util::CallbackReturn::FAILURE;
  }
  }

nav2_collision_monitor/src/collision_detector_node.cpp

@@ -224,7 +224,6 @@ bool CollisionDetector::configurePolygons(
  polygon_name.c_str());
  return false;
  }
-
  }
  } catch (const std::exception & ex) {
  RCLCPP_ERROR(get_logger(), "Error while getting parameters: %s", ex.what());

nav2_controller/src/controller_server.cpp

@@ -142,7 +142,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
  progress_checker_ids_[i].c_str(), progress_checker_types_[i].c_str());
  progress_checker->initialize(node, progress_checker_ids_[i]);
  progress_checkers_.insert({progress_checker_ids_[i], progress_checker});
- } catch (const pluginlib::PluginlibException & ex) {
+ } catch (const std::exception & ex) {
  RCLCPP_FATAL(
  get_logger(),
  "Failed to create progress_checker. Exception: %s", ex.what());

nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp

@@ -72,5 +72,5 @@ static constexpr unsigned char LETHAL_OBSTACLE = 254;
 static constexpr unsigned char INSCRIBED_INFLATED_OBSTACLE = 253;
 static constexpr unsigned char MAX_NON_OBSTACLE = 252;
 static constexpr unsigned char FREE_SPACE = 0;
-}
+} // namespace nav2_costmap_2d
 #endif // NAV2_COSTMAP_2D__COST_VALUES_HPP_

nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp

@@ -21,6 +21,8 @@
 #include <array>
 #include <memory>
 #include <limits>
+#include <string>
+#include <utility>
 
 namespace nav2_costmap_2d
 {
@@ -77,7 +79,7 @@ void morphologyOperation(
  const Image<uint8_t> & input, Image<uint8_t> & output,
  const Image<uint8_t> & shape, AggregateFn aggregate);
 
-using ShapeBuffer3x3 = std::array<uint8_t, 9>;
+using ShapeBuffer3x3 = std::array<uint8_t, 9>; // NOLINT
 inline Image<uint8_t> createShape(ShapeBuffer3x3 & buffer, ConnectivityType connectivity);
 } // namespace imgproc_impl
 
@@ -95,7 +97,7 @@ inline void dilate(
  const Image<uint8_t> & input, Image<uint8_t> & output,
  ConnectivityType connectivity, Max && max_function)
 {
- using namespace imgproc_impl;
+ using namespace imgproc_impl; // NOLINT
  ShapeBuffer3x3 shape_buffer;
  Image<uint8_t> shape = createShape(shape_buffer, connectivity);
  morphologyOperation(input, output, shape, max_function);
@@ -376,7 +378,7 @@ struct EquivalenceLabelTreesBase
 
 struct LabelOverflow : public std::runtime_error
 {
- LabelOverflow(const std::string & message)
+ explicit LabelOverflow(const std::string & message)
  : std::runtime_error(message) {}
 };
 
@@ -464,7 +466,6 @@ class EquivalenceLabelTrees : public EquivalenceLabelTreesBase
  {
  Label k = 1;
  for (Label i = 1; i < next_free_; ++i) {
-
  if (labels_[i] < i) {
  labels_[i] = labels_[labels_[i]];
  } else {
@@ -504,7 +505,7 @@ class EquivalenceLabelTrees : public EquivalenceLabelTreesBase
  * '~' - row continuation in the same style */
  max_labels = (rows * columns) / 3 + 1;
  }
- ++max_labels; // add zero label
+ ++max_labels;  // add zero label
  max_labels = std::min(max_labels, size_t(std::numeric_limits<Label>::max()));
  return max_labels;
  }
@@ -568,7 +569,7 @@ struct ProcessPixel<ConnectivityType::Way8>
  {
  Label & current = label.e();
 
- //The decision tree traversal. See reference article for details
+ // The decision tree traversal. See reference article for details
  if (!is_bg(image.e())) {
  if (label.b()) {
  current = label.b();
@@ -760,17 +761,20 @@ void morphologyOperation(
  };
 
  // Apply the central shape row.
- // This operation is applicable to all rows of the image, because at any position of the sliding window,
+ // This operation is applicable to all rows of the image,
+ // because at any position of the sliding window,
  // its central row is located on the image. So we start from the zero line of input and output
  probeRows(input, 0, output, 0, shape.row(1), set);
 
  if (input.rows() > 1) {
  // Apply the top shape row.
  // In the uppermost position of the sliding window, its first row is outside the image border.
- // Therefore, we start filling the output image starting from the line 1 and will process input.rows() - 1 lines in total
+ // Therefore, we start filling the output image starting from the line 1 and will process
+ // input.rows() - 1 lines in total
  probeRows(input, 0, output, 1, shape.row(0), update);
  // Apply the bottom shape row.
- // Similarly, the input image starting from the line 1 and will process input.rows() - 1 lines in total
+ // Similarly, the input image starting from the line 1 and will process
+ // input.rows() - 1 lines in total
  probeRows(input, 1, output, 0, shape.row(2), update);
  }
 }
@@ -812,8 +816,8 @@ Label connectedComponentsImpl(
  const Image<uint8_t> & image, Image<Label> & labels,
  imgproc_impl::EquivalenceLabelTrees<Label> & label_trees, const IsBg & is_background)
 {
- using namespace imgproc_impl;
- using PixelPass = ProcessPixel<connectivity>;
+ using namespace imgproc_impl; // NOLINT
+ using PixelPass = ProcessPixel<connectivity>; // NOLINT
 
  // scanning phase
  // scan row 0
@@ -1036,7 +1040,7 @@ Image<Label> connectedComponents(
  const IsBg & is_background,
  Label & total_labels)
 {
- using namespace imgproc_impl;
+ using namespace imgproc_impl; // NOLINT
  const size_t pixels = image.rows() * image.columns();
 
  if (pixels == 0) {

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -144,7 +144,13 @@ nav2_util::CallbackReturn
 Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
 {
  RCLCPP_INFO(get_logger(), "Configuring");
- getParameters();
+ try {
+ getParameters();
+ } catch (const std::exception & e) {
+ RCLCPP_ERROR(
+ get_logger(), "Failed to configure costmap! %s.", e.what());
+ return nav2_util::CallbackReturn::FAILURE;
+ }
 
  callback_group_ = create_callback_group(
  rclcpp::CallbackGroupType::MutuallyExclusive, false);

nav2_planner/src/planner_server.cpp

@@ -110,7 +110,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
  planner_ids_[i].c_str(), planner_types_[i].c_str());
  planner->configure(node, planner_ids_[i], tf_, costmap_ros_);
  planners_.insert({planner_ids_[i], planner});
- } catch (const pluginlib::PluginlibException & ex) {
+ } catch (const std::exception & ex) {
  RCLCPP_FATAL(
  get_logger(), "Failed to create global planner. Exception: %s",
  ex.what());

nav2_smoother/src/nav2_smoother.cpp

@@ -142,7 +142,7 @@ bool SmootherServer::loadSmootherPlugins()
  node, smoother_ids_[i], tf_, costmap_sub_,
  footprint_sub_);
  smoothers_.insert({smoother_ids_[i], smoother});
- } catch (const pluginlib::PluginlibException & ex) {
+ } catch (const std::exception & ex) {
  RCLCPP_FATAL(
  get_logger(), "Failed to create smoother. Exception: %s",
  ex.what());