feat(franka_gazebo): add incompatibly physics engine warning

frankaemika · Mar 18, 2022 · 756bc65 · 756bc65
1 parent 4e4157d
commit 756bc65
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 1 deletion.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -6,7 +6,7 @@
   - Add JointPosition and JointVelocity Interface
   - Fix: Robot now keeps position when no controller is running
   - joint_{position,velocity}_example controller are now available in `franka_gazebo`
-    
+  - Add Gazebo simulation physics engine command line argument.
 
 ## 0.8.2 - 2022-02-22
 

diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp
@@ -43,7 +43,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<std::string> 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.