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
This is an archived project. Repository and other project resources are read-only.
Show more breadcrumbs
CeTI
ROS
ROS Packages
panda_simulation
Commits
48e97549
Commit
48e97549
authored
5 years ago
by
Johannes Mey
Browse files
Options
Downloads
Patches
Plain Diff
extract Gazebo safety zone spawner to library
parent
21f63d8d
Branches
Branches containing commit
No related tags found
No related merge requests found
Changes
4
Hide whitespace changes
Inline
Side-by-side
Showing
4 changed files
CMakeLists.txt
+4
-3
4 additions, 3 deletions
CMakeLists.txt
src/GazeboZoneSpawner.cpp
+61
-0
61 additions, 0 deletions
src/GazeboZoneSpawner.cpp
src/GazeboZoneSpawner.h
+20
-0
20 additions, 0 deletions
src/GazeboZoneSpawner.h
src/robot_control_node.cpp
+2
-52
2 additions, 52 deletions
src/robot_control_node.cpp
with
87 additions
and
55 deletions
CMakeLists.txt
+
4
−
3
View file @
48e97549
...
@@ -60,7 +60,7 @@ include_directories(${catkin_INCLUDE_DIRS} ${SDF_INCLUDE_DIRS})
...
@@ -60,7 +60,7 @@ include_directories(${catkin_INCLUDE_DIRS} ${SDF_INCLUDE_DIRS})
# add custom controller as library
# add custom controller as library
add_library
(
${
PROJECT_NAME
}
_controllers_lib src/joint_position_controller.cpp
)
add_library
(
${
PROJECT_NAME
}
_controllers_lib src/joint_position_controller.cpp
)
#
add_library(
SafetyEnvironmentCreator src/SafetyEnvironmentCreator.cpp
)
add_library
(
GazeboZoneSpawner src/GazeboZoneSpawner.cpp src/GazeboZoneSpawner.h
)
# Specify libraries to link a library or executable target against
# Specify libraries to link a library or executable target against
target_link_libraries
(
${
PROJECT_NAME
}
_controllers_lib
${
catkin_LIBRARIES
}
)
target_link_libraries
(
${
PROJECT_NAME
}
_controllers_lib
${
catkin_LIBRARIES
}
)
...
@@ -71,9 +71,10 @@ add_executable(box_publisher_node src/box_publisher.cpp)
...
@@ -71,9 +71,10 @@ add_executable(box_publisher_node src/box_publisher.cpp)
add_executable
(
robot_state_initializer_node src/robot_state_initializer.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 src/SafetyEnvironmentCreator.cpp
)
target_link_libraries
(
robot_control_node
${
catkin_LIBRARIES
}
boost_filesystem
${
JSONCPP_LIBRARIES
}
${
SDF_LIBRARIES
}
)
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
(
box_publisher_node
${
catkin_LIBRARIES
}
)
target_link_libraries
(
robot_state_initializer_node
${
catkin_LIBRARIES
}
)
target_link_libraries
(
robot_state_initializer_node
${
catkin_LIBRARIES
}
)
target_link_libraries
(
SafetyAwarePlanner
${
catkin_LIBRARIES
}
)
target_link_libraries
(
SafetyAwarePlanner
${
catkin_LIBRARIES
}
)
#target_link_libraries(constraint_planner ${catkin_LIBRARIES} ${Boost_LIBRARIES} SafetyEnvironmentCreator)
#
target_link_libraries(constraint_planner ${catkin_LIBRARIES} ${Boost_LIBRARIES} SafetyEnvironmentCreator)
This diff is collapsed.
Click to expand it.
src/GazeboZoneSpawner.cpp
0 → 100644
+
61
−
0
View file @
48e97549
//
// Created by Johannes Mey on 31.03.20.
//
#include
"GazeboZoneSpawner.h"
#include
<sdformat-6.0/sdf/sdf.hh>
// for sdf model parsing
#include
<gazebo_msgs/SpawnModel.h>
// for spawning stuff in gazebo
#include
<ros/ros.h>
#include
<ros/package.h>
void
GazeboZoneSpawner
::
spawnCollisionBox
(
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"
);
return
;
}
sdf
::
SDFPtr
sdf
(
new
sdf
::
SDF
());
sdf
::
init
(
sdf
);
auto
sdfFileName
=
ros
::
package
::
getPath
(
ROS_PACKAGE_NAME
)
+
"/models/box.sdf"
;
assert
(
sdf
::
readFile
(
sdfFileName
,
sdf
));
auto
visualElement
=
sdf
->
Root
()
->
GetElement
(
"model"
)
->
GetElement
(
"link"
)
->
GetElement
(
"visual"
);
auto
poseElement
=
visualElement
->
GetElement
(
"pose"
);
auto
sizeElement
=
visualElement
->
GetElement
(
"geometry"
)
->
GetElement
(
"box"
)
->
GetElement
(
"size"
);
std
::
ostringstream
poseStream
;
poseStream
<<
pose
.
position
.
x
<<
" "
<<
pose
.
position
.
y
<<
" "
<<
pose
.
position
.
z
<<
" 0 0 0"
;
poseElement
->
Set
(
poseStream
.
str
());
ROS_INFO_NAMED
(
"GazeboZoneSpawner"
,
"Set pose in SDF file to '%s'."
,
poseElement
->
GetValue
()
->
GetAsString
().
c_str
());
std
::
ostringstream
sizeStream
;
sizeStream
<<
shape
.
dimensions
[
0
]
<<
" "
<<
shape
.
dimensions
[
1
]
<<
" "
<<
shape
.
dimensions
[
2
];
sizeElement
->
Set
(
sizeStream
.
str
());
ROS_INFO_NAMED
(
"GazeboZoneSpawner"
,
"Set size in SDF file to '%s'."
,
sizeElement
->
GetValue
()
->
GetAsString
().
c_str
());
// FIXME orientation is currently ignored
ros
::
NodeHandle
n
;
ros
::
ServiceClient
client
=
n
.
serviceClient
<
gazebo_msgs
::
SpawnModel
>
(
"/gazebo/spawn_sdf_model"
);
gazebo_msgs
::
SpawnModel
spawnService
;
spawnService
.
request
.
robot_namespace
=
"box space"
;
// spawnModelService.request.initial_pose = pose; // not required here
spawnService
.
request
.
model_name
=
sdf
->
Root
()
->
GetElement
(
"model"
)
->
GetAttribute
(
"name"
)
->
GetAsString
();
spawnService
.
request
.
model_xml
=
sdf
->
ToString
().
c_str
();
// spawnModelService.request.reference_frame = ; // if left empty, world is used
if
(
client
.
call
(
spawnService
))
{
if
(
spawnService
.
response
.
success
)
{
ROS_INFO_NAMED
(
"GazeboZoneSpawner"
,
"Successfully spawned Box. %s"
,
spawnService
.
response
.
status_message
.
c_str
());
}
else
{
ROS_INFO_NAMED
(
"GazeboZoneSpawner"
,
"Failed to spawn Box. %s"
,
spawnService
.
response
.
status_message
.
c_str
());
}
}
else
{
ROS_ERROR_NAMED
(
"GazeboZoneSpawner"
,
"Failed to call service '/gazebo/spawn_sdf_model'"
);
}
}
This diff is collapsed.
Click to expand it.
src/GazeboZoneSpawner.h
0 → 100644
+
20
−
0
View file @
48e97549
//
// Created by Johannes Mey on 31.03.20.
//
#ifndef PANDA_SIMULATION_GAZEBOZONESPAWNER_H
#define PANDA_SIMULATION_GAZEBOZONESPAWNER_H
#include
<shape_msgs/SolidPrimitive.h>
#include
<geometry_msgs/Pose.h>
class
GazeboZoneSpawner
{
public:
static
void
spawnCollisionBox
(
shape_msgs
::
SolidPrimitive
shape
,
geometry_msgs
::
Pose
pose
);
};
#endif //PANDA_SIMULATION_GAZEBOZONESPAWNER_H
This diff is collapsed.
Click to expand it.
src/robot_control_node.cpp
+
2
−
52
View file @
48e97549
...
@@ -6,62 +6,12 @@
...
@@ -6,62 +6,12 @@
#include
<ros/ros.h>
#include
<ros/ros.h>
#include
<boost/filesystem.hpp>
#include
<boost/filesystem.hpp>
#include
<sdformat-6.0/sdf/sdf.hh>
// for sdf model parsing
#include
"GazeboZoneSpawner.h"
#include
<gazebo_msgs/SpawnModel.h>
// for spawning stuff in gazebo
static
const
std
::
string
PLANNING_GROUP_ARM
=
"panda_arm"
;
static
const
std
::
string
PLANNING_GROUP_ARM
=
"panda_arm"
;
static
const
std
::
string
APP_DIRECTORY_NAME
=
".panda_simulation"
;
static
const
std
::
string
APP_DIRECTORY_NAME
=
".panda_simulation"
;
void
spawnCollisionBox
(
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"
);
return
;
}
sdf
::
SDFPtr
sdf
(
new
sdf
::
SDF
());
sdf
::
init
(
sdf
);
auto
sdfFileName
=
ros
::
package
::
getPath
(
ROS_PACKAGE_NAME
)
+
"/models/box.sdf"
;
assert
(
sdf
::
readFile
(
sdfFileName
,
sdf
));
ROS_INFO_NAMED
(
"model-loader"
,
"successfully read SDF file: %s"
,
sdf
->
ToString
().
c_str
());
auto
visualElement
=
sdf
->
Root
()
->
GetElement
(
"model"
)
->
GetElement
(
"link"
)
->
GetElement
(
"visual"
);
auto
poseElement
=
visualElement
->
GetElement
(
"pose"
);
auto
sizeElement
=
visualElement
->
GetElement
(
"geometry"
)
->
GetElement
(
"box"
)
->
GetElement
(
"size"
);
std
::
ostringstream
poseStream
;
poseStream
<<
pose
.
position
.
x
<<
" "
<<
pose
.
position
.
y
<<
" "
<<
pose
.
position
.
z
<<
" 0 0 0"
;
poseElement
->
Set
(
poseStream
.
str
());
ROS_INFO_NAMED
(
"model-loader"
,
"successfully changed SDF file, pose is: %s"
,
poseElement
->
GetValue
()
->
GetAsString
().
c_str
());
std
::
ostringstream
sizeStream
;
sizeStream
<<
shape
.
dimensions
[
0
]
<<
" "
<<
shape
.
dimensions
[
1
]
<<
" "
<<
shape
.
dimensions
[
2
];
sizeElement
->
Set
(
sizeStream
.
str
());
ROS_INFO_NAMED
(
"model-loader"
,
"successfully changed SDF file, size is: '%s', should be '%s'"
,
sizeElement
->
GetValue
()
->
GetAsString
().
c_str
(),
sizeStream
.
str
().
c_str
());
ros
::
NodeHandle
n
;
ros
::
ServiceClient
client
=
n
.
serviceClient
<
gazebo_msgs
::
SpawnModel
>
(
"/gazebo/spawn_sdf_model"
);
gazebo_msgs
::
SpawnModel
spawnModelService
;
spawnModelService
.
request
.
robot_namespace
=
"box space"
;
// spawnModelService.request.initial_pose = pose; // not required here
spawnModelService
.
request
.
model_name
=
sdf
->
Root
()
->
GetElement
(
"model"
)
->
GetAttribute
(
"name"
)
->
GetAsString
();
spawnModelService
.
request
.
model_xml
=
sdf
->
ToString
().
c_str
();
// spawnModelService.request.reference_frame = ; // if left empty, world is used
if
(
client
.
call
(
spawnModelService
))
{
ROS_INFO
(
"result: %s"
,
spawnModelService
.
response
.
success
?
"success!"
:
"failed!"
);
ROS_INFO
(
"result: %s"
,
spawnModelService
.
response
.
status_message
.
c_str
());
}
else
{
ROS_ERROR
(
"Failed to call service '/gazebo/spawn_sdf_model'"
);
}
}
moveit_msgs
::
CollisionObject
extractObstacleFromJson
(
Json
::
Value
&
root
,
std
::
string
name
)
moveit_msgs
::
CollisionObject
extractObstacleFromJson
(
Json
::
Value
&
root
,
std
::
string
name
)
{
{
...
@@ -101,7 +51,7 @@ moveit_msgs::CollisionObject extractObstacleFromJson(Json::Value &root, std::str
...
@@ -101,7 +51,7 @@ moveit_msgs::CollisionObject extractObstacleFromJson(Json::Value &root, std::str
collision_object
.
primitive_poses
.
push_back
(
box_pose
);
collision_object
.
primitive_poses
.
push_back
(
box_pose
);
collision_object
.
operation
=
collision_object
.
ADD
;
collision_object
.
operation
=
collision_object
.
ADD
;
spawnCollisionBox
(
primitive
,
box_pose
);
GazeboZoneSpawner
::
spawnCollisionBox
(
primitive
,
box_pose
);
return
std
::
move
(
collision_object
);
return
std
::
move
(
collision_object
);
}
}
...
...
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