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

add zone monitor node

parent b0d4a41f
Branches
No related tags found
No related merge requests found
...@@ -163,12 +163,15 @@ add_executable(TimedPlanner src/TimedPlanner.cpp src/TimedPlanner.h) ...@@ -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(MqttToRosNode src/MqttToRosNode.cpp ${PROTO_SRCS} ${PROTO_HDRS})
add_executable(MqttRosConnectionTestNode src/MqttRosConnectionTestNode.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_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_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_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 # Specify libraries to link a library or executable target against
target_link_libraries(TrajectoryUtil target_link_libraries(TrajectoryUtil
...@@ -195,3 +198,5 @@ target_link_libraries(MqttRosConnectionTestNode LINK_PUBLIC ...@@ -195,3 +198,5 @@ target_link_libraries(MqttRosConnectionTestNode LINK_PUBLIC
target_link_libraries(TimedPlanner ${catkin_LIBRARIES} TrajectoryUtil) target_link_libraries(TimedPlanner ${catkin_LIBRARIES} TrajectoryUtil)
target_link_libraries(${PROJECT_NAME}_safety_zone_spawner ${catkin_LIBRARIES}) 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
...@@ -10,5 +10,6 @@ ...@@ -10,5 +10,6 @@
<node name="MqttToRosNode" pkg="panda_mqtt_connector" type="MqttToRosNode" respawn="false" output="screen"/> <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="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_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"/> <node name="MqttRosConnectionTestNode" pkg="panda_mqtt_connector" type="MqttRosConnectionTestNode" respawn="false" output="screen"/>
</launch> </launch>
//
// 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;
}
//
// 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
/*! \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
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment