void VirtualWallLayer::updateWalls() {
resetMap(0, 0, getSizeInCellsX(), getSizeInCellsY());
for (std::map<std::string, WallInfo>::iterator it=walls.begin(); it!=walls.end(); ++it) {
drawWallLine((int) (*it).second.x1, (int) (*it).second.y1, (int) (*it).second.x2, (int) (*it).second.y2);
}
}
void StaticLayer::onInitialize()
{
ros::NodeHandle nh("~/" + name_), g_nh;
current_ = true;
global_frame_ = layered_costmap_->getGlobalFrameID();
std::string map_topic;
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);
nh.param("track_unknown_space", track_unknown_space_, true);
nh.param("use_maximum", use_maximum_, false);
int temp_lethal_threshold, temp_unknown_cost_value;
nh.param("lethal_cost_threshold", temp_lethal_threshold, int(100));
nh.param("unknown_cost_value", temp_unknown_cost_value, int(-1));
nh.param("trinary_costmap", trinary_costmap_, true);
lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0);
unknown_cost_value_ = temp_unknown_cost_value;
// we'll subscribe to the latched topic that the map server uses
ROS_INFO("Requesting the map...");
map_sub_ = g_nh.subscribe(map_topic, 1, &StaticLayer::incomingMap, this);
map_received_ = false;
has_updated_data_ = false;
ros::Rate r(10);
while (!map_received_ && g_nh.ok())
{
ros::spinOnce();
r.sleep();
}
ROS_INFO("Received a %d X %d map at %f m/pix", getSizeInCellsX(), getSizeInCellsY(), getResolution());
if (subscribe_to_updates_)
{
ROS_INFO("Subscribing to updates");
map_update_sub_ = g_nh.subscribe(map_topic + "_updates", 10, &StaticLayer::incomingUpdate, this);
}
if (dsrv_)
{
delete dsrv_;
}
dsrv_ = new dynamic_reconfigure::Server<layered_costmap_2d::GenericPluginConfig>(nh);
dynamic_reconfigure::Server<layered_costmap_2d::GenericPluginConfig>::CallbackType cb = boost::bind(
&StaticLayer::reconfigureCB, this, _1, _2);
dsrv_->setCallback(cb);
}
void BallPickerLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if (!enabled_)
return;
const unsigned char* master_array = master_grid.getCharMap();
for (int j = min_j; j < max_j; j++)
{
for (int i = min_i; i < max_i; i++)
{
if (clear_flag)
{
master_grid.setCost(i, j, FREE_SPACE);
}
else
{
int index = getIndex(i, j);
if (costmap_[index] == NO_INFORMATION)
continue;
unsigned char old_cost = master_array[index];
if (old_cost == NO_INFORMATION || old_cost < costmap_[index])
master_grid.setCost(i, j, costmap_[index]);
}
}
}
for (int i=0; i < getSizeInCellsX()*getSizeInCellsY(); i++)
costmap_[i] == NO_INFORMATION;
if (!obstacle_buffer.empty())
{
ROS_INFO("Costmap updated with ball obstacles.");
obstacle_buffer.clear();
std_srvs::Empty emptysrv;
confirm_update_client.call(emptysrv);
}
}
