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 = std::string("box") + poseStream.str() + sizeStream.str();
  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'.  %s", spawnService.request.model_name, spawnService.response.status_message.c_str());
    } else {
      ROS_INFO_NAMED("GazeboZoneSpawner", "Failed to spawn Box '%s'. %s", spawnService.request.model_name, spawnService.response.status_message.c_str());
    }
  } else {
    ROS_ERROR_NAMED("GazeboZoneSpawner", "Failed to call service '/gazebo/spawn_sdf_model'");
  }
}