From 5aaf32fa062d27d3107e5dd4d9eb51c6d4d7e82d Mon Sep 17 00:00:00 2001 From: Johannes Mey <johannes.mey@tu-dresden.de> Date: Mon, 27 Jul 2020 16:32:54 +0200 Subject: [PATCH] implement safety zone monitor --- msg/StampedInZone.msg | 5 +- src/RobotStateProvider.cpp | 1 + src/SafetyZoneMonitor.cpp | 119 +++++++++++++++++++++++++++++++++++- src/SafetyZoneMonitor.h | 70 +++++++-------------- src/safety_zone_monitor.cpp | 11 ++-- 5 files changed, 147 insertions(+), 59 deletions(-) diff --git a/msg/StampedInZone.msg b/msg/StampedInZone.msg index a7f236b..2d1e7f1 100644 --- a/msg/StampedInZone.msg +++ b/msg/StampedInZone.msg @@ -1,3 +1,6 @@ # information if robot is in zone with a time stamp Header header -bool zone \ No newline at end of file +bool zone +uint32 zone_changes +uint32 mode_changes +bool mode_changed \ No newline at end of file diff --git a/src/RobotStateProvider.cpp b/src/RobotStateProvider.cpp index 21970e5..def699a 100644 --- a/src/RobotStateProvider.cpp +++ b/src/RobotStateProvider.cpp @@ -137,6 +137,7 @@ int main(int argc, char **argv) { // publish the ROS message geometry_msgs::PoseStamped ros_msg; ros_msg.header.stamp = transformStamped.header.stamp; + ros_msg.header.frame_id = topic.first; ros_msg.pose.position.x = transformStamped.transform.translation.x; ros_msg.pose.position.y = transformStamped.transform.translation.y; ros_msg.pose.position.z = transformStamped.transform.translation.z; diff --git a/src/SafetyZoneMonitor.cpp b/src/SafetyZoneMonitor.cpp index dcaade9..89ca887 100644 --- a/src/SafetyZoneMonitor.cpp +++ b/src/SafetyZoneMonitor.cpp @@ -4,10 +4,74 @@ #include "SafetyZoneMonitor.h" -SafetyZoneMonitor::SafetyZoneMonitor(double zoneSize, ros::Publisher &publisher) : zone_size(zoneSize), publisher(&publisher) {} +SafetyZoneMonitor::SafetyZoneMonitor(double zoneSize, ros::Publisher &publisher) : zone_size(zoneSize), inZonePublisher(&publisher) {} void SafetyZoneMonitor::addZone(geometry_msgs::Point zone) { - zones.push_back(Zone(zone, zone_size)); + zones[Zone(zone, zone_size)] = std::map<std::string, bool>(); +} + +void SafetyZoneMonitor::providerCallback(const geometry_msgs::PoseStamped &msg) { + static bool initialized; + + bool zoneChanged = false; + + bool inZoneNow = false; + for (auto zone : zones) { + bool containsNow = zone.first.contains(msg.pose.position); + if (containsNow) { + inZoneNow = true; + } + if (containsNow != zone.second[msg.header.frame_id]) { + ++zone_changes; + ROS_ERROR_STREAM("zone changed from " << zone.second[msg.header.frame_id] <<" to " << containsNow); + zoneChanged = true; + zones[zone.first][msg.header.frame_id] = containsNow; + ROS_ERROR_STREAM("zone changed for " << msg.header.frame_id << " to " << zones[zone.first][msg.header.frame_id]); + } + } + + bool modeChanged = false; + if (in_zone != inZoneNow) { + + if (in_zone) { + // check for all other zones, too, if we left the zone with the current link + in_zone = false; + for (const auto& zone : zones) { + for (const auto& link : zone.second) { + if (link.second) { + in_zone = true; + break; + } + } + if (in_zone) { + break; + } + } + if (!in_zone) { + ROS_ERROR_STREAM("mode for link " << msg.header.frame_id << "changed to " << in_zone); + ++mode_changes; + modeChanged = true; + } + } else { + in_zone = true; + ++mode_changes; + modeChanged = true; + ROS_ERROR_STREAM("mode for link " << msg.header.frame_id << "changed to " << in_zone); + } + } + + + if (zoneChanged || !initialized) { +// ROS_ERROR_STREAM("publishing message."); + initialized = true; + panda_mqtt_connector::StampedInZone result; + result.zone = in_zone; + result.zone_changes = zone_changes; + result.mode_changes = mode_changes; + result.mode_changed = modeChanged; + result.header.stamp = ros::Time::now(); + inZonePublisher->publish(result); + } } SafetyZoneMonitor::Zone::Zone(geometry_msgs::Point zone, double zone_size) : @@ -19,8 +83,57 @@ SafetyZoneMonitor::Zone::Zone(geometry_msgs::Point zone, double zone_size) : y_max((zone.y + 0.5) * zone_size), z_max((zone.z + 1.0) * zone_size) {} -bool SafetyZoneMonitor::Zone::contains(const geometry_msgs::Point &point) { +bool SafetyZoneMonitor::Zone::contains(const geometry_msgs::Point &point) const { return point.x >= x_min && point.x <= x_max && point.y >= y_min && point.y <= y_max && point.z >= z_min && point.z <= z_max; } + +bool SafetyZoneMonitor::Zone::operator<(const SafetyZoneMonitor::Zone &rhs) const { + if (x_min < rhs.x_min) + return true; + if (rhs.x_min < x_min) + return false; + if (y_min < rhs.y_min) + return true; + if (rhs.y_min < y_min) + return false; + if (z_min < rhs.z_min) + return true; + if (rhs.z_min < z_min) + return false; + if (x_max < rhs.x_max) + return true; + if (rhs.x_max < x_max) + return false; + if (y_max < rhs.y_max) + return true; + if (rhs.y_max < y_max) + return false; + return z_max < rhs.z_max; +} + +bool SafetyZoneMonitor::Zone::operator>(const SafetyZoneMonitor::Zone &rhs) const { + return rhs < *this; +} + +bool SafetyZoneMonitor::Zone::operator<=(const SafetyZoneMonitor::Zone &rhs) const { + return !(rhs < *this); +} + +bool SafetyZoneMonitor::Zone::operator>=(const SafetyZoneMonitor::Zone &rhs) const { + return !(*this < rhs); +} + +bool SafetyZoneMonitor::Zone::operator==(const SafetyZoneMonitor::Zone &rhs) const { + return x_min == rhs.x_min && + y_min == rhs.y_min && + z_min == rhs.z_min && + x_max == rhs.x_max && + y_max == rhs.y_max && + z_max == rhs.z_max; +} + +bool SafetyZoneMonitor::Zone::operator!=(const SafetyZoneMonitor::Zone &rhs) const { + return !(rhs == *this); +} diff --git a/src/SafetyZoneMonitor.h b/src/SafetyZoneMonitor.h index 3731521..004fbdf 100644 --- a/src/SafetyZoneMonitor.h +++ b/src/SafetyZoneMonitor.h @@ -6,7 +6,8 @@ #define SRC_SAFETYZONEMONITOR_H #include <geometry_msgs/Point.h> -#include <gazebo_msgs/LinkStates.h> +#include <geometry_msgs/PoseStamped.h> + #include <panda_mqtt_connector/StampedInZone.h> #include "ros/ros.h" @@ -26,64 +27,35 @@ private: Zone(geometry_msgs::Point zone, double zone_size); - bool contains(const geometry_msgs::Point &point); + bool operator==(const Zone &rhs) const; + + bool operator!=(const Zone &rhs) const; + + bool operator<(const Zone &rhs) const; + + bool operator>(const Zone &rhs) const; + + bool operator<=(const Zone &rhs) const; + + bool operator>=(const Zone &rhs) const; + + bool contains(const geometry_msgs::Point &point) const; }; - std::vector<Zone> zones; + std::map<Zone, std::map<std::string,bool>> zones; double zone_size; - std::set<std::string> topics; - std::shared_ptr<ros::Publisher> publisher; + std::shared_ptr<ros::Publisher> inZonePublisher; + bool in_zone{}; + unsigned int zone_changes{}; + unsigned int mode_changes{}; public: SafetyZoneMonitor(double zoneSize, ros::Publisher &publisher); void addZone(geometry_msgs::Point zone); - void addTopic(std::string topic) { - topics.insert(topic); - } - - void providerCallback(const gazebo_msgs::LinkStates &msg) { - - static int counter; - static bool initialized; - static bool res; - - counter++; - - std::vector<std::string> names{msg.name}; - - for (int link_number = 0; link_number < names.size(); ++link_number) { - if (topics.find(names[link_number]) != topics.end()) { - for (auto zone : zones) { - if (zone.contains(msg.pose.at(link_number).position)) { - // the point is contained in the zone, so we can stop checking and send an according message - if ((counter%100==1) || !res || !initialized) { - initialized = true; - res = true; - 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; - } - } - } - } - // no part of the robot is in a zone - if ((counter%100==1) || res || !initialized) { - ROS_INFO_STREAM("left zone in run " << counter); - initialized = true; - res = false; - panda_mqtt_connector::StampedInZone result; - result.zone = false; - result.header.stamp = ros::Time::now(); - publisher->publish(result); - } - } + void providerCallback(const geometry_msgs::PoseStamped &msg); }; diff --git a/src/safety_zone_monitor.cpp b/src/safety_zone_monitor.cpp index 948309e..d99c2bf 100644 --- a/src/safety_zone_monitor.cpp +++ b/src/safety_zone_monitor.cpp @@ -39,7 +39,7 @@ namespace safety_zone_monitor { } for (const auto &pair : element_topics) { std::string mqttTopic{group + "/" + pair.first}; - topics.push_back(pair.second); + topics.push_back(mqttTopic); } } } @@ -59,7 +59,7 @@ namespace safety_zone_monitor { int main(int argc, char **argv) { // setup this ros-node - ros::init(argc, argv, "safety_zone_spawner"); + ros::init(argc, argv, "safety_zone_monitor"); ros::NodeHandle n("panda_mqtt_connector"); // wait for gazebo @@ -77,8 +77,6 @@ int main(int argc, char **argv) { SafetyZoneMonitor monitor{size, zoneStatePublisher}; - auto sub = n.subscribe("/gazebo/link_states", 10000, &SafetyZoneMonitor::providerCallback, &monitor); - // get list of zone voxels std::vector<std::string> voxels; if (!n.getParam("zones", voxels)) { @@ -98,8 +96,9 @@ int main(int argc, char **argv) { monitor.addZone(point); } - for (auto topic : safety_zone_monitor::readTopicList(n)) { - monitor.addTopic(topic); + std::set<ros::Subscriber> topicSubscribers; + for (const auto& topic : safety_zone_monitor::readTopicList(n)) { + topicSubscribers.insert(n.subscribe(n.getNamespace() + "/" + topic, 1000, &SafetyZoneMonitor::providerCallback, &monitor)); } ros::MultiThreadedSpinner spinner(1); -- GitLab