From 83b7de490e9b62ce9c62a33009a2cfd46edb0653 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 16 Mar 2016 23:33:34 +0100 Subject: [PATCH] added tactile sensors --- urdf_parser/CMakeLists.txt | 5 +- .../urdf_parser/tactile_sensor_parsers.h | 51 ++++++ urdf_parser/src/tactile_sensor_parsers.cpp | 168 ++++++++++++++++++ urdf_parser/test/basic.urdf | 25 +++ urdf_parser/test/urdf_unit_test.cpp | 15 ++ 5 files changed, 262 insertions(+), 2 deletions(-) create mode 100644 urdf_parser/include/urdf_parser/tactile_sensor_parsers.h create mode 100644 urdf_parser/src/tactile_sensor_parsers.cpp diff --git a/urdf_parser/CMakeLists.txt b/urdf_parser/CMakeLists.txt index 387af47e..4348fac2 100644 --- a/urdf_parser/CMakeLists.txt +++ b/urdf_parser/CMakeLists.txt @@ -9,8 +9,9 @@ target_link_libraries(urdfdom_model ${TinyXML_LIBRARIES} ${console_bridge_LIBRAR set_target_properties(urdfdom_model PROPERTIES SOVERSION ${URDF_MAJOR_MINOR_VERSION}) add_library(urdfdom_sensor SHARED - src/sensor_parser.cpp src/visual_sensor_parsers.cpp - include/urdf_parser/sensor_parser.h include/urdf_parser/visual_sensor_parsers.h include/urdf_parser/utils.h) + src/sensor_parser.cpp src/visual_sensor_parsers.cpp src/tactile_sensor_parsers.cpp + include/urdf_parser/sensor_parser.h include/urdf_parser/visual_sensor_parsers.h include/urdf_parser/utils.h + include/urdf_parser/tactile_sensor_parsers.h) target_link_libraries(urdfdom_sensor urdfdom_model) set_target_properties(urdfdom_sensor PROPERTIES SOVERSION ${URDF_MAJOR_MINOR_VERSION}) diff --git a/urdf_parser/include/urdf_parser/tactile_sensor_parsers.h b/urdf_parser/include/urdf_parser/tactile_sensor_parsers.h new file mode 100644 index 00000000..e803954c --- /dev/null +++ b/urdf_parser/include/urdf_parser/tactile_sensor_parsers.h @@ -0,0 +1,51 @@ +/********************************************************************* +* 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 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_TACTILE_SENSOR_PARSERS_H +#define URDF_PARSER_TACTILE_SENSOR_PARSERS_H + +#include "sensor_parser.h" + +namespace urdf { + + class URDFDOM_DLLAPI TactileSensorParser : public SensorParser { + public: + SensorBaseSharedPtr parse(TiXmlElement &sensor_element); + }; + +} + +#endif diff --git a/urdf_parser/src/tactile_sensor_parsers.cpp b/urdf_parser/src/tactile_sensor_parsers.cpp new file mode 100644 index 00000000..5fb01d64 --- /dev/null +++ b/urdf_parser/src/tactile_sensor_parsers.cpp @@ -0,0 +1,168 @@ +/********************************************************************* +* 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 author 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 */ + +#include "urdf_parser/tactile_sensor_parsers.h" +#include "urdf_parser/utils.h" +#include "urdf_parser/pose.h" +#include "urdf_parser/link.h" +#include +#include + +namespace urdf { + +/* specialization of parseAttribute(const char* value) for TactileArray::DataOrder */ +template <> +TactileArray::DataOrder parseAttribute(const char* value) +{ + if (strcmp(value, "col-major") == 0) return TactileArray::COLUMNMAJOR; + else if (strcmp(value, "row-major") == 0) return TactileArray::ROWMAJOR; + else throw ParseError("invalid order, expecting 'col-major'' or 'row-major'"); +} + +/* specialization of parseAttribute(const char* value) for Vector2 */ +template <> +Vector2 parseAttribute >(const char* value) +{ + std::vector pieces; + std::vector xy; + boost::split(pieces, value, boost::is_any_of(" ")); + for (unsigned int i = 0; i < pieces.size(); ++i){ + if (pieces[i] != ""){ + xy.push_back(boost::lexical_cast(pieces[i].c_str())); + } + } + + if (xy.size() != 2) + throw ParseError("expecting 2, but found " + boost::lexical_cast(xy.size()) + " elements"); + + return Vector2(xy[0], xy[1]); +} + + +bool parseTactileTaxel(TactileTaxel &taxel, TiXmlElement *config) +{ + taxel.clear(); + + // taxel frame + if (!parsePose(taxel.origin, config)) + return false; + + // Geometry + taxel.geometry = parseGeometry(config->FirstChildElement("geometry")); + if (!taxel.geometry) + return false; + + // Idx + try { + taxel.idx = parseAttribute(*config, "idx"); + } catch (const ParseError &e) { + CONSOLE_BRIDGE_logError(e.what()); + return false; + } + + return true; +} + +bool parseTactileArray(TactileArray &array, TiXmlElement *config) +{ + array.clear(); + try { + array.rows = parseAttribute(*config, "rows"); + array.cols = parseAttribute(*config, "cols"); + + array.size = parseAttribute >(*config, "size"); + array.spacing = parseAttribute >(*config, "spacing", &array.size); + Vector2 origin; + array.offset = parseAttribute >(*config, "offset", &origin); + + TactileArray::DataOrder order = TactileArray::ROWMAJOR; + array.order = parseAttribute(*config, "order", &order); + } + catch (const ParseError &e) { + CONSOLE_BRIDGE_logError(e.what()); + return false; + } + return true; +} + + +SensorBaseSharedPtr TactileSensorParser::parse(TiXmlElement &config) +{ + TactileSensorSharedPtr tactile(new TactileSensor()); + + // multiple Taxels (optional) + for (TiXmlElement* taxel_xml = config.FirstChildElement("taxel"); taxel_xml; taxel_xml = taxel_xml->NextSiblingElement("taxel")) + { + TactileTaxelSharedPtr taxel; + taxel.reset(new TactileTaxel()); + if (parseTactileTaxel(*taxel, taxel_xml)) + { + tactile->taxels_.push_back(taxel); + } + else + { + taxel.reset(); + CONSOLE_BRIDGE_logError("Could not parse taxel element for tactile sensor"); + return TactileSensorSharedPtr(); + } + } + + // a single array (optional) + for (TiXmlElement* array_xml = config.FirstChildElement("array"); array_xml; array_xml = array_xml->NextSiblingElement("array")) + { + if (tactile->array_) + { + CONSOLE_BRIDGE_logWarn("Only a single array element is allowed for a tactile sensor"); + break; // only warn once + } + tactile->array_.reset(new TactileArray()); + if (!parseTactileArray(*tactile->array_, array_xml)) + { + tactile->array_.reset(); + CONSOLE_BRIDGE_logError("Could not parse array element for tactile sensor"); + return TactileSensorSharedPtr(); + } + } + + if (tactile->array_ && tactile->taxels_.size()) + { + CONSOLE_BRIDGE_logWarn("Either an array or multiple taxel elements are allowed for a tactile sensor"); + tactile->array_.reset(); + } + return tactile; +} + +} diff --git a/urdf_parser/test/basic.urdf b/urdf_parser/test/basic.urdf index 7dcf76b1..cc8f14fd 100644 --- a/urdf_parser/test/basic.urdf +++ b/urdf_parser/test/basic.urdf @@ -31,4 +31,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf_parser/test/urdf_unit_test.cpp b/urdf_parser/test/urdf_unit_test.cpp index ea5b0d23..4f984a39 100644 --- a/urdf_parser/test/urdf_unit_test.cpp +++ b/urdf_parser/test/urdf_unit_test.cpp @@ -1,8 +1,10 @@ #include #include #include +#include #include #include +#include #include #include @@ -185,6 +187,7 @@ BOOST_AUTO_TEST_CASE(test_sensor_parsing) 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))); + parsers.insert(std::make_pair("tactile", urdf::SensorParserSharedPtr(new urdf::TactileSensorParser))); urdf::SensorMap sensors = urdf::parseSensors(*xml_doc, parsers); @@ -214,4 +217,16 @@ BOOST_AUTO_TEST_CASE(test_sensor_parsing) BOOST_CHECK_EQUAL(ray->vertical_min_angle, 0); BOOST_CHECK_EQUAL(ray->vertical_max_angle, 0); } + + + urdf::TactileSensorSharedPtr tactile = urdf::getSensor("tactile_taxel_sensor", sensors); + BOOST_REQUIRE(tactile); + BOOST_CHECK(tactile->taxels_.size() == 2); + + tactile = urdf::getSensor("tactile_array_sensor", sensors); + BOOST_REQUIRE(tactile); + BOOST_CHECK(tactile->array_); + BOOST_CHECK(tactile->array_->order == urdf::TactileArray::ROWMAJOR); + BOOST_CHECK(tactile->array_->spacing.x == tactile->array_->size.x); + BOOST_CHECK(tactile->array_->spacing.y == tactile->array_->size.y); }