From 77c2a669b2ffcd053ed60efd87cc2ba5059ec797 Mon Sep 17 00:00:00 2001
From: Johannes Mey <johannes.mey@tu-dresden.de>
Date: Tue, 21 Jul 2020 01:15:41 +0200
Subject: [PATCH] publish smooth and exact velocity
---
src/acceleration_publisher.cpp | 67 ++++++++++++++++++++--------------
1 file changed, 40 insertions(+), 27 deletions(-)
diff --git a/src/acceleration_publisher.cpp b/src/acceleration_publisher.cpp
index 8844cab..d4584d1 100644
--- a/src/acceleration_publisher.cpp
+++ b/src/acceleration_publisher.cpp
@@ -10,9 +10,39 @@
#include <tf2_ros/transform_listener.h>
#include <panda_mqtt_connector/StampedVelocity.h>
+void
+publishVelocity(const ros::Publisher &vel_smoothed, const tf2_ros::Buffer &tfBuffer, const ros::Duration &dt);
+
namespace acceleration_publisher {
}
+void publishVelocity(const ros::Publisher &vel_smoothed, const tf2_ros::Buffer &tfBuffer, const ros::Duration &dt) {
+ geometry_msgs::TransformStamped transformStamped;
+ try {
+ ros::Time now = ros::Time::now();
+ ros::Time past = now - dt;
+
+ transformStamped = tfBuffer.lookupTransform("panda_hand", now,
+ "panda_hand", past,
+ "world", ros::Duration(1.0));
+
+ double dx = transformStamped.transform.translation.x;
+ double dy = transformStamped.transform.translation.y;
+ double dz = transformStamped.transform.translation.z;
+ double smoothed_velocity = sqrt(dx * dx + dy * dz + dz * dz) / dt.toSec();
+
+ if (!isnan(smoothed_velocity)) {
+ panda_mqtt_connector::StampedVelocity msg;
+ msg.header.stamp = now - (dt * .5);
+ msg.velocity = smoothed_velocity;
+ vel_smoothed.publish(msg);
+ }
+ } catch (tf2::TransformException &ex) {
+ ROS_WARN("%s", ex.what());
+ ros::Duration(1.0).sleep();
+ }
+}
+
/**
* The main method of the ROS node
*
@@ -24,43 +54,26 @@ int main(int argc, char **argv) {
ros::NodeHandle n("panda_mqtt_connector");
ros::Publisher vel = n.advertise<panda_mqtt_connector::StampedVelocity>("velocity", 10);
+ ros::Publisher vel_smoothed = n.advertise<panda_mqtt_connector::StampedVelocity>("velocity_smoothed", 10);
tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer);
// the duration to compute the velocity
- ros::Duration dt{1, 500000}; // 0,10000
+ ros::Duration dt{0, 10000000}; // 0,10000
+ ros::Duration dt_smooth{0, 330000000}; // 0,10000
// wait for the duration. this way the old time at least exists
- dt.sleep();
+ dt_smooth.sleep();
while (n.ok()) {
- geometry_msgs::TransformStamped transformStamped;
- try {
- ros::Time now = ros::Time::now();
- ros::Time past = now - dt;
+ ros::Duration(0.01).sleep();
- transformStamped = tfBuffer.lookupTransform("panda_hand", now,
- "panda_hand", past,
- "world", ros::Duration(1.0));
-
- panda_mqtt_connector::StampedVelocity msg;
-
- double dx = transformStamped.transform.translation.x;
- double dy = transformStamped.transform.translation.y;
- double dz = transformStamped.transform.translation.z;
-
- double velocity = sqrt(dx*dx + dy*dz + dz*dz)/dt.toSec();
- msg.header.stamp = now - (dt * .5);
- msg.velocity = velocity;
- vel.publish(msg);
-
- } catch (tf2::TransformException &ex) {
- ROS_WARN("%s", ex.what());
- ros::Duration(1.0).sleep();
- continue;
- }
+ publishVelocity(vel, tfBuffer, dt);
+ publishVelocity(vel_smoothed, tfBuffer, dt_smooth);
}
return 0;
-}
\ No newline at end of file
+}
+
+
--
GitLab