From a640a57d9eb31a2dff39e616cf158dd92072968b Mon Sep 17 00:00:00 2001
From: Johannes Mey <johannes.mey@tu-dresden.de>
Date: Mon, 6 Apr 2020 09:05:10 +0200
Subject: [PATCH] use correct orientation in gazebo display of safety zones
---
src/SafetyZones/GazeboZoneSpawner.cpp | 10 +++++++---
1 file changed, 7 insertions(+), 3 deletions(-)
diff --git a/src/SafetyZones/GazeboZoneSpawner.cpp b/src/SafetyZones/GazeboZoneSpawner.cpp
index 3102ce4..ea268c9 100644
--- a/src/SafetyZones/GazeboZoneSpawner.cpp
+++ b/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;
--
GitLab