implement safety zone monitor

msg/StampedInZone.msg

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

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;

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);
+}

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);
 
 };
 

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);