add acceleration publisher

CMakeLists.txt

@@ -164,14 +164,17 @@ add_executable(MqttToRosNode src/MqttToRosNode.cpp ${PROTO_SRCS} ${PROTO_HDRS})
 add_executable(MqttRosConnectionTestNode src/MqttRosConnectionTestNode.cpp ${PROTO_SRCS} ${PROTO_HDRS})
 add_executable(${PROJECT_NAME}_safety_zone_spawner src/safety_zone_spawner.cpp)
 add_executable(${PROJECT_NAME}_safety_zone_monitor src/safety_zone_monitor.cpp src/SafetyZoneMonitor.cpp src/SafetyZoneMonitor.h)
+add_executable(${PROJECT_NAME}_acceleration_publisher src/acceleration_publisher.cpp)
 
 
 set_target_properties(${PROJECT_NAME}_safety_zone_spawner PROPERTIES OUTPUT_NAME safety_zone_spawner PREFIX "")
 set_target_properties(${PROJECT_NAME}_safety_zone_monitor PROPERTIES OUTPUT_NAME safety_zone_monitor PREFIX "")
+set_target_properties(${PROJECT_NAME}_acceleration_publisher PROPERTIES OUTPUT_NAME acceleration_publisher PREFIX "")
 
 
 add_dependencies(${PROJECT_NAME}_safety_zone_spawner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 add_dependencies(${PROJECT_NAME}_safety_zone_monitor ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(${PROJECT_NAME}_acceleration_publisher ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 # Specify libraries to link a library or executable target against
 target_link_libraries(TrajectoryUtil
@@ -199,4 +202,6 @@ target_link_libraries(TimedPlanner ${catkin_LIBRARIES} TrajectoryUtil)
 
 target_link_libraries(${PROJECT_NAME}_safety_zone_spawner ${catkin_LIBRARIES})
 
-target_link_libraries(${PROJECT_NAME}_safety_zone_monitor ${catkin_LIBRARIES})
\ No newline at end of file
+target_link_libraries(${PROJECT_NAME}_safety_zone_monitor ${catkin_LIBRARIES})
+
+target_link_libraries(${PROJECT_NAME}_acceleration_publisher ${catkin_LIBRARIES})
\ No newline at end of file

launch/simulation_rosrag_test.launch

@@ -12,4 +12,7 @@
     <node name="safety_zone_spawner" pkg="panda_mqtt_connector" type="safety_zone_spawner" respawn="false" output="screen"/>
     <node name="safety_zone_monitor" pkg="panda_mqtt_connector" type="safety_zone_monitor" respawn="false" output="screen"/>
     <node name="MqttRosConnectionTestNode" pkg="panda_mqtt_connector" type="MqttRosConnectionTestNode" respawn="false" output="screen"/>
+    <node name="acceleration_publisher" pkg="panda_mqtt_connector" type="acceleration_publisher" respawn="false" output="screen"/>
+    <node name="transform-riz" pkg="topic_tools" type="transform" args="/panda_mqtt_connector/robot_in_zone /panda_mqtt_connector/riz std_msgs/Float64 -- 'm.data*-1'" />
+    <node name="rqt_plot" pkg="rqt_plot" type="rqt_plot" respawn="false" output="screen" args="/gazebo/link_states/pose[10]/position/x:y:z /panda_mqtt_connector/velocity/data /panda_mqtt_connector/max_velocity/data /panda_mqtt_connector/riz" />
 </launch>

src/acceleration_publisher.cpp

+/*! \file acceleration_publisher.cpp
+    \brief ROS node main file that publishes
+
+    \author Johannes Mey
+    \date 18.07.20
+*/
+
+#include <ros/ros.h>
+
+#include <ros/ros.h>
+#include <tf2_ros/transform_listener.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <std_msgs/Float64.h>
+
+namespace acceleration_publisher {
+}
+
+/**
+ * The main method of the ROS node
+ *
+ */
+int main(int argc, char **argv) {
+
+  // setup this ros-node
+  ros::init(argc, argv, "acceleration_publisher");
+  ros::NodeHandle n("panda_mqtt_connector");
+
+  ros::Publisher vel = n.advertise<std_msgs::Float64>("velocity", 10);
+
+  tf2_ros::Buffer tfBuffer;
+  tf2_ros::TransformListener tfListener(tfBuffer);
+
+  // the duration to compute the velocity
+  ros::Duration dt{1, 500000}; // 0,10000
+  // wait for the duration. this way the old time at least exists
+  dt.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));
+
+    } catch (tf2::TransformException &ex) {
+      ROS_WARN("%s", ex.what());
+      ros::Duration(1.0).sleep();
+      continue;
+    }
+
+    std_msgs::Float64 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.data = velocity;
+    vel.publish(msg);
+  }
+
+  return 0;
+}
\ No newline at end of file