diff --git a/CMakeLists.txt b/CMakeLists.txt
index bddcdef0349f720859a73b497b917cd1f86df451..e92922516d8c0b8c38f6dd51173dc7c36b15127c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -80,6 +80,8 @@ add_message_files(
FILES
Trajectory.msg
Waypoint.msg
+ StampedVelocity.msg
+ StampedInZone.msg
)
## Generate services in the 'srv' folder
@@ -204,4 +206,8 @@ 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})
\ No newline at end of file
+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
diff --git a/launch/simulation_rosrag_test.launch b/launch/simulation_rosrag_test.launch
index 7dbd74a800a2ecf7b34ba4998d45c69a3d97de28..280283a84815fe4ba45b91840828c160dfd9f1c5 100644
--- a/launch/simulation_rosrag_test.launch
+++ b/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>
diff --git a/msg/StampedInZone.msg b/msg/StampedInZone.msg
new file mode 100644
index 0000000000000000000000000000000000000000..a7f236b917c901a53dab39f9eae1dec12d2b1979
--- /dev/null
+++ b/msg/StampedInZone.msg
@@ -0,0 +1,3 @@
+# information if robot is in zone with a time stamp
+Header header
+bool zone
\ No newline at end of file
diff --git a/msg/StampedVelocity.msg b/msg/StampedVelocity.msg
new file mode 100644
index 0000000000000000000000000000000000000000..90fc133804ae827c08a7c763823a3f2d2093505a
--- /dev/null
+++ b/msg/StampedVelocity.msg
@@ -0,0 +1,3 @@
+# a velocity with a time stamp
+Header header
+float64 velocity
\ No newline at end of file
diff --git a/scripts/plotter.py b/scripts/plotter.py
new file mode 100755
index 0000000000000000000000000000000000000000..bb3445c85085f38d7e6ca8623fd0922a466e8c27
--- /dev/null
+++ b/scripts/plotter.py
@@ -0,0 +1,18 @@
+#!/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()
diff --git a/src/SafetyZoneMonitor.h b/src/SafetyZoneMonitor.h
index 1e12abd5e3681e7ed4f595354a765d99b4b28243..85ff823d663aac1bb9d45fc0c8dfda1aefe1e57e 100644
--- a/src/SafetyZoneMonitor.h
+++ b/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);
}
}
diff --git a/src/acceleration_publisher.cpp b/src/acceleration_publisher.cpp
index c6e7572345f853b50e8039b6d17d28fda60dcad5..8844cabb0adc0939eaf3f4b429364d419ce367bc 100644
--- a/src/acceleration_publisher.cpp
+++ b/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));
+ 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;
}
-
- 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;
diff --git a/src/safety_zone_monitor.cpp b/src/safety_zone_monitor.cpp
index 21b129b5da0507fda336be2e6edd96bfe4af3785..948309e48e23737730547f6ab1ca2f08a3c97960 100644
--- a/src/safety_zone_monitor.cpp
+++ b/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};