diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_math.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_math.hpp
index 83d656842a254cd791bed16fe92ef1b99c12b5b5..795bac53fc46dde14b79a6ffc128ef9998b0d9f5 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_math.hpp
+++ b/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_
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp
index 4f7aa00ac0c50bd379a833981ac517606a546baf..2012dbf8f633a9bb2951c0b68507bef14508acbc 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp
@@ -129,7 +129,6 @@ public:
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:
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp
index 30b03f33161ceaf648c2b7b366596ed56b2d0196..ba619e14b138ff6f256c523f7f1e096dc81ffc03 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp
@@ -87,15 +87,6 @@ public:
*/
~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>
diff --git a/nav2_costmap_2d/src/costmap_math.cpp b/nav2_costmap_2d/src/costmap_math.cpp
index 7de87fa6f2f0f13268f4323e27d296a35ebc30aa..9d376e6e6d579b4e11ba3a40fe84c0658fc3b46e 100644
--- a/nav2_costmap_2d/src/costmap_math.cpp
+++ 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);
-}
diff --git a/nav2_costmap_2d/src/layer.cpp b/nav2_costmap_2d/src/layer.cpp
index aad00008a4f802a434ca69d05c85ad17db95ec73..0eacd5c3e6375c0d67cd241513cf4c62830bf1f4 100644
--- a/nav2_costmap_2d/src/layer.cpp
+++ b/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)
{
diff --git a/nav2_costmap_2d/src/observation_buffer.cpp b/nav2_costmap_2d/src/observation_buffer.cpp
index e771b2f96f039c5a5cda4139f6648b95376cce97..9d37eacf696f89cc547e9fc461340256b141c13a 100644
--- a/nav2_costmap_2d/src/observation_buffer.cpp
+++ b/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;