diff --git a/CMakeLists.txt b/CMakeLists.txt
index b9da816581a1293d9d81875cf6f89bb64e509282..5d364d891ba61031680e95c66693dc60bb2b63eb 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -56,27 +56,22 @@ catkin_package(CATKIN_DEPENDS
# Specify additional locations of header files Your package locations should be listed before other locations
include_directories(${catkin_INCLUDE_DIRS})
+# add custom controller as library
+add_library(${PROJECT_NAME}_controllers_lib src/joint_position_controller.cpp)
+#add_library(SafetyEnvironmentCreator src/SafetyEnvironmentCreator.cpp)
+
+# 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})
+
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(move_group_interface_tutorial src/move_group_interface_tutorial.cpp)
-#add_executable(motion_planning_api_tutorial src/motion_planning_api_tutorial.cpp)
-add_executable(constraint_planner src/constraint_planner.cpp)
+add_executable(constraint_planner src/constraint_planner.cpp src/SafetyEnvironmentCreator.cpp)
target_link_libraries(robot_control_node ${catkin_LIBRARIES} boost_filesystem ${JSONCPP_LIBRARIES})
target_link_libraries(box_publisher_node ${catkin_LIBRARIES})
target_link_libraries(robot_state_initializer_node ${catkin_LIBRARIES})
-#target_link_libraries(move_group_interface_tutorial ${catkin_LIBRARIES} ${Boost_LIBRARIES})
-#target_link_libraries(motion_planning_api_tutorial ${catkin_LIBRARIES} ${Boost_LIBRARIES})
-target_link_libraries(constraint_planner ${catkin_LIBRARIES} ${Boost_LIBRARIES})
-
-# add custom controller as library
-add_library(${PROJECT_NAME}_controllers_lib src/joint_position_controller.cpp)
-
-# Specify libraries to link a library or executable target against
-target_link_libraries(${PROJECT_NAME}_controllers_lib ${catkin_LIBRARIES})
-
-#install(TARGETS move_group_interface_tutorial DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
-#install(TARGETS motion_planning_api_tutorial DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+target_link_libraries(constraint_planner ${catkin_LIBRARIES})
+#target_link_libraries(constraint_planner ${catkin_LIBRARIES} ${Boost_LIBRARIES} SafetyEnvironmentCreator)
-#install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
\ No newline at end of file
diff --git a/src/SafetyEnvironmentCreator.cpp b/src/SafetyEnvironmentCreator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..171342a66621b0bf3fb3e4a1b44089bda00f18a8
--- /dev/null
+++ b/src/SafetyEnvironmentCreator.cpp
@@ -0,0 +1,217 @@
+//
+// 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;
+
+ public:
+ // ctor
+ SafetyEnvironmentCreator(double zone_width, double zone_height, double zone_depth) {
+ safety_zone_width = zone_width;
+ safety_zone_height = zone_height;
+ safety_zone_depth = zone_depth;
+ }
+
+ double getZoneWidth() {
+ return safety_zone_width;
+ }
+
+ double getZoneHeight() {
+ return safety_zone_height;
+ }
+
+ double getZoneDepth() {
+ return safety_zone_depth;
+ }
+
+ 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 = 0.0;
+ box_pose.position.y = -1 * ((getZoneWidth() / 2) + 0.05);
+ box_pose.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 = 0.0;
+ box_pose_2.position.y = ((getZoneWidth() / 2) + 0.05);
+ box_pose_2.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 = ((getZoneDepth() / 2) + 0.05);
+ box_pose_3.position.y = 0.0;
+ box_pose_3.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 = -1 * ((getZoneDepth() / 2) + 0.05);
+ box_pose_4.position.y = 0.0;
+ box_pose_4.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 = 0.0;
+ box_pose_5.position.y = 0.0;
+ box_pose_5.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 5 (floor / -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 = "box5";
+
+ // 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 = 0.0;
+ box_pose_6.position.y = 0.0;
+ box_pose_6.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);
+
+ return collision_objects;
+ }
+};
\ No newline at end of file
diff --git a/src/constraint_planner.cpp b/src/constraint_planner.cpp
index 95259b9685588c1f0f7ddc92f53d02f95c508044..966f9291675afc2f9193d54a530095b82f06ba22 100644
--- a/src/constraint_planner.cpp
+++ b/src/constraint_planner.cpp
@@ -6,9 +6,10 @@
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>
-
#include <moveit_visual_tools/moveit_visual_tools.h>
+#include "SafetyEnvironmentCreator.cpp"
+
int main(int argc, char** argv)
{
ros::init(argc, argv, "CONSTRAINT PLANNER");
@@ -25,8 +26,9 @@ int main(int argc, char** argv)
// Visualization Setup
namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
- visual_tools.deleteAllMarkers();
+ //visual_tools.deleteAllMarkers();
visual_tools.loadRemoteControl();
+ visual_tools.setAlpha(0.5);
Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 1.75;
@@ -42,37 +44,15 @@ int main(int argc, char** argv)
std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
std::ostream_iterator<std::string>(std::cout, ", "));
- // 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] = 0.4;
- primitive.dimensions[1] = 0.1;
- primitive.dimensions[2] = 0.4;
-
- // 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 = 0.4;
- box_pose.position.y = -0.2;
- box_pose.position.z = 1.0;
-
- collision_object.primitives.push_back(primitive);
- collision_object.primitive_poses.push_back(box_pose);
- collision_object.operation = collision_object.ADD;
-
- std::vector<moveit_msgs::CollisionObject> collision_objects;
- collision_objects.push_back(collision_object);
+ // Safety Box
+ SafetyEnvironmentCreator sec(2.0, 2.0 , 2.0);
+ std::vector<moveit_msgs::CollisionObject> collision_objects = sec.createCentralizedSafetyEnvironment(move_group);
+ //visual_tools.processCollisionObjectMsg(collision_objects[2], rviz_visual_tools::RED);
+ // visual_tools.setAlpha(0.0);
// Now, let's add the collision object into the world
ROS_INFO_NAMED("tutorial", "Add an object into the world");
+
planning_scene_interface.addCollisionObjects(collision_objects);
// Show text in RViz of status
@@ -89,6 +69,12 @@ int main(int argc, char** argv)
another_pose.position.z = 0.9;
move_group.setPoseTarget(another_pose);
+
+ //moveit_msgs::MotionPlanRequest mr;
+ //mr.max_velocity_scaling_factor = 0.5;
+ //move_group.constructMotionPlanRequest(mr);
+ move_group.setMaxVelocityScalingFactor(0.1);
+ move_group.setWorkspace(-0.5, -0.5, -0.5, 0, 0, 0);
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("constraint_planner", "Visualizing constraint plan %s", success ? "" : "FAILED");