diff --git a/rviz_tactile_plugins/src/tactile_state_display.cpp b/rviz_tactile_plugins/src/tactile_state_display.cpp index b5c1f66..9d0dc9f 100644 --- a/rviz_tactile_plugins/src/tactile_state_display.cpp +++ b/rviz_tactile_plugins/src/tactile_state_display.cpp @@ -34,9 +34,7 @@ #include #include -#include -#include -#include +#include #include #include @@ -222,14 +220,11 @@ void TactileStateDisplay::onRobotDescriptionChanged() const std::string &tf_prefix = tf_prefix_property_->getStdString(); try { - auto parsers = urdf::getSensorParser("tactile"); - auto sensors = urdf::parseSensorsFromParam(robot_description_property_->getStdString(), parsers); + auto sensors = urdf::tactile::parseSensorsFromParam(robot_description_property_->getStdString()); // create a TactileVisual for each tactile sensor listed in the URDF model for (const auto &sensor : sensors) { - urdf::tactile::TactileSensorConstSharedPtr tactile = urdf::tactile::tactile_sensor_cast(sensor.second); - if (!tactile) - continue; // some other sensor than tactile + const TactileSensorConstSharedPtr &tactile = sensor.second; TactileVisualBase *visual = nullptr; if (tactile->array_) { diff --git a/tactile_merger/include/tactile_merger/merger.h b/tactile_merger/include/tactile_merger/merger.h index 8c77e5e..03c867c 100644 --- a/tactile_merger/include/tactile_merger/merger.h +++ b/tactile_merger/include/tactile_merger/merger.h @@ -27,7 +27,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ #pragma once -#include +#include #include "taxel_group.h" #include #include @@ -38,7 +38,6 @@ class Merger { public: Merger(); - ~Merger(); void init(const std::string ¶m = "robot_description"); @@ -53,7 +52,6 @@ class Merger struct GroupData; typedef boost::shared_ptr GroupDataPtr; - urdf::ManagedSensorParserMap parsers_; std::map groups_; std::multimap > sensors_; }; diff --git a/tactile_merger/include/tactile_merger/taxel_group.h b/tactile_merger/include/tactile_merger/taxel_group.h index 423e427..a4dae47 100644 --- a/tactile_merger/include/tactile_merger/taxel_group.h +++ b/tactile_merger/include/tactile_merger/taxel_group.h @@ -28,7 +28,6 @@ */ #pragma once #include "taxel.h" -#include #include #include #include @@ -54,10 +53,10 @@ class TaxelGroup using SensorToTaxelMapping = std::map; /// load TaxelGroups from robot_description, identified by group name - static TaxelGroupMap load(const std::string &desc_param, const urdf::SensorParserMap &parsers); + static TaxelGroupMap load(const std::string &desc_param); TaxelGroup(const std::string &frame); - void addTaxels(const urdf::SensorConstSharedPtr &sensor); + void addTaxels(const urdf::tactile::TactileSensorConstSharedPtr &sensor); const std::string &frame() const { return frame_; } const std::vector &taxels() const { return taxels_; } diff --git a/tactile_merger/src/merger.cpp b/tactile_merger/src/merger.cpp index f1eff42..ed7dff1 100644 --- a/tactile_merger/src/merger.cpp +++ b/tactile_merger/src/merger.cpp @@ -47,20 +47,13 @@ struct Merger::GroupData Merger::GroupData::GroupData(const TaxelGroupPtr &group) : group(group) {} Merger::Merger() {} -Merger::~Merger() -{ - sensors_.clear(); - groups_.clear(); - parsers_.clear(); -} void Merger::init(const std::string ¶m) { sensors_.clear(); groups_.clear(); - parsers_ = std::move(urdf::getSensorParser("tactile")); - const TaxelGroupMap &groups = TaxelGroup::load(param, parsers_); + const TaxelGroupMap &groups = TaxelGroup::load(param); for (const auto &name_group : groups) { TaxelGroupPtr group = name_group.second; GroupDataPtr data(new GroupData(group)); diff --git a/tactile_merger/src/taxel_group.cpp b/tactile_merger/src/taxel_group.cpp index 81f82c3..9551757 100644 --- a/tactile_merger/src/taxel_group.cpp +++ b/tactile_merger/src/taxel_group.cpp @@ -28,10 +28,11 @@ */ #include -#include #include #include -#include +#include + +using namespace urdf::tactile; namespace tactile { @@ -51,34 +52,28 @@ static TaxelGroupPtr &getGroup(TaxelGroupMap &groups, const std::string &frame) return res.first->second; } -void TaxelGroup::addTaxels(const urdf::SensorConstSharedPtr &sensor) +void TaxelGroup::addTaxels(const TactileSensorConstSharedPtr &sensor) { TaxelGroup::TaxelMapping mapping; - const urdf::tactile::TactileSensor &tactile = urdf::tactile::tactile_sensor_cast(*sensor); - auto taxels = urdf::tactile::TaxelInfoIterable(sensor); + auto taxels = TaxelInfoIterable(sensor); for (const auto &taxel : taxels) { mapping[taxel.idx] = size(); addTaxel(Taxel(taxel.position, taxel.normal)); } - mappings_.insert(std::make_pair(tactile.channel_, mapping)); + mappings_.insert(std::make_pair(sensor->channel_, mapping)); } /** Create a TaxelGroup for each link for which we have a tactile sensor * A TaxelGroup is identified by link name. * A TaxelGroup can hold several tactile sensors if they are attached to the same link. */ -TaxelGroupMap TaxelGroup::load(const std::string &desc_param, const urdf::SensorParserMap &parsers) +TaxelGroupMap TaxelGroup::load(const std::string &desc_param) { TaxelGroupMap result; - urdf::SensorMap sensors = urdf::parseSensorsFromParam(desc_param, parsers); // create a TaxelGroup for each tactile sensor - for (auto &sensor : sensors) { - urdf::tactile::TactileSensorConstSharedPtr tactile = urdf::tactile::tactile_sensor_cast(sensor.second); - if (!tactile) - continue; // some other sensor than tactile - + for (auto &sensor : parseSensorsFromParam(desc_param)) { TaxelGroupPtr &group = getGroup(result, sensor.second->parent_link_); group->addTaxels(sensor.second); } diff --git a/tactile_pcl/CMakeLists.txt b/tactile_pcl/CMakeLists.txt index 391c922..39881ae 100644 --- a/tactile_pcl/CMakeLists.txt +++ b/tactile_pcl/CMakeLists.txt @@ -9,6 +9,7 @@ find_package(catkin REQUIRED COMPONENTS tactile_msgs pcl_conversions tf2_ros + urdf urdf_tactile ) diff --git a/tactile_pcl/package.xml b/tactile_pcl/package.xml index 46aa5d6..0d77f06 100644 --- a/tactile_pcl/package.xml +++ b/tactile_pcl/package.xml @@ -17,6 +17,7 @@ tactile_msgs pcl_conversions tf2_ros + urdf urdf_tactile diff --git a/tactile_pcl/src/pcl_collector.cpp b/tactile_pcl/src/pcl_collector.cpp index 83d35e8..286ee60 100644 --- a/tactile_pcl/src/pcl_collector.cpp +++ b/tactile_pcl/src/pcl_collector.cpp @@ -2,8 +2,8 @@ #include "conversions.h" #include -#include #include +#include using namespace tactile_msgs; @@ -48,9 +48,7 @@ void PCLCollector::initFromRobotDescription(const std::string ¶m) ROS_WARN_STREAM("failed to parse " << param); // fetch sensor descriptions - sensors_.clear(); - parsers_ = urdf::getSensorParser("tactile"); - sensors_ = parseSensors(xml_string, parsers_); + sensors_ = urdf::tactile::parseSensors(xml_string); } catch (const std::exception &e) { ROS_WARN_STREAM("failed to parse robot description:" << e.what()); } diff --git a/tactile_pcl/src/pcl_collector.h b/tactile_pcl/src/pcl_collector.h index 138c8c5..9da882a 100644 --- a/tactile_pcl/src/pcl_collector.h +++ b/tactile_pcl/src/pcl_collector.h @@ -2,7 +2,7 @@ #include #include -#include +#include #include #include @@ -50,8 +50,7 @@ class PCLCollector : public boost::mutex protected: std::string robot_root_frame_; - urdf::ManagedSensorParserMap parsers_; - urdf::SensorMap sensors_; //< tactile sensors + urdf::tactile::SensorMap sensors_; //< tactile sensors std::string target_frame_; //< target frame, the PCL should be expressed in pcl::PointCloud pcl_; diff --git a/tactile_state_publisher/src/tactile_state_publisher.cpp b/tactile_state_publisher/src/tactile_state_publisher.cpp index 9f48447..c55f7c7 100644 --- a/tactile_state_publisher/src/tactile_state_publisher.cpp +++ b/tactile_state_publisher/src/tactile_state_publisher.cpp @@ -7,10 +7,7 @@ */ #include "tactile_state_publisher.h" - -#include #include -#include #include #include @@ -36,8 +33,7 @@ void TactileStatePublisher::config() Due to a bug in pluginlib, the unloading of the lib might throw on destruction of SensorParserMap. */ try { - auto parsers = urdf::getSensorParser("tactile"); - createSensorDataMap(urdf::parseSensorsFromParam("robot_description", parsers)); + createSensorDataMap(parseSensorsFromParam("robot_description")); } catch (const std::exception &e) { ROS_ERROR_STREAM(e.what()); return; @@ -67,14 +63,11 @@ void TactileStatePublisher::config() } } -void TactileStatePublisher::createSensorDataMap(const urdf::SensorMap &sensors) +void TactileStatePublisher::createSensorDataMap(const SensorMap &sensors) { // loop over all the sensor found in the URDF - for (const auto &sensor : sensors) { - TactileSensorSharedPtr tactile = tactile_sensor_cast(sensor.second); - if (!tactile) - continue; // some other sensor than tactile - + for (const auto &item : sensors) { + const TactileSensorSharedPtr &tactile = item.second; int sensor_idx = -1; // find if the channel exists in the map diff --git a/tactile_state_publisher/src/tactile_state_publisher.h b/tactile_state_publisher/src/tactile_state_publisher.h index 2c707de..087b1b5 100644 --- a/tactile_state_publisher/src/tactile_state_publisher.h +++ b/tactile_state_publisher/src/tactile_state_publisher.h @@ -13,7 +13,7 @@ #include #include #include -#include +#include #include #include @@ -49,7 +49,7 @@ class TactileStatePublisher * initiliaze subscribers and publisher */ void init(); - void createSensorDataMap(const urdf::SensorMap& sensors); + void createSensorDataMap(const urdf::tactile::SensorMap& sensors); void config(); /** diff --git a/urdf_tactile/CMakeLists.txt b/urdf_tactile/CMakeLists.txt index 5d8e9a8..71fa679 100644 --- a/urdf_tactile/CMakeLists.txt +++ b/urdf_tactile/CMakeLists.txt @@ -7,7 +7,9 @@ set(CMAKE_CXX_STANDARD 14) ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(Boost REQUIRED system) -find_package(catkin REQUIRED pluginlib urdf) +find_package(urdfdom REQUIRED) +find_package(catkin REQUIRED COMPONENTS roscpp cmake_modules) +find_package(TinyXML REQUIRED) ################################### ## catkin specific configuration ## @@ -20,10 +22,9 @@ find_package(catkin REQUIRED pluginlib urdf) ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( INCLUDE_DIRS include - # Don't export the plugin-lib! This should be loaded dynamically only. - LIBRARIES ${PROJECT_NAME}_tools - CATKIN_DEPENDS urdf - DEPENDS Boost + LIBRARIES ${PROJECT_NAME} ${PROJECT_NAME}_tools + CATKIN_DEPENDS roscpp + DEPENDS urdfdom Boost TinyXML ) include_directories(SYSTEM ${Boost_INCLUDE_DIR}) @@ -42,10 +43,6 @@ endif() # all install targets should use catkin DESTINATION variables # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html -install(FILES - plugin_description.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) diff --git a/urdf_tactile/include/urdf_tactile/cast.h b/urdf_tactile/include/urdf_tactile/cast.h deleted file mode 100644 index 87dd313..0000000 --- a/urdf_tactile/include/urdf_tactile/cast.h +++ /dev/null @@ -1,73 +0,0 @@ -/* - * Copyright (C) 2016, Bielefeld University, CITEC - * 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, Inc. 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. - */ -#pragma once - -#include - -namespace urdf { -namespace tactile { - -inline const urdf::tactile::TactileSensor* tactile_sensor_cast(const urdf::SensorBase* sensor) -{ - return dynamic_cast(sensor); -} -inline urdf::tactile::TactileSensor* tactile_sensor_cast(urdf::SensorBase* sensor) -{ - return dynamic_cast(sensor); -} - -inline const urdf::tactile::TactileSensor& tactile_sensor_cast(const urdf::Sensor& sensor) -{ - return dynamic_cast(*sensor.sensor_); -} -inline urdf::tactile::TactileSensor& tactile_sensor_cast(urdf::Sensor& sensor) -{ - return dynamic_cast(*sensor.sensor_); -} - -inline TactileSensorSharedPtr tactile_sensor_cast(const urdf::SensorBaseSharedPtr& sensor) -{ - return urdf::dynamic_pointer_cast(sensor); -} -inline TactileSensorConstSharedPtr tactile_sensor_cast(const urdf::SensorBaseConstSharedPtr& sensor) -{ - return urdf::dynamic_pointer_cast(sensor); -} - -inline TactileSensorSharedPtr tactile_sensor_cast(const urdf::SensorSharedPtr& sensor) -{ - return tactile_sensor_cast(sensor->sensor_); -} -inline TactileSensorConstSharedPtr tactile_sensor_cast(const urdf::SensorConstSharedPtr& sensor) -{ - return tactile_sensor_cast(sensor->sensor_); -} - -} // end namespace tactile -} // end namespace urdf diff --git a/urdf_tactile/src/parser.h b/urdf_tactile/include/urdf_tactile/parser.h similarity index 85% rename from urdf_tactile/src/parser.h rename to urdf_tactile/include/urdf_tactile/parser.h index 004534a..94d8090 100644 --- a/urdf_tactile/src/parser.h +++ b/urdf_tactile/include/urdf_tactile/parser.h @@ -36,16 +36,22 @@ #pragma once -#include +#include "sensor.h" +#include namespace urdf { namespace tactile { -class URDFDOM_DLLAPI TactileSensorParser : public urdf::SensorParser +class TactileSensorParser { public: - SensorBase* parse(TiXmlElement& sensor_element) override; + TactileSensor *parse(TiXmlElement &sensor_element) const; }; +SensorMap parseSensors(TiXmlDocument &urdf_xml); +SensorMap parseSensors(const std::string &xml); +SensorMap parseSensorsFromParam(const std::string ¶m); +SensorMap parseSensorsFromFile(const std::string &filename); + } // namespace tactile } // namespace urdf diff --git a/urdf_tactile/include/urdf_tactile/sensor.h b/urdf_tactile/include/urdf_tactile/sensor.h index 75da427..acb0d4d 100644 --- a/urdf_tactile/include/urdf_tactile/sensor.h +++ b/urdf_tactile/include/urdf_tactile/sensor.h @@ -36,12 +36,12 @@ #pragma once -#include #include #include #include #include #include +#include namespace urdf { namespace tactile { @@ -119,17 +119,26 @@ class TactileArray using TactileTaxelSharedPtr = std::shared_ptr; using TactileArraySharedPtr = std::shared_ptr; -class TactileSensor : public urdf::SensorBase +class TactileSensor { public: TactileSensor() { TactileSensor::clear(); } + + std::string name_; //< sensor name must be unique + double update_rate_; //< update rate in Hz + std::string parent_link_; //< name of parent link this sensor is attached to + Pose origin_; //< transform from parent frame sensor frame + std::vector taxels_; TactileArraySharedPtr array_; std::string channel_; std::string group_; - void clear() override + void clear() { + name_.clear(); + parent_link_.clear(); + origin_.clear(); taxels_.clear(); array_.reset(); channel_.clear(); @@ -138,5 +147,7 @@ class TactileSensor : public urdf::SensorBase }; URDF_TYPEDEF_CLASS_POINTER(TactileSensor); +using SensorMap = std::map; + } // namespace tactile } // namespace urdf diff --git a/urdf_tactile/include/urdf_tactile/sort.h b/urdf_tactile/include/urdf_tactile/sort.h index 453c9aa..ce6b85c 100644 --- a/urdf_tactile/include/urdf_tactile/sort.h +++ b/urdf_tactile/include/urdf_tactile/sort.h @@ -28,7 +28,7 @@ */ #pragma once -#include +#include #include #include @@ -38,7 +38,7 @@ namespace tactile { /****************************************************************************** * sort sensors by group or channel name ******************************************************************************/ -typedef urdf::SensorMap::const_iterator iterator; +using iterator = urdf::tactile::SensorMap::const_iterator; using iterator_list = std::deque; struct Sensors { @@ -55,9 +55,9 @@ struct SensorsTree : Sensors map_type children; }; -SensorsMap sortByGroups(const urdf::SensorMap &sensors); -SensorsTree sortByGroupsHierarchical(const urdf::SensorMap &sensors); -SensorsMap sortByChannels(const urdf::SensorMap &sensors); +SensorsMap sortByGroups(const SensorMap &sensors); +SensorsTree sortByGroupsHierarchical(const SensorMap &sensors); +SensorsMap sortByChannels(const SensorMap &sensors); /****************************************************************************** * retrieving taxels diff --git a/urdf_tactile/include/urdf_tactile/taxel_info_iterator.h b/urdf_tactile/include/urdf_tactile/taxel_info_iterator.h index 37e7e22..d8b2d82 100644 --- a/urdf_tactile/include/urdf_tactile/taxel_info_iterator.h +++ b/urdf_tactile/include/urdf_tactile/taxel_info_iterator.h @@ -85,10 +85,10 @@ class TaxelInfoIterator class TaxelInfoIterable { - const urdf::SensorConstSharedPtr sensor_; + const TactileSensorConstSharedPtr sensor_; public: - TaxelInfoIterable(const urdf::SensorConstSharedPtr& sensor) : sensor_(sensor) {} + TaxelInfoIterable(const TactileSensorConstSharedPtr& sensor) : sensor_(sensor) {} TaxelInfoIterator begin(); TaxelInfoIterator end(); }; diff --git a/urdf_tactile/package.xml b/urdf_tactile/package.xml index 165b989..2389073 100644 --- a/urdf_tactile/package.xml +++ b/urdf_tactile/package.xml @@ -11,13 +11,11 @@ catkin - urdf + cmake_modules urdfdom - pluginlib + roscpp - - diff --git a/urdf_tactile/plugin_description.xml b/urdf_tactile/plugin_description.xml deleted file mode 100644 index f9c6fb3..0000000 --- a/urdf_tactile/plugin_description.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - parse tactile elements in URDF - - - diff --git a/urdf_tactile/src/CMakeLists.txt b/urdf_tactile/src/CMakeLists.txt index c7bf2ef..d8d0e65 100644 --- a/urdf_tactile/src/CMakeLists.txt +++ b/urdf_tactile/src/CMakeLists.txt @@ -1,8 +1,9 @@ add_library(${PROJECT_NAME} SHARED parser.cpp + utils.cpp ${PROJECT_INCLUDES} ) -target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES} ${TinyXML_LIBRARIES} ${urdfdom_LIBRARIES} ${catkin_LIBRARIES}) add_library(${PROJECT_NAME}_tools SHARED taxel_info_iterator.cpp diff --git a/urdf_tactile/src/parser.cpp b/urdf_tactile/src/parser.cpp index 93c6941..d0d3d54 100644 --- a/urdf_tactile/src/parser.cpp +++ b/urdf_tactile/src/parser.cpp @@ -34,16 +34,17 @@ /* Author: Robert Haschke */ -#include "parser.h" -#include "urdf_tactile/sensor.h" -#include -#include -#include +#include "urdf_tactile/parser.h" +#include "utils.h" +#include #include #include #include +#include +#include namespace urdf { +namespace tactile { /* specialization of parseAttribute(const char* value) for TactileArray::DataOrder */ template <> @@ -76,8 +77,6 @@ tactile::Vector2 parseAttribute >(const char *v return tactile::Vector2(xy[0], xy[1]); } -namespace tactile { - bool parseTactileTaxel(TactileTaxel &taxel, TiXmlElement *config) { taxel.clear(); @@ -87,7 +86,7 @@ bool parseTactileTaxel(TactileTaxel &taxel, TiXmlElement *config) return false; // Geometry - taxel.geometry = urdf::parseGeometry(config->FirstChildElement("geometry")); + taxel.geometry = parseGeometry(config->FirstChildElement("geometry")); if (!taxel.geometry) return false; @@ -123,9 +122,22 @@ bool parseTactileArray(TactileArray &array, TiXmlElement *config) return true; } -SensorBase *TactileSensorParser::parse(TiXmlElement &config) +TactileSensor *TactileSensorParser::parse(TiXmlElement &config) const { + TiXmlElement *parent = config.Parent()->ToElement()->FirstChildElement("parent"); + if (!parent) { + CONSOLE_BRIDGE_logError("No tag given for the sensor."); + return nullptr; + } + auto tactile = std::make_unique(); + tactile->name_ = parseAttribute(*config.Parent()->ToElement(), "name"); + tactile->parent_link_ = parseAttribute(*parent, "link"); + if (TiXmlElement *o = config.Parent()->ToElement()->FirstChildElement("origin")) { + if (!parsePose(tactile->origin_, o)) + return nullptr; + } + tactile->channel_ = parseAttribute(config, "channel"); tactile->group_ = parseAttribute(*config.Parent()->ToElement(), "group"); @@ -163,8 +175,72 @@ SensorBase *TactileSensorParser::parse(TiXmlElement &config) return tactile.release(); } +SensorMap parseSensorsFromFile(const std::string &filename) +{ + SensorMap result; + std::ifstream stream(filename.c_str()); + if (!stream.is_open()) { + throw std::runtime_error("Could not open file [" + filename + "] for parsing."); + } + + std::string xml_string((std::istreambuf_iterator(stream)), std::istreambuf_iterator()); + return parseSensors(xml_string); +} + +SensorMap parseSensorsFromParam(const std::string ¶m) +{ + ros::NodeHandle nh; + std::string xml_string; + + // gets the location of the robot description on the parameter server + std::string full_param; + if (!nh.searchParam(param, full_param)) { + throw std::runtime_error("Could not find parameter " + param + " on parameter server"); + } + + // read the robot description from the parameter server + if (!nh.getParam(full_param, xml_string)) { + throw std::runtime_error("Could not read parameter " + param + " on parameter server"); + } + return parseSensors(xml_string); +} + +SensorMap parseSensors(const std::string &xml_string) +{ + TiXmlDocument xml_doc; + xml_doc.Parse(xml_string.c_str()); + if (xml_doc.Error()) + throw std::runtime_error(std::string("Could not parse the xml document: ") + xml_doc.ErrorDesc()); + return parseSensors(xml_doc); +} + +SensorMap parseSensors(TiXmlDocument &urdf_xml) +{ + TiXmlElement *robot_xml = urdf_xml.FirstChildElement("robot"); + if (!robot_xml) { + CONSOLE_BRIDGE_logError("Could not find the 'robot' element in the URDF"); + return SensorMap(); + } + + TactileSensorParser parser; + SensorMap results; + // Get all sensor elements + for (TiXmlElement *sensor_xml = robot_xml->FirstChildElement("sensor"); sensor_xml; + sensor_xml = sensor_xml->NextSiblingElement("sensor")) { + if (TiXmlElement *tactile_xml = sensor_xml->FirstChildElement("tactile")) { + if (TactileSensor *sensor = parser.parse(*tactile_xml)) { + auto res = results.insert(make_pair(sensor->name_, sensor)); + if (!res.second) + CONSOLE_BRIDGE_logWarn("Sensor '%s' is not unique. Ignoring consecutive ones.", sensor->name_.c_str()); + else + 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; +} + } // namespace tactile } // namespace urdf - -#include -PLUGINLIB_EXPORT_CLASS(urdf::tactile::TactileSensorParser, urdf::SensorParser) diff --git a/urdf_tactile/src/sort.cpp b/urdf_tactile/src/sort.cpp index f6a269d..6e1c440 100644 --- a/urdf_tactile/src/sort.cpp +++ b/urdf_tactile/src/sort.cpp @@ -31,8 +31,6 @@ #include #include -#include -#include #include #include @@ -47,16 +45,16 @@ enum SortKey BY_CHANNEL }; template -const std::string &get(const urdf::SensorConstSharedPtr &sensor); +const std::string &get(const TactileSensorConstSharedPtr &sensor); template <> -const std::string &get(const urdf::SensorConstSharedPtr &sensor) +const std::string &get(const TactileSensorConstSharedPtr &sensor) { - return tactile_sensor_cast(*sensor).group_; + return sensor->group_; } template <> -const std::string &get(const urdf::SensorConstSharedPtr &sensor) +const std::string &get(const TactileSensorConstSharedPtr &sensor) { - return tactile_sensor_cast(*sensor).channel_; + return sensor->channel_; } template @@ -84,14 +82,10 @@ Result sort(const SensorMap &sensors) Result result; for (auto it = sensors.begin(), end = sensors.end(); it != end; ++it) { - TactileSensorSharedPtr tactile = tactile_sensor_cast(it->second); - if (!tactile) - continue; // some other sensor than tactile - Sensors &g = getOrInsertEntry(result, get(it->second)); - if (tactile->array_) + if (it->second->array_) g.arrays.push_back(it); - else if (!tactile->taxels_.empty()) + else if (!it->second->taxels_.empty()) g.taxels.push_back(it); } return result; diff --git a/urdf_tactile/src/taxel_info_iterator.cpp b/urdf_tactile/src/taxel_info_iterator.cpp index ecc0267..41c158f 100644 --- a/urdf_tactile/src/taxel_info_iterator.cpp +++ b/urdf_tactile/src/taxel_info_iterator.cpp @@ -30,7 +30,6 @@ /* Author: Robert Haschke */ #include -#include #include namespace urdf { @@ -57,7 +56,7 @@ urdf::Pose compose(const urdf::Pose &a, const urdf::Pose &b) class TaxelInfoIteratorI : public std::iterator { public: - TaxelInfoIteratorI(const urdf::SensorConstSharedPtr &sensor) : sensor(sensor) {} + TaxelInfoIteratorI(const TactileSensorConstSharedPtr &sensor) : sensor(sensor) {} virtual TaxelInfoIteratorIPtr clone() const = 0; virtual ~TaxelInfoIteratorI() {} @@ -71,16 +70,15 @@ class TaxelInfoIteratorI : public std::iteratorparent_link_; - const auto &ts = tactile_sensor_cast(*sensor); - info.group = ts.group_; - info.channel = ts.channel_; + info.group = sensor->group_; + info.channel = sensor->channel_; } void TaxelInfoIteratorI::finishInfo(TaxelInfo &info) @@ -104,7 +102,7 @@ class TaxelInfoIteratorBase : public TaxelInfoIteratorI public: typedef Iterator iterator; - explicit TaxelInfoIteratorBase(const urdf::SensorConstSharedPtr &sensor, Iterator it) + explicit TaxelInfoIteratorBase(const TactileSensorConstSharedPtr &sensor, Iterator it) : TaxelInfoIteratorI(sensor), it(it) {} @@ -175,7 +173,7 @@ void TaxelInfoIteratorBase::initInfo(TaxelInfo &info) info.geometry_origin = sensor->origin_; info.taxel_origin.rotation = info.geometry_origin.rotation; - const TactileArray &array = *tactile_sensor_cast(*sensor).array_; + const TactileArray &array = *sensor->array_; urdf::Box *box = new urdf::Box(); box->dim = urdf::Vector3(array.size.x, array.size.y, 0); info.geometry.reset(box); @@ -184,7 +182,7 @@ void TaxelInfoIteratorBase::initInfo(TaxelInfo &info) template <> void TaxelInfoIteratorBase::updateInfo(TaxelInfo &info) { - const TactileArray &array = *tactile_sensor_cast(*sensor).array_; + const TactileArray &array = *sensor->array_; info.idx = it; @@ -290,28 +288,26 @@ void TaxelInfoIterator::validate() const TaxelInfoIterator TaxelInfoIterable::begin() { - const TactileSensor &tactile = tactile_sensor_cast(*sensor_); TaxelInfoIteratorIPtr impl; bool valid = true; - if (tactile.array_) + if (sensor_->array_) impl.reset(new TaxelInfoIteratorBase(sensor_, 0)); else { - impl.reset(new TaxelInfoIteratorBase(sensor_, tactile.taxels_.begin())); - valid = (!tactile.taxels_.empty()); + impl.reset(new TaxelInfoIteratorBase(sensor_, sensor_->taxels_.begin())); + valid = (!sensor_->taxels_.empty()); } return TaxelInfoIterator(impl, valid); } TaxelInfoIterator TaxelInfoIterable::end() { - const TactileSensor &tactile = tactile_sensor_cast(*sensor_); TaxelInfoIteratorIPtr impl; - if (tactile.array_) - impl.reset(new TaxelInfoIteratorBase(sensor_, tactile.array_->rows * tactile.array_->cols)); + if (sensor_->array_) + impl.reset(new TaxelInfoIteratorBase(sensor_, sensor_->array_->rows * sensor_->array_->cols)); else - impl.reset(new TaxelInfoIteratorBase(sensor_, tactile.taxels_.end())); + impl.reset(new TaxelInfoIteratorBase(sensor_, sensor_->taxels_.end())); return TaxelInfoIterator(impl, false); } diff --git a/urdf_tactile/src/utils.cpp b/urdf_tactile/src/utils.cpp new file mode 100644 index 0000000..a363c36 --- /dev/null +++ b/urdf_tactile/src/utils.cpp @@ -0,0 +1,188 @@ +/********************************************************************* + * 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: Wim Meeussen, functions copied from urdfdom/urdf_parser/link.cpp */ + +#include "utils.h" +#include +#include + +namespace { +using namespace urdf; + +bool parseSphere(Sphere &s, TiXmlElement *c) +{ + s.clear(); + + s.type = Geometry::SPHERE; + if (!c->Attribute("radius")) { + CONSOLE_BRIDGE_logError("Sphere shape must have a radius attribute"); + return false; + } + + try { + s.radius = strToDouble(c->Attribute("radius")); + } catch (std::runtime_error &) { + std::stringstream stm; + stm << "radius [" << c->Attribute("radius") << "] is not a valid float"; + CONSOLE_BRIDGE_logError(stm.str().c_str()); + return false; + } + + return true; +} + +bool parseBox(Box &b, TiXmlElement *c) +{ + b.clear(); + + b.type = Geometry::BOX; + if (!c->Attribute("size")) { + CONSOLE_BRIDGE_logError("Box shape has no size attribute"); + return false; + } + try { + b.dim.init(c->Attribute("size")); + } catch (ParseError &e) { + b.dim.clear(); + CONSOLE_BRIDGE_logError(e.what()); + return false; + } + return true; +} + +bool parseCylinder(Cylinder &y, TiXmlElement *c) +{ + y.clear(); + + y.type = Geometry::CYLINDER; + if (!c->Attribute("length") || !c->Attribute("radius")) { + CONSOLE_BRIDGE_logError("Cylinder shape must have both length and radius attributes"); + return false; + } + + try { + y.length = strToDouble(c->Attribute("length")); + } catch (std::runtime_error &) { + std::stringstream stm; + stm << "length [" << c->Attribute("length") << "] is not a valid float"; + CONSOLE_BRIDGE_logError(stm.str().c_str()); + return false; + } + + try { + y.radius = strToDouble(c->Attribute("radius")); + } catch (std::runtime_error &) { + std::stringstream stm; + stm << "radius [" << c->Attribute("radius") << "] is not a valid float"; + CONSOLE_BRIDGE_logError(stm.str().c_str()); + return false; + } + + return true; +} + +bool parseMesh(Mesh &m, TiXmlElement *c) +{ + m.clear(); + + m.type = Geometry::MESH; + if (!c->Attribute("filename")) { + CONSOLE_BRIDGE_logError("Mesh must contain a filename attribute"); + return false; + } + + m.filename = c->Attribute("filename"); + + if (c->Attribute("scale")) { + try { + m.scale.init(c->Attribute("scale")); + } catch (ParseError &e) { + m.scale.clear(); + CONSOLE_BRIDGE_logError("Mesh scale was specified, but could not be parsed: %s", e.what()); + return false; + } + } else { + m.scale.x = m.scale.y = m.scale.z = 1; + } + return true; +} +} // namespace + +namespace urdf { +namespace tactile { + +GeometrySharedPtr parseGeometry(TiXmlElement *g) +{ + GeometrySharedPtr geom; + if (!g) + return geom; + + TiXmlElement *shape = g->FirstChildElement(); + if (!shape) { + CONSOLE_BRIDGE_logError("Geometry tag contains no child element."); + return geom; + } + + std::string type_name = shape->ValueStr(); + if (type_name == "sphere") { + Sphere *s = new Sphere(); + geom.reset(s); + if (parseSphere(*s, shape)) + return geom; + } else if (type_name == "box") { + Box *b = new Box(); + geom.reset(b); + if (parseBox(*b, shape)) + return geom; + } else if (type_name == "cylinder") { + Cylinder *c = new Cylinder(); + geom.reset(c); + if (parseCylinder(*c, shape)) + return geom; + } else if (type_name == "mesh") { + Mesh *m = new Mesh(); + geom.reset(m); + if (parseMesh(*m, shape)) + return geom; + } else { + CONSOLE_BRIDGE_logError("Unknown geometry type '%s'", type_name.c_str()); + return geom; + } + + return GeometrySharedPtr(); +} + +} // namespace tactile +} // namespace urdf diff --git a/urdf_tactile/src/utils.h b/urdf_tactile/src/utils.h new file mode 100644 index 0000000..9c8e0b4 --- /dev/null +++ b/urdf_tactile/src/utils.h @@ -0,0 +1,91 @@ + +/********************************************************************* + * 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 */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace urdf { +namespace tactile { + +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 = nullptr) +{ + 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()); + } +} + +urdf::GeometrySharedPtr parseGeometry(TiXmlElement* g); + +} // namespace tactile +} // namespace urdf diff --git a/urdf_tactile/test/parser.cpp b/urdf_tactile/test/parser.cpp index 3cf6261..98921e1 100644 --- a/urdf_tactile/test/parser.cpp +++ b/urdf_tactile/test/parser.cpp @@ -1,6 +1,4 @@ -#include "urdf_tactile/sensor.h" -#include "urdf_tactile/cast.h" -#include +#include "urdf_tactile/parser.h" #include using namespace urdf::tactile; @@ -25,13 +23,13 @@ static std::shared_ptr loadFromFile(const std::string &path) return xml_doc; } -void check_sensor_map(const urdf::SensorMap &sensors) +void check_sensor_map(const SensorMap &sensors) { - TactileSensorSharedPtr tactile = urdf::getSensor("tactile_taxel_sensor", sensors); + TactileSensorSharedPtr tactile = sensors.at("tactile_taxel_sensor"); BOOST_REQUIRE(tactile); BOOST_CHECK(tactile->taxels_.size() == 2); - tactile = urdf::getSensor("tactile_array_sensor", sensors); + tactile = sensors.at("tactile_array_sensor"); BOOST_REQUIRE(tactile); BOOST_CHECK(tactile->array_); BOOST_CHECK(tactile->array_->order == TactileArray::ROWMAJOR); @@ -41,31 +39,29 @@ void check_sensor_map(const urdf::SensorMap &sensors) BOOST_AUTO_TEST_CASE(test_pluginlib_parsing) { - auto parsers = urdf::getSensorParser("tactile"); - check_sensor_map(urdf::parseSensorsFromFile("tactile.urdf", parsers)); + check_sensor_map(parseSensorsFromFile("tactile.urdf")); } -void change_and_check_order_mode(const urdf::SensorParserSharedPtr &parser, TiXmlElement &tactile_xml, - const char *pcMode, TactileArray::DataOrder mode) +void change_and_check_order_mode(const TactileSensorParser &parser, TiXmlElement &tactile_xml, const char *pcMode, + TactileArray::DataOrder mode) { TiXmlElement *array_xml = tactile_xml.FirstChildElement("array"); array_xml->SetAttribute("order", pcMode); - TactileSensorSharedPtr tactile(tactile_sensor_cast(parser->parse(tactile_xml))); + TactileSensorSharedPtr tactile(parser.parse(tactile_xml)); BOOST_REQUIRE(tactile); BOOST_CHECK(tactile->array_->order == mode); } BOOST_AUTO_TEST_CASE(test_tactile_array) { - auto parsers = urdf::getSensorParser("tactile"); + TactileSensorParser parser; std::shared_ptr root = loadFromFile("tactile.urdf"); BOOST_REQUIRE(root); TiXmlElement *tactile_xml = root->RootElement()->FirstChildElement("sensor")->FirstChildElement("tactile"); BOOST_REQUIRE(tactile_xml); - urdf::SensorParserSharedPtr parser = parsers["tactile"]; - TactileSensorSharedPtr tactile(tactile_sensor_cast(parser->parse(*tactile_xml))); + TactileSensorSharedPtr tactile(parser.parse(*tactile_xml)); BOOST_REQUIRE(tactile); BOOST_CHECK(tactile->taxels_.empty()); @@ -79,14 +75,14 @@ BOOST_AUTO_TEST_CASE(test_tactile_array) // missing spacing attribute defaults to size array_xml->RemoveAttribute("spacing"); - tactile.reset(tactile_sensor_cast(parser->parse(*tactile_xml))); + tactile.reset(parser.parse(*tactile_xml)); BOOST_REQUIRE(tactile); BOOST_CHECK(tactile->array_->spacing.x == tactile->array_->size.x); BOOST_CHECK(tactile->array_->spacing.y == tactile->array_->size.y); // missing offset attribute defaults to zero array_xml->RemoveAttribute("offset"); - tactile.reset(tactile_sensor_cast(parser->parse(*tactile_xml))); + tactile.reset(parser.parse(*tactile_xml)); BOOST_REQUIRE(tactile); BOOST_CHECK(tactile->array_->offset.x == 0); BOOST_CHECK(tactile->array_->offset.y == 0); diff --git a/urdf_tactile/test/tools.cpp b/urdf_tactile/test/tools.cpp index 2e76c2a..c5cd703 100644 --- a/urdf_tactile/test/tools.cpp +++ b/urdf_tactile/test/tools.cpp @@ -13,25 +13,22 @@ using namespace urdf::tactile; #include -urdf::SensorSharedPtr create_array(unsigned int rows, unsigned int cols, - TactileArray::DataOrder order = TactileArray::ROWMAJOR) +TactileSensorSharedPtr create_array(unsigned int rows, unsigned int cols, + TactileArray::DataOrder order = TactileArray::ROWMAJOR) { - urdf::SensorSharedPtr s(new urdf::Sensor); + auto s = std::make_shared(); s->name_ = "array"; s->parent_link_ = "link"; + s->channel_ = "channel"; + s->group_ = "array"; - TactileSensorSharedPtr tactile(new TactileSensor); - s->sensor_ = tactile; - tactile->channel_ = "channel"; - tactile->group_ = "array"; - - TactileArraySharedPtr array(new TactileArray); + auto array = std::make_shared(); array->order = order; array->rows = rows; array->cols = cols; array->size = urdf::tactile::Vector2(0.5, 0.5); array->spacing = urdf::tactile::Vector2(1, 1); - tactile->array_ = array; + s->array_ = array; return s; } @@ -77,22 +74,17 @@ BOOST_AUTO_TEST_CASE(test_array_iterator_colmajor) test_array_iterator(TactileArray::ROWMAJOR); } -urdf::SensorSharedPtr create_taxels(unsigned int num = 10) +TactileSensorSharedPtr create_taxels(unsigned int num = 10) { - urdf::SensorSharedPtr s(new urdf::Sensor); + auto s = std::make_shared(); s->name_ = "taxels"; s->parent_link_ = "link"; + s->channel_ = "channel"; + s->group_ = "taxels"; - TactileSensorSharedPtr tactile(new TactileSensor); - s->sensor_ = tactile; - tactile->channel_ = "channel"; - tactile->group_ = "taxels"; - - TactileTaxelSharedPtr taxel; for (unsigned int i = 0; i < num; ++i) { - taxel.reset(new TactileTaxel); - taxel->idx = i; - tactile->taxels_.push_back(taxel); + s->taxels_.emplace_back(std::make_shared()); + s->taxels_.back()->idx = i; } return s; } @@ -111,7 +103,7 @@ BOOST_AUTO_TEST_CASE(test_vector_iterator) void test_grouping(unsigned int taxels, unsigned int rows, unsigned int cols) { - urdf::SensorMap sensors; + SensorMap sensors; sensors["taxels"] = create_taxels(taxels); sensors["array"] = create_array(rows, cols); auto groups = sortByGroups(sensors);