From dcc5eb0476d05300b6a2b02d407581f3f1f2a8f9 Mon Sep 17 00:00:00 2001
From: rickstaa <rick.staa@outlook.com>
Date: Fri, 10 Dec 2021 10:34:48 +0100
Subject: [PATCH] feat(franka_gazebo): add incompatibly physics engine warning

---
 franka_gazebo/src/franka_hw_sim.cpp | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp
index 723ea4891..4e6485180 100644
--- a/franka_gazebo/src/franka_hw_sim.cpp
+++ b/franka_gazebo/src/franka_hw_sim.cpp
@@ -39,7 +39,16 @@ bool FrankaHWSim::initSim(const std::string& robot_namespace,
   gazebo::physics::PhysicsEnginePtr physics = gazebo::physics::get_world()->GetPhysicsEngine();
 #endif
 
+  // Print information about the used physics engine
+  std::vector supported_engines{"ode", "dart"};
+  std::string physics_engine = physics->GetType();
   ROS_INFO_STREAM_NAMED("franka_hw_sim", "Using physics type " << physics->GetType());
+  if (std::find(supported_engines.begin(), supported_engines.end(), physics_engine) ==
+      supported_engines.end()) {
+    ROS_ERROR_STREAM_NAMED("franka_hw_sim",
+                           "The Panda Gazebo model does not yet officially support the '" +
+                               physics_engine + "' physics engine.");
+  }
 
   // Retrieve initial gravity vector from Gazebo
   // NOTE: Can be overwritten by the user via the 'gravity_vector' ROS parameter.