laser.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2015 Aldebaran
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 /*
19 * LOCAL includes
20 */
21 #include "laser.hpp"
22 #include "../tools/from_any_value.hpp"
23 
24 /*
25 * BOOST includes
26 */
27 #include <boost/foreach.hpp>
28 #define for_each BOOST_FOREACH
29 
30 namespace naoqi
31 {
32 namespace converter
33 {
34 
35 static const char* laserMemoryKeys[] = {
36  // RIGHT LASER
37  "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg01/X/Sensor/Value",
38  "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg01/Y/Sensor/Value",
39  "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg02/X/Sensor/Value",
40  "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg02/Y/Sensor/Value",
41  "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg03/X/Sensor/Value",
42  "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg03/Y/Sensor/Value",
43  "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg04/X/Sensor/Value",
44  "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg04/Y/Sensor/Value",
45  "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg05/X/Sensor/Value",
46  "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg05/Y/Sensor/Value",
47  "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg06/X/Sensor/Value",
48  "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg06/Y/Sensor/Value",
49  "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg07/X/Sensor/Value",
50  "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg07/Y/Sensor/Value",
51  "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg08/X/Sensor/Value",
52  "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg08/Y/Sensor/Value",
53  "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg09/X/Sensor/Value",
54  "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg09/Y/Sensor/Value",
55  "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg10/X/Sensor/Value",
56  "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg10/Y/Sensor/Value",
57  "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg11/X/Sensor/Value",
58  "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg11/Y/Sensor/Value",
59  "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg12/X/Sensor/Value",
60  "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg12/Y/Sensor/Value",
61  "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg13/X/Sensor/Value",
62  "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg13/Y/Sensor/Value",
63  "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg14/X/Sensor/Value",
64  "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg14/Y/Sensor/Value",
65  "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg15/X/Sensor/Value",
66  "Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg15/Y/Sensor/Value",
67  // FRONT LASER
68  "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg01/X/Sensor/Value",
69  "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg01/Y/Sensor/Value",
70  "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg02/X/Sensor/Value",
71  "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg02/Y/Sensor/Value",
72  "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg03/X/Sensor/Value",
73  "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg03/Y/Sensor/Value",
74  "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg04/X/Sensor/Value",
75  "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg04/Y/Sensor/Value",
76  "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg05/X/Sensor/Value",
77  "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg05/Y/Sensor/Value",
78  "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg06/X/Sensor/Value",
79  "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg06/Y/Sensor/Value",
80  "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg07/X/Sensor/Value",
81  "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg07/Y/Sensor/Value",
82  "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg08/X/Sensor/Value",
83  "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg08/Y/Sensor/Value",
84  "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg09/X/Sensor/Value",
85  "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg09/Y/Sensor/Value",
86  "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg10/X/Sensor/Value",
87  "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg10/Y/Sensor/Value",
88  "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg11/X/Sensor/Value",
89  "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg11/Y/Sensor/Value",
90  "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg12/X/Sensor/Value",
91  "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg12/Y/Sensor/Value",
92  "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg13/X/Sensor/Value",
93  "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg13/Y/Sensor/Value",
94  "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg14/X/Sensor/Value",
95  "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg14/Y/Sensor/Value",
96  "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg15/X/Sensor/Value",
97  "Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg15/Y/Sensor/Value",
98  // LEFT LASER
99  "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg01/X/Sensor/Value",
100  "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg01/Y/Sensor/Value",
101  "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg02/X/Sensor/Value",
102  "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg02/Y/Sensor/Value",
103  "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg03/X/Sensor/Value",
104  "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg03/Y/Sensor/Value",
105  "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg04/X/Sensor/Value",
106  "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg04/Y/Sensor/Value",
107  "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg05/X/Sensor/Value",
108  "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg05/Y/Sensor/Value",
109  "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg06/X/Sensor/Value",
110  "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg06/Y/Sensor/Value",
111  "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg07/X/Sensor/Value",
112  "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg07/Y/Sensor/Value",
113  "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg08/X/Sensor/Value",
114  "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg08/Y/Sensor/Value",
115  "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg09/X/Sensor/Value",
116  "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg09/Y/Sensor/Value",
117  "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg10/X/Sensor/Value",
118  "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg10/Y/Sensor/Value",
119  "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg11/X/Sensor/Value",
120  "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg11/Y/Sensor/Value",
121  "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg12/X/Sensor/Value",
122  "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg12/Y/Sensor/Value",
123  "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg13/X/Sensor/Value",
124  "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg13/Y/Sensor/Value",
125  "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg14/X/Sensor/Value",
126  "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg14/Y/Sensor/Value",
127  "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg15/X/Sensor/Value",
128  "Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg15/Y/Sensor/Value",
129 };
130 
131 LaserConverter::LaserConverter( const std::string& name, const float& frequency, const qi::SessionPtr& session ):
132  BaseConverter( name, frequency, session ),
133  p_memory_(session->service("ALMemory"))
134 {
135 }
136 
137 void LaserConverter::registerCallback( message_actions::MessageAction action, Callback_t cb )
138 {
139  callbacks_[action] = cb;
140 }
141 
142 void LaserConverter::callAll( const std::vector<message_actions::MessageAction>& actions )
143 {
144  static const std::vector<std::string> laser_keys_value(laserMemoryKeys, laserMemoryKeys+90);
145 
146  std::vector<float> result_value;
147  try {
148  qi::AnyValue anyvalues = p_memory_.call<qi::AnyValue>("getListData", laser_keys_value);
149  tools::fromAnyValueToFloatVector(anyvalues, result_value);
150  } catch (const std::exception& e) {
151  std::cerr << "Exception caught in LaserConverter: " << e.what() << std::endl;
152  return;
153  }
154  msg_.header.stamp = ros::Time::now();
155  // prepare the right sensor frame
156  size_t pos = 0;
157 
165  // ranges between 0-29
166  for( size_t i=0; i<30; i=i+2, ++pos)
167  {
168  const float lx = result_value[28-i]; // segments are internally flipped
169  const float ly = result_value[28-i+1]; // segments are internally flipped
170  float bx = lx*std::cos(-1.757) - ly*std::sin(-1.757) - 0.018;
171  float by = lx*std::sin(-1.757) + ly*std::cos(-1.757) - 0.090;
172  float dist = std::sqrt( std::pow(bx,2) + std::pow(by,2) );
173  //float dist = std::sqrt( std::pow(lx,2) + std::pow(ly,2) );
174  //std::cout << "got a distance at "<< pos << " with " << dist << std::endl;
175  msg_.ranges[pos] = dist;
176  }
177 
178  pos = pos+8; // leave out 8 blanks ==> pos = 15+8
179 
180  // ranges between 30-59
181  for( size_t i=0; i<30; i=i+2, ++pos)
182  {
183  const float lx = result_value[58-i];
184  const float ly = result_value[58-i+1];
185  float bx = lx + 0.056 ;
186  float by = ly;
187  float dist = std::sqrt( std::pow(bx,2) + std::pow(by,2) );
188  //float dist = std::sqrt( std::pow(lx,2) + std::pow(ly,2) );
189  //std::cout << "got a distance at "<< pos << " with " << dist << std::endl;
190  msg_.ranges[pos] = dist;
191  }
192 
193  pos = pos+8; // leave out again 8 blanks ==> pos = 15+8+15+8
194 
195  for( size_t i=0; i<30; i=i+2, ++pos)
196  {
197  const float lx = result_value[88-i];
198  const float ly = result_value[88-i+1];
199  float bx = lx*std::cos(1.757) - ly*std::sin(1.757) - 0.018;
200  float by = lx*std::sin(1.757) + ly*std::cos(1.757) + 0.090;
201  float dist = std::sqrt( std::pow(bx,2) + std::pow(by,2) );
202  //float dist = std::sqrt( std::pow(lx,2) + std::pow(ly,2) );
203  //std::cout << "got a distance at "<< pos << " with " << dist << std::endl;
204  msg_.ranges[pos] = dist;
205  }
206 
207  for_each( message_actions::MessageAction action, actions )
208  {
209  callbacks_[action](msg_);
210  }
211 }
212 
213 void LaserConverter::reset( )
214 {
215  msg_.header.frame_id = "base_footprint";
216  msg_.angle_min = -2.0944; // -120
217  msg_.angle_max = 2.0944; // +120
218  msg_.angle_increment = (2*2.0944) / (15+15+15+8+8); // 240 deg FoV / 61 points (blind zones inc)
219  msg_.range_min = this->range_min_; // in m
220  msg_.range_max = this->range_max_; // in m
221  msg_.ranges = std::vector<float>(61, -1.0f);
222 }
223 
224 void LaserConverter::setLaserRanges(
225  const float &range_min,
226  const float &range_max) {
227 
228  if (range_min > 0)
229  this->range_min_ = range_min;
230  else
231  this->range_min_ = 0.1;
232 
233  if (range_max > this->range_min_)
234  this->range_max_ = range_max;
235  else
236  this->range_max_ = 3.0;
237 }
238 
239 } //converter
240 } // naoqi
naoqi::converter::LaserConverter::msg_
sensor_msgs::LaserScan msg_
Definition: laser.hpp:60
for_each
#define for_each
Definition: laser.cpp:28
naoqi::message_actions::MessageAction
MessageAction
Definition: message_actions.h:9
naoqi::converter::laserMemoryKeys
static const char * laserMemoryKeys[]
Definition: laser.cpp:35
naoqi::converter::LaserConverter::LaserConverter
LaserConverter(const std::string &name, const float &frequency, const qi::SessionPtr &session)
Definition: laser.cpp:131
naoqi::converter::LaserConverter::Callback_t
boost::function< void(sensor_msgs::LaserScan &)> Callback_t
Definition: laser.hpp:40
naoqi::converter::LaserConverter::callbacks_
std::map< message_actions::MessageAction, Callback_t > callbacks_
Definition: laser.hpp:59
naoqi::converter::LaserConverter::callAll
void callAll(const std::vector< message_actions::MessageAction > &actions)
Definition: laser.cpp:142
naoqi::converter::LaserConverter::registerCallback
void registerCallback(message_actions::MessageAction action, Callback_t cb)
Definition: laser.cpp:137
action
action
naoqi::converter::LaserConverter::setLaserRanges
void setLaserRanges(const float &range_min, const float &range_max)
Definition: laser.cpp:224
naoqi::tools::fromAnyValueToFloatVector
std::vector< float > fromAnyValueToFloatVector(qi::AnyValue &value, std::vector< float > &result)
Definition: from_any_value.cpp:185
ros::Time::now
static Time now()

naoqi_driver
Author(s): Karsten Knese
autogenerated on Sat Feb 15 2020 03:24:26