Implement tactile sensor parsing w/o plugin

This makes urdf_tactile independent from a custom urdfdom.

rviz_tactile_plugins/src/tactile_state_display.cpp

@@ -34,9 +34,7 @@
 #include <rviz_tactile_plugins/tactile_array_visual.h>
 #include <rviz_tactile_plugins/group_property.h>
 
-#include <urdf/sensor.h>
-#include <urdf_tactile/sensor.h>
-#include <urdf_tactile/cast.h>
+#include <urdf_tactile/parser.h>
 
 #include <rviz/visualization_manager.h>
 #include <rviz/frame_manager.h>
@@ -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_) {

tactile_merger/include/tactile_merger/merger.h

@@ -27,7 +27,7 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 #pragma once
-#include <urdf/sensor.h>
+#include <urdf_tactile/sensor.h>
 #include "taxel_group.h"
 #include <ros/time.h>
 #include <tactile_msgs/TactileContacts.h>
@@ -38,7 +38,6 @@ class Merger
 {
 public:
 	Merger();
-	~Merger();
 
 	void init(const std::string &param = "robot_description");
 
@@ -53,7 +52,6 @@ class Merger
 	struct GroupData;
 	typedef boost::shared_ptr<GroupData> GroupDataPtr;
 
-	urdf::ManagedSensorParserMap parsers_;
 	std::map<std::string, GroupDataPtr> groups_;
 	std::multimap<std::string, std::pair<GroupDataPtr, const TaxelGroup::TaxelMapping *> > sensors_;
 };

tactile_merger/include/tactile_merger/taxel_group.h

@@ -28,7 +28,6 @@
  */
 #pragma once
 #include "taxel.h"
-#include <urdf_parser/sensor_parser.h>
 #include <urdf_tactile/sensor.h>
 #include <tactile_msgs/TactileContact.h>
 #include <Eigen/Dense>
@@ -54,10 +53,10 @@ class TaxelGroup
 	using SensorToTaxelMapping = std::map<std::string, TaxelMapping>;
 
 	/// 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<Taxel> &taxels() const { return taxels_; }

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 &param)
 {
 	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));

tactile_merger/src/taxel_group.cpp

@@ -28,10 +28,11 @@
  */
 #include <tactile_merger/taxel_group.h>
 
-#include <urdf/sensor.h>
 #include <ros/console.h>
 #include <urdf_tactile/taxel_info_iterator.h>
-#include <urdf_tactile/cast.h>
+#include <urdf_tactile/parser.h>
+
+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);
 	}

tactile_pcl/CMakeLists.txt

@@ -9,6 +9,7 @@ find_package(catkin REQUIRED COMPONENTS
 	tactile_msgs
 	pcl_conversions
 	tf2_ros
+	urdf
 	urdf_tactile
 )
 

tactile_pcl/package.xml

@@ -17,6 +17,7 @@
   <depend>tactile_msgs</depend>
   <depend>pcl_conversions</depend>
   <depend>tf2_ros</depend>
+  <depend>urdf</depend>
   <depend>urdf_tactile</depend>
 
   <export>

tactile_pcl/src/pcl_collector.cpp

@@ -2,8 +2,8 @@
 #include "conversions.h"
 
 #include <boost/thread/locks.hpp>
-#include <urdf/sensor.h>
 #include <urdf/model.h>
+#include <urdf_tactile/parser.h>
 
 using namespace tactile_msgs;
 
@@ -48,9 +48,7 @@ void PCLCollector::initFromRobotDescription(const std::string &param)
 			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());
 	}

tactile_pcl/src/pcl_collector.h

@@ -2,7 +2,7 @@
 
 #include <tactile_msgs/TactileState.h>
 #include <tactile_msgs/TactileContacts.h>
-#include <urdf/sensor.h>
+#include <urdf_tactile/sensor.h>
 
 #include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
@@ -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<ContactPoint> pcl_;

tactile_state_publisher/src/tactile_state_publisher.cpp

@@ -7,10 +7,7 @@
  */
 
 #include "tactile_state_publisher.h"
-
-#include <urdf/sensor.h>
 #include <urdf_tactile/sensor.h>
-#include <urdf_tactile/cast.h>
 
 #include <boost/thread/locks.hpp>
 #include <map>
@@ -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

tactile_state_publisher/src/tactile_state_publisher.h

@@ -13,7 +13,7 @@
 #include <boost/thread/shared_mutex.hpp>
 #include <tactile_msgs/TactileState.h>
 #include <sensor_msgs/ChannelFloat32.h>
-#include <urdf_parser/sensor_parser.h>
+#include <urdf_tactile/parser.h>
 
 #include <string>
 #include <vector>
@@ -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();
 
 	/**

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})

urdf_tactile/src/parser.h → urdf_tactile/include/urdf_tactile/parser.h

@@ -36,16 +36,22 @@
 
 #pragma once
 
-#include <urdf_parser/sensor_parser.h>
+#include "sensor.h"
+#include <tinyxml.h>
 
 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 &param);
+SensorMap parseSensorsFromFile(const std::string &filename);
+
 }  // namespace tactile
 }  // namespace urdf