Go to the documentation of this file.00001 #ifndef RANGE_SENSOR_LAYER_H_
00002 #define RANGE_SENSOR_LAYER_H_
00003 #include <ros/ros.h>
00004 #include <costmap_2d/costmap_layer.h>
00005 #include <costmap_2d/layered_costmap.h>
00006 #include <sensor_msgs/Range.h>
00007 #include <range_sensor_layer/RangeSensorLayerConfig.h>
00008 #include <dynamic_reconfigure/server.h>
00009
00010 namespace range_sensor_layer
00011 {
00012
00013 class RangeSensorLayer : public costmap_2d::CostmapLayer
00014 {
00015 public:
00016 enum InputSensorType
00017 {
00018 VARIABLE,
00019 FIXED,
00020 ALL
00021 };
00022
00023 RangeSensorLayer();
00024
00025 virtual void onInitialize();
00026 virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x,
00027 double* max_y);
00028 virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
00029 virtual void reset();
00030 virtual void deactivate();
00031 virtual void activate();
00032
00033 private:
00034 void reconfigureCB(range_sensor_layer::RangeSensorLayerConfig &config, uint32_t level);
00035
00036 void bufferIncomingRangeMsg(const sensor_msgs::RangeConstPtr& range_message);
00037 void processRangeMsg(sensor_msgs::Range& range_message);
00038 void processFixedRangeMsg(sensor_msgs::Range& range_message);
00039 void processVariableRangeMsg(sensor_msgs::Range& range_message);
00040
00041 void updateCostmap();
00042 void updateCostmap(sensor_msgs::Range& range_message, bool clear_sensor_cone);
00043
00044 double gamma(double theta);
00045 double delta(double phi);
00046 double sensor_model(double r, double phi, double theta);
00047
00048 void get_deltas(double angle, double *dx, double *dy);
00049 void update_cell(double ox, double oy, double ot, double r, double nx, double ny, bool clear);
00050
00051 double to_prob(unsigned char c){ return double(c)/costmap_2d::LETHAL_OBSTACLE; }
00052 unsigned char to_cost(double p){ return (unsigned char)(p*costmap_2d::LETHAL_OBSTACLE); }
00053
00054 boost::function<void (sensor_msgs::Range& range_message)> processRangeMessageFunc_;
00055 boost::mutex range_message_mutex_;
00056 std::list<sensor_msgs::Range> range_msgs_buffer_;
00057
00058 double max_angle_, phi_v_;
00059 std::string global_frame_;
00060
00061 double clear_threshold_, mark_threshold_;
00062 bool clear_on_max_reading_;
00063
00064 double no_readings_timeout_;
00065 ros::Time last_reading_time_;
00066 unsigned int buffered_readings_;
00067 std::vector<ros::Subscriber> range_subs_;
00068 double min_x_, min_y_, max_x_, max_y_;
00069
00070 dynamic_reconfigure::Server<range_sensor_layer::RangeSensorLayerConfig> *dsrv_;
00071 };
00072 }
00073 #endif