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 83d656842a2..795bac53fc4 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 & polygon, float testx, float testy); - -bool intersects( - std::vector & polygon1, - std::vector & 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 4f7aa00ac0c..2012dbf8f63 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 @@ 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: 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 30b03f33161..ba619e14b13 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 @@ 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 * Note: The burden is on the user to make sure the transform is available... ie they should use a MessageNotifier diff --git a/nav2_costmap_2d/src/costmap_math.cpp b/nav2_costmap_2d/src/costmap_math.cpp index 7de87fa6f2f..9d376e6e6d5 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 & 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 & polygon1, - std::vector & 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 & polygon1, - std::vector & 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 aad00008a4f..0eacd5c3e63 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 e771b2f96f0..9d37eacf696 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::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;