Skip to content
Snippets Groups Projects
Commit 1ef086ae authored by Sebastian Ebert's avatar Sebastian Ebert
Browse files

the safety zone can now be moved, todo: rotation

parent 989d4788
No related branches found
No related tags found
No related merge requests found
#include <moveit/move_group_interface/move_group_interface.h> #include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_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_msgs/CollisionObject.h>
#include <moveit_visual_tools/moveit_visual_tools.h> #include <moveit_visual_tools/moveit_visual_tools.h>
...@@ -44,17 +39,26 @@ int main(int argc, char** argv) ...@@ -44,17 +39,26 @@ int main(int argc, char** argv)
std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(), std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
std::ostream_iterator<std::string>(std::cout, ", ")); std::ostream_iterator<std::string>(std::cout, ", "));
shape_msgs::SolidPrimitive safetybox; shape_msgs::SolidPrimitive safety_box;
geometry_msgs::Pose box_pose; geometry_msgs::Pose safety_box_pose;
safetybox.type = safetybox.BOX;
safetybox.dimensions.resize(3); 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; safety_box.dimensions[0] = 2.0;
safetybox.dimensions[1] = 2.0; safety_box.dimensions[1] = 2.0;
safetybox.dimensions[2] = 2.0; safety_box.dimensions[2] = 2.0;
// Safety Box // 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); std::vector<moveit_msgs::CollisionObject> collision_objects = sec.createCentralizedSafetyEnvironment(move_group);
//visual_tools.processCollisionObjectMsg(collision_objects[2], rviz_visual_tools::RED); //visual_tools.processCollisionObjectMsg(collision_objects[2], rviz_visual_tools::RED);
...@@ -94,7 +98,6 @@ int main(int argc, char** argv) ...@@ -94,7 +98,6 @@ int main(int argc, char** argv)
// Move the simulated robot in gazebo // Move the simulated robot in gazebo
move_group.move(); move_group.move();
move_group.stop();
ros::shutdown(); ros::shutdown();
return 0; return 0;
......
...@@ -12,6 +12,13 @@ class SafetyEnvironmentCreator { ...@@ -12,6 +12,13 @@ class SafetyEnvironmentCreator {
double safety_zone_width; double safety_zone_width;
double safety_zone_height; double safety_zone_height;
double safety_zone_depth; 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: public:
/** /**
...@@ -26,28 +33,56 @@ class SafetyEnvironmentCreator { ...@@ -26,28 +33,56 @@ class SafetyEnvironmentCreator {
safety_zone_depth = 0.0; safety_zone_depth = 0.0;
safety_zone_width = 0.0; safety_zone_width = 0.0;
safety_zone_height = 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 { }else {
safety_zone_depth = shape.dimensions[0]; safety_zone_depth = shape.dimensions[0];
safety_zone_width = shape.dimensions[1]; safety_zone_width = shape.dimensions[1];
safety_zone_height = shape.dimensions[2]; 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; return safety_zone_width;
} }
double getZoneHeight() { double getZoneHeight() const {
return safety_zone_height; return safety_zone_height;
} }
double getZoneDepth() { double getZoneDepth() const {
return safety_zone_depth; 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> createCentralizedSafetyEnvironment(moveit::planning_interface::MoveGroupInterface &move_group) {
std::vector<moveit_msgs::CollisionObject> collision_objects; std::vector<moveit_msgs::CollisionObject> collision_objects;
// OBJECT 1 ( left box / -y ) // OBJECT 1 ( left box / -y )
// ^^^^^^^^ // ^^^^^^^^
// Define a collision object ROS message. // Define a collision object ROS message.
...@@ -69,9 +104,9 @@ class SafetyEnvironmentCreator { ...@@ -69,9 +104,9 @@ class SafetyEnvironmentCreator {
// Define a pose for the box (specified relative to frame_id) // Define a pose for the box (specified relative to frame_id)
geometry_msgs::Pose box_pose; geometry_msgs::Pose box_pose;
box_pose.orientation.w = 1.0; box_pose.orientation.w = 1.0;
box_pose.position.x = 0.0; box_pose.position.x = zone_position_x;
box_pose.position.y = -1 * ((getZoneWidth() / 2) + 0.05); box_pose.position.y = zone_position_y + (-1 * ((getZoneWidth() / 2) + 0.05));
box_pose.position.z = primitive.dimensions[2] / 2; box_pose.position.z = zone_position_z + (primitive.dimensions[2] / 2);
collision_object.primitives.push_back(primitive); collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose); collision_object.primitive_poses.push_back(box_pose);
...@@ -98,9 +133,9 @@ class SafetyEnvironmentCreator { ...@@ -98,9 +133,9 @@ class SafetyEnvironmentCreator {
// Define a pose for the box (specified relative to frame_id) // Define a pose for the box (specified relative to frame_id)
geometry_msgs::Pose box_pose_2; geometry_msgs::Pose box_pose_2;
box_pose_2.orientation.w = 1.0; box_pose_2.orientation.w = 1.0;
box_pose_2.position.x = 0.0; box_pose_2.position.x = zone_position_x;
box_pose_2.position.y = ((getZoneWidth() / 2) + 0.05); box_pose_2.position.y = zone_position_y + ((getZoneWidth() / 2) + 0.05);
box_pose_2.position.z = primitive_2.dimensions[2] / 2; box_pose_2.position.z = zone_position_z + (primitive_2.dimensions[2] / 2);
collision_object_2.primitives.push_back(primitive_2); collision_object_2.primitives.push_back(primitive_2);
collision_object_2.primitive_poses.push_back(box_pose_2); collision_object_2.primitive_poses.push_back(box_pose_2);
...@@ -127,9 +162,9 @@ class SafetyEnvironmentCreator { ...@@ -127,9 +162,9 @@ class SafetyEnvironmentCreator {
// Define a pose for the box (specified relative to frame_id) // Define a pose for the box (specified relative to frame_id)
geometry_msgs::Pose box_pose_3; geometry_msgs::Pose box_pose_3;
box_pose_3.orientation.w = 1.0; box_pose_3.orientation.w = 1.0;
box_pose_3.position.x = ((getZoneDepth() / 2) + 0.05); box_pose_3.position.x = zone_position_x + ((getZoneDepth() / 2) + 0.05);
box_pose_3.position.y = 0.0; box_pose_3.position.y = zone_position_y;
box_pose_3.position.z = primitive_3.dimensions[2] / 2; box_pose_3.position.z = zone_position_z + (primitive_3.dimensions[2] / 2);
collision_object_3.primitives.push_back(primitive_3); collision_object_3.primitives.push_back(primitive_3);
collision_object_3.primitive_poses.push_back(box_pose_3); collision_object_3.primitive_poses.push_back(box_pose_3);
...@@ -156,9 +191,9 @@ class SafetyEnvironmentCreator { ...@@ -156,9 +191,9 @@ class SafetyEnvironmentCreator {
// Define a pose for the box (specified relative to frame_id) // Define a pose for the box (specified relative to frame_id)
geometry_msgs::Pose box_pose_4; geometry_msgs::Pose box_pose_4;
box_pose_4.orientation.w = 1.0; box_pose_4.orientation.w = 1.0;
box_pose_4.position.x = -1 * ((getZoneDepth() / 2) + 0.05); box_pose_4.position.x = zone_position_x + (-1 * ((getZoneDepth() / 2) + 0.05));
box_pose_4.position.y = 0.0; box_pose_4.position.y = zone_position_y;
box_pose_4.position.z = primitive_4.dimensions[2] / 2; box_pose_4.position.z = zone_position_z + (primitive_4.dimensions[2] / 2);
collision_object_4.primitives.push_back(primitive_4); collision_object_4.primitives.push_back(primitive_4);
collision_object_4.primitive_poses.push_back(box_pose_4); collision_object_4.primitive_poses.push_back(box_pose_4);
...@@ -185,9 +220,9 @@ class SafetyEnvironmentCreator { ...@@ -185,9 +220,9 @@ class SafetyEnvironmentCreator {
// Define a pose for the box (specified relative to frame_id) // Define a pose for the box (specified relative to frame_id)
geometry_msgs::Pose box_pose_5; geometry_msgs::Pose box_pose_5;
box_pose_5.orientation.w = 1.0; box_pose_5.orientation.w = 1.0;
box_pose_5.position.x = 0.0; box_pose_5.position.x = zone_position_x;
box_pose_5.position.y = 0.0; box_pose_5.position.y = zone_position_y;
box_pose_5.position.z = -0.05; box_pose_5.position.z = zone_position_z - 0.05;
collision_object_5.primitives.push_back(primitive_5); collision_object_5.primitives.push_back(primitive_5);
collision_object_5.primitive_poses.push_back(box_pose_5); collision_object_5.primitive_poses.push_back(box_pose_5);
...@@ -195,13 +230,13 @@ class SafetyEnvironmentCreator { ...@@ -195,13 +230,13 @@ class SafetyEnvironmentCreator {
collision_objects.push_back(collision_object_5); collision_objects.push_back(collision_object_5);
// OBJECT 5 (floor / -z) // OBJECT 6 (top / +z)
// ^^^^^^^^ // ^^^^^^^^
moveit_msgs::CollisionObject collision_object_6; moveit_msgs::CollisionObject collision_object_6;
collision_object_6.header.frame_id = move_group.getPlanningFrame(); collision_object_6.header.frame_id = move_group.getPlanningFrame();
// The id of the object is used to identify it. // 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. // Define a box to add to the world.
shape_msgs::SolidPrimitive primitive_6; shape_msgs::SolidPrimitive primitive_6;
...@@ -214,16 +249,22 @@ class SafetyEnvironmentCreator { ...@@ -214,16 +249,22 @@ class SafetyEnvironmentCreator {
// Define a pose for the box (specified relative to frame_id) // Define a pose for the box (specified relative to frame_id)
geometry_msgs::Pose box_pose_6; geometry_msgs::Pose box_pose_6;
box_pose_6.orientation.w = 1.0; box_pose_6.orientation.w = 1.0;
box_pose_6.position.x = 0.0; box_pose_6.position.x = zone_position_x;
box_pose_6.position.y = 0.0; box_pose_6.position.y = zone_position_y;
box_pose_6.position.z = getZoneHeight() + 0.05; box_pose_6.position.z = zone_position_z + getZoneHeight() + 0.05;
collision_object_6.primitives.push_back(primitive_6); collision_object_6.primitives.push_back(primitive_6);
collision_object_6.primitive_poses.push_back(box_pose_6); collision_object_6.primitive_poses.push_back(box_pose_6);
collision_object_6.operation = collision_object_6.ADD; collision_object_6.operation = collision_object_6.ADD;
collision_objects.push_back(collision_object_6); 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; return collision_objects;
} }
}; };
\ 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