extract Gazebo safety zone spawner to library

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,7 +71,8 @@ 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})

src/GazeboZoneSpawner.cpp

+//
+// 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'");
+  }
+}

src/GazeboZoneSpawner.h

+//
+// 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

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);
 }