Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
P
panda_simulation
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Container registry
Model registry
Operate
Environments
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
Nikhil Ambardar
panda_simulation
Commits
1ef086ae
Commit
1ef086ae
authored
5 years ago
by
Sebastian Ebert
Browse files
Options
Downloads
Patches
Plain Diff
the safety zone can now be moved, todo: rotation
parent
989d4788
Branches
Branches containing commit
Tags
Tags containing commit
No related merge requests found
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
src/SafetyAwarePlanner.cpp
+17
-14
17 additions, 14 deletions
src/SafetyAwarePlanner.cpp
src/SafetyEnvironmentCreator.cpp
+66
-25
66 additions, 25 deletions
src/SafetyEnvironmentCreator.cpp
with
83 additions
and
39 deletions
src/SafetyAwarePlanner.cpp
+
17
−
14
View file @
1ef086ae
#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
;
...
...
This diff is collapsed.
Click to expand it.
src/SafetyEnvironmentCreator.cpp
+
66
−
25
View file @
1ef086ae
...
...
@@ -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
=
"box
5
"
;
collision_object_6
.
id
=
"box
6
"
;
// 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
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment