Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add width and height params to static if mask #104

Merged
merged 2 commits into from
Dec 9, 2024
Merged

Add width and height params to static if mask #104

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
86e5d34
add mask and width height to static
renan028 Nov 8, 2024
6117373
fix test
renan028 Nov 14, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 12 additions & 4 deletions costmap_2d/include/costmap_2d/static_layer.h
View file
Open in desktop
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
Open in desktop
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
Loading