1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34
35 /* Author: John Hsu */
36
37
38 #include <urdf_sensor/sensor.h>
39 #include <fstream>
40 #include <locale>
41 #include <sstream>
42 #include <stdexcept>
43 #include <string>
44 #include <algorithm>
45 #include <tinyxml.h>
46 #include <console_bridge/console.h>
47
48 namespace urdf{
49
50 bool parsePose(Pose &pose, TiXmlElement* xml);
51
parseCamera(Camera & camera,TiXmlElement * config)52 bool parseCamera(Camera &camera, TiXmlElement* config)
53 {
54 camera.clear();
55 camera.type = VisualSensor::CAMERA;
56
57 TiXmlElement *image = config->FirstChildElement("image");
58 if (image)
59 {
60 const char* width_char = image->Attribute("width");
61 if (width_char)
62 {
63 try
64 {
65 camera.width = std::stoul(width_char);
66 }
67 catch (std::invalid_argument &e)
68 {
69 CONSOLE_BRIDGE_logError("Camera image width [%s] is not a valid int: %s", width_char, e.what());
70 return false;
71 }
72 catch (std::out_of_range &e)
73 {
74 CONSOLE_BRIDGE_logError("Camera image width [%s] is out of range: %s", width_char, e.what());
75 return false;
76 }
77 }
78 else
79 {
80 CONSOLE_BRIDGE_logError("Camera sensor needs an image width attribute");
81 return false;
82 }
83
84 const char* height_char = image->Attribute("height");
85 if (height_char)
86 {
87 try
88 {
89 camera.height = std::stoul(height_char);
90 }
91 catch (std::invalid_argument &e)
92 {
93 CONSOLE_BRIDGE_logError("Camera image height [%s] is not a valid int: %s", height_char, e.what());
94 return false;
95 }
96 catch (std::out_of_range &e)
97 {
98 CONSOLE_BRIDGE_logError("Camera image height [%s] is out of range: %s", height_char, e.what());
99 return false;
100 }
101 }
102 else
103 {
104 CONSOLE_BRIDGE_logError("Camera sensor needs an image height attribute");
105 return false;
106 }
107
108 const char* format_char = image->Attribute("format");
109 if (format_char)
110 camera.format = std::string(format_char);
111 else
112 {
113 CONSOLE_BRIDGE_logError("Camera sensor needs an image format attribute");
114 return false;
115 }
116
117 const char* hfov_char = image->Attribute("hfov");
118 if (hfov_char)
119 {
120 try {
121 camera.hfov = strToDouble(hfov_char);
122 } catch(std::runtime_error &) {
123 CONSOLE_BRIDGE_logError("Camera image hfov [%s] is not a valid float", hfov_char);
124 return false;
125 }
126 }
127 else
128 {
129 CONSOLE_BRIDGE_logError("Camera sensor needs an image hfov attribute");
130 return false;
131 }
132
133 const char* near_char = image->Attribute("near");
134 if (near_char)
135 {
136 try {
137 camera.near = strToDouble(near_char);
138 } catch(std::runtime_error &) {
139 CONSOLE_BRIDGE_logError("Camera image near [%s] is not a valid float", near_char);
140 return false;
141 }
142 }
143 else
144 {
145 CONSOLE_BRIDGE_logError("Camera sensor needs an image near attribute");
146 return false;
147 }
148
149 const char* far_char = image->Attribute("far");
150 if (far_char)
151 {
152 try {
153 camera.far = strToDouble(far_char);
154 } catch(std::runtime_error &) {
155 CONSOLE_BRIDGE_logError("Camera image far [%s] is not a valid float", far_char);
156 return false;
157 }
158 }
159 else
160 {
161 CONSOLE_BRIDGE_logError("Camera sensor needs an image far attribute");
162 return false;
163 }
164
165 }
166 else
167 {
168 CONSOLE_BRIDGE_logError("Camera sensor has no <image> element");
169 return false;
170 }
171 return true;
172 }
173
parseRay(Ray & ray,TiXmlElement * config)174 bool parseRay(Ray &ray, TiXmlElement* config)
175 {
176 ray.clear();
177 ray.type = VisualSensor::RAY;
178
179 TiXmlElement *horizontal = config->FirstChildElement("horizontal");
180 if (horizontal)
181 {
182 const char* samples_char = horizontal->Attribute("samples");
183 if (samples_char)
184 {
185 try
186 {
187 ray.horizontal_samples = std::stoul(samples_char);
188 }
189 catch (std::invalid_argument &e)
190 {
191 CONSOLE_BRIDGE_logError("Ray horizontal samples [%s] is not a valid float: %s", samples_char, e.what());
192 return false;
193 }
194 catch (std::out_of_range &e)
195 {
196 CONSOLE_BRIDGE_logError("Ray horizontal samples [%s] is out of range: %s", samples_char, e.what());
197 return false;
198 }
199 }
200
201 const char* resolution_char = horizontal->Attribute("resolution");
202 if (resolution_char)
203 {
204 try {
205 ray.horizontal_resolution = strToDouble(resolution_char);
206 } catch(std::runtime_error &) {
207 CONSOLE_BRIDGE_logError("Ray horizontal resolution [%s] is not a valid float", resolution_char);
208 return false;
209 }
210 }
211
212 const char* min_angle_char = horizontal->Attribute("min_angle");
213 if (min_angle_char)
214 {
215 try {
216 ray.horizontal_min_angle = strToDouble(min_angle_char);
217 } catch(std::runtime_error &) {
218 CONSOLE_BRIDGE_logError("Ray horizontal min_angle [%s] is not a valid float", min_angle_char);
219 return false;
220 }
221 }
222
223 const char* max_angle_char = horizontal->Attribute("max_angle");
224 if (max_angle_char)
225 {
226 try {
227 ray.horizontal_max_angle = strToDouble(max_angle_char);
228 } catch(std::runtime_error &) {
229 CONSOLE_BRIDGE_logError("Ray horizontal max_angle [%s] is not a valid float", max_angle_char);
230 return false;
231 }
232 }
233 }
234
235 TiXmlElement *vertical = config->FirstChildElement("vertical");
236 if (vertical)
237 {
238 const char* samples_char = vertical->Attribute("samples");
239 if (samples_char)
240 {
241 try
242 {
243 ray.vertical_samples = std::stoul(samples_char);
244 }
245 catch (std::invalid_argument &e)
246 {
247 CONSOLE_BRIDGE_logError("Ray vertical samples [%s] is not a valid float: %s", samples_char, e.what());
248 return false;
249 }
250 catch (std::out_of_range &e)
251 {
252 CONSOLE_BRIDGE_logError("Ray vertical samples [%s] is out of range: %s", samples_char, e.what());
253 return false;
254 }
255 }
256
257 const char* resolution_char = vertical->Attribute("resolution");
258 if (resolution_char)
259 {
260 try {
261 ray.vertical_resolution = strToDouble(resolution_char);
262 } catch(std::runtime_error &) {
263 CONSOLE_BRIDGE_logError("Ray vertical resolution [%s] is not a valid float", resolution_char);
264 return false;
265 }
266 }
267
268 const char* min_angle_char = vertical->Attribute("min_angle");
269 if (min_angle_char)
270 {
271 try {
272 ray.vertical_min_angle = strToDouble(min_angle_char);
273 } catch(std::runtime_error &) {
274 CONSOLE_BRIDGE_logError("Ray vertical min_angle [%s] is not a valid float", min_angle_char);
275 return false;
276 }
277 }
278
279 const char* max_angle_char = vertical->Attribute("max_angle");
280 if (max_angle_char)
281 {
282 try {
283 ray.vertical_max_angle = strToDouble(max_angle_char);
284 } catch(std::runtime_error &) {
285 CONSOLE_BRIDGE_logError("Ray vertical max_angle [%s] is not a valid float", max_angle_char);
286 return false;
287 }
288 }
289 }
290 return false;
291 }
292
parseVisualSensor(TiXmlElement * g)293 VisualSensorSharedPtr parseVisualSensor(TiXmlElement *g)
294 {
295 VisualSensorSharedPtr visual_sensor;
296
297 // get sensor type
298 TiXmlElement *sensor_xml;
299 if (g->FirstChildElement("camera"))
300 {
301 Camera *camera = new Camera();
302 visual_sensor.reset(camera);
303 sensor_xml = g->FirstChildElement("camera");
304 if (!parseCamera(*camera, sensor_xml))
305 visual_sensor.reset();
306 }
307 else if (g->FirstChildElement("ray"))
308 {
309 Ray *ray = new Ray();
310 visual_sensor.reset(ray);
311 sensor_xml = g->FirstChildElement("ray");
312 if (!parseRay(*ray, sensor_xml))
313 visual_sensor.reset();
314 }
315 else
316 {
317 CONSOLE_BRIDGE_logError("No know sensor types [camera|ray] defined in <sensor> block");
318 }
319 return visual_sensor;
320 }
321
322
parseSensor(Sensor & sensor,TiXmlElement * config)323 bool parseSensor(Sensor &sensor, TiXmlElement* config)
324 {
325 sensor.clear();
326
327 const char *name_char = config->Attribute("name");
328 if (!name_char)
329 {
330 CONSOLE_BRIDGE_logError("No name given for the sensor.");
331 return false;
332 }
333 sensor.name = std::string(name_char);
334
335 // parse parent_link_name
336 const char *parent_link_name_char = config->Attribute("parent_link_name");
337 if (!parent_link_name_char)
338 {
339 CONSOLE_BRIDGE_logError("No parent_link_name given for the sensor.");
340 return false;
341 }
342 sensor.parent_link_name = std::string(parent_link_name_char);
343
344 // parse origin
345 TiXmlElement *o = config->FirstChildElement("origin");
346 if (o)
347 {
348 if (!parsePose(sensor.origin, o))
349 return false;
350 }
351
352 // parse sensor
353 sensor.sensor = parseVisualSensor(config);
354 return true;
355 }
356
357
358 }
359
360
361