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); } }