proxemic_layer.cpp
Go to the documentation of this file.
00001 #include <social_navigation_layers/proxemic_layer.h>
00002 #include <math.h>
00003 #include <angles/angles.h>
00004 #include <pluginlib/class_list_macros.h>
00005 PLUGINLIB_EXPORT_CLASS(social_navigation_layers::ProxemicLayer, costmap_2d::Layer)
00006 
00007 using costmap_2d::NO_INFORMATION;
00008 using costmap_2d::LETHAL_OBSTACLE;
00009 using costmap_2d::FREE_SPACE;
00010 
00011 double gaussian(double x, double y, double x0, double y0, double A, double varx, double vary, double skew){
00012     double dx = x-x0, dy = y-y0;
00013     double h = sqrt(dx*dx+dy*dy);
00014     double angle = atan2(dy,dx);
00015     double mx = cos(angle-skew) * h;
00016     double my = sin(angle-skew) * h;
00017     double f1 = pow(mx, 2.0)/(2.0 * varx), 
00018            f2 = pow(my, 2.0)/(2.0 * vary);
00019     return A * exp(-(f1 + f2));
00020 }
00021 
00022 double get_radius(double cutoff, double A, double var){
00023     return sqrt(-2*var * log(cutoff/A) );
00024 }
00025 
00026 
00027 namespace social_navigation_layers
00028 {
00029     void ProxemicLayer::onInitialize()
00030     {
00031         SocialLayer::onInitialize();
00032         ros::NodeHandle nh("~/" + name_), g_nh;
00033         server_ = new dynamic_reconfigure::Server<ProxemicLayerConfig>(nh);
00034         f_ = boost::bind(&ProxemicLayer::configure, this, _1, _2);
00035         server_->setCallback(f_);
00036     }
00037     
00038     void ProxemicLayer::updateBoundsFromPeople(double* min_x, double* min_y, double* max_x, double* max_y)
00039     {
00040         std::list<people_msgs::Person>::iterator p_it;
00041         
00042         for(p_it = transformed_people_.begin(); p_it != transformed_people_.end(); ++p_it){
00043             people_msgs::Person person = *p_it;
00044              
00045             double mag = sqrt(pow(person.velocity.x,2) + pow(person.velocity.y, 2));
00046             double factor = 1.0 + mag * factor_;
00047             double point = get_radius(cutoff_, amplitude_, covar_ * factor );
00048               
00049             *min_x = std::min(*min_x, person.position.x - point);
00050             *min_y = std::min(*min_y, person.position.y - point);
00051             *max_x = std::max(*max_x, person.position.x + point);
00052             *max_y = std::max(*max_y, person.position.y + point);
00053               
00054         }
00055     }
00056     
00057     void ProxemicLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j){
00058         boost::recursive_mutex::scoped_lock lock(lock_);
00059         if(!enabled_) return;
00060 
00061         if( people_list_.people.size() == 0 )
00062           return;
00063         if( cutoff_ >= amplitude_)
00064             return;
00065         
00066         std::list<people_msgs::Person>::iterator p_it;
00067         costmap_2d::Costmap2D* costmap = layered_costmap_->getCostmap();
00068         double res = costmap->getResolution();
00069         
00070         for(p_it = transformed_people_.begin(); p_it != transformed_people_.end(); ++p_it){
00071             people_msgs::Person person = *p_it;
00072             double angle = atan2(person.velocity.y, person.velocity.x);
00073             double mag = sqrt(pow(person.velocity.x,2) + pow(person.velocity.y, 2));
00074             double factor = 1.0 + mag * factor_;
00075             double base = get_radius(cutoff_, amplitude_, covar_);
00076             double point = get_radius(cutoff_, amplitude_, covar_ * factor );
00077             
00078             unsigned int width = std::max(1, int( (base + point) / res )),
00079                           height = std::max(1, int( (base + point) / res ));
00080                           
00081             double cx = person.position.x, cy = person.position.y;
00082 
00083             double ox, oy;
00084             if(sin(angle)>0)
00085                 oy = cy - base;
00086             else
00087                 oy = cy + (point-base) * sin(angle) - base;
00088 
00089             if(cos(angle)>=0)
00090                 ox = cx - base;
00091             else
00092                 ox = cx + (point-base) * cos(angle) - base;
00093 
00094 
00095             int dx, dy;
00096             costmap->worldToMapNoBounds(ox, oy, dx, dy);
00097 
00098             int start_x = 0, start_y=0, end_x=width, end_y = height;
00099             if(dx < 0)
00100                 start_x = -dx;
00101             else if(dx + width > costmap->getSizeInCellsX())
00102                 end_x = std::max(0, (int)costmap->getSizeInCellsX() - dx);
00103 
00104             if((int)(start_x+dx) < min_i)
00105                 start_x = min_i - dx;
00106             if((int)(end_x+dx) > max_i)
00107                 end_x = max_i - dx;
00108 
00109             if(dy < 0)
00110                 start_y = -dy;
00111             else if(dy + height > costmap->getSizeInCellsY())
00112                 end_y = std::max(0, (int) costmap->getSizeInCellsY() - dy);
00113 
00114             if((int)(start_y+dy) < min_j)
00115                 start_y = min_j - dy;
00116             if((int)(end_y+dy) > max_j)
00117                 end_y = max_j - dy;
00118 
00119             double bx = ox + res / 2,
00120                    by = oy + res / 2;
00121             for(int i=start_x;i<end_x;i++){
00122                 for(int j=start_y;j<end_y;j++){
00123                   unsigned char old_cost = costmap->getCost(i+dx, j+dy);
00124                   if(old_cost == costmap_2d::NO_INFORMATION)
00125                     continue;
00126 
00127                   double x = bx+i*res, y = by+j*res;
00128                   double ma = atan2(y-cy,x-cx);
00129                   double diff = angles::shortest_angular_distance(angle, ma);
00130                   double a;
00131                   if(fabs(diff)<M_PI/2)
00132                       a = gaussian(x,y,cx,cy,amplitude_,covar_*factor,covar_,angle);
00133                   else
00134                       a = gaussian(x,y,cx,cy,amplitude_,covar_,       covar_,0);
00135 
00136                   if(a < cutoff_)
00137                     continue;
00138                   unsigned char cvalue = (unsigned char) a;
00139                   costmap->setCost(i+dx, j+dy, std::max(cvalue, old_cost));
00140 
00141               }
00142             } 
00143 
00144             
00145         }
00146     }
00147 
00148     void ProxemicLayer::configure(ProxemicLayerConfig &config, uint32_t level) {
00149         cutoff_ = config.cutoff;
00150         amplitude_ = config.amplitude;
00151         covar_ = config.covariance;
00152         factor_ = config.factor;
00153         people_keep_time_ = ros::Duration(config.keep_time);
00154         enabled_ = config.enabled;
00155     }
00156 
00157 
00158 };


social_navigation_layers
Author(s): David V. Lu!!
autogenerated on Tue Dec 27 2016 03:52:47