diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3957731f565633f22226199ea77c43c603602ed3..6c5493304b8826dff78f6d2a4ecb6d39f6eeef79 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -60,21 +60,23 @@ include_directories(${catkin_INCLUDE_DIRS} ${SDF_INCLUDE_DIRS})
# add custom controller as library
add_library(${PROJECT_NAME}_controllers_lib src/joint_position_controller.cpp)
-add_library(GazeboZoneSpawner src/GazeboZoneSpawner.cpp src/GazeboZoneSpawner.h)
+add_library(GazeboZoneSpawner src/SafetyZones/GazeboZoneSpawner.cpp src/SafetyZones/GazeboZoneSpawner.h)
+add_library(SafetyEnvironmentCreator src/SafetyZones/SafetyEnvironmentCreator.cpp src/SafetyZones/SafetyEnvironmentCreator.h)
# Specify libraries to link a library or executable target against
target_link_libraries(${PROJECT_NAME}_controllers_lib ${catkin_LIBRARIES})
-#target_link_libraries(SafetyEnvironmentCreator ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+target_link_libraries(GazeboZoneSpawner ${catkin_LIBRARIES} ${SDF_LIBRARIES})
+target_link_libraries(SafetyEnvironmentCreator ${catkin_LIBRARIES})
add_executable(robot_control_node src/robot_control_node.cpp)
add_executable(box_publisher_node src/box_publisher.cpp)
add_executable(robot_state_initializer_node src/robot_state_initializer.cpp)
-add_executable(SafetyAwarePlanner src/SafetyAwarePlanner.cpp src/SafetyEnvironmentCreator.cpp)
+add_executable(SafetyAwarePlanner src/SafetyAwarePlanner.cpp)
+
-target_link_libraries(GazeboZoneSpawner ${catkin_LIBRARIES} ${SDF_LIBRARIES})
target_link_libraries(robot_control_node ${catkin_LIBRARIES} boost_filesystem ${JSONCPP_LIBRARIES} ${SDF_LIBRARIES} GazeboZoneSpawner)
target_link_libraries(box_publisher_node ${catkin_LIBRARIES})
target_link_libraries(robot_state_initializer_node ${catkin_LIBRARIES})
-target_link_libraries(SafetyAwarePlanner ${catkin_LIBRARIES})
+target_link_libraries(SafetyAwarePlanner ${catkin_LIBRARIES} SafetyEnvironmentCreator)
# target_link_libraries(constraint_planner ${catkin_LIBRARIES} ${Boost_LIBRARIES} SafetyEnvironmentCreator)
diff --git a/src/SafetyAwarePlanner.cpp b/src/SafetyAwarePlanner.cpp
index 460e9ae5efdee20eb5d37305c246c8a659916a8e..b75d10b35e93594a2b687d8e4a44ed6fc1bb5c20 100644
--- a/src/SafetyAwarePlanner.cpp
+++ b/src/SafetyAwarePlanner.cpp
@@ -3,7 +3,7 @@
#include <moveit_msgs/CollisionObject.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
-#include "SafetyEnvironmentCreator.cpp"
+#include "SafetyZones/SafetyEnvironmentCreator.cpp"
int main(int argc, char** argv)
{
diff --git a/src/SafetyEnvironmentCreator.cpp b/src/SafetyEnvironmentCreator.cpp
deleted file mode 100644
index f6ccedb8084b4e384add979d5a65d05fd07e0b7d..0000000000000000000000000000000000000000
--- a/src/SafetyEnvironmentCreator.cpp
+++ /dev/null
@@ -1,270 +0,0 @@
-//
-// Created by Sebastian Ebert
-//
-
-#include <moveit/move_group_interface/move_group_interface.h>
-#include <moveit_msgs/CollisionObject.h>
-#include <moveit_visual_tools/moveit_visual_tools.h>
-
-class SafetyEnvironmentCreator {
-
- private:
- double safety_zone_width;
- double safety_zone_height;
- double safety_zone_depth;
- double zone_orientation_w;
- double zone_orientation_x;
- double zone_orientation_y;
- double zone_orientation_z;
- double zone_position_x;
- double zone_position_y;
- double zone_position_z;
-
- public:
- /**
- * ctor
- * @param shape describes the inner dimensions of the safety zone
- * @param pose describes position and orientation of the safety zone
- * note: current zone-threshold is 0.1
- */
- SafetyEnvironmentCreator(shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose) {
- if(shape.type != shape.BOX){
- ROS_ERROR("Safety-zone could not be created due to wrong shape-type");
- safety_zone_depth = 0.0;
- safety_zone_width = 0.0;
- safety_zone_height = 0.0;
- zone_orientation_w = 1.0;
- zone_orientation_x = 1.0;
- zone_orientation_y = 1.0;
- zone_orientation_z = 1.0;
- }else {
- safety_zone_depth = shape.dimensions[0];
- safety_zone_width = shape.dimensions[1];
- safety_zone_height = shape.dimensions[2];
- zone_orientation_w = pose.orientation.w;
- zone_orientation_x = pose.orientation.x;
- zone_orientation_y = pose.orientation.y;
- zone_orientation_z = pose.orientation.z;
- zone_position_x = pose.position.x;
- zone_position_y = pose.position.y;
- zone_position_z = pose.position.z;
- }
- }
-
- double getZoneWidth() const {
- return safety_zone_width;
- }
-
- double getZoneHeight() const {
- return safety_zone_height;
- }
-
- double getZoneDepth() const {
- return safety_zone_depth;
- }
-
- double getZoneOrientationW() const {
- return zone_orientation_w;
- }
-
- double getZoneOrientationX() const {
- return zone_orientation_x;
- }
-
- double getZoneOrientationY() const {
- return zone_orientation_y;
- }
-
- double getZoneOrientationZ() const {
- return zone_orientation_z;
- }
-
- std::vector<moveit_msgs::CollisionObject> createCentralizedSafetyEnvironment(moveit::planning_interface::MoveGroupInterface &move_group) {
-
- std::vector<moveit_msgs::CollisionObject> collision_objects;
-
- // OBJECT 1 ( left box / -y )
- // ^^^^^^^^
- // Define a collision object ROS message.
- moveit_msgs::CollisionObject collision_object;
- collision_object.header.frame_id = move_group.getPlanningFrame();
-
- // The id of the object is used to identify it.
- collision_object.id = "box1";
-
- // Define a box to add to the world.
- shape_msgs::SolidPrimitive primitive;
- primitive.type = primitive.BOX;
- primitive.dimensions.resize(3);
-
- primitive.dimensions[0] = getZoneDepth() + 0.2; // x
- primitive.dimensions[1] = 0.1; // y
- primitive.dimensions[2] = getZoneHeight(); // z
-
- // Define a pose for the box (specified relative to frame_id)
- geometry_msgs::Pose box_pose;
- box_pose.orientation.w = 1.0;
- box_pose.position.x = zone_position_x;
- box_pose.position.y = zone_position_y + (-1 * ((getZoneWidth() / 2) + 0.05));
- box_pose.position.z = zone_position_z + (primitive.dimensions[2] / 2);
-
- collision_object.primitives.push_back(primitive);
- collision_object.primitive_poses.push_back(box_pose);
- collision_object.operation = collision_object.ADD;
-
- collision_objects.push_back(collision_object);
-
- // OBJECT 2 right box / +y )
- // ^^^^^^^^
- moveit_msgs::CollisionObject collision_object_2;
- collision_object_2.header.frame_id = move_group.getPlanningFrame();
-
- // The id of the object is used to identify it.
- collision_object_2.id = "box2";
-
- // Define a box to add to the world.
- shape_msgs::SolidPrimitive primitive_2;
- primitive_2.type = primitive.BOX;
- primitive_2.dimensions.resize(3);
- primitive_2.dimensions[0] = getZoneDepth() + 0.2;
- primitive_2.dimensions[1] = 0.1;
- primitive_2.dimensions[2] = getZoneHeight();
-
- // Define a pose for the box (specified relative to frame_id)
- geometry_msgs::Pose box_pose_2;
- box_pose_2.orientation.w = 1.0;
- box_pose_2.position.x = zone_position_x;
- box_pose_2.position.y = zone_position_y + ((getZoneWidth() / 2) + 0.05);
- box_pose_2.position.z = zone_position_z + (primitive_2.dimensions[2] / 2);
-
- collision_object_2.primitives.push_back(primitive_2);
- collision_object_2.primitive_poses.push_back(box_pose_2);
- collision_object_2.operation = collision_object_2.ADD;
-
- collision_objects.push_back(collision_object_2);
-
- // OBJECT 3 (front box / +x)
- // ^^^^^^^^
- moveit_msgs::CollisionObject collision_object_3;
- collision_object_3.header.frame_id = move_group.getPlanningFrame();
-
- // The id of the object is used to identify it.
- collision_object_3.id = "box3";
-
- // Define a box to add to the world.
- shape_msgs::SolidPrimitive primitive_3;
- primitive_3.type = primitive.BOX;
- primitive_3.dimensions.resize(3);
- primitive_3.dimensions[0] = 0.1;
- primitive_3.dimensions[1] = getZoneWidth() + 0.2;
- primitive_3.dimensions[2] = getZoneHeight();
-
- // Define a pose for the box (specified relative to frame_id)
- geometry_msgs::Pose box_pose_3;
- box_pose_3.orientation.w = 1.0;
- box_pose_3.position.x = zone_position_x + ((getZoneDepth() / 2) + 0.05);
- box_pose_3.position.y = zone_position_y;
- box_pose_3.position.z = zone_position_z + (primitive_3.dimensions[2] / 2);
-
- collision_object_3.primitives.push_back(primitive_3);
- collision_object_3.primitive_poses.push_back(box_pose_3);
- collision_object_3.operation = collision_object_3.ADD;
-
- collision_objects.push_back(collision_object_3);
-
- // OBJECT 4 (back box / -x)
- // ^^^^^^^^
- moveit_msgs::CollisionObject collision_object_4;
- collision_object_4.header.frame_id = move_group.getPlanningFrame();
-
- // The id of the object is used to identify it.
- collision_object_4.id = "box4";
-
- // Define a box to add to the world.
- shape_msgs::SolidPrimitive primitive_4;
- primitive_4.type = primitive.BOX;
- primitive_4.dimensions.resize(3);
- primitive_4.dimensions[0] = 0.1;
- primitive_4.dimensions[1] = getZoneWidth() + 0.2;
- primitive_4.dimensions[2] = getZoneHeight();
-
- // Define a pose for the box (specified relative to frame_id)
- geometry_msgs::Pose box_pose_4;
- box_pose_4.orientation.w = 1.0;
- box_pose_4.position.x = zone_position_x + (-1 * ((getZoneDepth() / 2) + 0.05));
- box_pose_4.position.y = zone_position_y;
- box_pose_4.position.z = zone_position_z + (primitive_4.dimensions[2] / 2);
-
- collision_object_4.primitives.push_back(primitive_4);
- collision_object_4.primitive_poses.push_back(box_pose_4);
- collision_object_4.operation = collision_object_4.ADD;
-
- collision_objects.push_back(collision_object_4);
-
- // OBJECT 5 (floor / -z)
- // ^^^^^^^^
- moveit_msgs::CollisionObject collision_object_5;
- collision_object_5.header.frame_id = move_group.getPlanningFrame();
-
- // The id of the object is used to identify it.
- collision_object_5.id = "box5";
-
- // Define a box to add to the world.
- shape_msgs::SolidPrimitive primitive_5;
- primitive_5.type = primitive.BOX;
- primitive_5.dimensions.resize(3);
- primitive_5.dimensions[0] = getZoneDepth() + 0.2;
- primitive_5.dimensions[1] = getZoneWidth() + 0.2;
- primitive_5.dimensions[2] = 0.1;
-
- // Define a pose for the box (specified relative to frame_id)
- geometry_msgs::Pose box_pose_5;
- box_pose_5.orientation.w = 1.0;
- box_pose_5.position.x = zone_position_x;
- box_pose_5.position.y = zone_position_y;
- box_pose_5.position.z = zone_position_z - 0.05;
-
- collision_object_5.primitives.push_back(primitive_5);
- collision_object_5.primitive_poses.push_back(box_pose_5);
- collision_object_5.operation = collision_object_5.ADD;
-
- collision_objects.push_back(collision_object_5);
-
- // OBJECT 6 (top / +z)
- // ^^^^^^^^
- moveit_msgs::CollisionObject collision_object_6;
- collision_object_6.header.frame_id = move_group.getPlanningFrame();
-
- // The id of the object is used to identify it.
- collision_object_6.id = "box6";
-
- // Define a box to add to the world.
- shape_msgs::SolidPrimitive primitive_6;
- primitive_6.type = primitive.BOX;
- primitive_6.dimensions.resize(3);
- primitive_6.dimensions[0] = getZoneDepth() + 0.2;
- primitive_6.dimensions[1] = getZoneWidth() + 0.2;
- primitive_6.dimensions[2] = 0.1;
-
- // Define a pose for the box (specified relative to frame_id)
- geometry_msgs::Pose box_pose_6;
- box_pose_6.orientation.w = 1.0;
- box_pose_6.position.x = zone_position_x;
- box_pose_6.position.y = zone_position_y;
- box_pose_6.position.z = zone_position_z + getZoneHeight() + 0.05;
-
- collision_object_6.primitives.push_back(primitive_6);
- collision_object_6.primitive_poses.push_back(box_pose_6);
- collision_object_6.operation = collision_object_6.ADD;
- collision_objects.push_back(collision_object_6);
-
- /*for(std::size_t i=0; i<collision_objects.size(); ++i) {
- collision_objects[i].primitive_poses[0].orientation.w = getZoneOrientationW();
- collision_objects[i].primitive_poses[0].orientation.x = getZoneOrientationX();
- collision_objects[i].primitive_poses[0].orientation.y = getZoneOrientationY();
- collision_objects[i].primitive_poses[0].orientation.z = getZoneOrientationZ();
- }*/
-
- return collision_objects;
- }
-};
\ No newline at end of file
diff --git a/src/GazeboZoneSpawner.cpp b/src/SafetyZones/GazeboZoneSpawner.cpp
similarity index 100%
rename from src/GazeboZoneSpawner.cpp
rename to src/SafetyZones/GazeboZoneSpawner.cpp
diff --git a/src/GazeboZoneSpawner.h b/src/SafetyZones/GazeboZoneSpawner.h
similarity index 100%
rename from src/GazeboZoneSpawner.h
rename to src/SafetyZones/GazeboZoneSpawner.h
diff --git a/src/SafetyZones/SafetyEnvironmentCreator.cpp b/src/SafetyZones/SafetyEnvironmentCreator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..76835262f9043a0ecb5f52dfc28e40003fd253cb
--- /dev/null
+++ b/src/SafetyZones/SafetyEnvironmentCreator.cpp
@@ -0,0 +1,250 @@
+//
+// Created by Sebastian Ebert
+//
+
+#include "SafetyEnvironmentCreator.h"
+
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit_msgs/CollisionObject.h>
+
+SafetyEnvironmentCreator::SafetyEnvironmentCreator(shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose) {
+ if(shape.type != shape.BOX){
+ ROS_ERROR("Safety-zone could not be created due to wrong shape-type");
+ safety_zone_depth = 0.0;
+ safety_zone_width = 0.0;
+ safety_zone_height = 0.0;
+ zone_orientation_w = 1.0;
+ zone_orientation_x = 1.0;
+ zone_orientation_y = 1.0;
+ zone_orientation_z = 1.0;
+ }else {
+ safety_zone_depth = shape.dimensions[0];
+ safety_zone_width = shape.dimensions[1];
+ safety_zone_height = shape.dimensions[2];
+ zone_orientation_w = pose.orientation.w;
+ zone_orientation_x = pose.orientation.x;
+ zone_orientation_y = pose.orientation.y;
+ zone_orientation_z = pose.orientation.z;
+ zone_position_x = pose.position.x;
+ zone_position_y = pose.position.y;
+ zone_position_z = pose.position.z;
+ }
+}
+
+double SafetyEnvironmentCreator::getZoneWidth() const {
+ return safety_zone_width;
+}
+
+double SafetyEnvironmentCreator::getZoneHeight() const {
+ return safety_zone_height;
+}
+
+double SafetyEnvironmentCreator::getZoneDepth() const {
+ return safety_zone_depth;
+}
+
+double SafetyEnvironmentCreator::getZoneOrientationW() const {
+ return zone_orientation_w;
+}
+
+double SafetyEnvironmentCreator::getZoneOrientationX() const {
+ return zone_orientation_x;
+}
+
+double SafetyEnvironmentCreator::getZoneOrientationY() const {
+ return zone_orientation_y;
+}
+
+double SafetyEnvironmentCreator::getZoneOrientationZ() const {
+ return zone_orientation_z;
+}
+
+std::vector<moveit_msgs::CollisionObject>
+SafetyEnvironmentCreator::createCentralizedSafetyEnvironment(moveit::planning_interface::MoveGroupInterface &move_group) {
+
+ std::vector<moveit_msgs::CollisionObject> collision_objects;
+
+ // OBJECT 1 ( left box / -y )
+ // ^^^^^^^^
+ // Define a collision object ROS message.
+ moveit_msgs::CollisionObject collision_object;
+ collision_object.header.frame_id = move_group.getPlanningFrame();
+
+ // The id of the object is used to identify it.
+ collision_object.id = "box1";
+
+ // Define a box to add to the world.
+ shape_msgs::SolidPrimitive primitive;
+ primitive.type = primitive.BOX;
+ primitive.dimensions.resize(3);
+
+ primitive.dimensions[0] = getZoneDepth() + 0.2; // x
+ primitive.dimensions[1] = 0.1; // y
+ primitive.dimensions[2] = getZoneHeight(); // z
+
+ // Define a pose for the box (specified relative to frame_id)
+ geometry_msgs::Pose box_pose;
+ box_pose.orientation.w = 1.0;
+ box_pose.position.x = zone_position_x;
+ box_pose.position.y = zone_position_y + (-1 * ((getZoneWidth() / 2) + 0.05));
+ box_pose.position.z = zone_position_z + (primitive.dimensions[2] / 2);
+
+ collision_object.primitives.push_back(primitive);
+ collision_object.primitive_poses.push_back(box_pose);
+ collision_object.operation = collision_object.ADD;
+
+ collision_objects.push_back(collision_object);
+
+ // OBJECT 2 right box / +y )
+ // ^^^^^^^^
+ moveit_msgs::CollisionObject collision_object_2;
+ collision_object_2.header.frame_id = move_group.getPlanningFrame();
+
+ // The id of the object is used to identify it.
+ collision_object_2.id = "box2";
+
+ // Define a box to add to the world.
+ shape_msgs::SolidPrimitive primitive_2;
+ primitive_2.type = primitive.BOX;
+ primitive_2.dimensions.resize(3);
+ primitive_2.dimensions[0] = getZoneDepth() + 0.2;
+ primitive_2.dimensions[1] = 0.1;
+ primitive_2.dimensions[2] = getZoneHeight();
+
+ // Define a pose for the box (specified relative to frame_id)
+ geometry_msgs::Pose box_pose_2;
+ box_pose_2.orientation.w = 1.0;
+ box_pose_2.position.x = zone_position_x;
+ box_pose_2.position.y = zone_position_y + ((getZoneWidth() / 2) + 0.05);
+ box_pose_2.position.z = zone_position_z + (primitive_2.dimensions[2] / 2);
+
+ collision_object_2.primitives.push_back(primitive_2);
+ collision_object_2.primitive_poses.push_back(box_pose_2);
+ collision_object_2.operation = collision_object_2.ADD;
+
+ collision_objects.push_back(collision_object_2);
+
+ // OBJECT 3 (front box / +x)
+ // ^^^^^^^^
+ moveit_msgs::CollisionObject collision_object_3;
+ collision_object_3.header.frame_id = move_group.getPlanningFrame();
+
+ // The id of the object is used to identify it.
+ collision_object_3.id = "box3";
+
+ // Define a box to add to the world.
+ shape_msgs::SolidPrimitive primitive_3;
+ primitive_3.type = primitive.BOX;
+ primitive_3.dimensions.resize(3);
+ primitive_3.dimensions[0] = 0.1;
+ primitive_3.dimensions[1] = getZoneWidth() + 0.2;
+ primitive_3.dimensions[2] = getZoneHeight();
+
+ // Define a pose for the box (specified relative to frame_id)
+ geometry_msgs::Pose box_pose_3;
+ box_pose_3.orientation.w = 1.0;
+ box_pose_3.position.x = zone_position_x + ((getZoneDepth() / 2) + 0.05);
+ box_pose_3.position.y = zone_position_y;
+ box_pose_3.position.z = zone_position_z + (primitive_3.dimensions[2] / 2);
+
+ collision_object_3.primitives.push_back(primitive_3);
+ collision_object_3.primitive_poses.push_back(box_pose_3);
+ collision_object_3.operation = collision_object_3.ADD;
+
+ collision_objects.push_back(collision_object_3);
+
+ // OBJECT 4 (back box / -x)
+ // ^^^^^^^^
+ moveit_msgs::CollisionObject collision_object_4;
+ collision_object_4.header.frame_id = move_group.getPlanningFrame();
+
+ // The id of the object is used to identify it.
+ collision_object_4.id = "box4";
+
+ // Define a box to add to the world.
+ shape_msgs::SolidPrimitive primitive_4;
+ primitive_4.type = primitive.BOX;
+ primitive_4.dimensions.resize(3);
+ primitive_4.dimensions[0] = 0.1;
+ primitive_4.dimensions[1] = getZoneWidth() + 0.2;
+ primitive_4.dimensions[2] = getZoneHeight();
+
+ // Define a pose for the box (specified relative to frame_id)
+ geometry_msgs::Pose box_pose_4;
+ box_pose_4.orientation.w = 1.0;
+ box_pose_4.position.x = zone_position_x + (-1 * ((getZoneDepth() / 2) + 0.05));
+ box_pose_4.position.y = zone_position_y;
+ box_pose_4.position.z = zone_position_z + (primitive_4.dimensions[2] / 2);
+
+ collision_object_4.primitives.push_back(primitive_4);
+ collision_object_4.primitive_poses.push_back(box_pose_4);
+ collision_object_4.operation = collision_object_4.ADD;
+
+ collision_objects.push_back(collision_object_4);
+
+ // OBJECT 5 (floor / -z)
+ // ^^^^^^^^
+ moveit_msgs::CollisionObject collision_object_5;
+ collision_object_5.header.frame_id = move_group.getPlanningFrame();
+
+ // The id of the object is used to identify it.
+ collision_object_5.id = "box5";
+
+ // Define a box to add to the world.
+ shape_msgs::SolidPrimitive primitive_5;
+ primitive_5.type = primitive.BOX;
+ primitive_5.dimensions.resize(3);
+ primitive_5.dimensions[0] = getZoneDepth() + 0.2;
+ primitive_5.dimensions[1] = getZoneWidth() + 0.2;
+ primitive_5.dimensions[2] = 0.1;
+
+ // Define a pose for the box (specified relative to frame_id)
+ geometry_msgs::Pose box_pose_5;
+ box_pose_5.orientation.w = 1.0;
+ box_pose_5.position.x = zone_position_x;
+ box_pose_5.position.y = zone_position_y;
+ box_pose_5.position.z = zone_position_z - 0.05;
+
+ collision_object_5.primitives.push_back(primitive_5);
+ collision_object_5.primitive_poses.push_back(box_pose_5);
+ collision_object_5.operation = collision_object_5.ADD;
+
+ collision_objects.push_back(collision_object_5);
+
+ // OBJECT 6 (top / +z)
+ // ^^^^^^^^
+ moveit_msgs::CollisionObject collision_object_6;
+ collision_object_6.header.frame_id = move_group.getPlanningFrame();
+
+ // The id of the object is used to identify it.
+ collision_object_6.id = "box6";
+
+ // Define a box to add to the world.
+ shape_msgs::SolidPrimitive primitive_6;
+ primitive_6.type = primitive.BOX;
+ primitive_6.dimensions.resize(3);
+ primitive_6.dimensions[0] = getZoneDepth() + 0.2;
+ primitive_6.dimensions[1] = getZoneWidth() + 0.2;
+ primitive_6.dimensions[2] = 0.1;
+
+ // Define a pose for the box (specified relative to frame_id)
+ geometry_msgs::Pose box_pose_6;
+ box_pose_6.orientation.w = 1.0;
+ box_pose_6.position.x = zone_position_x;
+ box_pose_6.position.y = zone_position_y;
+ box_pose_6.position.z = zone_position_z + getZoneHeight() + 0.05;
+
+ collision_object_6.primitives.push_back(primitive_6);
+ collision_object_6.primitive_poses.push_back(box_pose_6);
+ collision_object_6.operation = collision_object_6.ADD;
+ collision_objects.push_back(collision_object_6);
+
+ /*for(std::size_t i=0; i<collision_objects.size(); ++i) {
+ collision_objects[i].primitive_poses[0].orientation.w = getZoneOrientationW();
+ collision_objects[i].primitive_poses[0].orientation.x = getZoneOrientationX();
+ collision_objects[i].primitive_poses[0].orientation.y = getZoneOrientationY();
+ collision_objects[i].primitive_poses[0].orientation.z = getZoneOrientationZ();
+ }*/
+
+ return collision_objects;
+}
diff --git a/src/SafetyZones/SafetyEnvironmentCreator.h b/src/SafetyZones/SafetyEnvironmentCreator.h
new file mode 100644
index 0000000000000000000000000000000000000000..6fbeb83d06c5d7929b0f08d6c5d44b43be17115d
--- /dev/null
+++ b/src/SafetyZones/SafetyEnvironmentCreator.h
@@ -0,0 +1,53 @@
+//
+// Created by Johannes Mey on 31.03.20.
+//
+
+#ifndef PANDA_SIMULATION_SAFETYENVIRONMENTCREATOR_H
+#define PANDA_SIMULATION_SAFETYENVIRONMENTCREATOR_H
+
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit_msgs/CollisionObject.h>
+#include <moveit_visual_tools/moveit_visual_tools.h>
+
+class SafetyEnvironmentCreator {
+
+private:
+ double safety_zone_width;
+ double safety_zone_height;
+ double safety_zone_depth;
+ double zone_orientation_w;
+ double zone_orientation_x;
+ double zone_orientation_y;
+ double zone_orientation_z;
+ double zone_position_x;
+ double zone_position_y;
+ double zone_position_z;
+public:
+ double getZoneWidth() const;
+
+ double getZoneHeight() const;
+
+ double getZoneDepth() const;
+
+ double getZoneOrientationW() const;
+
+ double getZoneOrientationX() const;
+
+ double getZoneOrientationY() const;
+
+ double getZoneOrientationZ() const;
+
+ /**
+ * ctor
+ * @param shape describes the inner dimensions of the safety zone
+ * @param pose describes position and orientation of the safety zone
+ * note: current zone-threshold is 0.1
+ */
+ SafetyEnvironmentCreator(shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose);
+
+
+ std::vector<moveit_msgs::CollisionObject>
+ createCentralizedSafetyEnvironment(moveit::planning_interface::MoveGroupInterface &move_group);
+};
+
+#endif //PANDA_SIMULATION_SAFETYENVIRONMENTCREATOR_H
diff --git a/src/robot_control_node.cpp b/src/robot_control_node.cpp
index 27551f0c5e831e52a580b8c982c6c426eb589909..1c98ec7349271327c9273beb44aa5674d16280a6 100644
--- a/src/robot_control_node.cpp
+++ b/src/robot_control_node.cpp
@@ -6,7 +6,7 @@
#include <ros/ros.h>
#include <boost/filesystem.hpp>
-#include "GazeboZoneSpawner.h"
+#include "SafetyZones/GazeboZoneSpawner.h"
static const std::string PLANNING_GROUP_ARM = "panda_arm";
static const std::string APP_DIRECTORY_NAME = ".panda_simulation";