108 lines
2.9 KiB
C++
108 lines
2.9 KiB
C++
|
|
#include "toid_costmaps/rival_layer.hpp"
|
|
|
|
#include "geometry_msgs/msg/transform_stamped.hpp"
|
|
#include "nav2_util/node_utils.hpp"
|
|
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
|
|
|
|
namespace toid
|
|
{
|
|
|
|
void RivalLayer::onInitialize()
|
|
{
|
|
auto node = node_.lock();
|
|
if (!node) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
void RivalLayer::activate()
|
|
{
|
|
auto node = node_.lock();
|
|
|
|
nav2_util::declare_parameter_if_not_declared(
|
|
node, name_ + ".rival_size", rclcpp::ParameterValue(0.15));
|
|
node->get_parameter(name_ + ".rival_size", rival_size_);
|
|
|
|
using namespace std::placeholders;
|
|
rival_sub_ = node->create_subscription<Rivals>(
|
|
"/dynamicObstacle", 1, std::bind(&RivalLayer::rival_cb, this, _1));
|
|
}
|
|
|
|
void RivalLayer::deactivate()
|
|
{
|
|
auto node = node_.lock();
|
|
rival_sub_.reset();
|
|
rivals_.reset();
|
|
}
|
|
|
|
void RivalLayer::rival_cb(Rivals::SharedPtr msg)
|
|
{
|
|
if (msg->point.size() != 0 || debounce_++ > 10) {
|
|
rivals_ = msg;
|
|
debounce_ = 0;
|
|
}
|
|
}
|
|
|
|
void RivalLayer::updateBounds(
|
|
double, double, double, double * min_x, double * min_y, double * max_x, double * max_y)
|
|
{
|
|
touch(1.50, 1.0, min_x, min_y, max_x, max_y);
|
|
touch(-1.50, -1.0, min_x, min_y, max_x, max_y);
|
|
}
|
|
|
|
void RivalLayer::updateCosts(nav2_costmap_2d::Costmap2D & grid, int, int, int, int)
|
|
{
|
|
geometry_msgs::msg::TransformStamped tf_msg;
|
|
if(!rivals_) {
|
|
return;
|
|
}
|
|
|
|
try {
|
|
tf_msg = tf_->lookupTransform(
|
|
layered_costmap_->getGlobalFrameID(), rivals_->header.frame_id, rivals_->header.stamp);
|
|
} catch (const tf2::TransformException & e) {
|
|
RCLCPP_WARN_THROTTLE(logger_, *clock_, 1000, "Failed to transform rival message to costmap");
|
|
return;
|
|
}
|
|
|
|
for (const auto & rival : rivals_->point) {
|
|
geometry_msgs::msg::Point point;
|
|
tf2::doTransform(rival, point, tf_msg);
|
|
placeRival(grid, point.x, point.y);
|
|
}
|
|
}
|
|
|
|
void RivalLayer::placeRival(nav2_costmap_2d::Costmap2D & grid, const double x, const double y)
|
|
{
|
|
unsigned int mx, my;
|
|
if (!grid.worldToMap(x, y, mx, my)) {
|
|
return; // Center is outside the map bounds
|
|
}
|
|
|
|
double res = grid.getResolution();
|
|
int cell_radius = static_cast<int>(rival_size_ / res);
|
|
|
|
int min_i = std::max(0, static_cast<int>(mx) - cell_radius);
|
|
int max_i =
|
|
std::min(static_cast<int>(grid.getSizeInCellsX()) - 1, static_cast<int>(mx) + cell_radius);
|
|
int min_j = std::max(0, static_cast<int>(my) - cell_radius);
|
|
int max_j =
|
|
std::min(static_cast<int>(grid.getSizeInCellsY()) - 1, static_cast<int>(my) + cell_radius);
|
|
|
|
for (int i = min_i; i <= max_i; ++i) {
|
|
for (int j = min_j; j <= max_j; ++j) {
|
|
double di = static_cast<double>(i) - static_cast<double>(mx);
|
|
double dj = static_cast<double>(j) - static_cast<double>(my);
|
|
double distance_sq = di * di + dj * dj;
|
|
if (distance_sq <= static_cast<double>(cell_radius * cell_radius)) {
|
|
grid.setCost(i, j, nav2_costmap_2d::LETHAL_OBSTACLE);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
} // namespace toid
|
|
|
|
#include "pluginlib/class_list_macros.hpp"
|
|
PLUGINLIB_EXPORT_CLASS(toid::RivalLayer, nav2_costmap_2d::Layer); |