00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 #include <costmap_2d/static_layer.h>
00040 #include <costmap_2d/costmap_math.h>
00041 #include <pluginlib/class_list_macros.h>
00042
00043 PLUGINLIB_EXPORT_CLASS(costmap_2d::StaticLayer, costmap_2d::Layer)
00044
00045 using costmap_2d::NO_INFORMATION;
00046 using costmap_2d::LETHAL_OBSTACLE;
00047 using costmap_2d::FREE_SPACE;
00048
00049 namespace costmap_2d
00050 {
00051
00052 StaticLayer::StaticLayer() : dsrv_(NULL) {}
00053
00054 StaticLayer::~StaticLayer()
00055 {
00056 if (dsrv_)
00057 delete dsrv_;
00058 }
00059
00060 void StaticLayer::onInitialize()
00061 {
00062 ros::NodeHandle nh("~/" + name_), g_nh;
00063 current_ = true;
00064
00065 global_frame_ = layered_costmap_->getGlobalFrameID();
00066
00067 std::string map_topic;
00068 nh.param("map_topic", map_topic, std::string("map"));
00069 nh.param("first_map_only", first_map_only_, false);
00070 nh.param("subscribe_to_updates", subscribe_to_updates_, false);
00071
00072 nh.param("track_unknown_space", track_unknown_space_, true);
00073 nh.param("use_maximum", use_maximum_, false);
00074
00075 int temp_lethal_threshold, temp_unknown_cost_value;
00076 nh.param("lethal_cost_threshold", temp_lethal_threshold, int(100));
00077 nh.param("unknown_cost_value", temp_unknown_cost_value, int(-1));
00078 nh.param("trinary_costmap", trinary_costmap_, true);
00079
00080 lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0);
00081 unknown_cost_value_ = temp_unknown_cost_value;
00082
00083
00084 if (map_sub_.getTopic() != ros::names::resolve(map_topic))
00085 {
00086
00087 ROS_INFO("Requesting the map...");
00088 map_sub_ = g_nh.subscribe(map_topic, 1, &StaticLayer::incomingMap, this);
00089 map_received_ = false;
00090 has_updated_data_ = false;
00091
00092 ros::Rate r(10);
00093 while (!map_received_ && g_nh.ok())
00094 {
00095 ros::spinOnce();
00096 r.sleep();
00097 }
00098
00099 ROS_INFO("Received a %d X %d map at %f m/pix", getSizeInCellsX(), getSizeInCellsY(), getResolution());
00100
00101 if (subscribe_to_updates_)
00102 {
00103 ROS_INFO("Subscribing to updates");
00104 map_update_sub_ = g_nh.subscribe(map_topic + "_updates", 10, &StaticLayer::incomingUpdate, this);
00105
00106 }
00107 }
00108 else
00109 {
00110 has_updated_data_ = true;
00111 }
00112
00113 if (dsrv_)
00114 {
00115 delete dsrv_;
00116 }
00117
00118 dsrv_ = new dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>(nh);
00119 dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb = boost::bind(
00120 &StaticLayer::reconfigureCB, this, _1, _2);
00121 dsrv_->setCallback(cb);
00122 }
00123
00124 void StaticLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
00125 {
00126 if (config.enabled != enabled_)
00127 {
00128 enabled_ = config.enabled;
00129 has_updated_data_ = true;
00130 x_ = y_ = 0;
00131 width_ = size_x_;
00132 height_ = size_y_;
00133 }
00134 }
00135
00136 void StaticLayer::matchSize()
00137 {
00138
00139
00140 if (!layered_costmap_->isRolling())
00141 {
00142 Costmap2D* master = layered_costmap_->getCostmap();
00143 resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
00144 master->getOriginX(), master->getOriginY());
00145 }
00146 }
00147
00148 unsigned char StaticLayer::interpretValue(unsigned char value)
00149 {
00150
00151 if (track_unknown_space_ && value == unknown_cost_value_)
00152 return NO_INFORMATION;
00153 else if (!track_unknown_space_ && value == unknown_cost_value_)
00154 return FREE_SPACE;
00155 else if (value >= lethal_threshold_)
00156 return LETHAL_OBSTACLE;
00157 else if (trinary_costmap_)
00158 return FREE_SPACE;
00159
00160 double scale = (double) value / lethal_threshold_;
00161 return scale * LETHAL_OBSTACLE;
00162 }
00163
00164 void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map)
00165 {
00166 unsigned int size_x = new_map->info.width, size_y = new_map->info.height;
00167
00168 ROS_DEBUG("Received a %d X %d map at %f m/pix", size_x, size_y, new_map->info.resolution);
00169
00170
00171 Costmap2D* master = layered_costmap_->getCostmap();
00172 if (!layered_costmap_->isRolling() && (master->getSizeInCellsX() != size_x ||
00173 master->getSizeInCellsY() != size_y ||
00174 master->getResolution() != new_map->info.resolution ||
00175 master->getOriginX() != new_map->info.origin.position.x ||
00176 master->getOriginY() != new_map->info.origin.position.y ||
00177 !layered_costmap_->isSizeLocked()))
00178 {
00179
00180 ROS_INFO("Resizing costmap to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
00181 layered_costmap_->resizeMap(size_x, size_y, new_map->info.resolution, new_map->info.origin.position.x,
00182 new_map->info.origin.position.y, true);
00183 }
00184 else if (size_x_ != size_x || size_y_ != size_y ||
00185 resolution_ != new_map->info.resolution ||
00186 origin_x_ != new_map->info.origin.position.x ||
00187 origin_y_ != new_map->info.origin.position.y)
00188 {
00189
00190 ROS_INFO("Resizing static layer to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
00191 resizeMap(size_x, size_y, new_map->info.resolution,
00192 new_map->info.origin.position.x, new_map->info.origin.position.y);
00193 }
00194
00195 unsigned int index = 0;
00196
00197
00198 for (unsigned int i = 0; i < size_y; ++i)
00199 {
00200 for (unsigned int j = 0; j < size_x; ++j)
00201 {
00202 unsigned char value = new_map->data[index];
00203 costmap_[index] = interpretValue(value);
00204 ++index;
00205 }
00206 }
00207 map_frame_ = new_map->header.frame_id;
00208
00209
00210 x_ = y_ = 0;
00211 width_ = size_x_;
00212 height_ = size_y_;
00213 map_received_ = true;
00214 has_updated_data_ = true;
00215
00216
00217 if (first_map_only_)
00218 {
00219 ROS_INFO("Shutting down the map subscriber. first_map_only flag is on");
00220 map_sub_.shutdown();
00221 }
00222 }
00223
00224 void StaticLayer::incomingUpdate(const map_msgs::OccupancyGridUpdateConstPtr& update)
00225 {
00226 unsigned int di = 0;
00227 for (unsigned int y = 0; y < update->height ; y++)
00228 {
00229 unsigned int index_base = (update->y + y) * size_x_;
00230 for (unsigned int x = 0; x < update->width ; x++)
00231 {
00232 unsigned int index = index_base + x + update->x;
00233 costmap_[index] = interpretValue(update->data[di++]);
00234 }
00235 }
00236 x_ = update->x;
00237 y_ = update->y;
00238 width_ = update->width;
00239 height_ = update->height;
00240 has_updated_data_ = true;
00241 }
00242
00243 void StaticLayer::activate()
00244 {
00245 onInitialize();
00246 }
00247
00248 void StaticLayer::deactivate()
00249 {
00250 map_sub_.shutdown();
00251 if (subscribe_to_updates_)
00252 map_update_sub_.shutdown();
00253 }
00254
00255 void StaticLayer::reset()
00256 {
00257 if (first_map_only_)
00258 {
00259 has_updated_data_ = true;
00260 }
00261 else
00262 {
00263 onInitialize();
00264 }
00265 }
00266
00267 void StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
00268 double* max_x, double* max_y)
00269 {
00270
00271 if( !layered_costmap_->isRolling() ){
00272 if (!map_received_ || !(has_updated_data_ || has_extra_bounds_))
00273 return;
00274 }
00275
00276 useExtraBounds(min_x, min_y, max_x, max_y);
00277
00278 double wx, wy;
00279
00280 mapToWorld(x_, y_, wx, wy);
00281 *min_x = std::min(wx, *min_x);
00282 *min_y = std::min(wy, *min_y);
00283
00284 mapToWorld(x_ + width_, y_ + height_, wx, wy);
00285 *max_x = std::max(wx, *max_x);
00286 *max_y = std::max(wy, *max_y);
00287
00288 has_updated_data_ = false;
00289 }
00290
00291 void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
00292 {
00293 if (!map_received_)
00294 return;
00295
00296 if (!enabled_)
00297 return;
00298
00299 if (!layered_costmap_->isRolling())
00300 {
00301
00302 if (!use_maximum_)
00303 updateWithTrueOverwrite(master_grid, min_i, min_j, max_i, max_j);
00304 else
00305 updateWithMax(master_grid, min_i, min_j, max_i, max_j);
00306 }
00307 else
00308 {
00309
00310 unsigned int mx, my;
00311 double wx, wy;
00312
00313 tf::StampedTransform transform;
00314 try
00315 {
00316 tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0), transform);
00317 }
00318 catch (tf::TransformException ex)
00319 {
00320 ROS_ERROR("%s", ex.what());
00321 return;
00322 }
00323
00324 for (unsigned int i = min_i; i < max_i; ++i)
00325 {
00326 for (unsigned int j = min_j; j < max_j; ++j)
00327 {
00328
00329 layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);
00330
00331 tf::Point p(wx, wy, 0);
00332 p = transform(p);
00333
00334 if (worldToMap(p.x(), p.y(), mx, my))
00335 {
00336 if (!use_maximum_)
00337 master_grid.setCost(i, j, getCost(mx, my));
00338 else
00339 master_grid.setCost(i, j, std::max(getCost(mx, my), master_grid.getCost(i, j)));
00340 }
00341 }
00342 }
00343 }
00344 }
00345
00346 }