change messaged to stamped messages, add initial version of plotter

CMakeLists.txt

@@ -80,6 +80,8 @@ add_message_files(
         FILES
         Trajectory.msg
         Waypoint.msg
+        StampedVelocity.msg
+        StampedInZone.msg
 )
 
 ## Generate services in the 'srv' folder
@@ -205,3 +207,7 @@ target_link_libraries(${PROJECT_NAME}_safety_zone_spawner ${catkin_LIBRARIES})
 target_link_libraries(${PROJECT_NAME}_safety_zone_monitor ${catkin_LIBRARIES})
 
 target_link_libraries(${PROJECT_NAME}_acceleration_publisher ${catkin_LIBRARIES})
+
+catkin_install_python(PROGRAMS scripts/plotter.py
+        DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
\ No newline at end of file

launch/simulation_rosrag_test.launch

@@ -13,6 +13,9 @@
     <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" />
+    <node name="plotter" pkg="panda_mqtt_connector" type="plotter.py" respawn="false" output="screen"/>
+    <node name="transform_riz" pkg="topic_tools" type="transform" args="--wait-for-start /panda_mqtt_connector/robot_in_zone /panda_mqtt_connector/robot_in_zone_number std_msgs/Float64 -- 'm.zone*-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 /panda_mqtt_connector/max_velocity /panda_mqtt_connector/robot_in_zone_number"
+    />
 </launch>

msg/StampedInZone.msg

+# information if robot is in zone with a time stamp
+Header header
+bool zone
\ No newline at end of file

msg/StampedVelocity.msg

+# a velocity with a time stamp
+Header header
+float64 velocity
\ No newline at end of file

scripts/plotter.py

+#!/usr/bin/env python
+from matplotlib import pyplot as plt
+import rospy
+from panda_mqtt_connector.msg import StampedVelocity
+
+def plot_x(msg):
+    plt.plot(msg.header.stamp.to_sec(), msg.velocity, '*')
+    plt.axis("equal")
+    plt.draw()
+    plt.pause(0.00000000001)
+
+if __name__ == '__main__':
+
+    rospy.init_node("plotter")
+    rospy.Subscriber("panda_mqtt_connector/velocity", StampedVelocity, plot_x)
+    plt.ion()
+    plt.show()
+    rospy.spin()

src/SafetyZoneMonitor.h

@@ -7,7 +7,7 @@
 
 #include <geometry_msgs/Point.h>
 #include <gazebo_msgs/LinkStates.h>
-#include <std_msgs/Bool.h>
+#include <panda_mqtt_connector/StampedInZone.h>
 #include "ros/ros.h"
 
 
@@ -52,8 +52,6 @@ public:
 
     counter++;
 
-//    ROS_ERROR_STREAM("initialized " << initialized << " result " << res << " counter " << counter);
-
     std::vector<std::string> names{msg.name};
 
     for (int link_number = 0; link_number < names.size(); ++link_number) {
@@ -64,9 +62,10 @@ public:
             if (!res || !initialized) {
               initialized = true;
               res = true;
-              std_msgs::Bool result;
-              result.data = true;
-              ROS_ERROR_STREAM("entered zone [" << zone.zone.x << "," << zone.zone.y << "] in run " << counter);
+              panda_mqtt_connector::StampedInZone result;
+              result.zone = true;
+              result.header.stamp = ros::Time::now();
+              ROS_INFO_STREAM("entered zone [" << zone.zone.x << "," << zone.zone.y << "] in run " << counter);
               publisher->publish(result);
             }
             return;
@@ -76,11 +75,12 @@ public:
     }
     // no part of the robot is in a zone
     if (res || !initialized) {
-      ROS_ERROR_STREAM("left zone in run " << counter);
+      ROS_INFO_STREAM("left zone in run " << counter);
       initialized = true;
       res = false;
-      std_msgs::Bool result;
-      result.data = false;
+      panda_mqtt_connector::StampedInZone result;
+      result.zone = false;
+      result.header.stamp = ros::Time::now();
       publisher->publish(result);
     }
   }

src/acceleration_publisher.cpp

@@ -7,10 +7,8 @@
 
 #include <ros/ros.h>
 
-#include <ros/ros.h>
 #include <tf2_ros/transform_listener.h>
-#include <geometry_msgs/TransformStamped.h>
-#include <std_msgs/Float64.h>
+#include <panda_mqtt_connector/StampedVelocity.h>
 
 namespace acceleration_publisher {
 }
@@ -25,7 +23,7 @@ int main(int argc, char **argv) {
   ros::init(argc, argv, "acceleration_publisher");
   ros::NodeHandle n("panda_mqtt_connector");
 
-  ros::Publisher vel = n.advertise<std_msgs::Float64>("velocity", 10);
+  ros::Publisher vel = n.advertise<panda_mqtt_connector::StampedVelocity>("velocity", 10);
 
   tf2_ros::Buffer tfBuffer;
   tf2_ros::TransformListener tfListener(tfBuffer);
@@ -46,21 +44,22 @@ int main(int argc, char **argv) {
                                                   "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;
+      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.data = velocity;
+      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;
+    }
   }
 
   return 0;

src/safety_zone_monitor.cpp

@@ -8,7 +8,7 @@
 #include <ros/ros.h>
 #include <shape_msgs/SolidPrimitive.h>
 #include <geometry_msgs/Pose.h>
-#include <std_msgs/Bool.h>
+#include <panda_mqtt_connector/StampedInZone.h>
 #include "SafetyZoneMonitor.h"
 
 namespace safety_zone_monitor {
@@ -73,7 +73,7 @@ int main(int argc, char **argv) {
     exit(-1);
   }
 
-  ros::Publisher zoneStatePublisher = n.advertise<std_msgs::Bool>("robot_in_zone", 1000);
+  ros::Publisher zoneStatePublisher = n.advertise<panda_mqtt_connector::StampedInZone>("robot_in_zone", 1000);
 
   SafetyZoneMonitor monitor{size, zoneStatePublisher};