use correct orientation in gazebo display of safety zones

src/SafetyZones/GazeboZoneSpawner.cpp

@@ -9,6 +9,7 @@
 #include <gazebo_msgs/SpawnModel.h> // for spawning stuff in gazebo
 #include <ros/ros.h>
 #include <ros/package.h>
+#include <tf/tf.h>
 
 void GazeboZoneSpawner::spawnCollisionBox(shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose) {
   if (shape.type != shape.BOX) {
@@ -27,8 +28,13 @@ void GazeboZoneSpawner::spawnCollisionBox(shape_msgs::SolidPrimitive shape, geom
   auto poseElement = visualElement->GetElement("pose");
   auto sizeElement = visualElement->GetElement("geometry")->GetElement("box")->GetElement("size");
 
+  tf::Quaternion rot;
+  rot.setValue(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
+  double roll, pitch, yaw;
+  tf::Matrix3x3(rot).getRPY(roll, pitch, yaw);
+
   std::ostringstream poseStream;
-  poseStream << pose.position.x << " " << pose.position.y << " " << pose.position.z << " 0 0 0";
+  poseStream << pose.position.x << " " << pose.position.y << " " << pose.position.z << " " << roll << " " << pitch << " " << yaw;
   poseElement->Set(poseStream.str());
 
   ROS_INFO_NAMED("GazeboZoneSpawner", "Set pose in SDF file to '%s'.", poseElement->GetValue()->GetAsString().c_str());
@@ -39,8 +45,6 @@ void GazeboZoneSpawner::spawnCollisionBox(shape_msgs::SolidPrimitive shape, geom
 
   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;