static_layer.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2008, 2013, Willow Garage, Inc.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of Willow Garage, Inc. nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  * Author: Eitan Marder-Eppstein
00036  *         David V. Lu!!
00037  *********************************************************************/
00038 
00039 /*
00040  * Modified: added "layered_costmap_->updateMap(0,0,0);" below
00041  */
00042 
00043 #include "static_layer.h"
00044 #include <costmap_2d/costmap_math.h>
00045 #include <pluginlib/class_list_macros.h>
00046 
00047 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::StaticLayer, costmap_2d::Layer)
00048 
00049 using costmap_2d::NO_INFORMATION;
00050 using costmap_2d::LETHAL_OBSTACLE;
00051 using costmap_2d::FREE_SPACE;
00052 
00053 namespace rtabmap_ros
00054 {
00055 
00056 StaticLayer::StaticLayer() : dsrv_(NULL) {}
00057 
00058 StaticLayer::~StaticLayer()
00059 {
00060     if(dsrv_)
00061         delete dsrv_;
00062 }
00063 
00064 void StaticLayer::onInitialize()
00065 {
00066   ros::NodeHandle nh("~/" + name_), g_nh;
00067   current_ = true;
00068 
00069   global_frame_ = layered_costmap_->getGlobalFrameID();
00070 
00071   std::string map_topic;
00072   nh.param("map_topic", map_topic, std::string("map"));
00073   nh.param("subscribe_to_updates", subscribe_to_updates_, false);
00074   
00075   nh.param("track_unknown_space", track_unknown_space_, true);
00076   nh.param("use_maximum", use_maximum_, false);
00077 
00078   int temp_lethal_threshold, temp_unknown_cost_value;
00079   nh.param("lethal_cost_threshold", temp_lethal_threshold, int(100));
00080   nh.param("unknown_cost_value", temp_unknown_cost_value, int(-1));
00081   nh.param("trinary_costmap", trinary_costmap_, true);
00082 
00083   lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0);
00084   unknown_cost_value_ = temp_unknown_cost_value;
00085   //we'll subscribe to the latched topic that the map server uses
00086   ROS_INFO("Requesting the map...");
00087   map_sub_ = g_nh.subscribe(map_topic, 1, &StaticLayer::incomingMap, this);
00088   map_received_ = false;
00089   has_updated_data_ = false;
00090 
00091   ros::Rate r(10);
00092   while (!map_received_ && g_nh.ok())
00093   {
00094     ros::spinOnce();
00095     r.sleep();
00096   }
00097 
00098   ROS_INFO("Received a %d X %d map at %f m/pix", getSizeInCellsX(), getSizeInCellsY(), getResolution());
00099   
00100   if(subscribe_to_updates_)
00101   {
00102     ROS_INFO("Subscribing to updates");
00103     map_update_sub_ = g_nh.subscribe(map_topic + "_updates", 10, &StaticLayer::incomingUpdate, this);
00104   }
00105 
00106   if(dsrv_)
00107   {
00108     delete dsrv_;
00109   }
00110 
00111   dsrv_ = new dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>(nh);
00112   dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb = boost::bind(
00113       &StaticLayer::reconfigureCB, this, _1, _2);
00114   dsrv_->setCallback(cb);
00115 }
00116 
00117 void StaticLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
00118 {
00119   if (config.enabled != enabled_)
00120   {
00121     enabled_ = config.enabled;
00122     has_updated_data_ = true;
00123     x_ = y_ = 0;
00124     width_ = size_x_;
00125     height_ = size_y_;
00126   }
00127 }
00128 
00129 void StaticLayer::matchSize()
00130 {
00131   Costmap2D* master = layered_costmap_->getCostmap();
00132   resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
00133             master->getOriginX(), master->getOriginY());
00134 }
00135 
00136 unsigned char StaticLayer::interpretValue(unsigned char value)
00137 {
00138   //check if the static value is above the unknown or lethal thresholds
00139   if (track_unknown_space_ && value == unknown_cost_value_)
00140     return NO_INFORMATION;
00141   else if (value >= lethal_threshold_)
00142     return LETHAL_OBSTACLE;
00143   else if (trinary_costmap_)
00144     return FREE_SPACE;
00145 
00146   double scale = (double) value / lethal_threshold_;
00147   return scale * LETHAL_OBSTACLE;
00148 }
00149 
00150 void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map)
00151 {
00152   unsigned int size_x = new_map->info.width, size_y = new_map->info.height;
00153 
00154   ROS_DEBUG("Received a %d X %d map at %f m/pix", size_x, size_y, new_map->info.resolution);
00155 
00156   // resize costmap if size, resolution or origin do not match
00157   Costmap2D* master = layered_costmap_->getCostmap();
00158   if (master->getSizeInCellsX() != size_x ||
00159       master->getSizeInCellsY() != size_y ||
00160       master->getResolution() != new_map->info.resolution ||
00161       master->getOriginX() != new_map->info.origin.position.x ||
00162       master->getOriginY() != new_map->info.origin.position.y ||
00163       !layered_costmap_->isSizeLocked())
00164   {
00165     ROS_INFO("Resizing costmap to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
00166     layered_costmap_->resizeMap(size_x, size_y, new_map->info.resolution, new_map->info.origin.position.x,
00167                                 new_map->info.origin.position.y, true);
00168   }else if(size_x_ != size_x || size_y_ != size_y ||
00169       resolution_ != new_map->info.resolution ||
00170       origin_x_ != new_map->info.origin.position.x ||
00171       origin_y_ != new_map->info.origin.position.y){
00172     matchSize();
00173   }
00174 
00175   unsigned int index = 0;
00176 
00177   //initialize the costmap with static data
00178   for (unsigned int i = 0; i < size_y; ++i)
00179   {
00180     for (unsigned int j = 0; j < size_x; ++j)
00181     {
00182       unsigned char value = new_map->data[index];
00183       costmap_[index] = interpretValue(value);
00184       ++index;
00185     }
00186   }
00187   x_ = y_ = 0;
00188   width_ = size_x_;
00189   height_ = size_y_;
00190   map_received_ = true;
00191   has_updated_data_ = true;
00192 
00193   layered_costmap_->updateMap(0,0,0);
00194 }
00195 
00196 void StaticLayer::incomingUpdate(const map_msgs::OccupancyGridUpdateConstPtr& update)
00197 {
00198     unsigned int di = 0;
00199     for (unsigned int y = 0; y < update->height ; y++)
00200     {
00201         unsigned int index_base = (update->y + y) * size_x_;
00202         for (unsigned int x = 0; x < update->width ; x++)
00203         {
00204             unsigned int index = index_base + x + update->x;
00205             costmap_[index] = interpretValue( update->data[di++] );
00206         }
00207     }
00208     x_ = update->x;
00209     y_ = update->y;
00210     width_ = update->width;
00211     height_ = update->height;
00212     has_updated_data_ = true;
00213 
00214     layered_costmap_->updateMap(0,0,0);
00215 }
00216 
00217 void StaticLayer::activate()
00218 {
00219     onInitialize();
00220 }
00221 
00222 void StaticLayer::deactivate()
00223 {
00224     map_sub_.shutdown();
00225     if (subscribe_to_updates_)
00226         map_update_sub_.shutdown();
00227 }
00228 
00229 void StaticLayer::reset()
00230 {
00231     deactivate();
00232     activate();
00233 }
00234 
00235 void StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
00236                                double* max_x, double* max_y)
00237 {
00238   if (!map_received_ || !(has_updated_data_ || has_extra_bounds_))
00239     return;
00240     
00241   useExtraBounds(min_x, min_y, max_x, max_y);
00242 
00243   double mx, my;
00244   
00245   mapToWorld(x_, y_, mx, my);
00246   *min_x = std::min(mx, *min_x);
00247   *min_y = std::min(my, *min_y);
00248   
00249   mapToWorld(x_ + width_, y_ + height_, mx, my);
00250   *max_x = std::max(mx, *max_x);
00251   *max_y = std::max(my, *max_y);
00252   
00253   has_updated_data_ = false;
00254 }
00255 
00256 void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
00257 {
00258   if (!map_received_)
00259     return;
00260   if (!use_maximum_)
00261     updateWithTrueOverwrite(master_grid, min_i, min_j, max_i, max_j);
00262   else
00263     updateWithMax(master_grid, min_i, min_j, max_i, max_j);
00264 }
00265 
00266 }  // namespace rtabmap_ros


rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Sun Jul 24 2016 03:49:08