From 48e97549fc3d9d1b000b71353709e94b1ca999ca Mon Sep 17 00:00:00 2001
From: Johannes Mey <johannes.mey@tu-dresden.de>
Date: Tue, 31 Mar 2020 22:30:58 +0200
Subject: [PATCH] extract Gazebo safety zone spawner to library
---
CMakeLists.txt | 7 +++--
src/GazeboZoneSpawner.cpp | 61 ++++++++++++++++++++++++++++++++++++++
src/GazeboZoneSpawner.h | 20 +++++++++++++
src/robot_control_node.cpp | 54 ++-------------------------------
4 files changed, 87 insertions(+), 55 deletions(-)
create mode 100644 src/GazeboZoneSpawner.cpp
create mode 100644 src/GazeboZoneSpawner.h
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7667d14..3957731 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -60,7 +60,7 @@ include_directories(${catkin_INCLUDE_DIRS} ${SDF_INCLUDE_DIRS})
# add custom controller as library
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
target_link_libraries(${PROJECT_NAME}_controllers_lib ${catkin_LIBRARIES})
@@ -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(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(robot_state_initializer_node ${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)
diff --git a/src/GazeboZoneSpawner.cpp b/src/GazeboZoneSpawner.cpp
new file mode 100644
index 0000000..268fe5e
--- /dev/null
+++ b/src/GazeboZoneSpawner.cpp
@@ -0,0 +1,61 @@
+//
+// 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'");
+ }
+}
diff --git a/src/GazeboZoneSpawner.h b/src/GazeboZoneSpawner.h
new file mode 100644
index 0000000..b0c97a9
--- /dev/null
+++ b/src/GazeboZoneSpawner.h
@@ -0,0 +1,20 @@
+//
+// 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
diff --git a/src/robot_control_node.cpp b/src/robot_control_node.cpp
index bfe1f44..27551f0 100644
--- a/src/robot_control_node.cpp
+++ b/src/robot_control_node.cpp
@@ -6,62 +6,12 @@
#include <ros/ros.h>
#include <boost/filesystem.hpp>
-#include <sdformat-6.0/sdf/sdf.hh> // for sdf model parsing
-#include <gazebo_msgs/SpawnModel.h> // for spawning stuff in gazebo
+#include "GazeboZoneSpawner.h"
static const std::string PLANNING_GROUP_ARM = "panda_arm";
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)
{
@@ -101,7 +51,7 @@ moveit_msgs::CollisionObject extractObstacleFromJson(Json::Value &root, std::str
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;
- spawnCollisionBox(primitive, box_pose);
+ GazeboZoneSpawner::spawnCollisionBox(primitive, box_pose);
return std::move(collision_object);
}
--
GitLab