Skip to content

Commit

Permalink
initial gradient based mapping
Browse files Browse the repository at this point in the history
  • Loading branch information
@abhiramg2021
abhiramg2021 committed Sep 7, 2024
1 parent 015fb04 commit ddc0379
Showing 1 changed file with 176 additions and 140 deletions.
316 changes: 176 additions & 140 deletions urc_perception/src/elevation_mapping.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,185 +13,221 @@
namespace urc_perception
{

ElevationMapping::ElevationMapping(const rclcpp::NodeOptions & options)
: Node("elevation_mapping", options)
{
RCLCPP_INFO(this->get_logger(), "Mapping node has been started.");

declare_parameter("map_frame", "odom");
declare_parameter("camera_frame", "camera_depth_frame");
declare_parameter("depth_topic", "/depth_camera/points");

declare_parameter("resolution", 0.1);
declare_parameter("width", 60);
declare_parameter("min_z", 0.1);
declare_parameter("max_z", 2.0);

declare_parameter("inflation_radius", 0.1);
declare_parameter("inflate_obstacles", true);
ElevationMapping::ElevationMapping(const rclcpp::NodeOptions &options)
: Node("elevation_mapping", options)
{
RCLCPP_INFO(this->get_logger(), "Mapping node has been started.");

width_ = get_parameter("width").as_int();
resolution_ = get_parameter("resolution").as_double();
map_frame_ = get_parameter("map_frame").as_string();
camera_frame_ = get_parameter("camera_frame").as_string();
min_z_ = get_parameter("min_z").as_double();
max_z_ = get_parameter("max_z").as_double();
declare_parameter("map_frame", "odom");
declare_parameter("camera_frame", "camera_depth_frame");
declare_parameter("depth_topic", "/depth_camera/points");

cell_inflation_radius_ = cellDistance(get_parameter("inflation_radius").as_double());
inflate_obstacles_ = get_parameter("inflate_obstacles").as_bool();
declare_parameter("resolution", 0.1);
declare_parameter("width", 60);
declare_parameter("min_z", 0.1);
declare_parameter("max_z", 2.0);

RCLCPP_INFO(this->get_logger(), "Cell inflation radius set to %d", cell_inflation_radius_);
RCLCPP_INFO(
this->get_logger(), "Inflate obstacles set to %s.", inflate_obstacles_ ? "true" : "false");
declare_parameter("inflation_radius", 0.1);
declare_parameter("inflate_obstacles", true);

map_.header.frame_id = map_frame_;
map_.info.resolution = resolution_;
map_.info.width = map_.info.height = width_;
map_.data.resize(map_.info.width * map_.info.height);
width_ = get_parameter("width").as_int();
resolution_ = get_parameter("resolution").as_double();
map_frame_ = get_parameter("map_frame").as_string();
camera_frame_ = get_parameter("camera_frame").as_string();
min_z_ = get_parameter("min_z").as_double();
max_z_ = get_parameter("max_z").as_double();

depth_subscriber_ = create_subscription<sensor_msgs::msg::PointCloud2>(
get_parameter("depth_topic").as_string(), 10,
std::bind(&ElevationMapping::handlePointcloud, this, std::placeholders::_1));
cell_inflation_radius_ = cellDistance(get_parameter("inflation_radius").as_double());
inflate_obstacles_ = get_parameter("inflate_obstacles").as_bool();

map_publisher_ = create_publisher<nav_msgs::msg::OccupancyGrid>("/costmap", 10);
RCLCPP_INFO(this->get_logger(), "Cell inflation radius set to %d", cell_inflation_radius_);
RCLCPP_INFO(
this->get_logger(), "Inflate obstacles set to %s.", inflate_obstacles_ ? "true" : "false");

tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
}
map_.header.frame_id = map_frame_;
map_.info.resolution = resolution_;
map_.info.width = map_.info.height = width_;
map_.data.resize(map_.info.width * map_.info.height);

ElevationMapping::~ElevationMapping()
{
}
depth_subscriber_ = create_subscription<sensor_msgs::msg::PointCloud2>(
get_parameter("depth_topic").as_string(), 10,
std::bind(&ElevationMapping::handlePointcloud, this, std::placeholders::_1));

geometry_msgs::msg::TransformStamped ElevationMapping::lookup_transform(
std::string target_frame,
std::string source_frame,
rclcpp::Time time)
{
geometry_msgs::msg::TransformStamped transform;
map_publisher_ = create_publisher<nav_msgs::msg::OccupancyGrid>("/costmap", 10);

try {
transform = tf_buffer_->lookupTransform(target_frame, source_frame, time);
} catch (tf2::TransformException & ex) {
RCLCPP_ERROR(this->get_logger(), "Could not lookup transform: %s", ex.what());
tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
}

return transform;
}

bool ElevationMapping::worldToMap(
double x, double y, nav_msgs::msg::MapMetaData info,
std::pair<int, int> & out)
{
if (x < info.origin.position.x || x >= info.origin.position.x + info.width * info.resolution ||
y < info.origin.position.y || y >= info.origin.position.y + info.height * info.resolution)
ElevationMapping::~ElevationMapping()
{
return false;
}

out.first = (x - info.origin.position.x) / info.resolution;
out.second = (y - info.origin.position.y) / info.resolution;

return true;
}
geometry_msgs::msg::TransformStamped ElevationMapping::lookup_transform(
std::string target_frame,
std::string source_frame,
rclcpp::Time time)
{
geometry_msgs::msg::TransformStamped transform;

void ElevationMapping::handlePointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
{
// Transform the point cloud to the map frame
auto camera_to_map = lookup_transform(map_frame_, msg->header.frame_id, msg->header.stamp);
sensor_msgs::msg::PointCloud2 cloud_global_;
tf2::doTransform(*msg, cloud_global_, camera_to_map);

// Convert the transformed point cloud to a PCL point cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(cloud_global_, *cloud);

// Remove NaN values from the point cloud
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*cloud, *cloud, indices);

// Set the origin of the costmap to the current position of the robot
tf2::Stamped<tf2::Transform> trans;
tf2::fromMsg(lookup_transform(map_frame_, "base_link", msg->header.stamp), trans);

auto pos = trans.getOrigin();
double x = static_cast<int>(pos.x() / map_.info.resolution) * map_.info.resolution;
double y = static_cast<int>(pos.y() / map_.info.resolution) * map_.info.resolution;

map_.info.origin.position.x = x - map_.info.width * map_.info.resolution * 0.5;
map_.info.origin.position.y = y - map_.info.height * map_.info.resolution * 0.5;
map_.info.origin.position.z = 0.0;
map_.info.origin.orientation.w = 1.0;

// Reset the costmap
std::fill(map_.data.begin(), map_.data.end(), 0);

// Update the costmap with the point cloud
for (unsigned int i = 0; i < cloud->size(); i++) {
auto & point = cloud->points[i];

std::pair<int, int> map_coord;
if (!worldToMap(point.x, point.y, map_.info, map_coord)) {
continue;
try
{
transform = tf_buffer_->lookupTransform(target_frame, source_frame, time);
}
catch (tf2::TransformException &ex)
{
RCLCPP_ERROR(this->get_logger(), "Could not lookup transform: %s", ex.what());
}

double z = point.z - pos.z();
double y = point.y - pos.y();
double x = point.x - pos.x();
return transform;
}

if (z < min_z_ || std::sqrt(x * x + y * y) > 2.8) {
continue;
bool ElevationMapping::worldToMap(
double x, double y, nav_msgs::msg::MapMetaData info,
std::pair<int, int> &out)
{
if (x < info.origin.position.x || x >= info.origin.position.x + info.width * info.resolution ||
y < info.origin.position.y || y >= info.origin.position.y + info.height * info.resolution)
{
return false;
}

int costmap_index = map_coord.first + map_coord.second * map_.info.width;
out.first = (x - info.origin.position.x) / info.resolution;
out.second = (y - info.origin.position.y) / info.resolution;

double cost = 0.0;
return true;
}

if (z > max_z_) {
cost = max_cost_;
} else {
cost = (z - min_z_) / (max_z_ - min_z_) * max_cost_;
void ElevationMapping::handlePointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
{
// Transform the point cloud to the map frame
auto camera_to_map = lookup_transform(map_frame_, msg->header.frame_id, msg->header.stamp);
sensor_msgs::msg::PointCloud2 cloud_global_;
tf2::doTransform(*msg, cloud_global_, camera_to_map);

// Convert the transformed point cloud to a PCL point cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(cloud_global_, *cloud);

// Remove NaN values from the point cloud
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*cloud, *cloud, indices);

// Set the origin of the costmap to the current position of the robot
tf2::Stamped<tf2::Transform> trans;
tf2::fromMsg(lookup_transform(map_frame_, "base_link", msg->header.stamp), trans);

auto pos = trans.getOrigin();
double x = static_cast<int>(pos.x() / map_.info.resolution) * map_.info.resolution;
double y = static_cast<int>(pos.y() / map_.info.resolution) * map_.info.resolution;

map_.info.origin.position.x = x - map_.info.width * map_.info.resolution * 0.5;
map_.info.origin.position.y = y - map_.info.height * map_.info.resolution * 0.5;
map_.info.origin.position.z = 0.0;
map_.info.origin.orientation.w = 1.0;

int max_gradient = 0; // TODO: This is a hack to scale the gradients to a reasonable range.
std::vector<int> gradients(cloud->size()); // TODO:can collapse later into a single loop.
int neighborhood_size = 3;
for (unsigned int i = 0; i < cloud->size(); i++)
{
auto &current_point = cloud->points[i];
for (int dx = -neighborhood_size; dx <= neighborhood_size; dx++)
{
for (int dy = -neighborhood_size; dy <= neighborhood_size; dy++)
{
int x = i % cloud->row_step;
int y = i / cloud->row_step;

int nx = x + dx;
int ny = y + dy;

if (nx < 0 || nx >= map_.info.width || ny < 0 || ny >= map_.info.height)
{
continue;
}

int ni = nx + ny * map_.info.width;
auto &neighbor_point = cloud->points[ni];
int current_gradient = std::abs(current_point.z - neighbor_point.z);
gradients[i] = std::max(gradients[i], current_gradient);
}
}
max_gradient = std::max(max_gradient, gradients[i]);
}

if (cost > map_.data[costmap_index]) {
map_.data[costmap_index] = cost;
// Reset the costmap
std::fill(map_.data.begin(), map_.data.end(), 0);

if (inflate_obstacles_) {
inflate(map_coord.first, map_coord.second, cost, cell_inflation_radius_);
// Update the costmap using the gradients.
for (unsigned int i = 0; i < cloud->size(); i++)
{
auto &point = cloud->points[i];

std::pair<int, int> map_coord;
if (!worldToMap(point.x, point.y, map_.info, map_coord))
{
continue;
}
}
}

map_.header.stamp = get_clock()->now();
map_publisher_->publish(map_);
}
double z = point.z - pos.z();
double y = point.y - pos.y();
double x = point.x - pos.x();

void ElevationMapping::inflate(int cell_x, int cell_y, double cell_cost, int radius)
{
for (int x = cell_x - radius; x <= cell_x + radius; x++) {
for (int y = cell_y - radius; y <= cell_y + radius; y++) {
if (x < 0 || x >= map_.info.width || y < 0 || y >= map_.info.height) {
if (z < min_z_ || std::sqrt(x * x + y * y) > 2.8)
{
continue;
}

int dist = std::sqrt(std::pow(x - cell_x, 2) + std::pow(y - cell_y, 2));
int costmap_index = map_coord.first + map_coord.second * map_.info.width;

if (dist <= radius) {
int index = x + y * map_.info.width;
double inflated_cost = gaussian(dist) * cell_cost;
double cost = gradients[i] / max_gradient * max_cost_;
if (cost > map_.data[costmap_index])
{
map_.data[costmap_index] = cost;

if (inflated_cost > map_.data[index]) {
map_.data[index] = inflated_cost;
if (inflate_obstacles_)
{
inflate(map_coord.first, map_coord.second, cost, cell_inflation_radius_);
}
}
}

map_.header.stamp = get_clock()->now();
map_publisher_->publish(map_);
}
}

double ElevationMapping::gaussian(double x)
{
return std::exp(-0.5 * x * x / cell_inflation_radius_);
}
void ElevationMapping::inflate(int cell_x, int cell_y, double cell_cost, int radius)
{
for (int x = cell_x - radius; x <= cell_x + radius; x++)
{
for (int y = cell_y - radius; y <= cell_y + radius; y++)
{
if (x < 0 || x >= map_.info.width || y < 0 || y >= map_.info.height)
{
continue;
}

int dist = std::sqrt(std::pow(x - cell_x, 2) + std::pow(y - cell_y, 2));

if (dist <= radius)
{
int index = x + y * map_.info.width;
double inflated_cost = gaussian(dist) * cell_cost;

if (inflated_cost > map_.data[index])
{
map_.data[index] = inflated_cost;
}
}
}
}
}

double ElevationMapping::gaussian(double x)
{
return std::exp(-0.5 * x * x / cell_inflation_radius_);
}

} // namespace urc_perception

Expand Down

0 comments on commit ddc0379

Please sign in to comment.