Go to the documentation of this file.00001 #ifndef SOCIAL_LAYER_H_
00002 #define SOCIAL_LAYER_H_
00003 #include <ros/ros.h>
00004 #include <costmap_2d/layer.h>
00005 #include <costmap_2d/layered_costmap.h>
00006 #include <people_msgs/People.h>
00007 #include <boost/thread.hpp>
00008
00009 namespace social_navigation_layers
00010 {
00011 class SocialLayer : public costmap_2d::Layer
00012 {
00013 public:
00014 SocialLayer() { layered_costmap_ = NULL; }
00015
00016 virtual void onInitialize();
00017 virtual void updateBounds(double origin_x, double origin_y, double origin_yaw, double* min_x, double* min_y, double* max_x, double* max_y);
00018 virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) = 0;
00019
00020 virtual void updateBoundsFromPeople(double* min_x, double* min_y, double* max_x, double* max_y) = 0;
00021
00022 bool isDiscretized() { return false; }
00023
00024 protected:
00025 void peopleCallback(const people_msgs::People& people);
00026 ros::Subscriber people_sub_;
00027 people_msgs::People people_list_;
00028 std::list<people_msgs::Person> transformed_people_;
00029 ros::Duration people_keep_time_;
00030 boost::recursive_mutex lock_;
00031 tf::TransformListener tf_;
00032 bool first_time_;
00033 double last_min_x_, last_min_y_, last_max_x_, last_max_y_;
00034 };
00035 };
00036
00037
00038 #endif
00039