add zone monitor node

CMakeLists.txt

@@ -163,12 +163,15 @@ add_executable(TimedPlanner src/TimedPlanner.cpp src/TimedPlanner.h)
 add_executable(MqttToRosNode src/MqttToRosNode.cpp ${PROTO_SRCS} ${PROTO_HDRS})
 add_executable(MqttRosConnectionTestNode src/MqttRosConnectionTestNode.cpp ${PROTO_SRCS} ${PROTO_HDRS})
 add_executable(${PROJECT_NAME}_safety_zone_spawner src/safety_zone_spawner.cpp)
+add_executable(${PROJECT_NAME}_safety_zone_monitor src/safety_zone_monitor.cpp src/SafetyZoneMonitor.cpp src/SafetyZoneMonitor.h)
 
 
 set_target_properties(${PROJECT_NAME}_safety_zone_spawner PROPERTIES OUTPUT_NAME safety_zone_spawner PREFIX "")
+set_target_properties(${PROJECT_NAME}_safety_zone_monitor PROPERTIES OUTPUT_NAME safety_zone_monitor PREFIX "")
 
 
 add_dependencies(${PROJECT_NAME}_safety_zone_spawner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(${PROJECT_NAME}_safety_zone_monitor ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 # Specify libraries to link a library or executable target against
 target_link_libraries(TrajectoryUtil
@@ -195,3 +198,5 @@ target_link_libraries(MqttRosConnectionTestNode LINK_PUBLIC
 target_link_libraries(TimedPlanner ${catkin_LIBRARIES} TrajectoryUtil)
 
 target_link_libraries(${PROJECT_NAME}_safety_zone_spawner ${catkin_LIBRARIES})
+
+target_link_libraries(${PROJECT_NAME}_safety_zone_monitor ${catkin_LIBRARIES})
\ No newline at end of file

launch/simulation_rosrag_test.launch

@@ -10,5 +10,6 @@
     <node name="MqttToRosNode" pkg="panda_mqtt_connector" type="MqttToRosNode" respawn="false" output="screen"/>
     <node name="RobotStateProvider" pkg="panda_mqtt_connector" type="RobotStateProvider" respawn="false" output="screen"/>
     <node name="safety_zone_spawner" pkg="panda_mqtt_connector" type="safety_zone_spawner" respawn="false" output="screen"/>
+    <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"/>
 </launch>

src/SafetyZoneMonitor.cpp

+//
+// Created by jm on 18.07.20.
+//
+
+#include "SafetyZoneMonitor.h"
+
+SafetyZoneMonitor::SafetyZoneMonitor(double zoneSize, ros::Publisher &publisher) : zone_size(zoneSize), publisher(&publisher) {}
+
+void SafetyZoneMonitor::addZone(geometry_msgs::Point zone) {
+  zones.push_back(Zone(zone, zone_size));
+}
+
+SafetyZoneMonitor::Zone::Zone(geometry_msgs::Point zone, double zone_size) :
+    zone(zone),
+    x_min((zone.x - 0.5) * zone_size),
+    y_min((zone.y - 0.5) * zone_size),
+    z_min((zone.z) * zone_size),
+    x_max((zone.x + 0.5) * 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) {
+  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;
+}

src/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

src/safety_zone_monitor.cpp

+/*! \file safety_zone_spawner.cpp
+    \brief ROS node main file that spawns safety zones in gazebo.
+
+    \author Johannes Mey
+    \date 28.06.20
+*/
+
+#include <ros/ros.h>
+#include <shape_msgs/SolidPrimitive.h>
+#include <geometry_msgs/Pose.h>
+#include <std_msgs/Bool.h>
+#include "SafetyZoneMonitor.h"
+
+namespace safety_zone_monitor {
+  std::vector<std::string> readTopicList(const ros::NodeHandle &n) {
+    std::vector<std::string> topics;
+
+    const std::vector<std::string> elementTypes{"/parts/", "/end_effectors/"};
+    std::vector<std::string> parameterNames;
+    n.getParamNames(parameterNames);
+    std::set<std::string> groups{};
+    for (const auto &elementType : elementTypes) {
+      // get the groups
+      for (const auto &param : parameterNames) {
+        std::string prefix{n.getNamespace() + elementType};
+        if (param.rfind(prefix, 0) == 0) {
+          std::string rest{param.substr(prefix.size())};
+          std::string element{rest.substr(0, rest.rfind('/'))};
+          groups.insert(element);
+        }
+      }
+
+      // get the elements in the group
+      for (const auto &group: groups) {
+        std::map<std::string, std::string> element_topics{};
+        std::string key{n.getNamespace() + elementType + group};
+        if (!n.getParam(key, element_topics)) {
+          ROS_ERROR_STREAM("Unable to retrieve value for " << key);
+        }
+        for (const auto &pair : element_topics) {
+          std::string mqttTopic{group + "/" + pair.first};
+          topics.push_back(pair.second);
+        }
+      }
+    }
+
+    ROS_INFO_STREAM("Observing " << topics.size() << " Gazebo elements.");
+    for (const auto &topic : topics) {
+      ROS_INFO_STREAM("Observing gazebo element: " << topic);
+    }
+    return topics;
+  }
+}
+
+/**
+ * The main method of the ROS node
+ *
+ */
+int main(int argc, char **argv) {
+
+  // setup this ros-node
+  ros::init(argc, argv, "safety_zone_spawner");
+  ros::NodeHandle n("panda_mqtt_connector");
+
+  // wait for gazebo
+  // TODO this should actually wait until gazebo has started
+  ros::Duration(2.0).sleep();
+
+  // get size
+  double size = 0.5;
+  if (!n.getParam("zone_size", size)) {
+    ROS_ERROR_STREAM("Could not get string value for " << n.getNamespace() << "/size from param server.");
+    exit(-1);
+  }
+
+  ros::Publisher zoneStatePublisher = n.advertise<std_msgs::Bool>("robot_in_zone", 1000);
+
+  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)) {
+    ROS_WARN_STREAM("Could not get string values for " << n.getNamespace() << "/zones from param server. Exiting.");
+    exit(0);
+  }
+
+  for (const std::string &voxel: voxels) {
+    geometry_msgs::Point point;
+
+    std::stringstream stream(voxel);
+    stream >> point.x >> point.y;
+    if (stream) {
+      stream >> point.z; // the z value is 0 by default
+    }
+
+    monitor.addZone(point);
+  }
+
+  for (auto topic : safety_zone_monitor::readTopicList(n)) {
+    monitor.addTopic(topic);
+  }
+
+  ros::MultiThreadedSpinner spinner(1);
+  ros::spin(spinner);
+
+}
\ No newline at end of file