diff --git a/urdf_parser/CMakeLists.txt b/urdf_parser/CMakeLists.txt index ac7c0792..c92699a2 100644 --- a/urdf_parser/CMakeLists.txt +++ b/urdf_parser/CMakeLists.txt @@ -58,7 +58,8 @@ add_urdfdom_library( LIBNAME urdfdom_sensor SOURCES - src/urdf_sensor.cpp + src/sensor_parser.cpp + src/visual_sensor_parsers.cpp LINK urdfdom_model) diff --git a/urdf_parser/include/urdf_parser/joint.h b/urdf_parser/include/urdf_parser/joint.h new file mode 100644 index 00000000..a5edb45c --- /dev/null +++ b/urdf_parser/include/urdf_parser/joint.h @@ -0,0 +1,49 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +#ifndef URDF_PARSER_JOINT_H +#define URDF_PARSER_JOINT_H + +#include +#include + +namespace urdf { + +bool parseJoint(Joint &joint, TiXmlElement *config); + +bool exportJoint(Joint &joint, TiXmlElement *config); + +} + +#endif diff --git a/urdf_parser/include/urdf_parser/link.h b/urdf_parser/include/urdf_parser/link.h new file mode 100644 index 00000000..d83449d1 --- /dev/null +++ b/urdf_parser/include/urdf_parser/link.h @@ -0,0 +1,75 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +#ifndef URDF_PARSER_LINK_H +#define URDF_PARSER_LINK_H + +#include +#include +#include "exportdecl.h" + +namespace urdf { + +URDFDOM_DLLAPI bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_ok); + +URDFDOM_DLLAPI bool parseSphere(Sphere &s, TiXmlElement *c); +URDFDOM_DLLAPI bool parseBox(Box &b, TiXmlElement *c); +URDFDOM_DLLAPI bool parseCylinder(Cylinder &y, TiXmlElement *c); +URDFDOM_DLLAPI bool parseMesh(Mesh &m, TiXmlElement *c); +URDFDOM_DLLAPI GeometrySharedPtr parseGeometry(TiXmlElement *g); + +URDFDOM_DLLAPI bool parseInertial(Inertial &i, TiXmlElement *config); +URDFDOM_DLLAPI bool parseVisual(Visual &vis, TiXmlElement *config); +URDFDOM_DLLAPI bool parseCollision(Collision &col, TiXmlElement* config); + +URDFDOM_DLLAPI bool parseLink(Link &link, TiXmlElement *config); + + +URDFDOM_DLLAPI bool exportMaterial(Material &material, TiXmlElement *config); + +URDFDOM_DLLAPI bool exportSphere(Sphere &s, TiXmlElement *xml); +URDFDOM_DLLAPI bool exportBox(Box &b, TiXmlElement *xml); +URDFDOM_DLLAPI bool exportCylinder(Cylinder &y, TiXmlElement *xml); +URDFDOM_DLLAPI bool exportMesh(Mesh &m, TiXmlElement *xml); +URDFDOM_DLLAPI bool exportGeometry(GeometrySharedPtr &geom, TiXmlElement *xml); + +URDFDOM_DLLAPI bool exportInertial(Inertial &i, TiXmlElement *xml); +URDFDOM_DLLAPI bool exportVisual(Visual &vis, TiXmlElement *xml); +URDFDOM_DLLAPI bool exportCollision(Collision &col, TiXmlElement* xml); + +URDFDOM_DLLAPI bool exportLink(Link &link, TiXmlElement *config); + +} + +#endif diff --git a/urdf_parser/include/urdf_parser/pose.h b/urdf_parser/include/urdf_parser/pose.h new file mode 100644 index 00000000..8a2db93e --- /dev/null +++ b/urdf_parser/include/urdf_parser/pose.h @@ -0,0 +1,60 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +#ifndef URDF_PARSER_POSE_H +#define URDF_PARSER_POSE_H + +#include +#include +#include +#include "exportdecl.h" + +namespace urdf_export_helpers { + +URDFDOM_DLLAPI std::string values2str(unsigned int count, const double *values, double (*conv)(double) = NULL); +URDFDOM_DLLAPI std::string values2str(urdf::Vector3 vec); +URDFDOM_DLLAPI std::string values2str(urdf::Rotation rot); +URDFDOM_DLLAPI std::string values2str(urdf::Color c); +URDFDOM_DLLAPI std::string values2str(double d); + +} + +namespace urdf { + +URDFDOM_DLLAPI bool parsePose(Pose&, TiXmlElement*); +URDFDOM_DLLAPI bool exportPose(Pose &pose, TiXmlElement* xml); + +} + +#endif diff --git a/urdf_parser/include/urdf_parser/sensor_parser.h b/urdf_parser/include/urdf_parser/sensor_parser.h new file mode 100644 index 00000000..9cd67ce5 --- /dev/null +++ b/urdf_parser/include/urdf_parser/sensor_parser.h @@ -0,0 +1,75 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2016, Bielefeld University +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of Bielefeld University nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: Robert Haschke */ + +#ifndef URDF_PARSER_URDF_SENSOR_PARSER_H +#define URDF_PARSER_URDF_SENSOR_PARSER_H + +#include +#include +#include +#include + +#include "exportdecl.h" + +namespace urdf { + + /// API for any custom SensorParser + class URDFDOM_DLLAPI SensorParser { + public: + virtual ~SensorParser() = default; + virtual SensorBase* parse(TiXmlElement &sensor_element) = 0; + }; + URDF_TYPEDEF_CLASS_POINTER(SensorParser); + + /// map from sensor name to Sensor instance + using SensorMap = std::map; + /// map from sensor type to SensorParser instance + using SensorParserMap = std::map; + + /** parse tags in URDF document for which a parser exists in SensorParserMap */ + URDFDOM_DLLAPI SensorMap parseSensors(TiXmlDocument &urdf, const SensorParserMap &parsers); + + /** convienency function to fetch a sensor with given name and type from the map */ + template + URDFDOM_DLLAPI std::shared_ptr getSensor(const std::string &name, const SensorMap &sensors) { + SensorMap::const_iterator s = sensors.find(name); + if (s == sensors.end()) return std::shared_ptr(); + else return std::dynamic_pointer_cast(s->second->sensor_); + } + +} + +#endif diff --git a/urdf_parser/include/urdf_parser/urdf_parser.h b/urdf_parser/include/urdf_parser/urdf_parser.h index a293379e..2d178e5f 100644 --- a/urdf_parser/include/urdf_parser/urdf_parser.h +++ b/urdf_parser/include/urdf_parser/urdf_parser.h @@ -46,19 +46,13 @@ #include #include #include -#include #include #include "exportdecl.h" +#include "pose.h" namespace urdf_export_helpers { -URDFDOM_DLLAPI std::string values2str(unsigned int count, const double *values, double (*conv)(double) = NULL); -URDFDOM_DLLAPI std::string values2str(urdf::Vector3 vec); -URDFDOM_DLLAPI std::string values2str(urdf::Rotation rot); -URDFDOM_DLLAPI std::string values2str(urdf::Color c); -URDFDOM_DLLAPI std::string values2str(double d); - // This lives here (rather than in model.cpp) so we can run tests on it. class URDFVersion final { @@ -145,10 +139,6 @@ namespace urdf{ URDFDOM_DLLAPI ModelInterfaceSharedPtr parseURDFFile(const std::string &path); URDFDOM_DLLAPI TiXmlDocument* exportURDF(ModelInterfaceSharedPtr &model); URDFDOM_DLLAPI TiXmlDocument* exportURDF(const ModelInterface &model); - URDFDOM_DLLAPI bool parsePose(Pose&, TiXmlElement*); - URDFDOM_DLLAPI bool parseCamera(Camera&, TiXmlElement*); - URDFDOM_DLLAPI bool parseRay(Ray&, TiXmlElement*); - URDFDOM_DLLAPI bool parseSensor(Sensor&, TiXmlElement*); URDFDOM_DLLAPI bool parseModelState(ModelState&, TiXmlElement*); } diff --git a/urdf_parser/include/urdf_parser/utils.h b/urdf_parser/include/urdf_parser/utils.h new file mode 100644 index 00000000..9ea55743 --- /dev/null +++ b/urdf_parser/include/urdf_parser/utils.h @@ -0,0 +1,88 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2016, CITEC, Bielefeld University +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the copyright holder nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: Robert Haschke */ + +#ifndef URDF_PARSER_UTIL_H +#define URDF_PARSER_UTIL_H + +#include +#include + +namespace urdf { + +template +T parseAttribute(const char* value); + +template<> +inline std::string parseAttribute(const char* value) +{ + return value; +} + +template<> +inline double parseAttribute(const char* value) +{ + return strToDouble(value); +} + +template<> +inline unsigned int parseAttribute(const char* value) +{ + return std::stoul(value); +} + +template +T parseAttribute(const TiXmlElement &tag, const char* attr, const T* default_value=NULL) +{ + const char* value = tag.Attribute(attr); + if (!value) + { + if (default_value) return *default_value; + else throw ParseError(std::string("missing '") + attr + "' attribute"); + } + + try + { + return parseAttribute(value); + } + catch (const std::exception &e) + { + throw ParseError(std::string("failed to parse '") + attr + "' attribute: " + e.what()); + } +} + +} + +#endif diff --git a/urdf_parser/include/urdf_parser/visual_sensor_parsers.h b/urdf_parser/include/urdf_parser/visual_sensor_parsers.h new file mode 100644 index 00000000..6356a0c5 --- /dev/null +++ b/urdf_parser/include/urdf_parser/visual_sensor_parsers.h @@ -0,0 +1,56 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: Robert Haschke */ + +#ifndef URDF_PARSER_VISUAL_SENSOR_PARSERS_H +#define URDF_PARSER_VISUAL_SENSOR_PARSERS_H + +#include "sensor_parser.h" + +namespace urdf { + + class URDFDOM_DLLAPI CameraParser : public SensorParser { + public: + SensorBase* parse(TiXmlElement &sensor_element); + }; + + class URDFDOM_DLLAPI RayParser : public SensorParser { + public: + SensorBase* parse(TiXmlElement &sensor_element); + }; + +} + +#endif diff --git a/urdf_parser/src/joint.cpp b/urdf_parser/src/joint.cpp index 102be3f6..82aa51cb 100644 --- a/urdf_parser/src/joint.cpp +++ b/urdf_parser/src/joint.cpp @@ -42,11 +42,11 @@ #include #include #include +#include +#include namespace urdf{ -bool parsePose(Pose &pose, TiXmlElement* xml); - bool parseJointDynamics(JointDynamics &jd, TiXmlElement* config) { jd.clear(); @@ -522,10 +522,6 @@ bool parseJoint(Joint &joint, TiXmlElement* config) return true; } - -/* exports */ -bool exportPose(Pose &pose, TiXmlElement* xml); - bool exportJointDynamics(JointDynamics &jd, TiXmlElement* xml) { TiXmlElement *dynamics_xml = new TiXmlElement("dynamics"); diff --git a/urdf_parser/src/link.cpp b/urdf_parser/src/link.cpp index d3a7b2f2..040a96fc 100644 --- a/urdf_parser/src/link.cpp +++ b/urdf_parser/src/link.cpp @@ -36,7 +36,8 @@ #include -#include +#include +#include #include #include #include @@ -50,8 +51,6 @@ namespace urdf{ -bool parsePose(Pose &pose, TiXmlElement* xml); - bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_ok) { bool has_rgb = false; @@ -485,9 +484,6 @@ bool parseLink(Link &link, TiXmlElement* config) return true; } -/* exports */ -bool exportPose(Pose &pose, TiXmlElement* xml); - bool exportMaterial(Material &material, TiXmlElement *xml) { TiXmlElement *material_xml = new TiXmlElement("material"); diff --git a/urdf_parser/src/model.cpp b/urdf_parser/src/model.cpp index fc79f759..f233a1d6 100644 --- a/urdf_parser/src/model.cpp +++ b/urdf_parser/src/model.cpp @@ -39,14 +39,13 @@ #include #include #include "urdf_parser/urdf_parser.h" +#include "urdf_parser/pose.h" +#include "urdf_parser/link.h" +#include "urdf_parser/joint.h" #include namespace urdf{ -bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_ok); -bool parseLink(Link &link, TiXmlElement *config); -bool parseJoint(Joint &joint, TiXmlElement *config); - ModelInterfaceSharedPtr parseURDFFile(const std::string &path) { std::ifstream stream( path.c_str() ); @@ -269,9 +268,6 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) return model; } -bool exportMaterial(Material &material, TiXmlElement *config); -bool exportLink(Link &link, TiXmlElement *config); -bool exportJoint(Joint &joint, TiXmlElement *config); TiXmlDocument* exportURDF(const ModelInterface &model) { TiXmlDocument *doc = new TiXmlDocument(); diff --git a/urdf_parser/src/pose.cpp b/urdf_parser/src/pose.cpp index 56bedd48..a03076c7 100644 --- a/urdf_parser/src/pose.cpp +++ b/urdf_parser/src/pose.cpp @@ -35,13 +35,13 @@ /* Author: Wim Meeussen, John Hsu */ -#include #include #include #include #include #include #include +#include namespace urdf_export_helpers { diff --git a/urdf_parser/src/sensor_parser.cpp b/urdf_parser/src/sensor_parser.cpp new file mode 100644 index 00000000..f6f0715a --- /dev/null +++ b/urdf_parser/src/sensor_parser.cpp @@ -0,0 +1,156 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: John Hsu */ + + +#include "urdf_parser/sensor_parser.h" + +#include "urdf_parser/pose.h" +#include +#include +#include + +namespace urdf { + +SensorBaseSharedPtr parseSensorBase(TiXmlElement *sensor_xml, const SensorParserMap &parsers) +{ + // find first child element that is not or + const char* sensor_type; + TiXmlElement *sensor_base_xml = sensor_xml->FirstChildElement(); + while (sensor_base_xml) { + sensor_type = sensor_base_xml->Value(); + if (strcmp(sensor_type, "parent") && strcmp(sensor_type, "origin")) + break; + sensor_base_xml = sensor_base_xml->NextSiblingElement(); + } + + if (sensor_base_xml) + { + SensorParserMap::const_iterator parser = parsers.find(sensor_type); + if (parser != parsers.end() && parser->second) + { + // Link sensor's deleter to the parser. + // If the parser was loaded via a pluginlib, this is required to link + // the lifetime of the parser/library to the created sensor instances. + return SensorBaseSharedPtr(parser->second->parse(*sensor_base_xml), + [linked_parser = parser->second](auto *p) + { delete p; }); + } + else + { + CONSOLE_BRIDGE_logDebug("Sensor type not handled: %s", sensor_type); + } + } + else + { + CONSOLE_BRIDGE_logError("No child element defining the sensor."); + } + + return SensorBaseSharedPtr(); +} + + +bool parseSensor(Sensor &sensor, TiXmlElement* config, const SensorParserMap &parsers) +{ + sensor.clear(); + + const char *name_char = config->Attribute("name"); + if (!name_char) + { + CONSOLE_BRIDGE_logError("No name given for the sensor."); + return false; + } + sensor.name_ = std::string(name_char); + + // parse parent link name + TiXmlElement *parent_xml = config->FirstChildElement("parent"); + const char *parent_link_name_char = parent_xml ? parent_xml->Attribute("link") : NULL; + if (!parent_link_name_char) + { + CONSOLE_BRIDGE_logError("No parent link name given for the sensor."); + return false; + } + sensor.parent_link_ = std::string(parent_link_name_char); + + // parse origin + TiXmlElement *o = config->FirstChildElement("origin"); + if (o) + { + if (!parsePose(sensor.origin_, o)) + return false; + } + + // parse sensor + sensor.sensor_ = parseSensorBase(config, parsers); + return static_cast(sensor.sensor_); +} + +URDFDOM_DLLAPI +SensorMap parseSensors(TiXmlDocument &urdf_xml, const SensorParserMap &parsers) +{ + TiXmlElement *robot_xml = urdf_xml.FirstChildElement("robot"); + if (!robot_xml) { + CONSOLE_BRIDGE_logError("Could not find the 'robot' element in the URDF"); + } + + SensorMap results; + // Get all sensor elements + for (TiXmlElement* sensor_xml = robot_xml->FirstChildElement("sensor"); + sensor_xml; sensor_xml = sensor_xml->NextSiblingElement("sensor")) + { + SensorSharedPtr sensor; + sensor.reset(new Sensor); + + if (parseSensor(*sensor, sensor_xml, parsers)) + { + if (results.find(sensor->name_) != results.end()) + { + CONSOLE_BRIDGE_logWarn("Sensor '%s' is not unique. Ignoring consecutive ones.", sensor->name_.c_str()); + } + else + { + results.insert(make_pair(sensor->name_, sensor)); + CONSOLE_BRIDGE_logDebug("urdfdom: successfully added a new sensor '%s'", sensor->name_.c_str()); + } + } + else + { + CONSOLE_BRIDGE_logError("failed to parse sensor element"); + } + } + return results; +} + +} diff --git a/urdf_parser/src/urdf_sensor.cpp b/urdf_parser/src/urdf_sensor.cpp deleted file mode 100644 index ee863944..00000000 --- a/urdf_parser/src/urdf_sensor.cpp +++ /dev/null @@ -1,362 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: John Hsu */ - - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -namespace urdf{ - -bool parsePose(Pose &pose, TiXmlElement* xml); - -bool parseCamera(Camera &camera, TiXmlElement* config) -{ - camera.clear(); - camera.type = VisualSensor::CAMERA; - - TiXmlElement *image = config->FirstChildElement("image"); - if (image) - { - const char* width_char = image->Attribute("width"); - if (width_char) - { - try - { - camera.width = std::stoul(width_char); - } - catch (std::invalid_argument &e) - { - CONSOLE_BRIDGE_logError("Camera image width [%s] is not a valid int: %s", width_char, e.what()); - return false; - } - catch (std::out_of_range &e) - { - CONSOLE_BRIDGE_logError("Camera image width [%s] is out of range: %s", width_char, e.what()); - return false; - } - } - else - { - CONSOLE_BRIDGE_logError("Camera sensor needs an image width attribute"); - return false; - } - - const char* height_char = image->Attribute("height"); - if (height_char) - { - try - { - camera.height = std::stoul(height_char); - } - catch (std::invalid_argument &e) - { - CONSOLE_BRIDGE_logError("Camera image height [%s] is not a valid int: %s", height_char, e.what()); - return false; - } - catch (std::out_of_range &e) - { - CONSOLE_BRIDGE_logError("Camera image height [%s] is out of range: %s", height_char, e.what()); - return false; - } - } - else - { - CONSOLE_BRIDGE_logError("Camera sensor needs an image height attribute"); - return false; - } - - const char* format_char = image->Attribute("format"); - if (format_char) - camera.format = std::string(format_char); - else - { - CONSOLE_BRIDGE_logError("Camera sensor needs an image format attribute"); - return false; - } - - const char* hfov_char = image->Attribute("hfov"); - if (hfov_char) - { - try { - camera.hfov = strToDouble(hfov_char); - } catch(std::runtime_error &) { - CONSOLE_BRIDGE_logError("Camera image hfov [%s] is not a valid float", hfov_char); - return false; - } - } - else - { - CONSOLE_BRIDGE_logError("Camera sensor needs an image hfov attribute"); - return false; - } - - const char* near_char = image->Attribute("near"); - if (near_char) - { - try { - camera.near = strToDouble(near_char); - } catch(std::runtime_error &) { - CONSOLE_BRIDGE_logError("Camera image near [%s] is not a valid float", near_char); - return false; - } - } - else - { - CONSOLE_BRIDGE_logError("Camera sensor needs an image near attribute"); - return false; - } - - const char* far_char = image->Attribute("far"); - if (far_char) - { - try { - camera.far = strToDouble(far_char); - } catch(std::runtime_error &) { - CONSOLE_BRIDGE_logError("Camera image far [%s] is not a valid float", far_char); - return false; - } - } - else - { - CONSOLE_BRIDGE_logError("Camera sensor needs an image far attribute"); - return false; - } - - } - else - { - CONSOLE_BRIDGE_logError("Camera sensor has no element"); - return false; - } - return true; -} - -bool parseRay(Ray &ray, TiXmlElement* config) -{ - ray.clear(); - ray.type = VisualSensor::RAY; - - TiXmlElement *horizontal = config->FirstChildElement("horizontal"); - if (horizontal) - { - const char* samples_char = horizontal->Attribute("samples"); - if (samples_char) - { - try - { - ray.horizontal_samples = std::stoul(samples_char); - } - catch (std::invalid_argument &e) - { - CONSOLE_BRIDGE_logError("Ray horizontal samples [%s] is not a valid float: %s", samples_char, e.what()); - return false; - } - catch (std::out_of_range &e) - { - CONSOLE_BRIDGE_logError("Ray horizontal samples [%s] is out of range: %s", samples_char, e.what()); - return false; - } - } - - const char* resolution_char = horizontal->Attribute("resolution"); - if (resolution_char) - { - try { - ray.horizontal_resolution = strToDouble(resolution_char); - } catch(std::runtime_error &) { - CONSOLE_BRIDGE_logError("Ray horizontal resolution [%s] is not a valid float", resolution_char); - return false; - } - } - - const char* min_angle_char = horizontal->Attribute("min_angle"); - if (min_angle_char) - { - try { - ray.horizontal_min_angle = strToDouble(min_angle_char); - } catch(std::runtime_error &) { - CONSOLE_BRIDGE_logError("Ray horizontal min_angle [%s] is not a valid float", min_angle_char); - return false; - } - } - - const char* max_angle_char = horizontal->Attribute("max_angle"); - if (max_angle_char) - { - try { - ray.horizontal_max_angle = strToDouble(max_angle_char); - } catch(std::runtime_error &) { - CONSOLE_BRIDGE_logError("Ray horizontal max_angle [%s] is not a valid float", max_angle_char); - return false; - } - } - } - - TiXmlElement *vertical = config->FirstChildElement("vertical"); - if (vertical) - { - const char* samples_char = vertical->Attribute("samples"); - if (samples_char) - { - try - { - ray.vertical_samples = std::stoul(samples_char); - } - catch (std::invalid_argument &e) - { - CONSOLE_BRIDGE_logError("Ray vertical samples [%s] is not a valid float: %s", samples_char, e.what()); - return false; - } - catch (std::out_of_range &e) - { - CONSOLE_BRIDGE_logError("Ray vertical samples [%s] is out of range: %s", samples_char, e.what()); - return false; - } - } - - const char* resolution_char = vertical->Attribute("resolution"); - if (resolution_char) - { - try { - ray.vertical_resolution = strToDouble(resolution_char); - } catch(std::runtime_error &) { - CONSOLE_BRIDGE_logError("Ray vertical resolution [%s] is not a valid float", resolution_char); - return false; - } - } - - const char* min_angle_char = vertical->Attribute("min_angle"); - if (min_angle_char) - { - try { - ray.vertical_min_angle = strToDouble(min_angle_char); - } catch(std::runtime_error &) { - CONSOLE_BRIDGE_logError("Ray vertical min_angle [%s] is not a valid float", min_angle_char); - return false; - } - } - - const char* max_angle_char = vertical->Attribute("max_angle"); - if (max_angle_char) - { - try { - ray.vertical_max_angle = strToDouble(max_angle_char); - } catch(std::runtime_error &) { - CONSOLE_BRIDGE_logError("Ray vertical max_angle [%s] is not a valid float", max_angle_char); - return false; - } - } - } - return false; -} - -VisualSensorSharedPtr parseVisualSensor(TiXmlElement *g) -{ - VisualSensorSharedPtr visual_sensor; - - // get sensor type - TiXmlElement *sensor_xml; - if (g->FirstChildElement("camera")) - { - Camera *camera = new Camera(); - visual_sensor.reset(camera); - sensor_xml = g->FirstChildElement("camera"); - if (!parseCamera(*camera, sensor_xml)) - visual_sensor.reset(); - } - else if (g->FirstChildElement("ray")) - { - Ray *ray = new Ray(); - visual_sensor.reset(ray); - sensor_xml = g->FirstChildElement("ray"); - if (!parseRay(*ray, sensor_xml)) - visual_sensor.reset(); - } - else - { - CONSOLE_BRIDGE_logError("No know sensor types [camera|ray] defined in block"); - } - return visual_sensor; -} - - -bool parseSensor(Sensor &sensor, TiXmlElement* config) -{ - sensor.clear(); - - const char *name_char = config->Attribute("name"); - if (!name_char) - { - CONSOLE_BRIDGE_logError("No name given for the sensor."); - return false; - } - sensor.name = std::string(name_char); - - // parse parent_link_name - const char *parent_link_name_char = config->Attribute("parent_link_name"); - if (!parent_link_name_char) - { - CONSOLE_BRIDGE_logError("No parent_link_name given for the sensor."); - return false; - } - sensor.parent_link_name = std::string(parent_link_name_char); - - // parse origin - TiXmlElement *o = config->FirstChildElement("origin"); - if (o) - { - if (!parsePose(sensor.origin, o)) - return false; - } - - // parse sensor - sensor.sensor = parseVisualSensor(config); - return true; -} - - -} - - diff --git a/urdf_parser/src/visual_sensor_parsers.cpp b/urdf_parser/src/visual_sensor_parsers.cpp new file mode 100644 index 00000000..7906161e --- /dev/null +++ b/urdf_parser/src/visual_sensor_parsers.cpp @@ -0,0 +1,116 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* Author: John Hsu */ + +#include "urdf_parser/visual_sensor_parsers.h" +#include "urdf_parser/utils.h" +#include "urdf_parser/pose.h" +#include +#include + +#include + +namespace urdf { + +SensorBase* CameraParser::parse(TiXmlElement &config) +{ + TiXmlElement *image = config.FirstChildElement("image"); + if (image) + { + try { + std::unique_ptr camera(new Camera()); + camera->width = parseAttribute(*image, "width"); + camera->height = parseAttribute(*image, "height"); + camera->format = parseAttribute(*image, "format"); + camera->hfov = parseAttribute(*image, "hfov"); + camera->near = parseAttribute(*image, "near"); + camera->far = parseAttribute(*image, "far"); + return camera.release(); + } + catch (const std::exception &e) + { + CONSOLE_BRIDGE_logError("Camera sensor %s", e.what()); + return nullptr; + } + } + else + { + CONSOLE_BRIDGE_logError("Camera sensor has no element"); + return nullptr; + } +} + + +SensorBase* RayParser::parse(TiXmlElement &config) +{ + TiXmlElement *horizontal = config.FirstChildElement("horizontal"); + if (horizontal) + { + try { + std::unique_ptr ray(new Ray()); + ray->horizontal_samples = parseAttribute(*horizontal, "samples"); + ray->horizontal_resolution = parseAttribute(*horizontal, "resolution"); + ray->horizontal_min_angle = parseAttribute(*horizontal, "min_angle"); + ray->horizontal_max_angle = parseAttribute(*horizontal, "max_angle"); + return ray.release(); + } + catch (const std::exception &e) + { + CONSOLE_BRIDGE_logError("Ray horizontal: %s", e.what()); + return nullptr; + } + } + + TiXmlElement *vertical = config.FirstChildElement("vertical"); + if (vertical) + { + try { + std::unique_ptr ray(new Ray()); + ray->vertical_samples = parseAttribute(*vertical, "samples"); + ray->vertical_resolution = parseAttribute(*vertical, "resolution"); + ray->vertical_min_angle = parseAttribute(*vertical, "min_angle"); + ray->vertical_max_angle = parseAttribute(*vertical, "max_angle"); + return ray.release(); + } + catch (const std::exception &e) + { + CONSOLE_BRIDGE_logError("Ray horizontal: %s", e.what()); + return nullptr; + } + } + return nullptr; +} + +} diff --git a/urdf_parser/test/CMakeLists.txt b/urdf_parser/test/CMakeLists.txt index e845a799..8938390c 100644 --- a/urdf_parser/test/CMakeLists.txt +++ b/urdf_parser/test/CMakeLists.txt @@ -30,18 +30,21 @@ foreach(GTEST_SOURCE_file ${tests}) gtest_main gtest urdfdom_model + urdfdom_sensor ) if (UNIX) target_link_libraries(${BINARY_NAME} pthread) endif() add_test(NAME ${BINARY_NAME} + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} COMMAND ${BINARY_NAME} --gtest_output=xml:${CMAKE_BINARY_DIR}/test_results/${BINARY_NAME}.xml) set_tests_properties(${BINARY_NAME} PROPERTIES TIMEOUT 240 ENVIRONMENT LC_ALL=C) add_test(NAME ${BINARY_NAME}_locale + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} COMMAND ${BINARY_NAME} --gtest_output=xml:${CMAKE_BINARY_DIR}/test_results/${BINARY_NAME}_locale.xml) diff --git a/urdf_parser/test/basic.urdf b/urdf_parser/test/basic.urdf new file mode 100644 index 00000000..7dcf76b1 --- /dev/null +++ b/urdf_parser/test/basic.urdf @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf_parser/test/urdf_unit_test.cpp b/urdf_parser/test/urdf_unit_test.cpp index 8349b1de..b2ae2dd1 100644 --- a/urdf_parser/test/urdf_unit_test.cpp +++ b/urdf_parser/test/urdf_unit_test.cpp @@ -1,11 +1,15 @@ #include #include +#include #include #include #include #include "urdf_model/pose.h" +#include "urdf_sensor/camera.h" +#include "urdf_sensor/ray.h" #include "urdf_parser/urdf_parser.h" +#include "urdf_parser/visual_sensor_parsers.h" #ifndef M_PI # define M_PI 3.141592653589793 @@ -356,3 +360,58 @@ int main(int argc, char **argv) return RUN_ALL_TESTS(); } + +static std::shared_ptr loadFromFile(const std::string &path) +{ + std::shared_ptr xml_doc; + + std::ifstream stream(path.c_str()); + if (!stream) + return xml_doc; + + std::string xml_str((std::istreambuf_iterator(stream)), + std::istreambuf_iterator()); + + xml_doc.reset(new TiXmlDocument()); + xml_doc->Parse(xml_str.c_str()); + + if (xml_doc->Error()) + return std::shared_ptr(); + else + return xml_doc; +} + +TEST(URDF_UNIT_TEST, test_sensor_parsing) +{ + std::shared_ptr xml_doc = loadFromFile("basic.urdf"); + ASSERT_TRUE((bool)xml_doc) << "failed to load basic.urdf"; + + urdf::SensorParserMap parsers; + parsers.insert(std::make_pair("camera", urdf::SensorParserSharedPtr(new urdf::CameraParser))); + parsers.insert(std::make_pair("ray", urdf::SensorParserSharedPtr(new urdf::RayParser))); + + urdf::SensorMap sensors = urdf::parseSensors(*xml_doc, parsers); + + EXPECT_TRUE(!urdf::getSensor("camera1", sensors)); + urdf::CameraSharedPtr camera = urdf::getSensor("camera1", sensors); + ASSERT_TRUE((bool)camera); + EXPECT_EQ(camera->width, 640u); + EXPECT_EQ(camera->height, 480u); + EXPECT_EQ(camera->format, "RGB8"); + EXPECT_EQ(camera->hfov, 1.5708); + EXPECT_EQ(camera->near, 0.01); + EXPECT_EQ(camera->far, 50.0); + + EXPECT_TRUE(!urdf::getSensor("ray1", sensors)); + urdf::RaySharedPtr ray = urdf::getSensor("ray1", sensors); + ASSERT_TRUE((bool)ray); + EXPECT_EQ(ray->horizontal_samples, 100u); + EXPECT_EQ(ray->horizontal_resolution, 1); + EXPECT_EQ(ray->horizontal_min_angle, -1.5708); + EXPECT_EQ(ray->horizontal_max_angle, +1.5708); + + EXPECT_EQ(ray->vertical_samples, 1u); + EXPECT_EQ(ray->vertical_resolution, 1); + EXPECT_EQ(ray->vertical_min_angle, 0); + EXPECT_EQ(ray->vertical_max_angle, 0); +}