Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00037 #ifndef COSTMAP_LAYER_H
00038 #define COSTMAP_LAYER_H
00039
00040 #include <ros/ros.h>
00041 #include <costmap_2d/layered_costmap.h>
00042 #include <costmap_2d/footprint.h>
00043 #include <costmap_2d/layer.h>
00044 #include <pluginlib/class_list_macros.h>
00045
00046 #include <depth_nav_msgs/Point32List.h>
00047 #include <geometry_msgs/PointStamped.h>
00048
00049 #include <boost/thread.hpp>
00050
00051 #include <math.h>
00052 #include <algorithm>
00053
00054 #include <angles/angles.h>
00055
00056 #include <dynamic_reconfigure/server.h>
00057 #include <nav_layer_from_points/NavLayerFromPointsConfig.h>
00058
00059
00060 namespace nav_layer_from_points
00061 {
00062 class NavLayerFromPoints : public costmap_2d::Layer
00063 {
00064 public:
00065 NavLayerFromPoints() { layered_costmap_ = NULL; }
00066
00067 virtual void onInitialize();
00068
00069 virtual void updateBounds( double origin_x, double origin_y, double origin_yaw,
00070 double* min_x, double* min_y, double* max_x, double* max_y);
00071
00072 virtual void updateCosts( costmap_2d::Costmap2D& master_grid,
00073 int min_i, int min_j, int max_i, int max_j);
00074
00075 void updateBoundsFromPoints( double* min_x, double* min_y,
00076 double* max_x, double* max_y);
00077
00078 bool isDiscretized() { return false; }
00079
00080 protected:
00081
00082 void pointsCallback(const depth_nav_msgs::Point32List& points);
00083
00087 void clearTransformedPoints();
00088
00089 void configure(NavLayerFromPointsConfig &config, uint32_t level);
00090
00091 protected:
00092 ros::Subscriber sub_points_;
00093 depth_nav_msgs::Point32List points_list_;
00094
00095 tf::TransformListener tf_;
00096
00097 std::list<geometry_msgs::PointStamped> transformed_points_;
00098
00099
00100 ros::Duration points_keep_time_;
00101
00102 boost::recursive_mutex lock_;
00103 bool first_time_;
00104 double last_min_x_, last_min_y_, last_max_x_, last_max_y_;
00105
00106 private:
00107
00108
00109 double point_radius_;
00110 double robot_radius_;
00111
00112
00113
00114 dynamic_reconfigure::Server<NavLayerFromPointsConfig>* rec_server_;
00115 dynamic_reconfigure::Server<NavLayerFromPointsConfig>::CallbackType f_;
00116
00117 };
00118 }
00119
00120
00121 #endif
00122