DuongTD/costmap_2d
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

them file test static_layer

Browse Source
This commit is contained in:
duongtd
2025-11-07 17:44:42 +07:00
parent 79e706b798
commit 498b606e15
148 changed files with 6363 additions and 1599 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
plugins/critical_layer.cpp
View File

@@ -1,8 +1,5 @@
#include <costmap_2d/critical_layer.h>
#include <boost/dll/alias.hpp>
#include "shared.h"
PLUGINLIB_EXPORT_CLASS(costmap_2d::CriticalLayer, costmap_2d::Layer)
using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE;
@@ -35,4 +32,12 @@ void CriticalLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, i
updateWithMax(master_grid, min_i, min_j, max_i, max_j);
}
// Export factory function
static PluginPtr create_critical_plugin() {
return std::make_shared<CriticalLayer>();
}
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_critical_plugin, create_plugin)
}

748
plugins/directional_layer.cpp
View File

@@ -1,11 +1,11 @@
#include <costmap_2d/directional_layer.h>
#include <pluginlib/class_list_macros.hpp>
#include <tf/transform_listener.h>
#include <tf2/convert.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf/transform_datatypes.h>
// #include <pluginlib/class_list_macros.hpp>
// #include <tf/transform_listener.h>
// #include <tf2/convert.h>
// #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
// #include <tf/transform_datatypes.h>
PLUGINLIB_EXPORT_CLASS(costmap_2d::DirectionalLayer, costmap_2d::Layer)
// PLUGINLIB_EXPORT_CLASS(costmap_2d::DirectionalLayer, costmap_2d::Layer)
#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
@@ -15,398 +15,398 @@ using costmap_2d::NO_INFORMATION;
namespace costmap_2d
{
DirectionalLayer::DirectionalLayer()
{
ros::NodeHandle nh("~/" + name_);
lane_mask_pub_ = nh.advertise<nav_msgs::OccupancyGrid>("/direction_zones/lanes", 1);
}
DirectionalLayer::~DirectionalLayer() {}
// DirectionalLayer::DirectionalLayer()
// {
// ros::NodeHandle nh("~/" + name_);
// lane_mask_pub_ = nh.advertise<nav_msgs::OccupancyGrid>("/direction_zones/lanes", 1);
// }
// DirectionalLayer::~DirectionalLayer() {}
bool DirectionalLayer::laneFilter(const std::vector<geometry_msgs::PoseStamped> plan)
{
if (directional_areas_.empty())
throw "Has no map data";
nav_msgs::Path path;
path.header.stamp = ros::Time::now();
path.header.frame_id = map_frame_;
path.poses = plan;
return this->laneFilter(directional_areas_, path);
}
// bool DirectionalLayer::laneFilter(const std::vector<geometry_msgs::PoseStamped> plan)
// {
// if (directional_areas_.empty())
// throw "Has no map data";
// nav_msgs::Path path;
// path.header.stamp = ros::Time::now();
// path.header.frame_id = map_frame_;
// path.poses = plan;
// return this->laneFilter(directional_areas_, path);
// }
void DirectionalLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr &new_map)
{
unsigned int size_x = new_map->info.width, size_y = new_map->info.height;
// void DirectionalLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr &new_map)
// {
// unsigned int size_x = new_map->info.width, size_y = new_map->info.height;
ROS_DEBUG("Received a %d X %d map at %f m/pix", size_x / 2, size_y / 2, new_map->info.resolution);
// ROS_DEBUG("Received a %d X %d map at %f m/pix", size_x / 2, size_y / 2, new_map->info.resolution);
// resize costmap if size, resolution or origin do not match
Costmap2D *master = layered_costmap_->getCostmap();
if (!layered_costmap_->isRolling() &&
(master->getSizeInCellsX() != size_x / 2 ||
master->getSizeInCellsY() != size_y / 2 ||
master->getResolution() != new_map->info.resolution ||
master->getOriginX() != new_map->info.origin.position.x ||
master->getOriginY() != new_map->info.origin.position.y))
{
// Update the size of the layered costmap (and all layers, including this one)
ROS_INFO("Resizing costmap to %d X %d at %f m/pix", size_x / 2, size_y / 2, new_map->info.resolution);
layered_costmap_->resizeMap(size_x / 2, size_y / 2, new_map->info.resolution, new_map->info.origin.position.x,
new_map->info.origin.position.y,
true /* set size_locked to true, prevents reconfigureCb from overriding map size*/);
}
else if (size_x_ != size_x / 2 || size_y_ != size_y / 2 ||
resolution_ != new_map->info.resolution ||
origin_x_ != new_map->info.origin.position.x ||
origin_y_ != new_map->info.origin.position.y)
{
// only update the size of the costmap stored locally in this layer
ROS_INFO("Resizing static layer to %d X %d at %f m/pix", size_x / 2, size_y / 2, new_map->info.resolution);
resizeMap(size_x / 2, size_y / 2, new_map->info.resolution,
new_map->info.origin.position.x, new_map->info.origin.position.y);
}
unsigned char values[4];
directional_areas_.clear();
directional_areas_.resize(size_x / 2 * size_y / 2);
// initialize the costmap with static data
for (unsigned int iy = 0; iy < size_y; iy += 2)
{
for (unsigned int ix = 0; ix < size_x; ix += 2)
{
values[0] = new_map->data[MAP_IDX(size_x, ix, iy)];
values[1] = new_map->data[MAP_IDX(size_x, ix + 1, iy)];
values[2] = new_map->data[MAP_IDX(size_x, ix, iy + 1)];
values[3] = new_map->data[MAP_IDX(size_x, ix + 1, iy + 1)];
uint32_t color_avg = (uint32_t)(values[0] | values[1] << 8u | values[2] << 16u | values[3] << 24u);
int index = getIndex(ix / 2, iy / 2);
directional_areas_[index][0] = (uint16_t)(color_avg & 0xfff);
directional_areas_[index][1] = (uint16_t)(color_avg >> 12u);
costmap_[index] = costmap_2d::NO_INFORMATION;
}
}
map_frame_ = new_map->header.frame_id;
// // resize costmap if size, resolution or origin do not match
// Costmap2D *master = layered_costmap_->getCostmap();
// if (!layered_costmap_->isRolling() &&
// (master->getSizeInCellsX() != size_x / 2 ||
// master->getSizeInCellsY() != size_y / 2 ||
// master->getResolution() != new_map->info.resolution ||
// master->getOriginX() != new_map->info.origin.position.x ||
// master->getOriginY() != new_map->info.origin.position.y))
// {
// // Update the size of the layered costmap (and all layers, including this one)
// ROS_INFO("Resizing costmap to %d X %d at %f m/pix", size_x / 2, size_y / 2, new_map->info.resolution);
// layered_costmap_->resizeMap(size_x / 2, size_y / 2, new_map->info.resolution, new_map->info.origin.position.x,
// new_map->info.origin.position.y,
// true /* set size_locked to true, prevents reconfigureCb from overriding map size*/);
// }
// else if (size_x_ != size_x / 2 || size_y_ != size_y / 2 ||
// resolution_ != new_map->info.resolution ||
// origin_x_ != new_map->info.origin.position.x ||
// origin_y_ != new_map->info.origin.position.y)
// {
// // only update the size of the costmap stored locally in this layer
// ROS_INFO("Resizing static layer to %d X %d at %f m/pix", size_x / 2, size_y / 2, new_map->info.resolution);
// resizeMap(size_x / 2, size_y / 2, new_map->info.resolution,
// new_map->info.origin.position.x, new_map->info.origin.position.y);
// }
// unsigned char values[4];
// directional_areas_.clear();
// directional_areas_.resize(size_x / 2 * size_y / 2);
// // initialize the costmap with static data
// for (unsigned int iy = 0; iy < size_y; iy += 2)
// {
// for (unsigned int ix = 0; ix < size_x; ix += 2)
// {
// values[0] = new_map->data[MAP_IDX(size_x, ix, iy)];
// values[1] = new_map->data[MAP_IDX(size_x, ix + 1, iy)];
// values[2] = new_map->data[MAP_IDX(size_x, ix, iy + 1)];
// values[3] = new_map->data[MAP_IDX(size_x, ix + 1, iy + 1)];
// uint32_t color_avg = (uint32_t)(values[0] | values[1] << 8u | values[2] << 16u | values[3] << 24u);
// int index = getIndex(ix / 2, iy / 2);
// directional_areas_[index][0] = (uint16_t)(color_avg & 0xfff);
// directional_areas_[index][1] = (uint16_t)(color_avg >> 12u);
// costmap_[index] = costmap_2d::NO_INFORMATION;
// }
// }
// map_frame_ = new_map->header.frame_id;
// we have a new map, update full size of map
x_ = y_ = 0;
width_ = size_x_;
height_ = size_y_;
map_received_ = true;
has_updated_data_ = true;
// // we have a new map, update full size of map
// x_ = y_ = 0;
// width_ = size_x_;
// height_ = size_y_;
// map_received_ = true;
// has_updated_data_ = true;
new_map_.header = new_map->header;
new_map_.header.stamp = ros::Time::now();
new_map_.info = new_map->info;
new_map_.info.width = width_;
new_map_.info.height = height_;
// new_map_.header = new_map->header;
// new_map_.header.stamp = ros::Time::now();
// new_map_.info = new_map->info;
// new_map_.info.width = width_;
// new_map_.info.height = height_;
// shutdown the map subscrber if firt_map_only_ flag is on
if (first_map_only_)
{
ROS_INFO("Shutting down the map subscriber. first_map_only flag is on");
map_sub_.shutdown();
}
}
// // shutdown the map subscrber if firt_map_only_ flag is on
// if (first_map_only_)
// {
// ROS_INFO("Shutting down the map subscriber. first_map_only flag is on");
// map_sub_.shutdown();
// }
// }
bool DirectionalLayer::laneFilter(std::vector<std::array<uint16_t, 2>> new_map, const nav_msgs::Path path)
{
boost::unique_lock<mutex_t> lock(*layered_costmap_->getCostmap()->getMutex());
if (new_map.empty())
return false;
// bool DirectionalLayer::laneFilter(std::vector<std::array<uint16_t, 2>> new_map, const nav_msgs::Path path)
// {
// boost::unique_lock<mutex_t> lock(*layered_costmap_->getCostmap()->getMutex());
// if (new_map.empty())
// return false;
std::vector<std::pair<unsigned int, double>> X_List, Y_List;
std::vector<double> Yaw_list;
// std::vector<std::pair<unsigned int, double>> X_List, Y_List;
// std::vector<double> Yaw_list;
const geometry_msgs::PoseStamped &e = path.poses.back();
// const geometry_msgs::PoseStamped &e = path.poses.back();
bool get_success = getRobotPose(pose_x_, pose_y_, pose_yaw_);
if(!get_success) return false;
for (auto it = path.poses.begin(); it != path.poses.end(); ++it)
{
const geometry_msgs::PoseStamped &p = *it; // Get the pose from the iterator
unsigned int mx, my;
if (!worldToMap(p.pose.position.x, p.pose.position.y, mx, my))
{
ROS_ERROR("ERROR: trying to set a start cell %f %f that is outside of map\n", p.pose.position.x, p.pose.position.y);
return false;
}
// Convert to yaw
tf::Quaternion quaternion;
tf::quaternionMsgToTF(p.pose.orientation, quaternion);
double theta = tf::getYaw(quaternion);
unsigned char value = laneFilter(mx, my, theta);
if (get_success)
{
if (
(!inSkipErea(pose_x_, pose_y_, p.pose.position.x, p.pose.position.y, distance_skip_) &
!inSkipErea(p.pose.position.x, p.pose.position.y, e.pose.position.x, e.pose.position.y, distance_skip_))
|| p == path.poses.back()
)
{
if (value == costmap_2d::INSCRIBED_INFLATED_OBSTACLE || value == costmap_2d::LETHAL_OBSTACLE)
{
std::pair<unsigned int, double> x_val(mx, p.pose.position.x);
std::pair<unsigned int, double> y_val(my, p.pose.position.y);
X_List.push_back(x_val);
Y_List.push_back(y_val);
Yaw_list.push_back(theta);
// costmap_[getIndex(mx, my)] = value;
}
}
}
}
if (!Yaw_list.empty())
{
laneFilter(X_List, Y_List, Yaw_list);
nav_msgs::OccupancyGrid lanes;
convertToMap(costmap_, lanes, 0.65, 0.196);
lane_mask_pub_.publish(lanes);
return false;
}
return true;
// bool get_success = getRobotPose(pose_x_, pose_y_, pose_yaw_);
// if(!get_success) return false;
// for (auto it = path.poses.begin(); it != path.poses.end(); ++it)
// {
// const geometry_msgs::PoseStamped &p = *it; // Get the pose from the iterator
// unsigned int mx, my;
// if (!worldToMap(p.pose.position.x, p.pose.position.y, mx, my))
// {
// ROS_ERROR("ERROR: trying to set a start cell %f %f that is outside of map\n", p.pose.position.x, p.pose.position.y);
// return false;
// }
// // Convert to yaw
// tf::Quaternion quaternion;
// tf::quaternionMsgToTF(p.pose.orientation, quaternion);
// double theta = tf::getYaw(quaternion);
// unsigned char value = laneFilter(mx, my, theta);
// if (get_success)
// {
// if (
// (!inSkipErea(pose_x_, pose_y_, p.pose.position.x, p.pose.position.y, distance_skip_) &
// !inSkipErea(p.pose.position.x, p.pose.position.y, e.pose.position.x, e.pose.position.y, distance_skip_))
// || p == path.poses.back()
// )
// {
// if (value == costmap_2d::INSCRIBED_INFLATED_OBSTACLE || value == costmap_2d::LETHAL_OBSTACLE)
// {
// std::pair<unsigned int, double> x_val(mx, p.pose.position.x);
// std::pair<unsigned int, double> y_val(my, p.pose.position.y);
// X_List.push_back(x_val);
// Y_List.push_back(y_val);
// Yaw_list.push_back(theta);
// // costmap_[getIndex(mx, my)] = value;
// }
// }
// }
// }
// if (!Yaw_list.empty())
// {
// laneFilter(X_List, Y_List, Yaw_list);
// nav_msgs::OccupancyGrid lanes;
// convertToMap(costmap_, lanes, 0.65, 0.196);
// lane_mask_pub_.publish(lanes);
// return false;
// }
// return true;
}
// }
void DirectionalLayer::convertToMap(unsigned char *costmap, nav_msgs::OccupancyGrid &dir_map, double occ_th, double free_th)
{
dir_map.header = new_map_.header;
dir_map.header.stamp = ros::Time::now();
dir_map.info = new_map_.info;
int size_costmap = new_map_.info.width * new_map_.info.height;
dir_map.data.resize(size_costmap);
for (int i = 0; i < size_costmap; i++)
{
int8_t value;
if (costmap[i] == costmap_2d::NO_INFORMATION)
{
value = -1;
}
else
{
double occ = (costmap[i]) / 255.0;
if (occ > occ_th)
value = +100;
else if (occ < free_th)
value = 0;
else
{
double ratio = (occ - free_th) / (occ_th - free_th);
value = 1 + 98 * ratio;
}
}
dir_map.data[i] = value;
}
}
// void DirectionalLayer::convertToMap(unsigned char *costmap, nav_msgs::OccupancyGrid &dir_map, double occ_th, double free_th)
// {
// dir_map.header = new_map_.header;
// dir_map.header.stamp = ros::Time::now();
// dir_map.info = new_map_.info;
// int size_costmap = new_map_.info.width * new_map_.info.height;
// dir_map.data.resize(size_costmap);
// for (int i = 0; i < size_costmap; i++)
// {
// int8_t value;
// if (costmap[i] == costmap_2d::NO_INFORMATION)
// {
// value = -1;
// }
// else
// {
// double occ = (costmap[i]) / 255.0;
// if (occ > occ_th)
// value = +100;
// else if (occ < free_th)
// value = 0;
// else
// {
// double ratio = (occ - free_th) / (occ_th - free_th);
// value = 1 + 98 * ratio;
// }
// }
// dir_map.data[i] = value;
// }
// }
unsigned char DirectionalLayer::laneFilter(unsigned int x, unsigned int y, double yaw_robot)
{
double yaw_lane;
unsigned char cost = costmap_2d::NO_INFORMATION;
// unsigned char DirectionalLayer::laneFilter(unsigned int x, unsigned int y, double yaw_robot)
// {
// double yaw_lane;
// unsigned char cost = costmap_2d::NO_INFORMATION;
int index = getIndex(x, y);
for (auto &lane : directional_areas_[index])
{
if (lane > 359)
{
cost = std::min(cost, costmap_2d::NO_INFORMATION);
}
else
{
double yaw_lane = (double)lane / 180 * M_PI;
if (0.4 <= cos(yaw_robot - yaw_lane) && cos(yaw_robot - yaw_lane) <= 1.0)
cost = std::min(cost, costmap_2d::FREE_SPACE);
else if (-1.0 <= cos(yaw_robot - yaw_lane) && cos(yaw_robot - yaw_lane) <= -0.4)
cost = std::min(cost, costmap_2d::LETHAL_OBSTACLE);
else
cost = std::min(cost, costmap_2d::NO_INFORMATION);
}
}
return cost;
}
// int index = getIndex(x, y);
// for (auto &lane : directional_areas_[index])
// {
// if (lane > 359)
// {
// cost = std::min(cost, costmap_2d::NO_INFORMATION);
// }
// else
// {
// double yaw_lane = (double)lane / 180 * M_PI;
// if (0.4 <= cos(yaw_robot - yaw_lane) && cos(yaw_robot - yaw_lane) <= 1.0)
// cost = std::min(cost, costmap_2d::FREE_SPACE);
// else if (-1.0 <= cos(yaw_robot - yaw_lane) && cos(yaw_robot - yaw_lane) <= -0.4)
// cost = std::min(cost, costmap_2d::LETHAL_OBSTACLE);
// else
// cost = std::min(cost, costmap_2d::NO_INFORMATION);
// }
// }
// return cost;
// }
void DirectionalLayer::laneFilter(std::vector<std::pair<unsigned int, double>> x,
std::vector<std::pair<unsigned int, double>> y,
std::vector<double> yaw_robot)
{
if(x.empty() || y.empty() || yaw_robot.empty())
return;
// void DirectionalLayer::laneFilter(std::vector<std::pair<unsigned int, double>> x,
// std::vector<std::pair<unsigned int, double>> y,
// std::vector<double> yaw_robot)
// {
// if(x.empty() || y.empty() || yaw_robot.empty())
// return;
unsigned int x_min, y_min, x_max, y_max;
double x_min_w, y_min_w, x_max_w, y_max_w;
// unsigned int x_min, y_min, x_max, y_max;
// double x_min_w, y_min_w, x_max_w, y_max_w;
x_min = x.front().first; y_min = y.front().first;
x_max = x.front().first; y_max = y.front().first;
x_min_w = x.front().second; y_min_w = y.front().second;
x_max_w = x.front().second; y_max_w = y.front().second;
for (int i = 1; i < yaw_robot.size(); i++)
{
if(inSkipErea(pose_x_, pose_y_, x[i].second, y[i].second, distance_skip_))
continue;
x_min = std::min(x_min, x[i].first);
y_min = std::min(y_min, y[i].first);
x_max = std::max(x_max, x[i].first);
y_max = std::max(y_max, y[i].first);
// x_min = x.front().first; y_min = y.front().first;
// x_max = x.front().first; y_max = y.front().first;
// x_min_w = x.front().second; y_min_w = y.front().second;
// x_max_w = x.front().second; y_max_w = y.front().second;
// for (int i = 1; i < yaw_robot.size(); i++)
// {
// if(inSkipErea(pose_x_, pose_y_, x[i].second, y[i].second, distance_skip_))
// continue;
// x_min = std::min(x_min, x[i].first);
// y_min = std::min(y_min, y[i].first);
// x_max = std::max(x_max, x[i].first);
// y_max = std::max(y_max, y[i].first);
x_min_w = std::min(x_min_w, x[i].second);
y_min_w = std::min(y_min_w, y[i].second);
x_max_w = std::max(x_max_w, x[i].second);
y_max_w = std::max(y_max_w, y[i].second);
// x_min_w = std::min(x_min_w, x[i].second);
// y_min_w = std::min(y_min_w, y[i].second);
// x_max_w = std::max(x_max_w, x[i].second);
// y_max_w = std::max(y_max_w, y[i].second);
}
// ROS_INFO("%d %d %d %d", x_min, y_min, x_max, y_max);
// ROS_INFO("%f %f %f %f", x_min_w, y_min_w, x_max_w, y_max_w);
for (int i = 0; i < yaw_robot.size(); i++)
{
if(inSkipErea(pose_x_, pose_y_, x[i].second, y[i].second, distance_skip_))
continue;
// }
// // ROS_INFO("%d %d %d %d", x_min, y_min, x_max, y_max);
// // ROS_INFO("%f %f %f %f", x_min_w, y_min_w, x_max_w, y_max_w);
// for (int i = 0; i < yaw_robot.size(); i++)
// {
// if(inSkipErea(pose_x_, pose_y_, x[i].second, y[i].second, distance_skip_))
// continue;
double yaw_rad = (double)yaw_robot[i] / 180 * M_PI;
if (fabs(cos(yaw_rad)) > fabs(sin(yaw_rad)))
{
int x_ = x[i].first;
// Dưới lên trên
for (int j = y[i].first; j <= y_max; j++)
{
int y_ = j;
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
continue;
unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
costmap_[getIndex(x_, y_)] = value;
else
break;
}
// Trên xuống dưới
for (int k = y[i].first; k >= y_min; k--)
{
int y_ = k;
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
continue;
unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
costmap_[getIndex(x_, y_)] = value;
else
break;
}
// double yaw_rad = (double)yaw_robot[i] / 180 * M_PI;
// if (fabs(cos(yaw_rad)) > fabs(sin(yaw_rad)))
// {
// int x_ = x[i].first;
// // Dưới lên trên
// for (int j = y[i].first; j <= y_max; j++)
// {
// int y_ = j;
// if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
// continue;
// unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
// if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
// costmap_[getIndex(x_, y_)] = value;
// else
// break;
// }
// // Trên xuống dưới
// for (int k = y[i].first; k >= y_min; k--)
// {
// int y_ = k;
// if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
// continue;
// unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
// if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
// costmap_[getIndex(x_, y_)] = value;
// else
// break;
// }
int y_ = y[i].first;
// Phải qua trái
for (int j = x[i].first; j < layered_costmap_->getCostmap()->getSizeInCellsX(); j++)
{
int x_ = j;
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
continue;
unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
costmap_[getIndex(x_, y_)] = value;
else
break;
}
// Trái qua phải
for (int k = x[i].first; k >= 0; k--)
{
int x_ = k;
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
continue;
unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
costmap_[getIndex(x_, y_)] = value;
else
break;
}
}
else if(fabs(cos(yaw_rad)) < fabs(sin(yaw_rad)))
{
int y_ = y[i].first;
// Phải qua trái
for (int j = x[i].first; j <= x_max; j++)
{
int x_ = j;
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
continue;
unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
costmap_[getIndex(x_, y_)] = value;
else
break;
}
// Trái qua phải
for (int k = x[i].first; k >= x_min; k--)
{
int x_ = k;
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
continue;
unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
costmap_[getIndex(x_, y_)] = value;
else
break;
}
// int y_ = y[i].first;
// // Phải qua trái
// for (int j = x[i].first; j < layered_costmap_->getCostmap()->getSizeInCellsX(); j++)
// {
// int x_ = j;
// if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
// continue;
// unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
// if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
// costmap_[getIndex(x_, y_)] = value;
// else
// break;
// }
// // Trái qua phải
// for (int k = x[i].first; k >= 0; k--)
// {
// int x_ = k;
// if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
// continue;
// unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
// if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
// costmap_[getIndex(x_, y_)] = value;
// else
// break;
// }
// }
// else if(fabs(cos(yaw_rad)) < fabs(sin(yaw_rad)))
// {
// int y_ = y[i].first;
// // Phải qua trái
// for (int j = x[i].first; j <= x_max; j++)
// {
// int x_ = j;
// if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
// continue;
// unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
// if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
// costmap_[getIndex(x_, y_)] = value;
// else
// break;
// }
// // Trái qua phải
// for (int k = x[i].first; k >= x_min; k--)
// {
// int x_ = k;
// if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
// continue;
// unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
// if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
// costmap_[getIndex(x_, y_)] = value;
// else
// break;
// }
int x_ = x[i].first;
// Dưới lên trên
for (int j = y[i].first; j < layered_costmap_->getCostmap()->getSizeInCellsY(); j++)
{
int y_ = j;
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
continue;
unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
costmap_[getIndex(x_, y_)] = value;
else
break;
}
// Trên xuống dưới
for (int k = y[i].first; k >= 0; k--)
{
int y_ = k;
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
continue;
unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
costmap_[getIndex(x_, y_)] = value;
else
break;
}
}
}
}
// int x_ = x[i].first;
// // Dưới lên trên
// for (int j = y[i].first; j < layered_costmap_->getCostmap()->getSizeInCellsY(); j++)
// {
// int y_ = j;
// if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
// continue;
// unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
// if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
// costmap_[getIndex(x_, y_)] = value;
// else
// break;
// }
// // Trên xuống dưới
// for (int k = y[i].first; k >= 0; k--)
// {
// int y_ = k;
// if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
// continue;
// unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
// if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
// costmap_[getIndex(x_, y_)] = value;
// else
// break;
// }
// }
// }
// }
void DirectionalLayer::resetMap()
{
unsigned int size_x = layered_costmap_->getCostmap()->getSizeInCellsX();
unsigned int size_y = layered_costmap_->getCostmap()->getSizeInCellsY();
for (unsigned int iy = 0; iy < size_y; iy++)
{
for (unsigned int ix = 0; ix < size_x; ix++)
{
int index = getIndex(ix, iy);
costmap_[index] = costmap_2d::NO_INFORMATION;
}
}
}
// void DirectionalLayer::resetMap()
// {
// unsigned int size_x = layered_costmap_->getCostmap()->getSizeInCellsX();
// unsigned int size_y = layered_costmap_->getCostmap()->getSizeInCellsY();
// for (unsigned int iy = 0; iy < size_y; iy++)
// {
// for (unsigned int ix = 0; ix < size_x; ix++)
// {
// int index = getIndex(ix, iy);
// costmap_[index] = costmap_2d::NO_INFORMATION;
// }
// }
// }
bool DirectionalLayer::getRobotPose(double &x, double &y, double &yaw)
{
tf::StampedTransform transform;
try
{
listener_.lookupTransform(map_frame_, base_frame_id_, ros::Time(0), transform);
}
catch (tf::TransformException &ex)
{
ROS_ERROR("%s", ex.what());
return false;
}
x = transform.getOrigin().x();
y = transform.getOrigin().y();
// Extract the rotation as a quaternion from the transform
tf::Quaternion rotation = transform.getRotation();
// Convert the quaternion to a yaw angle (in radians)
yaw = tf::getYaw(rotation);
return true;
}
// bool DirectionalLayer::getRobotPose(double &x, double &y, double &yaw)
// {
// tf::StampedTransform transform;
// try
// {
// listener_.lookupTransform(map_frame_, base_frame_id_, ros::Time(0), transform);
// }
// catch (tf::TransformException &ex)
// {
// ROS_ERROR("%s", ex.what());
// return false;
// }
// x = transform.getOrigin().x();
// y = transform.getOrigin().y();
// // Extract the rotation as a quaternion from the transform
// tf::Quaternion rotation = transform.getRotation();
// // Convert the quaternion to a yaw angle (in radians)
// yaw = tf::getYaw(rotation);
// return true;
// }
bool DirectionalLayer::inSkipErea(double start_x, double start_y, double end_x, double end_y, double skip_distance)
{
return fabs(hypot(start_x - end_x, start_y - end_y)) <= skip_distance;
}
// bool DirectionalLayer::inSkipErea(double start_x, double start_y, double end_x, double end_y, double skip_distance)
// {
// return fabs(hypot(start_x - end_x, start_y - end_y)) <= skip_distance;
// }
}

15
plugins/inflation_layer.cpp
View File

@@ -2,9 +2,9 @@
#include <costmap_2d/inflation_layer.h>
#include <costmap_2d/costmap_math.h>
#include <costmap_2d/footprint.h>
#include <costmap_2d/utils.h>
#include <boost/thread.hpp>
// PLUGINLIB_EXPORT_CLASS(costmap_2d::InflationLayer, costmap_2d::Layer)
#include <boost/dll/alias.hpp>
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
@@ -54,13 +54,14 @@ bool InflationLayer::getParams()
{
try {
YAML::Node config = YAML::LoadFile("../cfg/config.yaml");
double cost_scaling_factor = config["inflation_layer"]["cost_scaling_factor"].as<double>();
double inflation_radius = config["inflation_layer"]["inflation_radius"].as<double>();
YAML::Node layer = config["inflation_layer"];
double cost_scaling_factor = loadParam(layer, "cost_scaling_factor", 15.0);
double inflation_radius = loadParam(layer, "inflation_radius", 0.55);
setInflationParameters(inflation_radius, cost_scaling_factor);
bool enabled = config["inflation_layer"]["enabled"].as<bool>();
bool inflate_unknown = config["inflation_layer"]["inflate_unknown"].as<bool>();
bool enabled = loadParam(layer, "enabled", true);
bool inflate_unknown = loadParam(layer, "inflate_unknown", false);
if (enabled_ != enabled || inflate_unknown_ != inflate_unknown) {
enabled_ = enabled;
inflate_unknown_ = inflate_unknown;

718
plugins/obstacle_layer.cpp
View File

@@ -1,185 +1,183 @@
#include <costmap_2d/obstacle_layer.h>
// #include <costmap_2d/costmap_math.h>
#include <costmap_2d/costmap_math.h>
// #include <tf2_ros/message_filter.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <tf2/utils.h>
#include <boost/dll/alias.hpp>
// #include <pluginlib/class_list_macros.hpp>
// #include <sensor_msgs/point_cloud2_iterator.h>
using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::FREE_SPACE;
// PLUGINLIB_EXPORT_CLASS(costmap_2d::ObstacleLayer, costmap_2d::Layer)
using costmap_2d::ObservationBuffer;
using costmap_2d::Observation;
// using costmap_2d::NO_INFORMATION;
// using costmap_2d::LETHAL_OBSTACLE;
// using costmap_2d::FREE_SPACE;
namespace costmap_2d
{
// using costmap_2d::ObservationBuffer;
// using costmap_2d::Observation;
// namespace costmap_2d
// {
// void ObstacleLayer::onInitialize()
// {
void ObstacleLayer::onInitialize()
{
// ros::NodeHandle nh("~/" + name_), g_nh;
// rolling_window_ = layered_costmap_->isRolling();
rolling_window_ = layered_costmap_->isRolling();
// bool track_unknown_space;
bool track_unknown_space;
// nh.param("track_unknown_space", track_unknown_space, layered_costmap_->isTrackingUnknown());
// if (track_unknown_space)
// default_value_ = NO_INFORMATION;
// else
// default_value_ = FREE_SPACE;
if (track_unknown_space)
default_value_ = NO_INFORMATION;
else
default_value_ = FREE_SPACE;
// ObstacleLayer::matchSize();
// current_ = true;
ObstacleLayer::matchSize();
current_ = true;
// global_frame_ = layered_costmap_->getGlobalFrameID();
// double transform_tolerance;
global_frame_ = layered_costmap_->getGlobalFrameID();
double transform_tolerance;
// nh.param("transform_tolerance", transform_tolerance, 0.2);
// std::string topics_string;
// // get the topics that we'll subscribe to from the parameter server
std::string topics_string;
// get the topics that we'll subscribe to from the parameter server
// nh.param("observation_sources", topics_string, std::string(""));
// ROS_INFO(" Subscribed to Topics: %s", topics_string.c_str());
// // now we need to split the topics based on whitespace which we can use a stringstream for
// std::stringstream ss(topics_string);
// now we need to split the topics based on whitespace which we can use a stringstream for
std::stringstream ss(topics_string);
/*
std::string source;
while (ss >> source)
{
ros::NodeHandle source_node(nh, source);
// std::string source;
// while (ss >> source)
// {
// ros::NodeHandle source_node(nh, source);
// get the parameters for the specific topic
double observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height;
std::string topic, sensor_frame, data_type;
bool inf_is_valid, clearing, marking;
// // get the parameters for the specific topic
// double observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height;
// std::string topic, sensor_frame, data_type;
// bool inf_is_valid, clearing, marking;
source_node.param("topic", topic, source);
source_node.param("sensor_frame", sensor_frame, std::string(""));
source_node.param("observation_persistence", observation_keep_time, 0.0);
source_node.param("expected_update_rate", expected_update_rate, 0.0);
source_node.param("data_type", data_type, std::string("PointCloud"));
source_node.param("min_obstacle_height", min_obstacle_height, 0.0);
source_node.param("max_obstacle_height", max_obstacle_height, 2.0);
source_node.param("inf_is_valid", inf_is_valid, false);
source_node.param("clearing", clearing, false);
source_node.param("marking", marking, true);
// source_node.param("topic", topic, source);
// source_node.param("sensor_frame", sensor_frame, std::string(""));
// source_node.param("observation_persistence", observation_keep_time, 0.0);
// source_node.param("expected_update_rate", expected_update_rate, 0.0);
// source_node.param("data_type", data_type, std::string("PointCloud"));
// source_node.param("min_obstacle_height", min_obstacle_height, 0.0);
// source_node.param("max_obstacle_height", max_obstacle_height, 2.0);
// source_node.param("inf_is_valid", inf_is_valid, false);
// source_node.param("clearing", clearing, false);
// source_node.param("marking", marking, true);
if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan"))
{
ROS_FATAL("Only topics that use point clouds or laser scans are currently supported");
throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported");
}
// if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan"))
// {
// ROS_FATAL("Only topics that use point clouds or laser scans are currently supported");
// throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported");
// }
std::string raytrace_range_param_name, obstacle_range_param_name;
// std::string raytrace_range_param_name, obstacle_range_param_name;
// get the obstacle range for the sensor
double obstacle_range = 2.5;
if (source_node.searchParam("obstacle_range", obstacle_range_param_name))
{
source_node.getParam(obstacle_range_param_name, obstacle_range);
}
// // get the obstacle range for the sensor
// double obstacle_range = 2.5;
// if (source_node.searchParam("obstacle_range", obstacle_range_param_name))
// {
// source_node.getParam(obstacle_range_param_name, obstacle_range);
// }
// get the raytrace range for the sensor
double raytrace_range = 3.0;
if (source_node.searchParam("raytrace_range", raytrace_range_param_name))
{
source_node.getParam(raytrace_range_param_name, raytrace_range);
}
// // get the raytrace range for the sensor
// double raytrace_range = 3.0;
// if (source_node.searchParam("raytrace_range", raytrace_range_param_name))
// {
// source_node.getParam(raytrace_range_param_name, raytrace_range);
// }
printf("Creating an observation buffer for source %s, topic %s, frame %s", source.c_str(), topic.c_str(),
sensor_frame.c_str());
// ROS_DEBUG("Creating an observation buffer for source %s, topic %s, frame %s", source.c_str(), topic.c_str(),
// sensor_frame.c_str());
// create an observation buffer
observation_buffers_.push_back(
boost::shared_ptr < ObservationBuffer
> (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height,
max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_,
sensor_frame, transform_tolerance)));
// // create an observation buffer
// observation_buffers_.push_back(
// boost::shared_ptr < ObservationBuffer
// > (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height,
// max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_,
// sensor_frame, transform_tolerance)));
// check if we'll add this buffer to our marking observation buffers
if (marking)
marking_buffers_.push_back(observation_buffers_.back());
// // check if we'll add this buffer to our marking observation buffers
// if (marking)
// marking_buffers_.push_back(observation_buffers_.back());
// check if we'll also add this buffer to our clearing observation buffers
if (clearing)
clearing_buffers_.push_back(observation_buffers_.back());
// // check if we'll also add this buffer to our clearing observation buffers
// if (clearing)
// clearing_buffers_.push_back(observation_buffers_.back());
printf(
"Created an observation buffer for source %s, topic %s, global frame: %s, "
"expected update rate: %.2f, observation persistence: %.2f",
source.c_str(), topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time);
// ROS_DEBUG(
// "Created an observation buffer for source %s, topic %s, global frame: %s, "
// "expected update rate: %.2f, observation persistence: %.2f",
// source.c_str(), topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time);
// create a callback for the topic
if (data_type == "LaserScan")
{
boost::shared_ptr < message_filters::Subscriber<sensor_msgs::LaserScan>
> sub(new message_filters::Subscriber<sensor_msgs::LaserScan>(g_nh, topic, 50));
// // create a callback for the topic
// if (data_type == "LaserScan")
// {
// boost::shared_ptr < message_filters::Subscriber<sensor_msgs::LaserScan>
// > sub(new message_filters::Subscriber<sensor_msgs::LaserScan>(g_nh, topic, 50));
boost::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::LaserScan> > filter(
new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*sub, *tf_, global_frame_, 50, g_nh));
// boost::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::LaserScan> > filter(
// new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*sub, *tf_, global_frame_, 50, g_nh));
if (inf_is_valid)
{
filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ laserScanValidInfCallback(msg, buffer); });
}
else
{
filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ laserScanCallback(msg, buffer); });
}
// if (inf_is_valid)
// {
// filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ laserScanValidInfCallback(msg, buffer); });
// }
// else
// {
// filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ laserScanCallback(msg, buffer); });
// }
observation_subscribers_.push_back(sub);
observation_notifiers_.push_back(filter);
// observation_subscribers_.push_back(sub);
// observation_notifiers_.push_back(filter);
observation_notifiers_.back()->setTolerance(ros::Duration(0.05));
}
else if (data_type == "PointCloud")
{
boost::shared_ptr < message_filters::Subscriber<sensor_msgs::PointCloud>
> sub(new message_filters::Subscriber<sensor_msgs::PointCloud>(g_nh, topic, 50));
// observation_notifiers_.back()->setTolerance(ros::Duration(0.05));
// }
// else if (data_type == "PointCloud")
// {
// boost::shared_ptr < message_filters::Subscriber<sensor_msgs::PointCloud>
// > sub(new message_filters::Subscriber<sensor_msgs::PointCloud>(g_nh, topic, 50));
if (inf_is_valid)
{
ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
}
// if (inf_is_valid)
// {
// ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
// }
boost::shared_ptr < tf2_ros::MessageFilter<sensor_msgs::PointCloud>
> filter(new tf2_ros::MessageFilter<sensor_msgs::PointCloud>(*sub, *tf_, global_frame_, 50, g_nh));
filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ pointCloudCallback(msg, buffer); });
// boost::shared_ptr < tf2_ros::MessageFilter<sensor_msgs::PointCloud>
// > filter(new tf2_ros::MessageFilter<sensor_msgs::PointCloud>(*sub, *tf_, global_frame_, 50, g_nh));
// filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ pointCloudCallback(msg, buffer); });
observation_subscribers_.push_back(sub);
observation_notifiers_.push_back(filter);
}
else
{
boost::shared_ptr < message_filters::Subscriber<sensor_msgs::PointCloud2>
> sub(new message_filters::Subscriber<sensor_msgs::PointCloud2>(g_nh, topic, 50));
// observation_subscribers_.push_back(sub);
// observation_notifiers_.push_back(filter);
// }
// else
// {
// boost::shared_ptr < message_filters::Subscriber<sensor_msgs::PointCloud2>
// > sub(new message_filters::Subscriber<sensor_msgs::PointCloud2>(g_nh, topic, 50));
if (inf_is_valid)
{
ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
}
// if (inf_is_valid)
// {
// ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
// }
boost::shared_ptr < tf2_ros::MessageFilter<sensor_msgs::PointCloud2>
> filter(new tf2_ros::MessageFilter<sensor_msgs::PointCloud2>(*sub, *tf_, global_frame_, 50, g_nh));
filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ pointCloud2Callback(msg, buffer); });
// boost::shared_ptr < tf2_ros::MessageFilter<sensor_msgs::PointCloud2>
// > filter(new tf2_ros::MessageFilter<sensor_msgs::PointCloud2>(*sub, *tf_, global_frame_, 50, g_nh));
// filter->registerCallback([this,buffer=observation_buffers_.back()](auto& msg){ pointCloud2Callback(msg, buffer); });
// observation_subscribers_.push_back(sub);
// observation_notifiers_.push_back(filter);
// }
// if (sensor_frame != "")
// {
// std::vector < std::string > target_frames;
// target_frames.push_back(global_frame_);
// target_frames.push_back(sensor_frame);
// observation_notifiers_.back()->setTargetFrames(target_frames);
// }
// }
observation_subscribers_.push_back(sub);
observation_notifiers_.push_back(filter);
}
if (sensor_frame != "")
{
std::vector < std::string > target_frames;
target_frames.push_back(global_frame_);
target_frames.push_back(sensor_frame);
observation_notifiers_.back()->setTargetFrames(target_frames);
}
}
*/
// dsrv_ = NULL;
// setupDynamicReconfigure(nh);
// }
}
// void ObstacleLayer::setupDynamicReconfigure(ros::NodeHandle& nh)
// {
@@ -189,11 +187,9 @@
// dsrv_->setCallback(cb);
// }
// ObstacleLayer::~ObstacleLayer()
// {
// if (dsrv_)
// delete dsrv_;
// }
ObstacleLayer::~ObstacleLayer()
{}
// void ObstacleLayer::reconfigureCB(costmap_2d::ObstaclePluginConfig &config, uint32_t level)
// {
// enabled_ = config.enabled;
@@ -300,246 +296,246 @@
// buffer->unlock();
// }
// void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
// double* min_y, double* max_x, double* max_y)
// {
// if (rolling_window_)
// updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
// useExtraBounds(min_x, min_y, max_x, max_y);
void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
double* min_y, double* max_x, double* max_y)
{
if (rolling_window_)
updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
useExtraBounds(min_x, min_y, max_x, max_y);
// bool current = true;
// std::vector<Observation> observations, clearing_observations;
bool current = true;
std::vector<Observation> observations, clearing_observations;
// // get the marking observations
// current = current && getMarkingObservations(observations);
// get the marking observations
current = current && getMarkingObservations(observations);
// // get the clearing observations
// current = current && getClearingObservations(clearing_observations);
// get the clearing observations
current = current && getClearingObservations(clearing_observations);
// // update the global current status
// current_ = current;
// update the global current status
current_ = current;
// // raytrace freespace
// for (unsigned int i = 0; i < clearing_observations.size(); ++i)
// {
// raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
// }
// raytrace freespace
for (unsigned int i = 0; i < clearing_observations.size(); ++i)
{
raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
}
// // place the new obstacles into a priority queue... each with a priority of zero to begin with
// for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
// {
// const Observation& obs = *it;
// place the new obstacles into a priority queue... each with a priority of zero to begin with
for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
{
const Observation& obs = *it;
// const sensor_msgs::PointCloud2& cloud = *(obs.cloud_);
const sensor_msgs::PointCloud2& cloud = *(obs.cloud_);
// double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
// sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
// sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
// sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
// for (; iter_x !=iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
// {
// double px = *iter_x, py = *iter_y, pz = *iter_z;
for (; iter_x !=iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
{
double px = *iter_x, py = *iter_y, pz = *iter_z;
// // if the obstacle is too high or too far away from the robot we won't add it
// if (pz > max_obstacle_height_)
// {
// ROS_DEBUG("The point is too high");
// continue;
// }
// if the obstacle is too high or too far away from the robot we won't add it
if (pz > max_obstacle_height_)
{
printf("The point is too high");
continue;
}
// // compute the squared distance from the hitpoint to the pointcloud's origin
// double sq_dist = (px - obs.origin_.x) * (px - obs.origin_.x) + (py - obs.origin_.y) * (py - obs.origin_.y)
// + (pz - obs.origin_.z) * (pz - obs.origin_.z);
// compute the squared distance from the hitpoint to the pointcloud's origin
double sq_dist = (px - obs.origin_.x) * (px - obs.origin_.x) + (py - obs.origin_.y) * (py - obs.origin_.y)
+ (pz - obs.origin_.z) * (pz - obs.origin_.z);
// // if the point is far enough away... we won't consider it
// if (sq_dist >= sq_obstacle_range)
// {
// ROS_DEBUG("The point is too far away");
// continue;
// }
// if the point is far enough away... we won't consider it
if (sq_dist >= sq_obstacle_range)
{
printf("The point is too far away");
continue;
}
// // now we need to compute the map coordinates for the observation
// unsigned int mx, my;
// if (!worldToMap(px, py, mx, my))
// {
// ROS_DEBUG("Computing map coords failed");
// continue;
// }
// now we need to compute the map coordinates for the observation
unsigned int mx, my;
if (!worldToMap(px, py, mx, my))
{
printf("Computing map coords failed");
continue;
}
// unsigned int index = getIndex(mx, my);
// costmap_[index] = LETHAL_OBSTACLE;
// touch(px, py, min_x, min_y, max_x, max_y);
// }
// }
unsigned int index = getIndex(mx, my);
costmap_[index] = LETHAL_OBSTACLE;
touch(px, py, min_x, min_y, max_x, max_y);
}
}
// updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
// }
updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
}
// void ObstacleLayer::updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
// double* max_x, double* max_y)
// {
// if (!footprint_clearing_enabled_) return;
// transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_);
void ObstacleLayer::updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
double* max_x, double* max_y)
{
if (!footprint_clearing_enabled_) return;
transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_);
// for (unsigned int i = 0; i < transformed_footprint_.size(); i++)
// {
// touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y);
// }
// }
for (unsigned int i = 0; i < transformed_footprint_.size(); i++)
{
touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y);
}
}
// void ObstacleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
// {
// if (footprint_clearing_enabled_)
// {
// setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE);
// }
void ObstacleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if (footprint_clearing_enabled_)
{
setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE);
}
// switch (combination_method_)
// {
// case 0: // Overwrite
// updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j);
// break;
// case 1: // Maximum
// updateWithMax(master_grid, min_i, min_j, max_i, max_j);
// break;
// default: // Nothing
// break;
// }
switch (combination_method_)
{
case 0: // Overwrite
updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j);
break;
case 1: // Maximum
updateWithMax(master_grid, min_i, min_j, max_i, max_j);
break;
default: // Nothing
break;
}
// }
}
// void ObstacleLayer::addStaticObservation(costmap_2d::Observation& obs, bool marking, bool clearing)
// {
// if (marking)
// static_marking_observations_.push_back(obs);
// if (clearing)
// static_clearing_observations_.push_back(obs);
// }
void ObstacleLayer::addStaticObservation(costmap_2d::Observation& obs, bool marking, bool clearing)
{
if (marking)
static_marking_observations_.push_back(obs);
if (clearing)
static_clearing_observations_.push_back(obs);
}
// void ObstacleLayer::clearStaticObservations(bool marking, bool clearing)
// {
// if (marking)
// static_marking_observations_.clear();
// if (clearing)
// static_clearing_observations_.clear();
// }
void ObstacleLayer::clearStaticObservations(bool marking, bool clearing)
{
if (marking)
static_marking_observations_.clear();
if (clearing)
static_clearing_observations_.clear();
}
// bool ObstacleLayer::getMarkingObservations(std::vector<Observation>& marking_observations) const
// {
// bool current = true;
// // get the marking observations
// for (unsigned int i = 0; i < marking_buffers_.size(); ++i)
// {
// marking_buffers_[i]->lock();
// marking_buffers_[i]->getObservations(marking_observations);
// current = marking_buffers_[i]->isCurrent() && current;
// marking_buffers_[i]->unlock();
// }
// marking_observations.insert(marking_observations.end(),
// static_marking_observations_.begin(), static_marking_observations_.end());
// return current;
// }
bool ObstacleLayer::getMarkingObservations(std::vector<Observation>& marking_observations) const
{
bool current = true;
// get the marking observations
for (unsigned int i = 0; i < marking_buffers_.size(); ++i)
{
marking_buffers_[i]->lock();
marking_buffers_[i]->getObservations(marking_observations);
current = marking_buffers_[i]->isCurrent() && current;
marking_buffers_[i]->unlock();
}
marking_observations.insert(marking_observations.end(),
static_marking_observations_.begin(), static_marking_observations_.end());
return current;
}
// bool ObstacleLayer::getClearingObservations(std::vector<Observation>& clearing_observations) const
// {
// bool current = true;
// // get the clearing observations
// for (unsigned int i = 0; i < clearing_buffers_.size(); ++i)
// {
// clearing_buffers_[i]->lock();
// clearing_buffers_[i]->getObservations(clearing_observations);
// current = clearing_buffers_[i]->isCurrent() && current;
// clearing_buffers_[i]->unlock();
// }
// clearing_observations.insert(clearing_observations.end(),
// static_clearing_observations_.begin(), static_clearing_observations_.end());
// return current;
// }
bool ObstacleLayer::getClearingObservations(std::vector<Observation>& clearing_observations) const
{
bool current = true;
// get the clearing observations
for (unsigned int i = 0; i < clearing_buffers_.size(); ++i)
{
clearing_buffers_[i]->lock();
clearing_buffers_[i]->getObservations(clearing_observations);
current = clearing_buffers_[i]->isCurrent() && current;
clearing_buffers_[i]->unlock();
}
clearing_observations.insert(clearing_observations.end(),
static_clearing_observations_.begin(), static_clearing_observations_.end());
return current;
}
// void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y,
// double* max_x, double* max_y)
// {
// double ox = clearing_observation.origin_.x;
// double oy = clearing_observation.origin_.y;
// const sensor_msgs::PointCloud2 &cloud = *(clearing_observation.cloud_);
void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y,
double* max_x, double* max_y)
{
double ox = clearing_observation.origin_.x;
double oy = clearing_observation.origin_.y;
const sensor_msgs::PointCloud2 &cloud = *(clearing_observation.cloud_);
// // get the map coordinates of the origin of the sensor
// unsigned int x0, y0;
// if (!worldToMap(ox, oy, x0, y0))
// {
// ROS_WARN_THROTTLE(
// 1.0, "The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.",
// ox, oy);
// return;
// }
// get the map coordinates of the origin of the sensor
unsigned int x0, y0;
if (!worldToMap(ox, oy, x0, y0))
{
printf(
"The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.",
ox, oy);
return;
}
// // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
// double origin_x = origin_x_, origin_y = origin_y_;
// double map_end_x = origin_x + size_x_ * resolution_;
// double map_end_y = origin_y + size_y_ * resolution_;
// we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
double origin_x = origin_x_, origin_y = origin_y_;
double map_end_x = origin_x + size_x_ * resolution_;
double map_end_y = origin_y + size_y_ * resolution_;
// touch(ox, oy, min_x, min_y, max_x, max_y);
touch(ox, oy, min_x, min_y, max_x, max_y);
// // for each point in the cloud, we want to trace a line from the origin and clear obstacles along it
// sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
// sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
// for each point in the cloud, we want to trace a line from the origin and clear obstacles along it
sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
// for (; iter_x != iter_x.end(); ++iter_x, ++iter_y)
// {
// double wx = *iter_x;
// double wy = *iter_y;
for (; iter_x != iter_x.end(); ++iter_x, ++iter_y)
{
double wx = *iter_x;
double wy = *iter_y;
// // now we also need to make sure that the enpoint we're raytracing
// // to isn't off the costmap and scale if necessary
// double a = wx - ox;
// double b = wy - oy;
// now we also need to make sure that the enpoint we're raytracing
// to isn't off the costmap and scale if necessary
double a = wx - ox;
double b = wy - oy;
// // the minimum value to raytrace from is the origin
// if (wx < origin_x)
// {
// double t = (origin_x - ox) / a;
// wx = origin_x;
// wy = oy + b * t;
// }
// if (wy < origin_y)
// {
// double t = (origin_y - oy) / b;
// wx = ox + a * t;
// wy = origin_y;
// }
// the minimum value to raytrace from is the origin
if (wx < origin_x)
{
double t = (origin_x - ox) / a;
wx = origin_x;
wy = oy + b * t;
}
if (wy < origin_y)
{
double t = (origin_y - oy) / b;
wx = ox + a * t;
wy = origin_y;
}
// // the maximum value to raytrace to is the end of the map
// if (wx > map_end_x)
// {
// double t = (map_end_x - ox) / a;
// wx = map_end_x - .001;
// wy = oy + b * t;
// }
// if (wy > map_end_y)
// {
// double t = (map_end_y - oy) / b;
// wx = ox + a * t;
// wy = map_end_y - .001;
// }
// the maximum value to raytrace to is the end of the map
if (wx > map_end_x)
{
double t = (map_end_x - ox) / a;
wx = map_end_x - .001;
wy = oy + b * t;
}
if (wy > map_end_y)
{
double t = (map_end_y - oy) / b;
wx = ox + a * t;
wy = map_end_y - .001;
}
// // now that the vector is scaled correctly... we'll get the map coordinates of its endpoint
// unsigned int x1, y1;
// now that the vector is scaled correctly... we'll get the map coordinates of its endpoint
unsigned int x1, y1;
// // check for legality just in case
// if (!worldToMap(wx, wy, x1, y1))
// continue;
// check for legality just in case
if (!worldToMap(wx, wy, x1, y1))
continue;
// unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
// MarkCell marker(costmap_, FREE_SPACE);
// // and finally... we can execute our trace to clear obstacles along that line
// raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range);
unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
MarkCell marker(costmap_, FREE_SPACE);
// and finally... we can execute our trace to clear obstacles along that line
raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range);
// updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
// }
// }
updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
}
}
// void ObstacleLayer::activate()
// {
@@ -565,15 +561,15 @@
// }
// }
// void ObstacleLayer::updateRaytraceBounds(double ox, double oy, double wx, double wy, double range,
// double* min_x, double* min_y, double* max_x, double* max_y)
// {
// double dx = wx-ox, dy = wy-oy;
// double full_distance = hypot(dx, dy);
// double scale = std::min(1.0, range / full_distance);
// double ex = ox + dx * scale, ey = oy + dy * scale;
// touch(ex, ey, min_x, min_y, max_x, max_y);
// }
void ObstacleLayer::updateRaytraceBounds(double ox, double oy, double wx, double wy, double range,
double* min_x, double* min_y, double* max_x, double* max_y)
{
double dx = wx-ox, dy = wy-oy;
double full_distance = hypot(dx, dy);
double scale = std::min(1.0, range / full_distance);
double ex = ox + dx * scale, ey = oy + dy * scale;
touch(ex, ey, min_x, min_y, max_x, max_y);
}
// void ObstacleLayer::reset()
// {
@@ -583,4 +579,12 @@
// activate();
// }
// } // namespace costmap_2d
// Export factory function
static PluginPtr create_obstacle_plugin() {
return std::make_shared<ObstacleLayer>();
}
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_obstacle_plugin, create_plugin)
} // namespace costmap_2d

12
plugins/preferred_layer.cpp
View File

@@ -1,7 +1,5 @@
#include <costmap_2d/preferred_layer.h>
#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(costmap_2d::PreferredLayer, costmap_2d::Layer)
#include <boost/dll/alias.hpp>
using costmap_2d::NO_INFORMATION;
using costmap_2d::FREE_SPACE;
@@ -25,4 +23,12 @@ unsigned char PreferredLayer::interpretValue(unsigned char value)
return scale * LETHAL_OBSTACLE;
}
// Export factory function
static PluginPtr create_preferred_plugin() {
return std::make_shared<PreferredLayer>();
}
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_preferred_plugin, create_plugin)
}

201
plugins/static_layer.cpp
View File

@@ -1,11 +1,14 @@
#include <costmap_2d/static_layer.h>
#include <costmap_2d/costmap_math.h>
#include <costmap_2d/data_convert.h>
#include <costmap_2d/utils.h>
#include <tf2/convert.h>
#include <tf2/utils.h>
// #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <boost/dll/alias.hpp>
using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::FREE_SPACE;
@@ -27,6 +30,7 @@ void StaticLayer::onInitialize()
current_ = true;
global_frame_ = layered_costmap_->getGlobalFrameID();
getParams();
// nh.param("map_topic", map_topic_, std::string("map"));
// nh.param("first_map_only", first_map_only_, false);
// nh.param("subscribe_to_updates", subscribe_to_updates_, false);
@@ -59,11 +63,11 @@ void StaticLayer::onInitialize()
// r.sleep();
// }
// printf("Received a %d X %d map at %f m/pix", getSizeInCellsX(), getSizeInCellsY(), getResolution());
// printf("Received a %d X %d map at %f m/pix", getSizeInCellsX(), getSizeInCellsY(), getResolution());
// if (subscribe_to_updates_)
// {
// printf("Subscribing to updates");
std::cout<<"Subscribing to updates"<<std::endl;
// map_update_sub_ = g_nh.subscribe(map_topic_ + "_updates", 10, &StaticLayer::incomingUpdate, this);
// }
// }
@@ -74,6 +78,48 @@ void StaticLayer::onInitialize()
}
bool StaticLayer::getParams()
{
try {
YAML::Node config = YAML::LoadFile("/home/duongtd/robotics_core/costmap_2d/config/config.yaml");
YAML::Node layer = config["static_layer"];
bool enabled = loadParam(layer, "enabled", true);
bool first_map_only = loadParam(layer, "first_map_only", false);
bool subscribe_to_updates = loadParam(layer, "subscribe_to_updates", false);
bool track_unknown_space = loadParam(layer, "track_unknown_space", true);
bool use_maximum = loadParam(layer, "use_maximum", false);
int lethal_cost_threshold = loadParam(layer, "lethal_cost_threshold", 100);
int unknown_cost_value = loadParam(layer, "unknown_cost_value", -1);
bool trinary_costmap = loadParam(layer, "trinary_costmap", true);
std::string base_frame_id = loadParam(layer, "base_frame_id", std::string("map"));
first_map_only_ = first_map_only;
subscribe_to_updates_ = subscribe_to_updates;
track_unknown_space_ = track_unknown_space;
use_maximum_ = use_maximum;
lethal_threshold_ = std::max(std::min(lethal_cost_threshold, 100), 0);
unknown_cost_value_ = unknown_cost_value;
trinary_costmap_ = trinary_costmap;
base_frame_id_ = base_frame_id;
if (enabled_ != enabled) {
enabled_ = enabled;
has_updated_data_ = true;
x_ = y_ = 0;
width_ = size_x_;
height_ = size_y_;
}
}
catch (const YAML::BadFile& e) {
std::cerr << "Cannot open YAML file: " << e.what() << std::endl;
return false;
}
return true;
}
// void StaticLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
// {
// if (config.enabled != enabled_)
@@ -116,82 +162,88 @@ unsigned char StaticLayer::interpretValue(unsigned char value)
void StaticLayer::incomingMap(const nav_msgs::OccupancyGrid& new_map)
{
unsigned int size_x = new_map.info.width, size_y = new_map.info.height;
printf("Received a %d X %d map at %f m/pix", size_x, size_y, new_map.info.resolution);
// resize costmap if size, resolution or origin do not match
Costmap2D* master = layered_costmap_->getCostmap();
if (!layered_costmap_->isRolling() &&
(master->getSizeInCellsX() != size_x ||
master->getSizeInCellsY() != size_y ||
master->getResolution() != new_map.info.resolution ||
master->getOriginX() != new_map.info.origin.position.x ||
master->getOriginY() != new_map.info.origin.position.y))
if(!map_shutdown_)
{
// Update the size of the layered costmap (and all layers, including this one)
printf("Resizing costmap to %d X %d at %f m/pix", size_x, size_y, new_map.info.resolution);
layered_costmap_->resizeMap(size_x, size_y, new_map.info.resolution, new_map.info.origin.position.x,
new_map.info.origin.position.y,
true /* set size_locked to true, prevents reconfigureCb from overriding map size*/);
}
else if (size_x_ != size_x || size_y_ != size_y ||
resolution_ != new_map.info.resolution ||
origin_x_ != new_map.info.origin.position.x ||
origin_y_ != new_map.info.origin.position.y)
{
// only update the size of the costmap stored locally in this layer
printf("Resizing static layer to %d X %d at %f m/pix", size_x, size_y, new_map.info.resolution);
resizeMap(size_x, size_y, new_map.info.resolution,
new_map.info.origin.position.x, new_map.info.origin.position.y);
}
unsigned int size_x = new_map.info.width, size_y = new_map.info.height;
unsigned int index = 0;
printf("Received a %d X %d map at %f m/pix", size_x, size_y, new_map.info.resolution);
// initialize the costmap with static data
for (unsigned int i = 0; i < size_y; ++i)
{
for (unsigned int j = 0; j < size_x; ++j)
// resize costmap if size, resolution or origin do not match
Costmap2D* master = layered_costmap_->getCostmap();
if (!layered_costmap_->isRolling() &&
(master->getSizeInCellsX() != size_x ||
master->getSizeInCellsY() != size_y ||
master->getResolution() != new_map.info.resolution ||
master->getOriginX() != new_map.info.origin.position.x ||
master->getOriginY() != new_map.info.origin.position.y))
{
unsigned char value = new_map.data[index];
costmap_[index] = interpretValue(value);
++index;
// Update the size of the layered costmap (and all layers, including this one)
printf("Resizing costmap to %d X %d at %f m/pix", size_x, size_y, new_map.info.resolution);
layered_costmap_->resizeMap(size_x, size_y, new_map.info.resolution, new_map.info.origin.position.x,
new_map.info.origin.position.y,
true /* set size_locked to true, prevents reconfigureCb from overriding map size*/);
}
else if (size_x_ != size_x || size_y_ != size_y ||
resolution_ != new_map.info.resolution ||
origin_x_ != new_map.info.origin.position.x ||
origin_y_ != new_map.info.origin.position.y)
{
// only update the size of the costmap stored locally in this layer
printf("Resizing static layer to %d X %d at %f m/pix", size_x, size_y, new_map.info.resolution);
resizeMap(size_x, size_y, new_map.info.resolution,
new_map.info.origin.position.x, new_map.info.origin.position.y);
}
unsigned int index = 0;
// initialize the costmap with static data
for (unsigned int i = 0; i < size_y; ++i)
{
for (unsigned int j = 0; j < size_x; ++j)
{
unsigned char value = new_map.data[index];
costmap_[index] = interpretValue(value);
++index;
}
}
map_frame_ = new_map.header.frame_id;
// we have a new map, update full size of map
x_ = y_ = 0;
width_ = size_x_;
height_ = size_y_;
map_received_ = true;
has_updated_data_ = true;
}
map_frame_ = new_map.header.frame_id;
// we have a new map, update full size of map
x_ = y_ = 0;
width_ = size_x_;
height_ = size_y_;
map_received_ = true;
has_updated_data_ = true;
// shutdown the map subscrber if firt_map_only_ flag is on
if (first_map_only_)
{
printf("Shutting down the map subscriber. first_map_only flag is on");
map_shutdown_ = true;
// map_sub_.shutdown();
}
}
void StaticLayer::incomingUpdate(const map_msgs::OccupancyGridUpdate& update)
{
unsigned int di = 0;
for (unsigned int y = 0; y < update.height ; y++)
if(!map_update_shutdown_)
{
unsigned int index_base = (update.y + y) * size_x_;
for (unsigned int x = 0; x < update.width ; x++)
unsigned int di = 0;
for (unsigned int y = 0; y < update.height ; y++)
{
unsigned int index = index_base + x + update.x;
costmap_[index] = interpretValue(update.data[di++]);
unsigned int index_base = (update.y + y) * size_x_;
for (unsigned int x = 0; x < update.width ; x++)
{
unsigned int index = index_base + x + update.x;
costmap_[index] = interpretValue(update.data[di++]);
}
}
x_ = update.x;
y_ = update.y;
width_ = update.width;
height_ = update.height;
has_updated_data_ = true;
}
x_ = update.x;
y_ = update.y;
width_ = update.width;
height_ = update.height;
has_updated_data_ = true;
}
void StaticLayer::activate()
@@ -199,23 +251,26 @@ void StaticLayer::activate()
onInitialize();
}
// void StaticLayer::deactivate()
// {
// map_sub_.shutdown();
// if (subscribe_to_updates_)
// map_update_sub_.shutdown();
// }
void StaticLayer::deactivate()
{
map_shutdown_ = true;
if (subscribe_to_updates_)
map_update_shutdown_ = true;
}
void StaticLayer::reset()
{
if (first_map_only_)
{
map_shutdown_ = false;
map_update_shutdown_ = false;
has_updated_data_ = true;
}
else
{
onInitialize();
}
printf("RESET MAP");
}
void StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
@@ -261,15 +316,16 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
unsigned int mx, my;
double wx, wy;
// Might even be in a different frame
geometry_msgs::TransformStamped transform;
// geometry_msgs::TransformStamped transform;
tf2::TransformStampedMsg transformMsg;
try
{
// transformMsg = tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0));
tf_->canTransform(map_frame_, global_frame_, tf2::Time());
tf_->lookupTransform(map_frame_,
global_frame_,
tf2::Time());
bool status =tf_->canTransform(map_frame_, global_frame_, tf2::Time());
if(!status) throw tf2::TransformException("[static_layer] Cannot transform");
transformMsg = tf_->lookupTransform(map_frame_,
global_frame_,
tf2::Time());
}
catch (tf2::TransformException ex)
{
@@ -278,7 +334,8 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
}
// Copy map data given proper transformations
tf2::Transform tf2_transform;
// tf2::convert(transform.transform, tf2_transform);
tf2_transform = convertToTf2Transform(transformMsg.transform);
// tf2::convert(transformMsg.transform, tf2_transform);
for (unsigned int i = min_i; i < max_i; ++i)
{
for (unsigned int j = min_j; j < max_j; ++j)
@@ -287,7 +344,7 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);
// Transform from global_frame_ to map_frame_
tf2::Vector3 p(wx, wy, 0);
// p = tf2_transform*p;
p = tf2_transform*p;
// Set master_grid with cell from map
if (worldToMap(p.x(), p.y(), mx, my))
{
@@ -302,7 +359,7 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
}
// Export factory function
static PluginPtr create_static_plugin() {
static PluginCostmapLayerPtr create_static_plugin() {
return std::make_shared<StaticLayer>();
}

12
plugins/unpreferred_layer.cpp
View File

@@ -1,7 +1,5 @@
#include <costmap_2d/unpreferred_layer.h>
#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(costmap_2d::UnPreferredLayer, costmap_2d::Layer)
#include <boost/dll/alias.hpp>
using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE;
@@ -24,4 +22,12 @@ unsigned char UnPreferredLayer::interpretValue(unsigned char value)
return scale * LETHAL_OBSTACLE;
}
// Export factory function
static PluginPtr create_unpreferred_plugin() {
return std::make_shared<UnPreferredLayer>();
}
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_unpreferred_plugin, create_plugin)
}

782
plugins/voxel_layer.cpp
View File

@@ -36,11 +36,12 @@
* David V. Lu!!
*********************************************************************/
#include <costmap_2d/voxel_layer.h>
#include <pluginlib/class_list_macros.hpp>
#include <costmap_2d/utils.h>
#include <boost/dll/alias.hpp>
#include <sensor_msgs/point_cloud2_iterator.h>
#define VOXEL_BITS 16
PLUGINLIB_EXPORT_CLASS(costmap_2d::VoxelLayer, costmap_2d::Layer)
using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE;
@@ -52,398 +53,403 @@ using costmap_2d::Observation;
namespace costmap_2d
{
void VoxelLayer::onInitialize()
{
ObstacleLayer::onInitialize();
ros::NodeHandle private_nh("~/" + name_);
// void VoxelLayer::onInitialize()
// {
// ObstacleLayer::onInitialize();
// ros::NodeHandle private_nh("~/" + name_);
private_nh.param("publish_voxel_map", publish_voxel_, false);
if (publish_voxel_)
voxel_pub_ = private_nh.advertise < costmap_2d::VoxelGrid > ("voxel_grid", 1);
// private_nh.param("publish_voxel_map", publish_voxel_, false);
// if (publish_voxel_)
// voxel_pub_ = private_nh.advertise < costmap_2d::VoxelGrid > ("voxel_grid", 1);
clearing_endpoints_pub_ = private_nh.advertise<sensor_msgs::PointCloud>("clearing_endpoints", 1);
}
// clearing_endpoints_pub_ = private_nh.advertise<sensor_msgs::PointCloud>("clearing_endpoints", 1);
// }
void VoxelLayer::setupDynamicReconfigure(ros::NodeHandle& nh)
{
voxel_dsrv_ = new dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig>(nh);
dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig>::CallbackType cb =
[this](auto& config, auto level){ reconfigureCB(config, level); };
voxel_dsrv_->setCallback(cb);
}
// void VoxelLayer::setupDynamicReconfigure(ros::NodeHandle& nh)
// {
// voxel_dsrv_ = new dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig>(nh);
// dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig>::CallbackType cb =
// [this](auto& config, auto level){ reconfigureCB(config, level); };
// voxel_dsrv_->setCallback(cb);
// }
VoxelLayer::~VoxelLayer()
{
if (voxel_dsrv_)
delete voxel_dsrv_;
{}
// void VoxelLayer::reconfigureCB(costmap_2d::VoxelPluginConfig &config, uint32_t level)
// {
// enabled_ = config.enabled;
// footprint_clearing_enabled_ = config.footprint_clearing_enabled;
// max_obstacle_height_ = config.max_obstacle_height;
// size_z_ = config.z_voxels;
// origin_z_ = config.origin_z;
// z_resolution_ = config.z_resolution;
// unknown_threshold_ = config.unknown_threshold + (VOXEL_BITS - size_z_);
// mark_threshold_ = config.mark_threshold;
// combination_method_ = config.combination_method;
// matchSize();
// }
// void VoxelLayer::matchSize()
// {
// ObstacleLayer::matchSize();
// voxel_grid_.resize(size_x_, size_y_, size_z_);
// ROS_ASSERT(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_);
// }
// void VoxelLayer::reset()
// {
// deactivate();
// resetMaps();
// voxel_grid_.reset();
// activate();
// }
// void VoxelLayer::resetMaps()
// {
// Costmap2D::resetMaps();
// voxel_grid_.reset();
// }
// void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
// double* min_y, double* max_x, double* max_y)
// {
// if (rolling_window_)
// updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
// useExtraBounds(min_x, min_y, max_x, max_y);
// bool current = true;
// std::vector<Observation> observations, clearing_observations;
// // get the marking observations
// current = getMarkingObservations(observations) && current;
// // get the clearing observations
// current = getClearingObservations(clearing_observations) && current;
// // update the global current status
// current_ = current;
// // raytrace freespace
// for (unsigned int i = 0; i < clearing_observations.size(); ++i)
// {
// raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
// }
// // place the new obstacles into a priority queue... each with a priority of zero to begin with
// for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
// {
// const Observation& obs = *it;
// const sensor_msgs::PointCloud2& cloud = *(obs.cloud_);
// double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
// sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
// sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
// sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
// for (unsigned int i = 0; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
// {
// // if the obstacle is too high or too far away from the robot we won't add it
// if (*iter_z > max_obstacle_height_)
// continue;
// // compute the squared distance from the hitpoint to the pointcloud's origin
// double sq_dist = (*iter_x - obs.origin_.x) * (*iter_x - obs.origin_.x)
// + (*iter_y - obs.origin_.y) * (*iter_y - obs.origin_.y)
// + (*iter_z - obs.origin_.z) * (*iter_z - obs.origin_.z);
// // if the point is far enough away... we won't consider it
// if (sq_dist >= sq_obstacle_range)
// continue;
// // now we need to compute the map coordinates for the observation
// unsigned int mx, my, mz;
// if (*iter_z < origin_z_)
// {
// if (!worldToMap3D(*iter_x, *iter_y, origin_z_, mx, my, mz))
// continue;
// }
// else if (!worldToMap3D(*iter_x, *iter_y, *iter_z, mx, my, mz))
// {
// continue;
// }
// // mark the cell in the voxel grid and check if we should also mark it in the costmap
// if (voxel_grid_.markVoxelInMap(mx, my, mz, mark_threshold_))
// {
// unsigned int index = getIndex(mx, my);
// costmap_[index] = LETHAL_OBSTACLE;
// touch(double(*iter_x), double(*iter_y), min_x, min_y, max_x, max_y);
// }
// }
// }
// if (publish_voxel_)
// {
// costmap_2d::VoxelGrid grid_msg;
// unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY();
// grid_msg.size_x = voxel_grid_.sizeX();
// grid_msg.size_y = voxel_grid_.sizeY();
// grid_msg.size_z = voxel_grid_.sizeZ();
// grid_msg.data.resize(size);
// memcpy(&grid_msg.data[0], voxel_grid_.getData(), size * sizeof(unsigned int));
// grid_msg.origin.x = origin_x_;
// grid_msg.origin.y = origin_y_;
// grid_msg.origin.z = origin_z_;
// grid_msg.resolutions.x = resolution_;
// grid_msg.resolutions.y = resolution_;
// grid_msg.resolutions.z = z_resolution_;
// grid_msg.header.frame_id = global_frame_;
// grid_msg.header.stamp = ros::Time::now();
// voxel_pub_.publish(grid_msg);
// }
// updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
// }
// void VoxelLayer::clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info)
// {
// // get the cell coordinates of the center point of the window
// unsigned int mx, my;
// if (!worldToMap(wx, wy, mx, my))
// return;
// // compute the bounds of the window
// double start_x = wx - w_size_x / 2;
// double start_y = wy - w_size_y / 2;
// double end_x = start_x + w_size_x;
// double end_y = start_y + w_size_y;
// // scale the window based on the bounds of the costmap
// start_x = std::max(origin_x_, start_x);
// start_y = std::max(origin_y_, start_y);
// end_x = std::min(origin_x_ + getSizeInMetersX(), end_x);
// end_y = std::min(origin_y_ + getSizeInMetersY(), end_y);
// // get the map coordinates of the bounds of the window
// unsigned int map_sx, map_sy, map_ex, map_ey;
// // check for legality just in case
// if (!worldToMap(start_x, start_y, map_sx, map_sy) || !worldToMap(end_x, end_y, map_ex, map_ey))
// return;
// // we know that we want to clear all non-lethal obstacles in this window to get it ready for inflation
// unsigned int index = getIndex(map_sx, map_sy);
// unsigned char* current = &costmap_[index];
// for (unsigned int j = map_sy; j <= map_ey; ++j)
// {
// for (unsigned int i = map_sx; i <= map_ex; ++i)
// {
// // if the cell is a lethal obstacle... we'll keep it and queue it, otherwise... we'll clear it
// if (*current != LETHAL_OBSTACLE)
// {
// if (clear_no_info || *current != NO_INFORMATION)
// {
// *current = FREE_SPACE;
// voxel_grid_.clearVoxelColumn(index);
// }
// }
// current++;
// index++;
// }
// current += size_x_ - (map_ex - map_sx) - 1;
// index += size_x_ - (map_ex - map_sx) - 1;
// }
// }
// void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y,
// double* max_x, double* max_y)
// {
// size_t clearing_observation_cloud_size = clearing_observation.cloud_->height * clearing_observation.cloud_->width;
// if (clearing_observation_cloud_size == 0)
// return;
// double sensor_x, sensor_y, sensor_z;
// double ox = clearing_observation.origin_.x;
// double oy = clearing_observation.origin_.y;
// double oz = clearing_observation.origin_.z;
// if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z))
// {
// ROS_WARN_THROTTLE(
// 1.0,
// "The origin for the sensor at (%.2f, %.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.",
// ox, oy, oz);
// return;
// }
// bool publish_clearing_points = (clearing_endpoints_pub_.getNumSubscribers() > 0);
// if (publish_clearing_points)
// {
// clearing_endpoints_.points.clear();
// clearing_endpoints_.points.reserve(clearing_observation_cloud_size);
// }
// // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
// double map_end_x = origin_x_ + getSizeInMetersX();
// double map_end_y = origin_y_ + getSizeInMetersY();
// sensor_msgs::PointCloud2ConstIterator<float> iter_x(*(clearing_observation.cloud_), "x");
// sensor_msgs::PointCloud2ConstIterator<float> iter_y(*(clearing_observation.cloud_), "y");
// sensor_msgs::PointCloud2ConstIterator<float> iter_z(*(clearing_observation.cloud_), "z");
// for (;iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
// {
// double wpx = *iter_x;
// double wpy = *iter_y;
// double wpz = *iter_z;
// double distance = dist(ox, oy, oz, wpx, wpy, wpz);
// double scaling_fact = 1.0;
// scaling_fact = std::max(std::min(scaling_fact, (distance - 2 * resolution_) / distance), 0.0);
// wpx = scaling_fact * (wpx - ox) + ox;
// wpy = scaling_fact * (wpy - oy) + oy;
// wpz = scaling_fact * (wpz - oz) + oz;
// double a = wpx - ox;
// double b = wpy - oy;
// double c = wpz - oz;
// double t = 1.0;
// // we can only raytrace to a maximum z height
// if (wpz > max_obstacle_height_)
// {
// // we know we want the vector's z value to be max_z
// t = std::max(0.0, std::min(t, (max_obstacle_height_ - 0.01 - oz) / c));
// }
// // and we can only raytrace down to the floor
// else if (wpz < origin_z_)
// {
// // we know we want the vector's z value to be 0.0
// t = std::min(t, (origin_z_ - oz) / c);
// }
// // the minimum value to raytrace from is the origin
// if (wpx < origin_x_)
// {
// t = std::min(t, (origin_x_ - ox) / a);
// }
// if (wpy < origin_y_)
// {
// t = std::min(t, (origin_y_ - oy) / b);
// }
// // the maximum value to raytrace to is the end of the map
// if (wpx > map_end_x)
// {
// t = std::min(t, (map_end_x - ox) / a);
// }
// if (wpy > map_end_y)
// {
// t = std::min(t, (map_end_y - oy) / b);
// }
// wpx = ox + a * t;
// wpy = oy + b * t;
// wpz = oz + c * t;
// double point_x, point_y, point_z;
// if (worldToMap3DFloat(wpx, wpy, wpz, point_x, point_y, point_z))
// {
// unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
// // voxel_grid_.markVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z);
// voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_,
// unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION,
// cell_raytrace_range);
// updateRaytraceBounds(ox, oy, wpx, wpy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
// if (publish_clearing_points)
// {
// geometry_msgs::Point32 point;
// point.x = wpx;
// point.y = wpy;
// point.z = wpz;
// clearing_endpoints_.points.push_back(point);
// }
// }
// }
// if (publish_clearing_points)
// {
// clearing_endpoints_.header.frame_id = global_frame_;
// clearing_endpoints_.header.stamp = clearing_observation.cloud_->header.stamp;
// clearing_endpoints_.header.seq = clearing_observation.cloud_->header.seq;
// clearing_endpoints_pub_.publish(clearing_endpoints_);
// }
// }
// void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
// {
// // project the new origin into the grid
// int cell_ox, cell_oy;
// cell_ox = int((new_origin_x - origin_x_) / resolution_);
// cell_oy = int((new_origin_y - origin_y_) / resolution_);
// // compute the associated world coordinates for the origin cell
// // beacuase we want to keep things grid-aligned
// double new_grid_ox, new_grid_oy;
// new_grid_ox = origin_x_ + cell_ox * resolution_;
// new_grid_oy = origin_y_ + cell_oy * resolution_;
// // To save casting from unsigned int to int a bunch of times
// int size_x = size_x_;
// int size_y = size_y_;
// // we need to compute the overlap of the new and existing windows
// int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
// lower_left_x = std::min(std::max(cell_ox, 0), size_x);
// lower_left_y = std::min(std::max(cell_oy, 0), size_y);
// upper_right_x = std::min(std::max(cell_ox + size_x, 0), size_x);
// upper_right_y = std::min(std::max(cell_oy + size_y, 0), size_y);
// unsigned int cell_size_x = upper_right_x - lower_left_x;
// unsigned int cell_size_y = upper_right_y - lower_left_y;
// // we need a map to store the obstacles in the window temporarily
// unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
// unsigned int* local_voxel_map = new unsigned int[cell_size_x * cell_size_y];
// unsigned int* voxel_map = voxel_grid_.getData();
// // copy the local window in the costmap to the local map
// copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
// copyMapRegion(voxel_map, lower_left_x, lower_left_y, size_x_, local_voxel_map, 0, 0, cell_size_x, cell_size_x,
// cell_size_y);
// // we'll reset our maps to unknown space if appropriate
// resetMaps();
// // update the origin with the appropriate world coordinates
// origin_x_ = new_grid_ox;
// origin_y_ = new_grid_oy;
// // compute the starting cell location for copying data back in
// int start_x = lower_left_x - cell_ox;
// int start_y = lower_left_y - cell_oy;
// // now we want to copy the overlapping information back into the map, but in its new location
// copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
// copyMapRegion(local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y, size_x_, cell_size_x, cell_size_y);
// // make sure to clean up
// delete[] local_map;
// delete[] local_voxel_map;
// }
// Export factory function
static PluginPtr create_voxel_plugin() {
return std::make_shared<VoxelLayer>();
}
void VoxelLayer::reconfigureCB(costmap_2d::VoxelPluginConfig &config, uint32_t level)
{
enabled_ = config.enabled;
footprint_clearing_enabled_ = config.footprint_clearing_enabled;
max_obstacle_height_ = config.max_obstacle_height;
size_z_ = config.z_voxels;
origin_z_ = config.origin_z;
z_resolution_ = config.z_resolution;
unknown_threshold_ = config.unknown_threshold + (VOXEL_BITS - size_z_);
mark_threshold_ = config.mark_threshold;
combination_method_ = config.combination_method;
matchSize();
}
void VoxelLayer::matchSize()
{
ObstacleLayer::matchSize();
voxel_grid_.resize(size_x_, size_y_, size_z_);
ROS_ASSERT(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_);
}
void VoxelLayer::reset()
{
deactivate();
resetMaps();
voxel_grid_.reset();
activate();
}
void VoxelLayer::resetMaps()
{
Costmap2D::resetMaps();
voxel_grid_.reset();
}
void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
double* min_y, double* max_x, double* max_y)
{
if (rolling_window_)
updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
useExtraBounds(min_x, min_y, max_x, max_y);
bool current = true;
std::vector<Observation> observations, clearing_observations;
// get the marking observations
current = getMarkingObservations(observations) && current;
// get the clearing observations
current = getClearingObservations(clearing_observations) && current;
// update the global current status
current_ = current;
// raytrace freespace
for (unsigned int i = 0; i < clearing_observations.size(); ++i)
{
raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
}
// place the new obstacles into a priority queue... each with a priority of zero to begin with
for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
{
const Observation& obs = *it;
const sensor_msgs::PointCloud2& cloud = *(obs.cloud_);
double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
for (unsigned int i = 0; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
{
// if the obstacle is too high or too far away from the robot we won't add it
if (*iter_z > max_obstacle_height_)
continue;
// compute the squared distance from the hitpoint to the pointcloud's origin
double sq_dist = (*iter_x - obs.origin_.x) * (*iter_x - obs.origin_.x)
+ (*iter_y - obs.origin_.y) * (*iter_y - obs.origin_.y)
+ (*iter_z - obs.origin_.z) * (*iter_z - obs.origin_.z);
// if the point is far enough away... we won't consider it
if (sq_dist >= sq_obstacle_range)
continue;
// now we need to compute the map coordinates for the observation
unsigned int mx, my, mz;
if (*iter_z < origin_z_)
{
if (!worldToMap3D(*iter_x, *iter_y, origin_z_, mx, my, mz))
continue;
}
else if (!worldToMap3D(*iter_x, *iter_y, *iter_z, mx, my, mz))
{
continue;
}
// mark the cell in the voxel grid and check if we should also mark it in the costmap
if (voxel_grid_.markVoxelInMap(mx, my, mz, mark_threshold_))
{
unsigned int index = getIndex(mx, my);
costmap_[index] = LETHAL_OBSTACLE;
touch(double(*iter_x), double(*iter_y), min_x, min_y, max_x, max_y);
}
}
}
if (publish_voxel_)
{
costmap_2d::VoxelGrid grid_msg;
unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY();
grid_msg.size_x = voxel_grid_.sizeX();
grid_msg.size_y = voxel_grid_.sizeY();
grid_msg.size_z = voxel_grid_.sizeZ();
grid_msg.data.resize(size);
memcpy(&grid_msg.data[0], voxel_grid_.getData(), size * sizeof(unsigned int));
grid_msg.origin.x = origin_x_;
grid_msg.origin.y = origin_y_;
grid_msg.origin.z = origin_z_;
grid_msg.resolutions.x = resolution_;
grid_msg.resolutions.y = resolution_;
grid_msg.resolutions.z = z_resolution_;
grid_msg.header.frame_id = global_frame_;
grid_msg.header.stamp = ros::Time::now();
voxel_pub_.publish(grid_msg);
}
updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
}
void VoxelLayer::clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info)
{
// get the cell coordinates of the center point of the window
unsigned int mx, my;
if (!worldToMap(wx, wy, mx, my))
return;
// compute the bounds of the window
double start_x = wx - w_size_x / 2;
double start_y = wy - w_size_y / 2;
double end_x = start_x + w_size_x;
double end_y = start_y + w_size_y;
// scale the window based on the bounds of the costmap
start_x = std::max(origin_x_, start_x);
start_y = std::max(origin_y_, start_y);
end_x = std::min(origin_x_ + getSizeInMetersX(), end_x);
end_y = std::min(origin_y_ + getSizeInMetersY(), end_y);
// get the map coordinates of the bounds of the window
unsigned int map_sx, map_sy, map_ex, map_ey;
// check for legality just in case
if (!worldToMap(start_x, start_y, map_sx, map_sy) || !worldToMap(end_x, end_y, map_ex, map_ey))
return;
// we know that we want to clear all non-lethal obstacles in this window to get it ready for inflation
unsigned int index = getIndex(map_sx, map_sy);
unsigned char* current = &costmap_[index];
for (unsigned int j = map_sy; j <= map_ey; ++j)
{
for (unsigned int i = map_sx; i <= map_ex; ++i)
{
// if the cell is a lethal obstacle... we'll keep it and queue it, otherwise... we'll clear it
if (*current != LETHAL_OBSTACLE)
{
if (clear_no_info || *current != NO_INFORMATION)
{
*current = FREE_SPACE;
voxel_grid_.clearVoxelColumn(index);
}
}
current++;
index++;
}
current += size_x_ - (map_ex - map_sx) - 1;
index += size_x_ - (map_ex - map_sx) - 1;
}
}
void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y,
double* max_x, double* max_y)
{
size_t clearing_observation_cloud_size = clearing_observation.cloud_->height * clearing_observation.cloud_->width;
if (clearing_observation_cloud_size == 0)
return;
double sensor_x, sensor_y, sensor_z;
double ox = clearing_observation.origin_.x;
double oy = clearing_observation.origin_.y;
double oz = clearing_observation.origin_.z;
if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z))
{
ROS_WARN_THROTTLE(
1.0,
"The origin for the sensor at (%.2f, %.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.",
ox, oy, oz);
return;
}
bool publish_clearing_points = (clearing_endpoints_pub_.getNumSubscribers() > 0);
if (publish_clearing_points)
{
clearing_endpoints_.points.clear();
clearing_endpoints_.points.reserve(clearing_observation_cloud_size);
}
// we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
double map_end_x = origin_x_ + getSizeInMetersX();
double map_end_y = origin_y_ + getSizeInMetersY();
sensor_msgs::PointCloud2ConstIterator<float> iter_x(*(clearing_observation.cloud_), "x");
sensor_msgs::PointCloud2ConstIterator<float> iter_y(*(clearing_observation.cloud_), "y");
sensor_msgs::PointCloud2ConstIterator<float> iter_z(*(clearing_observation.cloud_), "z");
for (;iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
{
double wpx = *iter_x;
double wpy = *iter_y;
double wpz = *iter_z;
double distance = dist(ox, oy, oz, wpx, wpy, wpz);
double scaling_fact = 1.0;
scaling_fact = std::max(std::min(scaling_fact, (distance - 2 * resolution_) / distance), 0.0);
wpx = scaling_fact * (wpx - ox) + ox;
wpy = scaling_fact * (wpy - oy) + oy;
wpz = scaling_fact * (wpz - oz) + oz;
double a = wpx - ox;
double b = wpy - oy;
double c = wpz - oz;
double t = 1.0;
// we can only raytrace to a maximum z height
if (wpz > max_obstacle_height_)
{
// we know we want the vector's z value to be max_z
t = std::max(0.0, std::min(t, (max_obstacle_height_ - 0.01 - oz) / c));
}
// and we can only raytrace down to the floor
else if (wpz < origin_z_)
{
// we know we want the vector's z value to be 0.0
t = std::min(t, (origin_z_ - oz) / c);
}
// the minimum value to raytrace from is the origin
if (wpx < origin_x_)
{
t = std::min(t, (origin_x_ - ox) / a);
}
if (wpy < origin_y_)
{
t = std::min(t, (origin_y_ - oy) / b);
}
// the maximum value to raytrace to is the end of the map
if (wpx > map_end_x)
{
t = std::min(t, (map_end_x - ox) / a);
}
if (wpy > map_end_y)
{
t = std::min(t, (map_end_y - oy) / b);
}
wpx = ox + a * t;
wpy = oy + b * t;
wpz = oz + c * t;
double point_x, point_y, point_z;
if (worldToMap3DFloat(wpx, wpy, wpz, point_x, point_y, point_z))
{
unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
// voxel_grid_.markVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z);
voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_,
unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION,
cell_raytrace_range);
updateRaytraceBounds(ox, oy, wpx, wpy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
if (publish_clearing_points)
{
geometry_msgs::Point32 point;
point.x = wpx;
point.y = wpy;
point.z = wpz;
clearing_endpoints_.points.push_back(point);
}
}
}
if (publish_clearing_points)
{
clearing_endpoints_.header.frame_id = global_frame_;
clearing_endpoints_.header.stamp = clearing_observation.cloud_->header.stamp;
clearing_endpoints_.header.seq = clearing_observation.cloud_->header.seq;
clearing_endpoints_pub_.publish(clearing_endpoints_);
}
}
void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
{
// project the new origin into the grid
int cell_ox, cell_oy;
cell_ox = int((new_origin_x - origin_x_) / resolution_);
cell_oy = int((new_origin_y - origin_y_) / resolution_);
// compute the associated world coordinates for the origin cell
// beacuase we want to keep things grid-aligned
double new_grid_ox, new_grid_oy;
new_grid_ox = origin_x_ + cell_ox * resolution_;
new_grid_oy = origin_y_ + cell_oy * resolution_;
// To save casting from unsigned int to int a bunch of times
int size_x = size_x_;
int size_y = size_y_;
// we need to compute the overlap of the new and existing windows
int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
lower_left_x = std::min(std::max(cell_ox, 0), size_x);
lower_left_y = std::min(std::max(cell_oy, 0), size_y);
upper_right_x = std::min(std::max(cell_ox + size_x, 0), size_x);
upper_right_y = std::min(std::max(cell_oy + size_y, 0), size_y);
unsigned int cell_size_x = upper_right_x - lower_left_x;
unsigned int cell_size_y = upper_right_y - lower_left_y;
// we need a map to store the obstacles in the window temporarily
unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
unsigned int* local_voxel_map = new unsigned int[cell_size_x * cell_size_y];
unsigned int* voxel_map = voxel_grid_.getData();
// copy the local window in the costmap to the local map
copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
copyMapRegion(voxel_map, lower_left_x, lower_left_y, size_x_, local_voxel_map, 0, 0, cell_size_x, cell_size_x,
cell_size_y);
// we'll reset our maps to unknown space if appropriate
resetMaps();
// update the origin with the appropriate world coordinates
origin_x_ = new_grid_ox;
origin_y_ = new_grid_oy;
// compute the starting cell location for copying data back in
int start_x = lower_left_x - cell_ox;
int start_y = lower_left_y - cell_oy;
// now we want to copy the overlapping information back into the map, but in its new location
copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
copyMapRegion(local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y, size_x_, cell_size_x, cell_size_y);
// make sure to clean up
delete[] local_map;
delete[] local_voxel_map;
}
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_voxel_plugin, create_plugin)
} // namespace costmap_2d