Improvements and fixes for various issues

.travis.rosinstall

@@ -1,5 +1,5 @@
 # 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: 0.5}
-- git: {local-name: urdfdom, uri: 'https://github.com/ubi-agni/urdfdom', version: 0.5}
-- git: {local-name: robot_model, uri: 'https://github.com/ubi-agni/robot_model', version: kinetic-devel}
+- 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: ros-sensor-refactoring}
+- git: {local-name: urdf, uri: 'https://github.com/ubi-agni/urdf', version: melodic-sensor-parsing}
 - git: {local-name: tactile_filters, uri: 'https://github.com/ubi-agni/tactile_filters', version: master}

.travis.sh

@@ -1,3 +1,6 @@
+#!/bin/bash
+set -e  # fail on any error
+
 export TRAVIS_FOLD_COUNTER=1
 
 # Display command in Travis console and fold output in dropdown section
@@ -35,10 +38,6 @@ travis_run wstool init
 travis_run wstool merge file://$REPO/.travis.rosinstall
 travis_run wstool update
 
-# only need to build package urdf
-mv robot_model/urdf .
-rm -rf robot_model
-
 # link in source
 ln -s $REPO .
 

.travis.yml

@@ -9,9 +9,9 @@ notifications:
       - [email protected]
 env:
   matrix:
-    - ROS_DISTRO="kinetic"
+    - ROS_DISTRO="melodic"
 
 before_script:
   - docker pull ros:$ROS_DISTRO-ros-base
 script:
-  - docker run -v $(pwd):$(pwd) ros:$ROS_DISTRO-ros-base /bin/bash -c "source $(pwd)/.travis.sh"
+  - docker run -v $(pwd):$(pwd) ros:$ROS_DISTRO-ros-base $(pwd)/.travis.sh

rviz_tactile_plugins/src/tactile_contact_display.cpp

@@ -37,6 +37,7 @@
 #include <rviz/properties/color_property.h>
 #include <rviz/default_plugin/wrench_visual.h>
 #include <QApplication>
+#include <QTimer>
 #include <boost/thread/locks.hpp>
 
 namespace rviz {
@@ -111,7 +112,9 @@ TactileContactDisplay::TactileContactDisplay()
   torque_scale_property_ = new rviz::FloatProperty
       ("Torque Arrow Scale", 1.0, "", scale_property_, SLOT(triggerFullUpdate()), this);
   width_property_ = new rviz::FloatProperty
-      ( "Arrow Width", 1.0, "", scale_property_, SLOT(triggerFullUpdate()), this);
+      ( "Arrow Width", 0.5, "", scale_property_, SLOT(triggerFullUpdate()), this);
+  hide_small_values_property_ = new rviz::BoolProperty("Hide Small Values", true, "Hide small values",
+                                                       this, SLOT(triggerFullUpdate()));
 }
 
 TactileContactDisplay::~TactileContactDisplay()
@@ -134,7 +137,9 @@ void TactileContactDisplay::subscribe()
     auto it = topics.begin(), end = topics.end();
     for (; it != end && it->name != topic; ++it);
     if (it == end) {
-      setStatus(StatusProperty::Error, "Topic", "not published, cannot infer msg type");
+      setStatus(StatusProperty::Error, "Topic", "Not yet published, cannot infer msg type");
+      // try again in a second
+      QTimer::singleShot(1000, this, &TactileContactDisplay::subscribe);
       return;
     }
 
@@ -148,7 +153,7 @@ void TactileContactDisplay::subscribe()
       // should not happen due to type filtering in TactileContactTopicProperty
       throw ros::Exception(std::string("unhandled msg type: " + it->datatype));
 
-    setStatus(StatusProperty::Ok, "Topic", "OK");
+    setStatusStd(StatusProperty::Ok, "Topic", it->datatype.substr(it->datatype.find('/')));
   } catch(const ros::Exception& e) {
     setStatus(StatusProperty::Error, "Topic", QString("error subscribing: ") + e.what());
   }
@@ -220,6 +225,7 @@ void TactileContactDisplay::processMessage(const tactile_msgs::TactileContact::C
   boost::unique_lock<boost::mutex> lock(mutex_);
   processMessage(*msg);
 }
+
 void TactileContactDisplay::processMessages(const tactile_msgs::TactileContacts::ConstPtr& msg)
 {
   boost::unique_lock<boost::mutex> lock(mutex_);
@@ -237,19 +243,23 @@ void TactileContactDisplay::update(float wall_dt, float ros_dt)
 
   ros::Time now = ros::Time::now();
   ros::Duration timeout(timeout_property_->getFloat());
-
   boost::unique_lock<boost::mutex> lock(mutex_);
+
+  if(now < last_update_) {
+    ROS_WARN_STREAM("Detected jump back in time of " << (last_update_ - now).toSec() << "s. Clearing contacts.");
+    contacts_.clear();
+  }
+  last_update_ = now;
+
   for (auto it = contacts_.begin(), end = contacts_.end(); it != end; ++it) {
     const tactile_msgs::TactileContact &msg = it->second.first;
     WrenchVisualPtr &visual = it->second.second;
     bool new_visual = !visual;
+
+    // hide visuals if they are outdated
     if (msg.header.stamp != zeroStamp && msg.header.stamp + timeout < now) {
-      const std::string& tf_prefix = tf_prefix_property_->getStdString();
-      const std::string& frame = tf_prefix.empty() ? msg.header.frame_id
-                                                   : tf::resolve(tf_prefix, msg.header.frame_id);
-      setStatusStd(StatusProperty::Warn, frame, "no recent msg");
       if (visual) visual->setVisible(false);
-      continue;
+      continue;  // skip further processing for this message
     }
 
     // Update pose of visual
@@ -260,15 +270,14 @@ void TactileContactDisplay::update(float wall_dt, float ros_dt)
     const std::string& frame = tf_prefix.empty() ? msg.header.frame_id
                                                  : tf::resolve(tf_prefix, msg.header.frame_id);
     // use zeroStamp to fetch most recent frame (tf is lacking behind our timestamps which caused issues)
-    if (!context_->getFrameManager()->getTransform(frame, zeroStamp,
-                                                   position, orientation)) {
+    if (!context_->getFrameManager()->getTransform(frame, zeroStamp, position, orientation)) {
       std::string error;
       context_->getFrameManager()->transformHasProblems(frame, msg.header.stamp, error);
       setStatusStd(StatusProperty::Error, frame, error);
       if (visual) visual->setVisible(false);
       continue;
     } else {
-      setStatusStd(StatusProperty::Ok, frame, "");
+      deleteStatusStd(frame);
     }
 
     // create visual if not yet done
@@ -289,6 +298,7 @@ void TactileContactDisplay::update(float wall_dt, float ros_dt)
       visual->setTorqueColor(torque_color.r, torque_color.g, torque_color.b, alpha);
       visual->setForceScale(force_scale);
       visual->setTorqueScale(torque_scale);
+      visual->setHideSmallValues(hide_small_values_property_->getBool());
       visual->setWidth(width);
     }
 

rviz_tactile_plugins/src/tactile_contact_display.h

@@ -96,12 +96,14 @@ protected Q_SLOTS:
   rviz::ColorProperty *force_color_property_, *torque_color_property_;
   rviz::FloatProperty *alpha_property_;
   rviz::FloatProperty *scale_property_, *force_scale_property_, *torque_scale_property_, *width_property_;
+  rviz::BoolProperty* hide_small_values_property_;
   bool full_update_; // update all visual properties?
 
   ros::NodeHandle  nh_;
   ros::Subscriber  sub_;
   std::map<std::string, std::pair<tactile_msgs::TactileContact, WrenchVisualPtr> > contacts_;
   boost::mutex mutex_;
+  ros::Time last_update_;
 };
 
 } // namespace tactile

rviz_tactile_plugins/src/tactile_state_display.cpp

@@ -320,12 +320,13 @@ void TactileStateDisplay::onAllVisibleChanged()
 // This is our callback to handle an incoming message.
 void TactileStateDisplay::processMessage(const tactile_msgs::TactileState::ConstPtr& msg)
 {
+  const ros::Time now = ros::Time::now();
   for (auto sensor = msg->sensors.begin(), end = msg->sensors.end(); sensor != end; ++sensor)
   {
     const std::string &channel = sensor->name;
     auto range = sensors_.equal_range(channel);
     for (auto s = range.first, range_end = range.second; s != range_end; ++s) {
-      s->second->update(ros::Time::now(), sensor->values);
+      s->second->update(now, sensor->values);
     }
   }
 }
@@ -336,19 +337,21 @@ void TactileStateDisplay::update(float wall_dt, float ros_dt)
 
   Display::update(wall_dt, ros_dt);
 
-  ros::Time timeout = ros::Time::now();
-  try {
-    timeout -= ros::Duration(timeout_property_->getFloat());
-  } catch (const std::runtime_error &e) {
-    // ros::Time::now was smaller than ros::Duration
+  ros::Time now = ros::Time::now();
+  ros::Duration timeout(timeout_property_->getFloat());
+  if(now < last_update_) {
+    ROS_WARN_STREAM("Detected jump back in time of " << (last_update_ - now).toSec() << "s. Clearing taxels.");
+    for (auto& sensor : sensors_)
+      sensor.second->resetTime();  // expire the sensor data
   }
+  last_update_ = now;
 
   for (auto it = sensors_.begin(), end = sensors_.end(); it != end; ++it) {
     TactileVisualBase &sensor = *it->second;
     sensor.updateRangeProperty();
     if (!sensor.isVisible()) continue;
 
-    bool enabled = !sensor.expired(timeout) && sensor.updatePose();
+    bool enabled = !sensor.expired(now, timeout) && sensor.updatePose();
     sensor.setEnabled(enabled);
     if (!enabled) continue;
 

rviz_tactile_plugins/src/tactile_state_display.h

@@ -102,6 +102,7 @@ protected Q_SLOTS:
   ros::Subscriber  sub_;
   /// list of all sensors, accessible by sensor name
   std::multimap<std::string, TactileVisualBase*> sensors_;
+  ros::Time last_update_;
 
   ::tactile::TactileValue::Mode mode_;
   ColorMap abs_color_map_;

rviz_tactile_plugins/src/tactile_visual_base.cpp

@@ -127,9 +127,9 @@ void TactileVisualBase::update(const ros::Time &stamp)
     raw_range_.update(it->absRange());
 }
 
-bool TactileVisualBase::expired(const ros::Time &timeout) const
+bool TactileVisualBase::expired(const ros::Time &now, const ros::Duration& timeout) const
 {
-  return last_update_time_ <= timeout;
+  return last_update_time_ + timeout < now;
 }
 
 bool TactileVisualBase::updatePose()
@@ -175,6 +175,11 @@ void TactileVisualBase::reset()
   setRawRangeFromProperty();
 }
 
+void TactileVisualBase::resetTime()
+{
+  last_update_time_ = ros::Time();
+}
+
 void TactileVisualBase::onVisibleChanged()
 {
   scene_node_->setVisible(isVisible() && isEnabled());

rviz_tactile_plugins/src/tactile_visual_base.h

@@ -81,9 +81,10 @@ Q_OBJECT
 
   /// reset ranges
   virtual void reset();
+  void resetTime();
 
-  /// most recent update older than timeout?
-  bool expired(const ros::Time &timeout) const;
+  /// most recent update time + timeout older than now?
+  bool expired(const ros::Time &now, const ros::Duration &timeout) const;
 
   /// isVisible() simply returns status of this' BoolProperty
   bool isVisible() const {return this->getBool();}

tactile_merger/include/tactile_merger/merger.h

@@ -42,6 +42,8 @@ class Merger
 	template <typename Iterator>
 	void update(const ros::Time &stamp, const std::string &channel,
 	            Iterator begin, Iterator end);
+	void reset();
+
 	tactile_msgs::TactileContacts getGroupAveragedContacts();
 	tactile_msgs::TactileContacts getAllTaxelContacts();
 

tactile_merger/include/tactile_merger/taxel_group.h

@@ -79,9 +79,6 @@ class TaxelGroup
 	std::vector<Taxel> taxels_;
 	/// mapping from channel name onto its TaxelMapping
 	SensorToTaxelMapping mappings_;
-	/// conversion from eigen pos, normal, amplitude to contact
-	void toContact(tactile_msgs::TactileContact &contact, const Eigen::Vector3d &pos,
-	               const Eigen::Vector3d &normal, const double force_amplitude);
 };
 
 } // namespace tactile

tactile_merger/src/merger.cpp

@@ -72,6 +72,12 @@ void Merger::init(const std::string &param)
 	}
 }
 
+void Merger::reset()
+{
+	for (auto& group : groups_)
+		group.second->timestamp = ros::Time();
+}
+
 template <typename Iterator>
 void Merger::update(const ros::Time &stamp, const std::string &channel,
                     Iterator begin, Iterator end) {

tactile_merger/src/merger_node.cpp

@@ -32,8 +32,11 @@
 #include <tactile_msgs/TactileState.h>
 #include <boost/bind.hpp>
 
+static bool have_new_data = false;
+
 void message_handler(tactile::Merger &merger,
                      const tactile_msgs::TactileStateConstPtr &msg) {
+	have_new_data = true;
 	for (auto it = msg->sensors.begin(), end = msg->sensors.end(); it != end; ++it) {
 		merger.update(msg->header.stamp, it->name, it->values.begin(), it->values.end());
 	}
@@ -54,15 +57,28 @@ int main(int argc, char *argv[])
 	      callback = boost::bind(message_handler, boost::ref(merger), _1);
 	ros::Subscriber sub = nh.subscribe("tactile_states", 1, callback);
 
+	ros::Time last_update_;
 	ros::Rate rate(nh_priv.param("rate", 100.));
 	bool no_clustering = nh_priv.param("no_clustering", false);
 	while (ros::ok())
 	{
 		ros::spinOnce();
-		if (no_clustering)
-			pub.publish(merger.getAllTaxelContacts());
-		else
-			pub.publish(merger.getGroupAveragedContacts());
+		ros::Time now = ros::Time::now();
+		if(now < last_update_) {
+			ROS_WARN_STREAM("Detected jump back in time of " << (last_update_ - now).toSec() << "s. Resetting data.");
+			merger.reset();
+			have_new_data = false;
+		}
+		last_update_ = now;
+
+		if (have_new_data) {
+			have_new_data = false;
+			if (no_clustering)
+				pub.publish(merger.getAllTaxelContacts());
+			else
+				pub.publish(merger.getGroupAveragedContacts());
+			ros::spinOnce();
+		}
 		rate.sleep();
 	}
 

tactile_merger/src/taxel_group.cpp

@@ -96,10 +96,9 @@ template void TaxelGroup::update<std::vector<float>::const_iterator>
 (const TaxelMapping &mapping,
 std::vector<float>::const_iterator begin, std::vector<float>::const_iterator end);
 
-void TaxelGroup::toContact(tactile_msgs::TactileContact &contact, const Eigen::Vector3d &pos,
-                           const Eigen::Vector3d &normal, const double force_amplitude)
+inline void toContact(tactile_msgs::TactileContact &contact, const Eigen::Vector3d &pos,
+                      const Eigen::Vector3d &normal, const Eigen::Vector3d &force, const Eigen::Vector3d& torque)
 {
-	Eigen::Vector3d force, torque;
 	contact.position.x = pos.x();
 	contact.position.y = pos.y();
 	contact.position.z = pos.z();
@@ -108,12 +107,10 @@ void TaxelGroup::toContact(tactile_msgs::TactileContact &contact, const Eigen::V
 	contact.normal.y = normal.y();
 	contact.normal.z = normal.z();
 
-	force = (-force_amplitude) * normal; // force acts opposite to normal
 	contact.wrench.force.x = force.x();
 	contact.wrench.force.y = force.y();
 	contact.wrench.force.z = force.z();
 
-	torque = pos.cross(force);
 	contact.wrench.torque.x = torque.x();
 	contact.wrench.torque.y = torque.y();
 	contact.wrench.torque.z = torque.z();
@@ -122,12 +119,14 @@ void TaxelGroup::toContact(tactile_msgs::TactileContact &contact, const Eigen::V
 bool TaxelGroup::all(std::vector<tactile_msgs::TactileContact> &contacts, const tactile_msgs::TactileContact &contact_template)
 {
 	unsigned int i=0;
+	Eigen::Vector3d force;
 	for (auto it = taxels_.begin(), end = taxels_.end(); it != end; ++it, ++i) {
 		tactile_msgs::TactileContact contact = contact_template;  // copy header and name
 		contact.name.append("_");
 		contact.name.append(std::to_string(i));
 
-		toContact(contact, it->position, it->normal, it->weight);  // add index
+		force = (-it->weight)*it->normal;
+		toContact(contact, it->position, it->normal, force, it->position.cross(force));
 		contacts.push_back(contact);
 	}
 	return true;
@@ -139,17 +138,23 @@ bool TaxelGroup::average(tactile_msgs::TactileContact &contact)
 	Eigen::Vector3d pos, normal, force, torque;
 	pos = normal = force = torque = Eigen::Vector3d::Zero();
 	for (auto it = taxels_.begin(), end = taxels_.end(); it != end; ++it) {
-		double w = it->weight;
+		double w = std::abs(it->weight);
 		sum += w;
 		pos += w * it->position;
 		normal += w * it->normal;
+		Eigen::Vector3d f = (-it->weight) * it->normal;  // force is pointing opposite to normal
+		force += f;
+		torque += it->position.cross(f);
 	}
-	if (sum > Eigen::NumTraits<float>::dummy_precision()) {
+	// Is overall force magnitude large enough to compute location?
+	if (sum > Eigen::NumTraits<float>::dummy_precision())
 		pos /= sum;
-		normal.normalize();
-	} else
+	else
 		return false;
-	toContact(contact, pos, normal, sum);
+
+	normal.normalize();
+
+	toContact(contact, pos, normal, force, torque);
 	return true;
 }