diff --git a/src/SafetyAwarePlanner.cpp b/src/SafetyAwarePlanner.cpp index b9fb3cf85cf9ec3b64071342b84e9436ff2605cc..460e9ae5efdee20eb5d37305c246c8a659916a8e 100644 --- a/src/SafetyAwarePlanner.cpp +++ b/src/SafetyAwarePlanner.cpp @@ -1,10 +1,5 @@ #include <moveit/move_group_interface/move_group_interface.h> #include <moveit/planning_scene_interface/planning_scene_interface.h> - -#include <moveit_msgs/DisplayRobotState.h> -#include <moveit_msgs/DisplayTrajectory.h> - -#include <moveit_msgs/AttachedCollisionObject.h> #include <moveit_msgs/CollisionObject.h> #include <moveit_visual_tools/moveit_visual_tools.h> @@ -44,17 +39,26 @@ int main(int argc, char** argv) std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(), std::ostream_iterator<std::string>(std::cout, ", ")); - shape_msgs::SolidPrimitive safetybox; - geometry_msgs::Pose box_pose; - safetybox.type = safetybox.BOX; - safetybox.dimensions.resize(3); + shape_msgs::SolidPrimitive safety_box; + geometry_msgs::Pose safety_box_pose; + + safety_box_pose.orientation.w = 1.0; + safety_box_pose.orientation.x = 0.0; + safety_box_pose.orientation.y = 0.0; + safety_box_pose.orientation.z = 0.0; + safety_box_pose.position.x = 0.0; + safety_box_pose.position.y = 0.0; + safety_box_pose.position.z = 0.0; + + safety_box.type = safety_box.BOX; + safety_box.dimensions.resize(3); - safetybox.dimensions[0] = 2.0; - safetybox.dimensions[1] = 2.0; - safetybox.dimensions[2] = 2.0; + safety_box.dimensions[0] = 2.0; + safety_box.dimensions[1] = 2.0; + safety_box.dimensions[2] = 2.0; // Safety Box - SafetyEnvironmentCreator sec(safetybox, box_pose); + SafetyEnvironmentCreator sec(safety_box, safety_box_pose); std::vector<moveit_msgs::CollisionObject> collision_objects = sec.createCentralizedSafetyEnvironment(move_group); //visual_tools.processCollisionObjectMsg(collision_objects[2], rviz_visual_tools::RED); @@ -94,7 +98,6 @@ int main(int argc, char** argv) // Move the simulated robot in gazebo move_group.move(); - move_group.stop(); ros::shutdown(); return 0; diff --git a/src/SafetyEnvironmentCreator.cpp b/src/SafetyEnvironmentCreator.cpp index 4e36529a174bb4845ab08f13ddb8ab7e3d25f1ea..f6ccedb8084b4e384add979d5a65d05fd07e0b7d 100644 --- a/src/SafetyEnvironmentCreator.cpp +++ b/src/SafetyEnvironmentCreator.cpp @@ -12,6 +12,13 @@ class SafetyEnvironmentCreator { 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: /** @@ -26,28 +33,56 @@ class SafetyEnvironmentCreator { safety_zone_depth = 0.0; safety_zone_width = 0.0; safety_zone_height = 0.0; - }else { + 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() { + double getZoneWidth() const { return safety_zone_width; } - double getZoneHeight() { + double getZoneHeight() const { return safety_zone_height; } - double getZoneDepth() { + 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. @@ -69,9 +104,9 @@ class SafetyEnvironmentCreator { // 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; + 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); @@ -98,9 +133,9 @@ class SafetyEnvironmentCreator { // 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; + 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); @@ -127,9 +162,9 @@ class SafetyEnvironmentCreator { // 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; + 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); @@ -156,9 +191,9 @@ class SafetyEnvironmentCreator { // 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; + 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); @@ -185,9 +220,9 @@ class SafetyEnvironmentCreator { // 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; + 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); @@ -195,13 +230,13 @@ class SafetyEnvironmentCreator { collision_objects.push_back(collision_object_5); - // OBJECT 5 (floor / -z) + // 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 = "box5"; + collision_object_6.id = "box6"; // Define a box to add to the world. shape_msgs::SolidPrimitive primitive_6; @@ -214,16 +249,22 @@ class SafetyEnvironmentCreator { // 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; + 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