Fix SmacPlanner memory leaks and costmap downsampler test (#2050)

naiveHobo · web-flow · commit 8ba925cb8dd7 · 2020-11-09T12:33:02.000-08:00
* Fix shared_ptr cyclic dependencies

Signed-off-by: Sarthak Mittal &lt;sarthakmittal2608@gmail.com&gt;

* Refactor costmap downsampler and fix test

Signed-off-by: Sarthak Mittal &lt;sarthakmittal2608@gmail.com&gt;

* Revert costmap downsampler test changes

Signed-off-by: Sarthak Mittal &lt;sarthakmittal2608@gmail.com&gt;
diff --git a/smac_planner/include/smac_planner/costmap_downsampler.hpp b/smac_planner/include/smac_planner/costmap_downsampler.hpp
@@ -34,23 +34,24 @@ class CostmapDownsampler
 public:
   /**
    * @brief A constructor for CostmapDownsampler
-   * @param node Lifecycle node pointer
    */
-  explicit CostmapDownsampler(const nav2_util::LifecycleNode::SharedPtr & node);
+  CostmapDownsampler();
 
   /**
    * @brief A destructor for CostmapDownsampler
    */
   ~CostmapDownsampler();
 
   /**
-   * @brief Initialize the downsampled costmap object and the ROS publisher
+   * @brief Configure the downsampled costmap object and the ROS publisher
+   * @param node Lifecycle node pointer
    * @param global_frame The ID of the global frame used by the costmap
    * @param topic_name The name of the topic to publish the downsampled costmap
    * @param costmap The costmap we want to downsample
    * @param downsampling_factor Multiplier for the costmap resolution
    */
-  void initialize(
+  void on_configure(
+    const nav2_util::LifecycleNode::WeakPtr & node,
     const std::string & global_frame,
     const std::string & topic_name,
     nav2_costmap_2d::Costmap2D * const costmap,
@@ -59,18 +60,17 @@ class CostmapDownsampler
   /**
    * @brief Activate the publisher of the downsampled costmap
    */
-  void activatePublisher()
-  {
-    _downsampled_costmap_pub->on_activate();
-  }
+  void on_activate();
 
   /**
    * @brief Deactivate the publisher of the downsampled costmap
    */
-  void deactivatePublisher()
-  {
-    _downsampled_costmap_pub->on_deactivate();
-  }
+  void on_deactivate();
+
+  /**
+   * @brief Cleanup the publisher of the downsampled costmap
+   */
+  void on_cleanup();
 
   /**
    * @brief Downsample the given costmap by the downsampling factor, and publish the downsampled costmap
@@ -105,8 +105,6 @@ class CostmapDownsampler
   unsigned int _downsampled_size_y;
   unsigned int _downsampling_factor;
   float _downsampled_resolution;
-  std::string _topic_name;
-  nav2_util::LifecycleNode::SharedPtr _node;
   nav2_costmap_2d::Costmap2D * _costmap;
   std::unique_ptr<nav2_costmap_2d::Costmap2D> _downsampled_costmap;
   std::unique_ptr<nav2_costmap_2d::Costmap2DPublisher> _downsampled_costmap_pub;
diff --git a/smac_planner/include/smac_planner/smac_planner.hpp b/smac_planner/include/smac_planner/smac_planner.hpp
@@ -111,7 +111,8 @@ class SmacPlanner : public nav2_core::GlobalPlanner
 protected:
   std::unique_ptr<AStarAlgorithm<NodeSE2>> _a_star;
   std::unique_ptr<Smoother> _smoother;
-  nav2_util::LifecycleNode::SharedPtr _node;
+  rclcpp::Clock::SharedPtr _clock;
+  rclcpp::Logger _logger{rclcpp::get_logger("SmacPlanner")};
   nav2_costmap_2d::Costmap2D * _costmap;
   std::unique_ptr<CostmapDownsampler> _costmap_downsampler;
   std::string _global_frame, _name;
diff --git a/smac_planner/include/smac_planner/smac_planner_2d.hpp b/smac_planner/include/smac_planner/smac_planner_2d.hpp
@@ -106,7 +106,8 @@ class SmacPlanner2D : public nav2_core::GlobalPlanner
   std::unique_ptr<Smoother> _smoother;
   nav2_costmap_2d::Costmap2D * _costmap;
   std::unique_ptr<CostmapDownsampler> _costmap_downsampler;
-  nav2_util::LifecycleNode::SharedPtr _node;
+  rclcpp::Clock::SharedPtr _clock;
+  rclcpp::Logger _logger{rclcpp::get_logger("SmacPlanner2D")};
   std::string _global_frame, _name;
   float _tolerance;
   int _downsampling_factor;
diff --git a/smac_planner/src/costmap_downsampler.cpp b/smac_planner/src/costmap_downsampler.cpp
@@ -22,9 +22,8 @@
 namespace smac_planner
 {
 
-CostmapDownsampler::CostmapDownsampler(const nav2_util::LifecycleNode::SharedPtr & node)
-: _node(node),
-  _costmap(nullptr),
+CostmapDownsampler::CostmapDownsampler()
+: _costmap(nullptr),
   _downsampled_costmap(nullptr),
   _downsampled_costmap_pub(nullptr)
 {
@@ -34,13 +33,13 @@ CostmapDownsampler::~CostmapDownsampler()
 {
 }
 
-void CostmapDownsampler::initialize(
+void CostmapDownsampler::on_configure(
+  const nav2_util::LifecycleNode::WeakPtr & node,
   const std::string & global_frame,
   const std::string & topic_name,
   nav2_costmap_2d::Costmap2D * const costmap,
   const unsigned int & downsampling_factor)
 {
-  _topic_name = topic_name;
   _costmap = costmap;
   _downsampling_factor = downsampling_factor;
   updateCostmapSize();
@@ -50,7 +49,24 @@ void CostmapDownsampler::initialize(
     _costmap->getOriginX(), _costmap->getOriginY(), UNKNOWN);
 
   _downsampled_costmap_pub = std::make_unique<nav2_costmap_2d::Costmap2DPublisher>(
-    _node, _downsampled_costmap.get(), global_frame, _topic_name, false);
+    node, _downsampled_costmap.get(), global_frame, topic_name, false);
+}
+
+void CostmapDownsampler::on_activate()
+{
+  _downsampled_costmap_pub->on_activate();
+}
+
+void CostmapDownsampler::on_deactivate()
+{
+  _downsampled_costmap_pub->on_deactivate();
+}
+
+void CostmapDownsampler::on_cleanup()
+{
+  _costmap = nullptr;
+  _downsampled_costmap.reset();
+  _downsampled_costmap_pub.reset();
 }
 
 nav2_costmap_2d::Costmap2D * CostmapDownsampler::downsample(
@@ -74,10 +90,7 @@ nav2_costmap_2d::Costmap2D * CostmapDownsampler::downsample(
     }
   }
 
-  if (_node->count_subscribers(_topic_name) > 0) {
-    _downsampled_costmap_pub->publishCostmap();
-  }
-
+  _downsampled_costmap_pub->publishCostmap();
   return _downsampled_costmap.get();
 }
 
diff --git a/smac_planner/src/smac_planner.cpp b/smac_planner/src/smac_planner.cpp
@@ -31,7 +31,6 @@ using namespace std::chrono;  // NOLINT
 SmacPlanner::SmacPlanner()
 : _a_star(nullptr),
   _smoother(nullptr),
-  _node(nullptr),
   _costmap(nullptr),
   _costmap_downsampler(nullptr)
 {
@@ -40,7 +39,7 @@ SmacPlanner::SmacPlanner()
 SmacPlanner::~SmacPlanner()
 {
   RCLCPP_INFO(
-    _node->get_logger(), "Destroying plugin %s of type SmacPlanner",
+    _logger, "Destroying plugin %s of type SmacPlanner",
     _name.c_str());
 }
 
@@ -49,7 +48,9 @@ void SmacPlanner::configure(
   std::string name, std::shared_ptr<tf2_ros::Buffer>/*tf*/,
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
 {
-  _node = parent.lock();
+  auto node = parent.lock();
+  _logger = node->get_logger();
+  _clock = node->get_clock();
   _costmap = costmap_ros->getCostmap();
   _name = name;
   _global_frame = costmap_ros->getGlobalFrameID();
@@ -64,77 +65,77 @@ void SmacPlanner::configure(
 
   // General planner params
   nav2_util::declare_parameter_if_not_declared(
-    _node, name + ".tolerance", rclcpp::ParameterValue(0.125));
-  _tolerance = static_cast<float>(_node->get_parameter(name + ".tolerance").as_double());
+    node, name + ".tolerance", rclcpp::ParameterValue(0.125));
+  _tolerance = static_cast<float>(node->get_parameter(name + ".tolerance").as_double());
   nav2_util::declare_parameter_if_not_declared(
-    _node, name + ".downsample_costmap", rclcpp::ParameterValue(true));
-  _node->get_parameter(name + ".downsample_costmap", _downsample_costmap);
+    node, name + ".downsample_costmap", rclcpp::ParameterValue(true));
+  node->get_parameter(name + ".downsample_costmap", _downsample_costmap);
   nav2_util::declare_parameter_if_not_declared(
-    _node, name + ".downsampling_factor", rclcpp::ParameterValue(1));
-  _node->get_parameter(name + ".downsampling_factor", _downsampling_factor);
+    node, name + ".downsampling_factor", rclcpp::ParameterValue(1));
+  node->get_parameter(name + ".downsampling_factor", _downsampling_factor);
 
   nav2_util::declare_parameter_if_not_declared(
-    _node, name + ".angle_quantization_bins", rclcpp::ParameterValue(1));
-  _node->get_parameter(name + ".angle_quantization_bins", angle_quantizations);
+    node, name + ".angle_quantization_bins", rclcpp::ParameterValue(1));
+  node->get_parameter(name + ".angle_quantization_bins", angle_quantizations);
   _angle_bin_size = 2.0 * M_PI / angle_quantizations;
   _angle_quantizations = static_cast<unsigned int>(angle_quantizations);
 
   nav2_util::declare_parameter_if_not_declared(
-    _node, name + ".allow_unknown", rclcpp::ParameterValue(true));
-  _node->get_parameter(name + ".allow_unknown", allow_unknown);
+    node, name + ".allow_unknown", rclcpp::ParameterValue(true));
+  node->get_parameter(name + ".allow_unknown", allow_unknown);
   nav2_util::declare_parameter_if_not_declared(
-    _node, name + ".max_iterations", rclcpp::ParameterValue(-1));
-  _node->get_parameter(name + ".max_iterations", max_iterations);
+    node, name + ".max_iterations", rclcpp::ParameterValue(-1));
+  node->get_parameter(name + ".max_iterations", max_iterations);
   nav2_util::declare_parameter_if_not_declared(
-    _node, name + ".smooth_path", rclcpp::ParameterValue(false));
-  _node->get_parameter(name + ".smooth_path", smooth_path);
+    node, name + ".smooth_path", rclcpp::ParameterValue(false));
+  node->get_parameter(name + ".smooth_path", smooth_path);
 
   nav2_util::declare_parameter_if_not_declared(
-    _node, name + ".minimum_turning_radius", rclcpp::ParameterValue(0.2));
-  _node->get_parameter(name + ".minimum_turning_radius", search_info.minimum_turning_radius);
+    node, name + ".minimum_turning_radius", rclcpp::ParameterValue(0.2));
+  node->get_parameter(name + ".minimum_turning_radius", search_info.minimum_turning_radius);
 
   nav2_util::declare_parameter_if_not_declared(
-    _node, name + ".reverse_penalty", rclcpp::ParameterValue(2.0));
-  _node->get_parameter(name + ".reverse_penalty", search_info.reverse_penalty);
+    node, name + ".reverse_penalty", rclcpp::ParameterValue(2.0));
+  node->get_parameter(name + ".reverse_penalty", search_info.reverse_penalty);
   nav2_util::declare_parameter_if_not_declared(
-    _node, name + ".change_penalty", rclcpp::ParameterValue(0.5));
-  _node->get_parameter(name + ".change_penalty", search_info.change_penalty);
+    node, name + ".change_penalty", rclcpp::ParameterValue(0.5));
+  node->get_parameter(name + ".change_penalty", search_info.change_penalty);
   nav2_util::declare_parameter_if_not_declared(
-    _node, name + ".non_straight_penalty", rclcpp::ParameterValue(1.05));
-  _node->get_parameter(name + ".non_straight_penalty", search_info.non_straight_penalty);
+    node, name + ".non_straight_penalty", rclcpp::ParameterValue(1.05));
+  node->get_parameter(name + ".non_straight_penalty", search_info.non_straight_penalty);
   nav2_util::declare_parameter_if_not_declared(
-    _node, name + ".cost_penalty", rclcpp::ParameterValue(1.2));
-  _node->get_parameter(name + ".cost_penalty", search_info.cost_penalty);
+    node, name + ".cost_penalty", rclcpp::ParameterValue(1.2));
+  node->get_parameter(name + ".cost_penalty", search_info.cost_penalty);
   nav2_util::declare_parameter_if_not_declared(
-    _node, name + ".analytic_expansion_ratio", rclcpp::ParameterValue(2.0));
-  _node->get_parameter(name + ".analytic_expansion_ratio", search_info.analytic_expansion_ratio);
+    node, name + ".analytic_expansion_ratio", rclcpp::ParameterValue(2.0));
+  node->get_parameter(name + ".analytic_expansion_ratio", search_info.analytic_expansion_ratio);
 
   nav2_util::declare_parameter_if_not_declared(
-    _node, name + ".max_planning_time_ms", rclcpp::ParameterValue(5000.0));
-  _node->get_parameter(name + ".max_planning_time_ms", _max_planning_time);
+    node, name + ".max_planning_time_ms", rclcpp::ParameterValue(5000.0));
+  node->get_parameter(name + ".max_planning_time_ms", _max_planning_time);
 
   nav2_util::declare_parameter_if_not_declared(
-    _node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("DUBIN")));
-  _node->get_parameter(name + ".motion_model_for_search", motion_model_for_search);
+    node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("DUBIN")));
+  node->get_parameter(name + ".motion_model_for_search", motion_model_for_search);
   MotionModel motion_model = fromString(motion_model_for_search);
   if (motion_model == MotionModel::UNKNOWN) {
     RCLCPP_WARN(
-      _node->get_logger(),
+      _logger,
       "Unable to get MotionModel search type. Given '%s', "
       "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.",
       motion_model_for_search.c_str());
   }
 
   if (max_on_approach_iterations <= 0) {
     RCLCPP_INFO(
-      _node->get_logger(), "On approach iteration selected as <= 0, "
+      _logger, "On approach iteration selected as <= 0, "
       "disabling tolerance and on approach iterations.");
     max_on_approach_iterations = std::numeric_limits<int>::max();
   }
 
   if (max_iterations <= 0) {
     RCLCPP_INFO(
-      _node->get_logger(), "maximum iteration selected as <= 0, "
+      _logger, "maximum iteration selected as <= 0, "
       "disabling maximum iterations.");
     max_iterations = std::numeric_limits<int>::max();
   }
@@ -153,22 +154,23 @@ void SmacPlanner::configure(
 
   if (smooth_path) {
     _smoother = std::make_unique<Smoother>();
-    _optimizer_params.get(_node.get(), name);
-    _smoother_params.get(_node.get(), name);
+    _optimizer_params.get(node.get(), name);
+    _smoother_params.get(node.get(), name);
     _smoother_params.max_curvature = 1.0f / minimum_turning_radius_global_coords;
     _smoother->initialize(_optimizer_params);
   }
 
   if (_downsample_costmap && _downsampling_factor > 1) {
     std::string topic_name = "downsampled_costmap";
-    _costmap_downsampler = std::make_unique<CostmapDownsampler>(_node);
-    _costmap_downsampler->initialize(_global_frame, topic_name, _costmap, _downsampling_factor);
+    _costmap_downsampler = std::make_unique<CostmapDownsampler>();
+    _costmap_downsampler->on_configure(
+      node, _global_frame, topic_name, _costmap, _downsampling_factor);
   }
 
-  _raw_plan_publisher = _node->create_publisher<nav_msgs::msg::Path>("unsmoothed_plan", 1);
+  _raw_plan_publisher = node->create_publisher<nav_msgs::msg::Path>("unsmoothed_plan", 1);
 
   RCLCPP_INFO(
-    _node->get_logger(), "Configured plugin %s of type SmacPlanner with "
+    _logger, "Configured plugin %s of type SmacPlanner with "
     "tolerance %.2f, maximum iterations %i, "
     "max on approach iterations %i, and %s. Using motion model: %s.",
     _name.c_str(), _tolerance, max_iterations, max_on_approach_iterations,
@@ -179,32 +181,33 @@ void SmacPlanner::configure(
 void SmacPlanner::activate()
 {
   RCLCPP_INFO(
-    _node->get_logger(), "Activating plugin %s of type SmacPlanner",
+    _logger, "Activating plugin %s of type SmacPlanner",
     _name.c_str());
   _raw_plan_publisher->on_activate();
   if (_costmap_downsampler) {
-    _costmap_downsampler->activatePublisher();
+    _costmap_downsampler->on_activate();
   }
 }
 
 void SmacPlanner::deactivate()
 {
   RCLCPP_INFO(
-    _node->get_logger(), "Deactivating plugin %s of type SmacPlanner",
+    _logger, "Deactivating plugin %s of type SmacPlanner",
     _name.c_str());
   _raw_plan_publisher->on_deactivate();
   if (_costmap_downsampler) {
-    _costmap_downsampler->deactivatePublisher();
+    _costmap_downsampler->on_deactivate();
   }
 }
 
 void SmacPlanner::cleanup()
 {
   RCLCPP_INFO(
-    _node->get_logger(), "Cleaning up plugin %s of type SmacPlanner",
+    _logger, "Cleaning up plugin %s of type SmacPlanner",
     _name.c_str());
   _a_star.reset();
   _smoother.reset();
+  _costmap_downsampler->on_cleanup();
   _costmap_downsampler.reset();
   _raw_plan_publisher.reset();
 }
@@ -251,7 +254,7 @@ nav_msgs::msg::Path SmacPlanner::createPlan(
 
   // Setup message
   nav_msgs::msg::Path plan;
-  plan.header.stamp = _node->now();
+  plan.header.stamp = _clock->now();
   plan.header.frame_id = _global_frame;
   geometry_msgs::msg::PoseStamped pose;
   pose.header = plan.header;
@@ -282,7 +285,7 @@ nav_msgs::msg::Path SmacPlanner::createPlan(
 
   if (!error.empty()) {
     RCLCPP_WARN(
-      _node->get_logger(),
+      _logger,
       "%s: failed to create plan, %s.",
       _name.c_str(), error.c_str());
     return plan;
@@ -304,7 +307,7 @@ nav_msgs::msg::Path SmacPlanner::createPlan(
   }
 
   // Publish raw path for debug
-  if (_node->count_subscribers(_raw_plan_publisher->get_topic_name()) > 0) {
+  if (_raw_plan_publisher->get_subscription_count() > 0) {
     _raw_plan_publisher->publish(plan);
   }
 
@@ -328,7 +331,7 @@ nav_msgs::msg::Path SmacPlanner::createPlan(
   // Smooth plan
   if (!_smoother->smooth(path_world, costmap, _smoother_params)) {
     RCLCPP_WARN(
-      _node->get_logger(),
+      _logger,
       "%s: failed to smooth plan, Ceres could not find a usable solution to optimize.",
       _name.c_str());
     return plan;
diff --git a/smac_planner/src/smac_planner_2d.cpp b/smac_planner/src/smac_planner_2d.cpp
diff --git a/smac_planner/test/test_costmap_downsampler.cpp b/smac_planner/test/test_costmap_downsampler.cpp
