diff --git a/src/acceleration_publisher.cpp b/src/acceleration_publisher.cpp index 8844cabb0adc0939eaf3f4b429364d419ce367bc..d4584d1da422cbee89a5cfc95d0449b2443e52bd 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 +} + +