range_sensor_layer.h
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


range_sensor_layer
Author(s): David!!
autogenerated on Tue Dec 27 2016 03:52:43