remove dead code from costmap_2d (#2020)

nav2_costmap_2d/include/nav2_costmap_2d/costmap_math.hpp

@@ -63,10 +63,4 @@ inline double distance(double x0, double y0, double x1, double y1)
 
 double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1);
 
-bool intersects(std::vector<geometry_msgs::msg::Point> & polygon, float testx, float testy);
-
-bool intersects(
-  std::vector<geometry_msgs::msg::Point> & polygon1,
-  std::vector<geometry_msgs::msg::Point> & polygon2);
-
 #endif  // NAV2_COSTMAP_2D__COSTMAP_MATH_HPP_

nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp

@@ -129,7 +129,6 @@ class Layer  // TODO(mjeronimo): public nav2_util::LifecycleHelperInterface
   void declareParameter(
     const std::string & param_name);
   bool hasParameter(const std::string & param_name);
-  void undeclareAllParameters();
   std::string getFullName(const std::string & param_name);
 
 protected:

nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp

@@ -87,15 +87,6 @@ class ObservationBuffer
    */
   ~ObservationBuffer();
 
-  /**
-   * @brief Sets the global frame of an observation buffer. This will
-   * transform all the currently cached observations to the new global
-   * frame
-   * @param new_global_frame The name of the new global frame.
-   * @return True if the operation succeeds, false otherwise
-   */
-  bool setGlobalFrame(const std::string new_global_frame);
-
   /**
    * @brief  Transforms a PointCloud to the global frame and buffers it
    * <b>Note: The burden is on the user to make sure the transform is available... ie they should use a MessageNotifier</b>

nav2_costmap_2d/src/costmap_math.cpp

@@ -57,36 +57,3 @@ double distanceToLine(double pX, double pY, double x0, double y0, double x1, dou
 
   return distance(pX, pY, xx, yy);
 }
-
-bool intersects(std::vector<geometry_msgs::msg::Point> & polygon, float testx, float testy)
-{
-  bool c = false;
-  int i, j, nvert = polygon.size();
-  for (i = 0, j = nvert - 1; i < nvert; j = i++) {
-    float yi = polygon[i].y, yj = polygon[j].y, xi = polygon[i].x, xj = polygon[j].x;
-
-    if (((yi > testy) != (yj > testy)) && (testx < (xj - xi) * (testy - yi) / (yj - yi) + xi)) {
-      c = !c;
-    }
-  }
-  return c;
-}
-
-bool intersects_helper(
-  std::vector<geometry_msgs::msg::Point> & polygon1,
-  std::vector<geometry_msgs::msg::Point> & polygon2)
-{
-  for (unsigned int i = 0; i < polygon1.size(); i++) {
-    if (intersects(polygon2, polygon1[i].x, polygon1[i].y)) {
-      return true;
-    }
-  }
-  return false;
-}
-
-bool intersects(
-  std::vector<geometry_msgs::msg::Point> & polygon1,
-  std::vector<geometry_msgs::msg::Point> & polygon2)
-{
-  return intersects_helper(polygon1, polygon2) || intersects_helper(polygon2, polygon1);
-}

nav2_costmap_2d/src/layer.cpp

@@ -112,19 +112,6 @@ Layer::hasParameter(const std::string & param_name)
   return node->has_parameter(getFullName(param_name));
 }
 
-void
-Layer::undeclareAllParameters()
-{
-  auto node = node_.lock();
-  if (!node) {
-    throw std::runtime_error{"Failed to lock node"};
-  }
-  for (auto & param_name : local_params_) {
-    node->undeclare_parameter(getFullName(param_name));
-  }
-  local_params_.clear();
-}
-
 std::string
 Layer::getFullName(const std::string & param_name)
 {

nav2_costmap_2d/src/observation_buffer.cpp

@@ -73,54 +73,6 @@ ObservationBuffer::~ObservationBuffer()
 {
 }
 
-bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
-{
-  rclcpp::Time transform_time = clock_->now();
-  std::string tf_error;
-
-  geometry_msgs::msg::TransformStamped transformStamped;
-  if (!tf2_buffer_.canTransform(
-      new_global_frame, global_frame_, tf2_ros::fromMsg(transform_time),
-      tf2::durationFromSec(tf_tolerance_), &tf_error))
-  {
-    RCLCPP_ERROR(
-      logger_, "Transform between %s and %s with tolerance %.2f failed: %s.",
-      new_global_frame.c_str(),
-      global_frame_.c_str(), tf_tolerance_, tf_error.c_str());
-    return false;
-  }
-
-  std::list<Observation>::iterator obs_it;
-  for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it) {
-    try {
-      Observation & obs = *obs_it;
-
-      geometry_msgs::msg::PointStamped origin;
-      origin.header.frame_id = global_frame_;
-      origin.header.stamp = transform_time;
-      origin.point = obs.origin_;
-
-      // we need to transform the origin of the observation to the new global frame
-      tf2_buffer_.transform(origin, origin, new_global_frame, tf2::durationFromSec(tf_tolerance_));
-      obs.origin_ = origin.point;
-
-      // we also need to transform the cloud of the observation to the new global frame
-      tf2_buffer_.transform(
-        *(obs.cloud_), *(obs.cloud_), new_global_frame, tf2::durationFromSec(tf_tolerance_));
-    } catch (tf2::TransformException & ex) {
-      RCLCPP_ERROR(
-        logger_, "TF Error attempting to transform an observation from %s to %s: %s",
-        global_frame_.c_str(),
-        new_global_frame.c_str(), ex.what());
-      return false;
-    }
-  }
-
-  // now we need to update our global_frame member
-  global_frame_ = new_global_frame;
-  return true;
-}
-
 void ObservationBuffer::bufferCloud(const sensor_msgs::msg::PointCloud2 & cloud)
 {
   geometry_msgs::msg::PointStamped global_origin;