publish smooth and exact velocity

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;
-
-      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;
+    ros::Duration(0.01).sleep();
 
-      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;
 }
+
+