range_sensor_layer.h
Go to the documentation of this file.
1 // Copyright 2018 David V. Lu!!
2 #ifndef RANGE_SENSOR_LAYER_RANGE_SENSOR_LAYER_H_
3 #define RANGE_SENSOR_LAYER_RANGE_SENSOR_LAYER_H_
4 #include <ros/ros.h>
7 #include <sensor_msgs/Range.h>
8 #include <range_sensor_layer/RangeSensorLayerConfig.h>
9 #include <dynamic_reconfigure/server.h>
10 #include <list>
11 #include <string>
12 #include <unordered_map>
13 #include <vector>
14 
16 {
17 
19 {
20 public:
22  {
26  };
27 
29 
30  virtual void onInitialize();
31  virtual void updateBounds(double robot_x, double robot_y, double robot_yaw,
32  double* min_x, double* min_y, double* max_x, double* max_y);
33  virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
34  virtual void reset();
35  virtual void deactivate();
36  virtual void activate();
37 
38 private:
39  void reconfigureCB(range_sensor_layer::RangeSensorLayerConfig &config, uint32_t level);
40 
41  void bufferIncomingRangeMsg(const sensor_msgs::RangeConstPtr& range_message, const std::string& topic);
42  void processRangeMsg(sensor_msgs::Range& range_message);
43  void processFixedRangeMsg(sensor_msgs::Range& range_message);
44  void processVariableRangeMsg(sensor_msgs::Range& range_message);
45 
46  void resetRange();
47  void updateCostmap();
48  void updateCostmap(sensor_msgs::Range& range_message, bool clear_sensor_cone);
49 
50  double gamma(double theta);
51  double delta(double phi);
52  double sensor_model(double r, double phi, double theta);
53 
54  void get_deltas(double angle, double *dx, double *dy);
55  void update_cell(double ox, double oy, double ot, double r, double nx, double ny, bool clear);
56 
57  double to_prob(unsigned char c)
58  {
59  return static_cast<double>(c) / costmap_2d::LETHAL_OBSTACLE;
60  }
61  unsigned char to_cost(double p)
62  {
63  return static_cast<unsigned char>(p * costmap_2d::LETHAL_OBSTACLE);
64  }
65 
66  boost::function<void(sensor_msgs::Range& range_message)> processRangeMessageFunc_;
67  boost::mutex range_message_mutex_;
68  size_t ranges_buffer_size_ { 0 };
69  std::unordered_map<std::string, std::list<sensor_msgs::Range>> range_msgs_buffers_;
70 
71  double max_angle_, phi_v_;
72  double inflate_cone_;
73  std::string global_frame_;
74 
77 
80  unsigned int buffered_readings_;
81  std::vector<ros::Subscriber> range_subs_;
83 
84  dynamic_reconfigure::Server<range_sensor_layer::RangeSensorLayerConfig> *dsrv_;
85 
86 
87  float area(int x1, int y1, int x2, int y2, int x3, int y3)
88  {
89  return fabs((x1 * (y2 - y3) + x2 * (y3 - y1) + x3 * (y1 - y2)) / 2.0);
90  };
91 
92  int orient2d(int Ax, int Ay, int Bx, int By, int Cx, int Cy)
93  {
94  return (Bx - Ax) * (Cy - Ay) - (By - Ay) * (Cx - Ax);
95  };
96 };
97 } // namespace range_sensor_layer
98 #endif // RANGE_SENSOR_LAYER_RANGE_SENSOR_LAYER_H
void get_deltas(double angle, double *dx, double *dy)
void processFixedRangeMsg(sensor_msgs::Range &range_message)
void processRangeMsg(sensor_msgs::Range &range_message)
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double *min_x, double *min_y, double *max_x, double *max_y)
void reconfigureCB(range_sensor_layer::RangeSensorLayerConfig &config, uint32_t level)
void processVariableRangeMsg(sensor_msgs::Range &range_message)
std::unordered_map< std::string, std::list< sensor_msgs::Range > > range_msgs_buffers_
void bufferIncomingRangeMsg(const sensor_msgs::RangeConstPtr &range_message, const std::string &topic)
std::vector< ros::Subscriber > range_subs_
boost::function< void(sensor_msgs::Range &range_message)> processRangeMessageFunc_
double sensor_model(double r, double phi, double theta)
dynamic_reconfigure::Server< range_sensor_layer::RangeSensorLayerConfig > * dsrv_
void update_cell(double ox, double oy, double ot, double r, double nx, double ny, bool clear)
int orient2d(int Ax, int Ay, int Bx, int By, int Cx, int Cy)
static const unsigned char LETHAL_OBSTACLE
float area(int x1, int y1, int x2, int y2, int x3, int y3)
virtual void updateCosts(costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)


range_sensor_layer
Author(s): David!!
autogenerated on Thu Mar 4 2021 04:02:55