SafetyZoneMonitor.h

//
// Created by jm on 18.07.20.
//

#ifndef SRC_SAFETYZONEMONITOR_H
#define SRC_SAFETYZONEMONITOR_H

#include <geometry_msgs/Point.h>
#include <gazebo_msgs/LinkStates.h>
#include <std_msgs/Bool.h>
#include "ros/ros.h"


class SafetyZoneMonitor {

private:

  struct Zone {
    geometry_msgs::Point zone;
    double x_min;
    double y_min;
    double z_min;
    double x_max;
    double y_max;
    double z_max;

    Zone(geometry_msgs::Point zone, double zone_size);

    bool contains(const geometry_msgs::Point &point);
  };

  std::vector<Zone> zones;
  double zone_size;
  std::set<std::string> topics;
  std::shared_ptr<ros::Publisher> publisher;

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

//    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) {
      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 (!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);
              publisher->publish(result);
            }
            return;
          }
        }
      }
    }
    // no part of the robot is in a zone
    if (res || !initialized) {
      ROS_ERROR_STREAM("left zone in run " << counter);
      initialized = true;
      res = false;
      std_msgs::Bool result;
      result.data = false;
      publisher->publish(result);
    }
  }

};


#endif //SRC_SAFETYZONEMONITOR_H