social_layer.cpp
Go to the documentation of this file.
00001 #include <social_navigation_layers/social_layer.h>
00002 #include <math.h>
00003 #include <angles/angles.h>
00004 #include <pluginlib/class_list_macros.h>
00005 
00006 using costmap_2d::NO_INFORMATION;
00007 using costmap_2d::LETHAL_OBSTACLE;
00008 using costmap_2d::FREE_SPACE;
00009 
00010 namespace social_navigation_layers
00011 {
00012     void SocialLayer::onInitialize()
00013     {
00014         ros::NodeHandle nh("~/" + name_), g_nh;
00015         current_ = true;
00016         first_time_ = true;
00017         people_sub_ = nh.subscribe("/people", 1, &SocialLayer::peopleCallback, this);
00018     }
00019     
00020     void SocialLayer::peopleCallback(const people_msgs::People& people) {
00021         boost::recursive_mutex::scoped_lock lock(lock_);
00022         people_list_ = people;
00023     }
00024 
00025 
00026     void SocialLayer::updateBounds(double origin_x, double origin_y, double origin_z, double* min_x, double* min_y, double* max_x, double* max_y){
00027         boost::recursive_mutex::scoped_lock lock(lock_);
00028         
00029         std::string global_frame = layered_costmap_->getGlobalFrameID();
00030         transformed_people_.clear();
00031         
00032         for(unsigned int i=0; i<people_list_.people.size(); i++){
00033             people_msgs::Person& person = people_list_.people[i];
00034             people_msgs::Person tpt;
00035             geometry_msgs::PointStamped pt, opt;
00036             
00037             try{
00038               pt.point.x = person.position.x;
00039               pt.point.y = person.position.y;
00040               pt.point.z = person.position.z;
00041               pt.header.frame_id = people_list_.header.frame_id;
00042               tf_.transformPoint(global_frame, pt, opt);
00043               tpt.position.x = opt.point.x;
00044               tpt.position.y = opt.point.y;
00045               tpt.position.z = opt.point.z;
00046 
00047               pt.point.x += person.velocity.x;
00048               pt.point.y += person.velocity.y;
00049               pt.point.z += person.velocity.z;
00050               tf_.transformPoint(global_frame, pt, opt);
00051               
00052               tpt.velocity.x = opt.point.x - tpt.position.x;
00053               tpt.velocity.y = opt.point.y - tpt.position.y;
00054               tpt.velocity.z = opt.point.z - tpt.position.z;
00055               
00056               transformed_people_.push_back(tpt);
00057               
00058             }
00059             catch(tf::LookupException& ex) {
00060               ROS_ERROR("No Transform available Error: %s\n", ex.what());
00061               continue;
00062             }
00063             catch(tf::ConnectivityException& ex) {
00064               ROS_ERROR("Connectivity Error: %s\n", ex.what());
00065               continue;
00066             }
00067             catch(tf::ExtrapolationException& ex) {
00068               ROS_ERROR("Extrapolation Error: %s\n", ex.what());
00069               continue;
00070             }
00071         }
00072         updateBoundsFromPeople(min_x, min_y, max_x, max_y);
00073         if(first_time_){
00074             last_min_x_ = *min_x;
00075             last_min_y_ = *min_y;    
00076             last_max_x_ = *max_x;
00077             last_max_y_ = *max_y;    
00078             first_time_ = false;
00079         }else{
00080             double a = *min_x, b = *min_y, c = *max_x, d = *max_y;
00081             *min_x = std::min(last_min_x_, *min_x);
00082             *min_y = std::min(last_min_y_, *min_y);
00083             *max_x = std::max(last_max_x_, *max_x);
00084             *max_y = std::max(last_max_y_, *max_y);
00085             last_min_x_ = a;
00086             last_min_y_ = b;
00087             last_max_x_ = c;
00088             last_max_y_ = d;
00089         
00090         }
00091         
00092     }
00093 };


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