From 9869b41ab6ab294c166b9b06ded318258d2adcca Mon Sep 17 00:00:00 2001
From: Johannes Mey <johannes.mey@tu-dresden.de>
Date: Sun, 19 Jul 2020 11:00:07 +0200
Subject: [PATCH] change messaged to stamped messages, add initial version of
plotter
---
CMakeLists.txt | 8 +++++++-
launch/simulation_rosrag_test.launch | 7 +++++--
msg/StampedInZone.msg | 3 +++
msg/StampedVelocity.msg | 3 +++
scripts/plotter.py | 18 ++++++++++++++++++
src/SafetyZoneMonitor.h | 18 +++++++++---------
src/acceleration_publisher.cpp | 27 +++++++++++++--------------
src/safety_zone_monitor.cpp | 4 ++--
8 files changed, 60 insertions(+), 28 deletions(-)
create mode 100644 msg/StampedInZone.msg
create mode 100644 msg/StampedVelocity.msg
create mode 100755 scripts/plotter.py
diff --git a/CMakeLists.txt b/CMakeLists.txt
index bddcdef..e929225 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 7dbd74a..280283a 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 0000000..a7f236b
--- /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 0000000..90fc133
--- /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 0000000..bb3445c
--- /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 1e12abd..85ff823 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 c6e7572..8844cab 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 21b129b..948309e 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};
--
GitLab