Skip to content

Commit 499de2b

Browse files
rhaschkerhaschke
committed
simplified visual sensor parsing
moved basic attribute parsing routine to utils.h
1 parent c43efd8 commit 499de2b

File tree

4 files changed

+102
-210
lines changed

4 files changed

+102
-210
lines changed

‎urdf_parser/CMakeLists.txt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -10,7 +10,7 @@ set_target_properties(urdfdom_model PROPERTIES SOVERSION ${URDF_MAJOR_MINOR_VERS
1010

1111
add_library(urdfdom_sensor SHARED
1212
src/sensor_parser.cpp src/visual_sensor_parsers.cpp
13-
include/urdf_parser/sensor_parser.h include/urdf_parser/visual_sensor_parsers.h)
13+
include/urdf_parser/sensor_parser.h include/urdf_parser/visual_sensor_parsers.h include/urdf_parser/utils.h)
1414
target_link_libraries(urdfdom_sensor urdfdom_model)
1515
set_target_properties(urdfdom_sensor PROPERTIES SOVERSION ${URDF_MAJOR_MINOR_VERSION})
1616

‎urdf_parser/include/urdf_parser/utils.h‎

Lines changed: 73 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,73 @@
1+
/*********************************************************************
2+
* Software License Agreement (BSD License)
3+
*
4+
* Copyright (c) 2016, CITEC, Bielefeld University
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 copyright holder 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: Robert Haschke */
36+
37+
#ifndef URDF_PARSER_UTIL_H
38+
#define URDF_PARSER_UTIL_H
39+
40+
#include <boost/lexical_cast.hpp>
41+
#include <urdf_exception/exception.h>
42+
43+
namespace urdf {
44+
45+
template <typename T>
46+
T parseAttribute(const char* value)
47+
{
48+
return boost::lexical_cast<T>(value);
49+
}
50+
51+
template <typename T>
52+
T parseAttribute(const TiXmlElement &tag, const char* attr, const T* default_value=NULL)
53+
{
54+
const char* value = tag.Attribute(attr);
55+
if (!value)
56+
{
57+
if (default_value) return *default_value;
58+
else throw ParseError(std::string("missing '") + attr + "'' attribute");
59+
}
60+
61+
try
62+
{
63+
return parseAttribute<T>(value);
64+
}
65+
catch (const std::exception &e)
66+
{
67+
throw ParseError(std::string("failed to parse '") + attr + "' attribute: " + e.what());
68+
}
69+
}
70+
71+
}
72+
73+
#endif

‎urdf_parser/src/sensor_parser.cpp‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -36,11 +36,10 @@
3636

3737

3838
#include "urdf_parser/sensor_parser.h"
39+
3940
#include "urdf_parser/pose.h"
4041
#include <urdf_sensor/camera.h>
4142
#include <urdf_sensor/ray.h>
42-
43-
#include <boost/lexical_cast.hpp>
4443
#include <console_bridge/console.h>
4544

4645
namespace urdf {

‎urdf_parser/src/visual_sensor_parsers.cpp‎

Lines changed: 27 additions & 207 deletions
Original file line numberDiff line numberDiff line change
@@ -35,12 +35,11 @@
3535
/* Author: John Hsu */
3636

3737
#include "urdf_parser/visual_sensor_parsers.h"
38+
#include "urdf_parser/utils.h"
39+
#include "urdf_parser/pose.h"
3840
#include <urdf_sensor/camera.h>
3941
#include <urdf_sensor/ray.h>
40-
41-
#include <boost/lexical_cast.hpp>
4242
#include <console_bridge/console.h>
43-
#include "urdf_parser/pose.h"
4443

4544
namespace urdf {
4645

@@ -51,110 +50,19 @@ SensorBaseSharedPtr CameraParser::parse(TiXmlElement &config)
5150
TiXmlElement *image = config.FirstChildElement("image");
5251
if (image)
5352
{
54-
const char* width_char = image->Attribute("width");
55-
if (width_char)
56-
{
57-
try
58-
{
59-
camera->width = boost::lexical_cast<unsigned int>(width_char);
60-
}
61-
catch (boost::bad_lexical_cast &e)
62-
{
63-
CONSOLE_BRIDGE_logError("Camera image width [%s] is not a valid int: %s", width_char, e.what());
64-
return CameraSharedPtr();
65-
}
53+
try {
54+
camera->width = parseAttribute<unsigned int>(*image, "width");
55+
camera->height = parseAttribute<unsigned int>(*image, "height");
56+
camera->format = parseAttribute<std::string>(*image, "format");
57+
camera->hfov = parseAttribute<double>(*image, "hfov");
58+
camera->near = parseAttribute<double>(*image, "near");
59+
camera->far = parseAttribute<double>(*image, "far");
6660
}
67-
else
61+
catch (const std::exception &e)
6862
{
69-
CONSOLE_BRIDGE_logError("Camera sensor needs an image width attribute");
63+
CONSOLE_BRIDGE_logError("Camera sensor %s", e.what());
7064
return CameraSharedPtr();
7165
}
72-
73-
const char* height_char = image->Attribute("height");
74-
if (height_char)
75-
{
76-
try
77-
{
78-
camera->height = boost::lexical_cast<unsigned int>(height_char);
79-
}
80-
catch (boost::bad_lexical_cast &e)
81-
{
82-
CONSOLE_BRIDGE_logError("Camera image height [%s] is not a valid int: %s", height_char, e.what());
83-
return CameraSharedPtr();
84-
}
85-
}
86-
else
87-
{
88-
CONSOLE_BRIDGE_logError("Camera sensor needs an image height attribute");
89-
return CameraSharedPtr();
90-
}
91-
92-
const char* format_char = image->Attribute("format");
93-
if (format_char)
94-
camera->format = std::string(format_char);
95-
else
96-
{
97-
CONSOLE_BRIDGE_logError("Camera sensor needs an image format attribute");
98-
return CameraSharedPtr();
99-
}
100-
101-
const char* hfov_char = image->Attribute("hfov");
102-
if (hfov_char)
103-
{
104-
try
105-
{
106-
camera->hfov = boost::lexical_cast<double>(hfov_char);
107-
}
108-
catch (boost::bad_lexical_cast &e)
109-
{
110-
CONSOLE_BRIDGE_logError("Camera image hfov [%s] is not a valid float: %s", hfov_char, e.what());
111-
return CameraSharedPtr();
112-
}
113-
}
114-
else
115-
{
116-
CONSOLE_BRIDGE_logError("Camera sensor needs an image hfov attribute");
117-
return CameraSharedPtr();
118-
}
119-
120-
const char* near_char = image->Attribute("near");
121-
if (near_char)
122-
{
123-
try
124-
{
125-
camera->near = boost::lexical_cast<double>(near_char);
126-
}
127-
catch (boost::bad_lexical_cast &e)
128-
{
129-
CONSOLE_BRIDGE_logError("Camera image near [%s] is not a valid float: %s", near_char, e.what());
130-
return CameraSharedPtr();
131-
}
132-
}
133-
else
134-
{
135-
CONSOLE_BRIDGE_logError("Camera sensor needs an image near attribute");
136-
return CameraSharedPtr();
137-
}
138-
139-
const char* far_char = image->Attribute("far");
140-
if (far_char)
141-
{
142-
try
143-
{
144-
camera->far = boost::lexical_cast<double>(far_char);
145-
}
146-
catch (boost::bad_lexical_cast &e)
147-
{
148-
CONSOLE_BRIDGE_logError("Camera image far [%s] is not a valid float: %s", far_char, e.what());
149-
return CameraSharedPtr();
150-
}
151-
}
152-
else
153-
{
154-
CONSOLE_BRIDGE_logError("Camera sensor needs an image far attribute");
155-
return CameraSharedPtr();
156-
}
157-
15866
}
15967
else
16068
{
@@ -172,120 +80,32 @@ SensorBaseSharedPtr RayParser::parse(TiXmlElement &config)
17280
TiXmlElement *horizontal = config.FirstChildElement("horizontal");
17381
if (horizontal)
17482
{
175-
const char* samples_char = horizontal->Attribute("samples");
176-
if (samples_char)
177-
{
178-
try
179-
{
180-
ray->horizontal_samples = boost::lexical_cast<unsigned int>(samples_char);
181-
}
182-
catch (boost::bad_lexical_cast &e)
183-
{
184-
CONSOLE_BRIDGE_logError("Ray horizontal samples [%s] is not a valid float: %s", samples_char, e.what());
185-
return RaySharedPtr();
186-
}
187-
}
188-
189-
const char* resolution_char = horizontal->Attribute("resolution");
190-
if (resolution_char)
191-
{
192-
try
193-
{
194-
ray->horizontal_resolution = boost::lexical_cast<double>(resolution_char);
195-
}
196-
catch (boost::bad_lexical_cast &e)
197-
{
198-
CONSOLE_BRIDGE_logError("Ray horizontal resolution [%s] is not a valid float: %s", resolution_char, e.what());
199-
return RaySharedPtr();
200-
}
83+
try {
84+
ray->horizontal_samples = parseAttribute<unsigned int>(*horizontal, "samples");
85+
ray->horizontal_resolution = parseAttribute<double>(*horizontal, "resolution");
86+
ray->horizontal_min_angle = parseAttribute<double>(*horizontal, "min_angle");
87+
ray->horizontal_max_angle = parseAttribute<double>(*horizontal, "max_angle");
20188
}
202-
203-
const char* min_angle_char = horizontal->Attribute("min_angle");
204-
if (min_angle_char)
205-
{
206-
try
207-
{
208-
ray->horizontal_min_angle = boost::lexical_cast<double>(min_angle_char);
209-
}
210-
catch (boost::bad_lexical_cast &e)
211-
{
212-
CONSOLE_BRIDGE_logError("Ray horizontal min_angle [%s] is not a valid float: %s", min_angle_char, e.what());
213-
return RaySharedPtr();
214-
}
215-
}
216-
217-
const char* max_angle_char = horizontal->Attribute("max_angle");
218-
if (max_angle_char)
89+
catch (const std::exception &e)
21990
{
220-
try
221-
{
222-
ray->horizontal_max_angle = boost::lexical_cast<double>(max_angle_char);
223-
}
224-
catch (boost::bad_lexical_cast &e)
225-
{
226-
CONSOLE_BRIDGE_logError("Ray horizontal max_angle [%s] is not a valid float: %s", max_angle_char, e.what());
227-
return RaySharedPtr();
228-
}
91+
CONSOLE_BRIDGE_logError("Ray horizontal: %s", e.what());
92+
return RaySharedPtr();
22993
}
23094
}
23195

23296
TiXmlElement *vertical = config.FirstChildElement("vertical");
23397
if (vertical)
23498
{
235-
const char* samples_char = vertical->Attribute("samples");
236-
if (samples_char)
237-
{
238-
try
239-
{
240-
ray->vertical_samples = boost::lexical_cast<unsigned int>(samples_char);
241-
}
242-
catch (boost::bad_lexical_cast &e)
243-
{
244-
CONSOLE_BRIDGE_logError("Ray vertical samples [%s] is not a valid float: %s", samples_char, e.what());
245-
return RaySharedPtr();
246-
}
247-
}
248-
249-
const char* resolution_char = vertical->Attribute("resolution");
250-
if (resolution_char)
251-
{
252-
try
253-
{
254-
ray->vertical_resolution = boost::lexical_cast<double>(resolution_char);
255-
}
256-
catch (boost::bad_lexical_cast &e)
257-
{
258-
CONSOLE_BRIDGE_logError("Ray vertical resolution [%s] is not a valid float: %s", resolution_char, e.what());
259-
return RaySharedPtr();
260-
}
261-
}
262-
263-
const char* min_angle_char = vertical->Attribute("min_angle");
264-
if (min_angle_char)
265-
{
266-
try
267-
{
268-
ray->vertical_min_angle = boost::lexical_cast<double>(min_angle_char);
269-
}
270-
catch (boost::bad_lexical_cast &e)
271-
{
272-
CONSOLE_BRIDGE_logError("Ray vertical min_angle [%s] is not a valid float: %s", min_angle_char, e.what());
273-
return RaySharedPtr();
274-
}
99+
try {
100+
ray->vertical_samples = parseAttribute<unsigned int>(*vertical, "samples");
101+
ray->vertical_resolution = parseAttribute<double>(*vertical, "resolution");
102+
ray->vertical_min_angle = parseAttribute<double>(*vertical, "min_angle");
103+
ray->vertical_max_angle = parseAttribute<double>(*vertical, "max_angle");
275104
}
276-
277-
const char* max_angle_char = vertical->Attribute("max_angle");
278-
if (max_angle_char)
105+
catch (const std::exception &e)
279106
{
280-
try
281-
{
282-
ray->vertical_max_angle = boost::lexical_cast<double>(max_angle_char);
283-
}
284-
catch (boost::bad_lexical_cast &e)
285-
{
286-
CONSOLE_BRIDGE_logError("Ray vertical max_angle [%s] is not a valid float: %s", max_angle_char, e.what());
287-
return RaySharedPtr();
288-
}
107+
CONSOLE_BRIDGE_logError("Ray horizontal: %s", e.what());
108+
return RaySharedPtr();
289109
}
290110
}
291111
return ray;

0 commit comments

Comments
 (0)