passing_layer.cpp
Go to the documentation of this file.
00001 #include <social_navigation_layers/proxemic_layer.h>
00002 #include <pluginlib/class_list_macros.h>
00003 
00004 #include <angles/angles.h>
00005 
00006 namespace social_navigation_layers
00007 {
00008   class PassingLayer : public ProxemicLayer
00009   {
00010     public:
00011         PassingLayer() : ProxemicLayer() {
00012 
00013     }
00014     
00015     virtual void updateBounds(double origin_x, double origin_y, double origin_z, double* min_x, double* min_y, double* max_x, double* max_y){
00016         boost::recursive_mutex::scoped_lock lock(lock_);
00017         
00018         std::string global_frame = layered_costmap_->getGlobalFrameID();
00019         transformed_people_.clear();
00020         
00021         for(unsigned int i=0; i<people_list_.people.size(); i++){
00022             people_msgs::Person& person = people_list_.people[i];
00023             people_msgs::Person tpt;
00024             geometry_msgs::PointStamped pt, opt;
00025             
00026             try{
00027               pt.point.x = person.position.x;
00028               pt.point.y = person.position.y;
00029               pt.point.z = person.position.z;
00030               pt.header.frame_id = people_list_.header.frame_id;
00031               tf_.transformPoint(global_frame, pt, opt);
00032               tpt.position.x = opt.point.x;
00033               tpt.position.y = opt.point.y;
00034               tpt.position.z = opt.point.z;
00035 
00036               pt.point.x += person.velocity.x;
00037               pt.point.y += person.velocity.y;
00038               pt.point.z += person.velocity.z;
00039               tf_.transformPoint(global_frame, pt, opt);
00040               
00041               tpt.velocity.x = tpt.position.x - opt.point.x;
00042               tpt.velocity.y = tpt.position.y - opt.point.y;
00043               tpt.velocity.z = tpt.position.z - opt.point.z;
00044               
00045               transformed_people_.push_back(tpt);
00046               
00047               double mag = sqrt(pow(tpt.velocity.x,2) + pow(person.velocity.y, 2));
00048               double factor = 1.0 + mag * factor_;
00049               double point = get_radius(cutoff_, amplitude_, covar_ * factor );
00050               
00051               *min_x = std::min(*min_x, tpt.position.x - point);
00052               *min_y = std::min(*min_y, tpt.position.y - point);
00053               *max_x = std::max(*max_x, tpt.position.x + point);
00054               *max_y = std::max(*max_y, tpt.position.y + point);
00055               
00056             }
00057             catch(tf::LookupException& ex) {
00058               ROS_ERROR("No Transform available Error: %s\n", ex.what());
00059               continue;
00060             }
00061             catch(tf::ConnectivityException& ex) {
00062               ROS_ERROR("Connectivity Error: %s\n", ex.what());
00063               continue;
00064             }
00065             catch(tf::ExtrapolationException& ex) {
00066               ROS_ERROR("Extrapolation Error: %s\n", ex.what());
00067               continue;
00068             }
00069         }
00070     }
00071     
00072     virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j){
00073         boost::recursive_mutex::scoped_lock lock(lock_);
00074         if(!enabled_) return;
00075 
00076         if( people_list_.people.size() == 0 )
00077           return;
00078         if( cutoff_ >= amplitude_)
00079             return;
00080         
00081         std::list<people_msgs::Person>::iterator p_it;
00082         costmap_2d::Costmap2D* costmap = layered_costmap_->getCostmap();
00083         double res = costmap->getResolution();
00084         
00085         for(p_it = transformed_people_.begin(); p_it != transformed_people_.end(); ++p_it){
00086             people_msgs::Person person = *p_it;
00087             double angle = atan2(person.velocity.y, person.velocity.x)+1.51;
00088             double mag = sqrt(pow(person.velocity.x,2) + pow(person.velocity.y, 2));
00089             double factor = 1.0 + mag * factor_;
00090             double base = get_radius(cutoff_, amplitude_, covar_);
00091             double point = get_radius(cutoff_, amplitude_, covar_ * factor );
00092             
00093             unsigned int width = std::max(1, int( (base + point) / res )),
00094                           height = std::max(1, int( (base + point) / res ));
00095                           
00096             double cx = person.position.x, cy = person.position.y;
00097 
00098             double ox, oy;
00099             if(sin(angle)>0)
00100                 oy = cy - base;
00101             else
00102                 oy = cy + (point-base) * sin(angle) - base;
00103 
00104             if(cos(angle)>=0)
00105                 ox = cx - base;
00106             else
00107                 ox = cx + (point-base) * cos(angle) - base;
00108 
00109 
00110             int dx, dy;
00111             costmap->worldToMapNoBounds(ox, oy, dx, dy);
00112 
00113             int start_x = 0, start_y=0, end_x=width, end_y = height;
00114             if(dx < 0)
00115                 start_x = -dx;
00116             else if(dx + width > costmap->getSizeInCellsX())
00117                 end_x = std::max(0, (int)costmap->getSizeInCellsX() - dx);
00118 
00119             if((int)(start_x+dx) < min_i)
00120                 start_x = min_i - dx;
00121             if((int)(end_x+dx) > max_i)
00122                 end_x = max_i - dx;
00123 
00124             if(dy < 0)
00125                 start_y = -dy;
00126             else if(dy + height > costmap->getSizeInCellsY())
00127                 end_y = std::max(0, (int) costmap->getSizeInCellsY() - dy);
00128 
00129             if((int)(start_y+dy) < min_j)
00130                 start_y = min_j - dy;
00131             if((int)(end_y+dy) > max_j)
00132                 end_y = max_j - dy;
00133 
00134             double bx = ox + res / 2,
00135                    by = oy + res / 2;
00136             for(int i=start_x;i<end_x;i++){
00137                 for(int j=start_y;j<end_y;j++){
00138                   unsigned char old_cost = costmap->getCost(i+dx, j+dy);
00139                   if(old_cost == costmap_2d::NO_INFORMATION)
00140                     continue;
00141 
00142                   double x = bx+i*res, y = by+j*res;
00143                   double ma = atan2(y-cy,x-cx);
00144                   double diff = angles::shortest_angular_distance(angle, ma);
00145                   double a;
00146                   if(fabs(diff)<M_PI/2)
00147                       a = gaussian(x,y,cx,cy,amplitude_,covar_*factor,covar_,angle);
00148                   else
00149                     continue;
00150                   
00151 
00152                   if(a < cutoff_)
00153                     continue;
00154                   unsigned char cvalue = (unsigned char) a;
00155                   costmap->setCost(i+dx, j+dy, std::max(cvalue, old_cost));
00156 
00157               }
00158             } 
00159 
00160             
00161         }
00162     }
00163 
00164 
00165 
00166   };
00167 };
00168 
00169 
00170 
00171 
00172 
00173 
00174 
00175 PLUGINLIB_EXPORT_CLASS(social_navigation_layers::PassingLayer, costmap_2d::Layer)
00176 
00177 
00178     
00179 
00180 


social_navigation_layers
Author(s): David V. Lu!!
autogenerated on Thu Aug 27 2015 14:18:02