Merge branch urdfdom-independence into obese-devel

.github/workflows/ci.yaml

@@ -33,8 +33,6 @@ jobs:
       CACHE_PREFIX: ${{ matrix.distro }}
       ROS_DISTRO: ${{ matrix.distro }}
       UPSTREAM_WORKSPACE: rosinstall
-      ROSDEP_SKIP_KEYS: tinyxml_vendor console_bridge_vendor  # *_vendor packages are ROS2 specific
-      CATKIN_LINT_ARGS: --config src/tactile_toolbox/.catkin_lint
 
     name: "${{ matrix.distro }}${{ matrix.env.CLANG_TIDY && (github.event_name != 'workflow_dispatch' && ' • clang-tidy (delta)' || ' • clang-tidy (all)') || '' }}"
     runs-on: ubuntu-latest

.pre-commit-config.yaml

@@ -49,7 +49,7 @@ repos:
         name: catkin_lint
         description: Check package.xml and cmake files
         entry: catkin_lint
-        args: [ "--config", ".catkin_lint", "." ]
+        args: [ "." ]
         language: system
         always_run: true
         pass_filenames: false

README.md

@@ -8,7 +8,7 @@ This software package adds tactile messages, a tactile sensor description to URD
   * `tactile_bias`: Re-publish [TactileState msgs](tactile_msgs/msg/TactileState.msg) allowing to reset tactile bias on demand (tare)
 * [tactile_calibration](tactile_calibration)
   * `tactile_state_calibrator`: maps raw tactile values to calibrated ones
-* [urdf_tactile](urdf_tactile/README.md): extension to URDF to describe tactile sensors, requires modified [`urdf`](https://github.com/ubi-agni/urdf) and [`urdfdom`](https://github.com/ubi-agni/urdfdom) packages.
+* [urdf_tactile](urdf_tactile/README.md): URDF parser to read tactile `<sensor>` tags
 * [tactile_merger](tactile_merger): Map [`tactile_msgs/TactileState`](tactile_msgs/msg/TactileState.msg) onto [`tactile_msgs/TactileContacts`](tactile_msgs/msg/TactileContacts.msg), i.e. contact position, force, and normal, merging contact data of all taxels per link into a single wrench vector
 * [tactile_pcl](tactile_pcl): Compute and publish Tactile Point Cloud data from [`tactile_msgs/TactileContacts`](tactile_msgs/msg/TactileState.msg).
 * [rviz_tactile_plugins](rviz_tactile_plugins): rviz visualization tools for [raw tactile data](tactile_msgs/msg/TactileState.msg) and [contact information](tactile_msgs/msg/TactileContacts.msg).

rosinstall

@@ -1,5 +1 @@
-# As tactile-toolbox relies on new, not yet released urdf features, we need to pull in those for travis testing
-- git: {local-name: urdfdom_headers, uri: 'https://github.com/ubi-agni/urdfdom_headers', version: ros-sensor-refactoring}
-- git: {local-name: urdfdom, uri: 'https://github.com/ubi-agni/urdfdom', version: sensor-refactoring}
-- git: {local-name: urdf, uri: 'https://github.com/ubi-agni/urdf', version: sensor-parsing}
 - git: {local-name: tactile_filters, uri: 'https://github.com/ubi-agni/tactile_filters', version: master}

rviz_tactile_plugins/include/rviz_tactile_plugins/tactile_array_visual.h

@@ -30,7 +30,7 @@
 #pragma once
 
 #include <rviz_tactile_plugins/tactile_visual_base.h>
-#include <urdf_tactile/tactile.h>
+#include <urdf_tactile/sensor.h>
 #include <rviz/ogre_helpers/point_cloud.h>
 
 namespace rviz {

rviz_tactile_plugins/include/rviz_tactile_plugins/tactile_taxels_visual.h

@@ -30,7 +30,7 @@
 #pragma once
 
 #include <rviz_tactile_plugins/tactile_visual_base.h>
-#include <urdf_tactile/tactile.h>
+#include <urdf_tactile/sensor.h>
 
 #define ENABLE_ARROWS 0
 

rviz_tactile_plugins/include/rviz_tactile_plugins/tactile_visual_base.h

@@ -30,7 +30,7 @@
 #pragma once
 
 #include <rviz_tactile_plugins/group_property.h>
-#include <urdf_tactile/tactile.h>
+#include <urdf_tactile/sensor.h>
 #include <tactile_msgs/TactileState.h>
 #include <geometry_msgs/Pose.h>
 #include <tactile_filters/TactileValueArray.h>

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/tactile.h>
-#include <urdf_tactile/cast.h>
+#include <urdf_tactile/parser.h>
 
 #include <rviz/visualization_manager.h>
 #include <rviz/frame_manager.h>
@@ -221,16 +219,12 @@ void TactileStateDisplay::onRobotDescriptionChanged()
 	sensors_.clear();
 	const std::string &tf_prefix = tf_prefix_property_->getStdString();
 
-	// Keep parsers outside of try block to keep them alive in case of an exception as well!
-	auto parsers = urdf::getSensorParser("tactile");
 	try {
-		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,8 +28,7 @@
  */
 #pragma once
 #include "taxel.h"
-#include <urdf_parser/sensor_parser.h>
-#include <urdf_tactile/tactile.h>
+#include <urdf_tactile/sensor.h>
 #include <tactile_msgs/TactileContact.h>
 #include <Eigen/Dense>
 #include <map>
@@ -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

@@ -16,6 +16,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;
 
@@ -41,9 +41,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>
@@ -53,8 +53,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/tactile.h>
-#include <urdf_tactile/cast.h>
+#include <urdf_tactile/sensor.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})