distApproxMode.h
Go to the documentation of this file.
1 /*
2  * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9 
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 
18 #ifndef DISTAPPROXMODE_H
19 #define DISTAPPROXMODE_H
20 
21 #include <mode.h>
22 #include <algorithm>
23 #include <limits>
24 
25 #include <std_msgs/ColorRGBA.h>
26 #include <sensor_msgs/LaserScan.h>
27 #include <cob_light/LightMode.h>
28 
29 #include <boost/thread.hpp>
30 #include <boost/algorithm/clamp.hpp>
31 
32 class DistApproxMode : public Mode
33 {
34 public:
35  DistApproxMode(size_t num_leds, int priority = 0, double freq = 5, int pulses = 0, double timeout = 0)
36  :Mode(priority, freq, pulses, timeout), _timer_inc(0), _num_leds(num_leds)
37  {
38  ros::NodeHandle nh;
39  sub_scan = nh.subscribe("/scan_unified", 1, &DistApproxMode::scan_callback, this);
40  //use static freq for this mode
41  _inc = (1. / UPDATE_RATE_HZ) * UPDATE_FREQ;
42 
43  _colors.resize(_num_leds);
44 
45  c_red.a = 1; c_red.r = 1; c_red.g = 0; c_red.b = 0;
46  c_green.a = 1; c_green.r = 0; c_green.g = 1; c_green.b = 0;
47  c_off.a = 0; c_off.r = 0; c_off.g = 0; c_off.b = 0;
48  c_default.a = 0.1; c_default.r = 0; c_default.g = 1.0; c_default.b = 0;
49  }
50 
51  void scan_callback(const sensor_msgs::LaserScanConstPtr& msg)
52  {
53  boost::mutex::scoped_lock lock(mutex);
54  scan = *msg;
55  }
56 
57  void execute()
58  {
59  if(_timer_inc >= 1.0)
60  {
61  //rotate scan array
62  mutex.lock();
63  std::vector<float> ranges = scan.ranges;
64  mutex.unlock();
65  float sector_size = (float)ranges.size() / (float)_num_leds;
66  std::vector<float> sectors;
67  sectors.assign(_num_leds, 0.0);
68 
69  //calculate sector values
70  for(int i = 0; i < _num_leds; i++)
71  {
72  float sum = std::numeric_limits<float>::max();
73  //float sum = 0;
74  for(int j = i*(int)sector_size; j < (i+1)*(int)sector_size; j++)
75  {
76  if(ranges.at(j) != 0){
77  sum = std::min(ranges.at(j), sum);
78  }
79  }
80  sectors.at(i) = sum;
81  }
82 
83  for(int i = 0; i < _num_leds; i++)
84  {
85  color::rgba col;
86 
87  float mean = 0;
88  if(i == 0)
89  {
90  mean = sectors.back()+sectors.at(i)/2.0f;
91  }
92  else
93  {
94  mean = (sectors.at(i)+sectors.at(i-1))/2.0f;
95  }
96  if(mean > DIST_MAX)
97  {
98  col = c_default;
99  }
100  else
101  {
102  float t = (boost::algorithm::clamp(mean, DIST_MIN, DIST_MAX) - DIST_MIN)/(DIST_MAX - DIST_MIN);
104  }
105 
106  _colors.at(i) = col;
107  }
108  _pulsed++;
110  _timer_inc = 0.0;
111  }
112  else
113  _timer_inc += _inc;
114  }
115 
116  std::string getName(){ return std::string("DistApproxMode"); }
117 
118  static constexpr float DIST_MIN = 0.3f;
119  static constexpr float DIST_MAX = 2.0f;
120  static constexpr double UPDATE_FREQ = 50.0;
121 
122 private:
123  double _timer_inc;
124  double _inc;
125  size_t _num_leds;
126 
127  sensor_msgs::LaserScan scan;
128 
130  boost::mutex mutex;
135 };
136 
137 constexpr float DistApproxMode::DIST_MIN;
138 constexpr float DistApproxMode::DIST_MAX;
139 constexpr double DistApproxMode::UPDATE_FREQ;
140 
141 #endif
static color::rgba interpolateColor(color::rgba start, color::rgba goal, float t)
Definition: colorUtils.h:104
std::string getName()
f
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
std::vector< color::rgba > _colors
Definition: mode.h:122
Definition: mode.h:26
int _pulsed
Definition: mode.h:119
static constexpr double UPDATE_FREQ
static const unsigned int UPDATE_RATE_HZ
Definition: mode.h:126
sensor_msgs::LaserScan scan
static constexpr float DIST_MIN
ros::Subscriber sub_scan
void scan_callback(const sensor_msgs::LaserScanConstPtr &msg)
DistApproxMode(size_t num_leds, int priority=0, double freq=5, int pulses=0, double timeout=0)
color::rgba c_red
color::rgba c_green
color::rgba c_off
boost::signals2::signal< void(std::vector< color::rgba > &colors)> m_sigColorsReady
Definition: mode.h:129
color::rgba c_default
static constexpr float DIST_MAX
boost::mutex mutex


cob_light
Author(s): Benjamin Maidel
autogenerated on Wed Apr 7 2021 02:11:39