Skip to content

Commit

Permalink
Add width and height params to static if mask (#104)
Browse files Browse the repository at this point in the history
  • Loading branch information
@renan028
renan028 authored Dec 9, 2024
1 parent 39ea032 commit ea69c68
Show file tree
Hide file tree
Showing 2 changed files with 76 additions and 31 deletions.
16 changes: 12 additions & 4 deletions costmap_2d/include/costmap_2d/static_layer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,13 @@ enum class CombinationMethod
MASK = 2
};

struct MapInfo
{
unsigned int width, height;
double origin_x, origin_y;
double resolution;
};

class StaticLayer : public CostmapLayer
{
public:
Expand All @@ -83,25 +90,26 @@ class StaticLayer : public CostmapLayer
*/
void incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map);
void incomingUpdate(const map_msgs::OccupancyGridUpdateConstPtr& update);
void reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level);
void reconfigureCB(costmap_2d::GenericPluginConfig& config, uint32_t level);

unsigned char interpretValue(unsigned char value);

std::string global_frame_; ///< @brief The global frame for the costmap
std::string map_frame_; /// @brief frame that map is located in
std::string map_frame_; /// @brief frame that map is located in
bool subscribe_to_updates_;
bool map_received_;
bool has_updated_data_;
unsigned int x_, y_, width_, height_;
bool track_unknown_space_;
bool first_map_only_; ///< @brief Store the first static map and reuse it on reinitializing
bool first_map_only_; ///< @brief Store the first static map and reuse it on reinitializing
bool trinary_costmap_;
ros::Subscriber map_sub_, map_update_sub_;

unsigned char lethal_threshold_, unknown_cost_value_;
CombinationMethod combination_method_;

dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig> *dsrv_;
dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>* dsrv_;
MapInfo* param_map_info_;
};

} // namespace costmap_2d
Expand Down
91 changes: 64 additions & 27 deletions costmap_2d/plugins/static_layer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,24 +44,28 @@

PLUGINLIB_EXPORT_CLASS(costmap_2d::StaticLayer, costmap_2d::Layer)

using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::FREE_SPACE;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::NO_INFORMATION;

namespace costmap_2d
{

StaticLayer::StaticLayer() : dsrv_(NULL) {}
StaticLayer::StaticLayer() : dsrv_(NULL), param_map_info_(NULL)
{
}

StaticLayer::~StaticLayer()
{
if (dsrv_)
delete dsrv_;
delete param_map_info_;
}

void StaticLayer::onInitialize()
{
ros::NodeHandle nh("~/" + name_), g_nh;
ros::NodeHandle pnh(ros::names::parentNamespace(nh.getNamespace()));
current_ = true;

global_frame_ = layered_costmap_->getGlobalFrameID();
Expand All @@ -82,6 +86,18 @@ void StaticLayer::onInitialize()
nh.param("combination_method", combination_method_int, 0);
combination_method_ = static_cast<CombinationMethod>(combination_method_int);

int width, height;
double origin_x, origin_y;
if (pnh.getParam("width", width) && pnh.getParam("height", height) && pnh.getParam("origin_x", origin_x) &&
pnh.getParam("origin_y", origin_y))
{
param_map_info_ = new MapInfo();
param_map_info_->width = width;
param_map_info_->height = height;
param_map_info_->origin_x = origin_x;
param_map_info_->origin_y = origin_y;
}

lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0);
unknown_cost_value_ = temp_unknown_cost_value;

Expand Down Expand Up @@ -144,8 +160,8 @@ void StaticLayer::matchSize()
if (!layered_costmap_->isRolling())
{
Costmap2D* master = layered_costmap_->getCostmap();
resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
master->getOriginX(), master->getOriginY());
resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(), master->getOriginX(),
master->getOriginY());
}
}

Expand All @@ -161,50 +177,71 @@ unsigned char StaticLayer::interpretValue(unsigned char value)
else if (trinary_costmap_)
return FREE_SPACE;

double scale = (double) value / lethal_threshold_;
double scale = (double)value / lethal_threshold_;
return scale * LETHAL_OBSTACLE;
}

void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map)
{
unsigned int size_x = new_map->info.width, size_y = new_map->info.height;
double origin_x = new_map->info.origin.position.x, origin_y = new_map->info.origin.position.y;
double resolution = new_map->info.resolution;

if (param_map_info_ && combination_method_ == CombinationMethod::MASK)
{
const double end_x = std::min(std::max(param_map_info_->origin_x + param_map_info_->width, origin_x),
origin_x + size_x * resolution);
const double end_y = std::min(std::max(param_map_info_->origin_y + param_map_info_->height, origin_y),
origin_y + size_y * resolution);
origin_x = std::min(std::max(param_map_info_->origin_x, origin_x), origin_x + size_x * resolution);
origin_y = std::min(std::max(param_map_info_->origin_y, origin_y), origin_y + size_y * resolution);
size_x = std::max(0u, static_cast<unsigned>((end_x - origin_x) / resolution));
size_y = std::max(0u, static_cast<unsigned>((end_y - origin_y) / resolution));
ROS_INFO_NAMED("static_layer", "new origin_x: %f, origin_y: %f, size_x: %d, size_y: %d", origin_x, origin_y, size_x,
size_y);
}

ROS_DEBUG("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 (!layered_costmap_->isRolling() && (master->getSizeInCellsX() != size_x || master->getSizeInCellsY() != size_y ||
master->getResolution() != new_map->info.resolution ||
master->getOriginX() != origin_x || master->getOriginY() != origin_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, 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,
layered_costmap_->resizeMap(size_x, size_y, new_map->info.resolution, origin_x, origin_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)
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
ROS_INFO("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);
resizeMap(size_x, size_y, new_map->info.resolution, origin_x, origin_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];
unsigned int map_index = index;
if (param_map_info_ && combination_method_ == CombinationMethod::MASK)
{
// need to compute the index in the map data array
int mx = (int)((origin_x + j * resolution - new_map->info.origin.position.x) / resolution);
int my = (int)((origin_y + i * resolution - new_map->info.origin.position.y) / resolution);

if (mx > new_map->info.width || my > new_map->info.height)
{
continue;
}
map_index = my * new_map->info.width + mx;
}
unsigned char value = new_map->data[map_index];
costmap_[index] = interpretValue(value);
++index;
}
Expand All @@ -218,7 +255,7 @@ void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map)
map_received_ = true;
has_updated_data_ = true;

// shutdown the map subscrber if firt_map_only_ flag is on
// shutdown the map subscrber if first_map_only_ flag is on
if (first_map_only_)
{
ROS_INFO("Shutting down the map subscriber. first_map_only flag is on");
Expand All @@ -229,10 +266,10 @@ void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map)
void StaticLayer::incomingUpdate(const map_msgs::OccupancyGridUpdateConstPtr& update)
{
unsigned int di = 0;
for (unsigned int y = 0; y < update->height ; y++)
for (unsigned int y = 0; y < update->height; y++)
{
unsigned int index_base = (update->y + y) * size_x_;
for (unsigned int x = 0; x < update->width ; x++)
for (unsigned int x = 0; x < update->width; x++)
{
unsigned int index = index_base + x + update->x;
costmap_[index] = interpretValue(update->data[di++]);
Expand Down Expand Up @@ -272,8 +309,8 @@ void StaticLayer::reset()
void StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
double* max_x, double* max_y)
{

if( !layered_costmap_->isRolling() ){
if (!layered_costmap_->isRolling())
{
if (!map_received_ || !(has_updated_data_ || has_extra_bounds_))
return;
}
Expand Down

0 comments on commit ea69c68

Please sign in to comment.