Skip to content
Snippets Groups Projects
Commit 5aaf32fa authored by Johannes Mey's avatar Johannes Mey
Browse files

implement safety zone monitor

parent 13116a14
No related branches found
No related tags found
No related merge requests found
# 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
......@@ -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;
......
......@@ -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);
}
......@@ -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);
};
......
......@@ -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);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment