X-lanni
105 lines
2.9 KiB
C++
105 lines
2.9 KiB
C++
// Copyright (c) 2022 Samsung R&D Institute Russia
|
|
//
|
|
// Licensed under the Apache License, Version 2.0 (the "License");
|
|
// you may not use this file except in compliance with the License.
|
|
// You may obtain a copy of the License at
|
|
//
|
|
// http://www.apache.org/licenses/LICENSE-2.0
|
|
//
|
|
// Unless required by applicable law or agreed to in writing, software
|
|
// distributed under the License is distributed on an "AS IS" BASIS,
|
|
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
// See the License for the specific language governing permissions and
|
|
// limitations under the License.
|
|
|
|
#include "nav2_collision_monitor/circle.hpp"
|
|
|
|
#include <math.h>
|
|
#include <cmath>
|
|
#include <exception>
|
|
|
|
#include "nav2_util/node_utils.hpp"
|
|
|
|
namespace nav2_collision_monitor
|
|
{
|
|
|
|
Circle::Circle(
|
|
const nav2_util::LifecycleNode::WeakPtr & node,
|
|
const std::string & polygon_name,
|
|
const std::shared_ptr<tf2_ros::Buffer> tf_buffer,
|
|
const std::string & base_frame_id,
|
|
const tf2::Duration & transform_tolerance)
|
|
: Polygon::Polygon(node, polygon_name, tf_buffer, base_frame_id, transform_tolerance)
|
|
{
|
|
RCLCPP_INFO(logger_, "[%s]: Creating Circle", polygon_name_.c_str());
|
|
}
|
|
|
|
Circle::~Circle()
|
|
{
|
|
RCLCPP_INFO(logger_, "[%s]: Destroying Circle", polygon_name_.c_str());
|
|
}
|
|
|
|
void Circle::getPolygon(std::vector<Point> & poly) const
|
|
{
|
|
// Number of polygon points. More edges means better approximation.
|
|
const double polygon_edges = 16;
|
|
// Increment of angle during points position calculation
|
|
double angle_increment = 2 * M_PI / polygon_edges;
|
|
|
|
// Clear polygon before filling
|
|
poly.clear();
|
|
|
|
// Making new polygon looks like a circle
|
|
Point p;
|
|
for (double angle = 0.0; angle < 2 * M_PI; angle += angle_increment) {
|
|
p.x = radius_ * std::cos(angle);
|
|
p.y = radius_ * std::sin(angle);
|
|
poly.push_back(p);
|
|
}
|
|
}
|
|
|
|
int Circle::getPointsInside(const std::vector<Point> & points) const
|
|
{
|
|
int num = 0;
|
|
for (Point point : points) {
|
|
if (point.x * point.x + point.y * point.y < radius_squared_) {
|
|
num++;
|
|
}
|
|
}
|
|
|
|
return num;
|
|
}
|
|
|
|
bool Circle::getParameters(std::string & polygon_pub_topic, std::string & footprint_topic)
|
|
{
|
|
auto node = node_.lock();
|
|
if (!node) {
|
|
throw std::runtime_error{"Failed to lock node"};
|
|
}
|
|
|
|
if (!getCommonParameters(polygon_pub_topic)) {
|
|
return false;
|
|
}
|
|
|
|
// There is no footprint subscription for the Circle. Thus, set string as empty.
|
|
footprint_topic.clear();
|
|
|
|
try {
|
|
// Leave it not initialized: the will cause an error if it will not set
|
|
nav2_util::declare_parameter_if_not_declared(
|
|
node, polygon_name_ + ".radius", rclcpp::PARAMETER_DOUBLE);
|
|
radius_ = node->get_parameter(polygon_name_ + ".radius").as_double();
|
|
radius_squared_ = radius_ * radius_;
|
|
} catch (const std::exception & ex) {
|
|
RCLCPP_ERROR(
|
|
logger_,
|
|
"[%s]: Error while getting circle parameters: %s",
|
|
polygon_name_.c_str(), ex.what());
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
} // namespace nav2_collision_monitor
|