Add service for deferred initialization

Launching a simulation with a robot that has ray sensors may make
Gazebo/ODE crash. The part that makes the difference is the creation
of the scene (if skipped, the crash goes away). Having found no
sensible reason why this is, I add this service to give control to
the user when to initialize the plugin.

README.md

@@ -104,5 +104,6 @@ A set of utility plugins is also available to be used along with the monitor plu
 Known Issues
 ------------
 
-* The camera visualizations (not the cameras used for recording) are created and set correctly, but they don't get attached to the models.
-* The cameras are not always moved correctly during initialization. The numberOfInitialAttachRetries parameter was introduced as a temporary fix.
+* The camera visualizations (not the cameras used for the recordings) are created and set correctly, but they don't get attached to the models.
+* The cameras are not always positioned correctly during initialization. The [numberOfInitialAttachRetries](gazebo_video_monitor_plugins/include/gazebo_video_monitor_plugins/gazebo_monitor_base_plugin.h#L43) parameter was introduced as a temporary fix.
+* Gazebo may or may not crash upon launch if your robot has ray sensors (somehow creating the scene early makes collision detection explode; your guess is as good as mine). The [initService](gazebo_video_monitor_plugins/include/gazebo_video_monitor_plugins/gvm_multicamera_bootstrapper_plugin.h#L38) parameter was introduced to trigger the initialization of the plugin by calling a service at the right moment (e.g. after your robot has been initialized).

gazebo_video_monitor_plugins/include/gazebo_video_monitor_plugins/gvm_multicamera_bootstrapper_plugin.h

@@ -18,6 +18,10 @@
 #ifndef GAZEBO_VIDEO_MONITOR_PLUGINS_GVM_MULTICAMERA_BOOTSTRAPPER_PLUGIN_H
 #define GAZEBO_VIDEO_MONITOR_PLUGINS_GVM_MULTICAMERA_BOOTSTRAPPER_PLUGIN_H
 
+#include <ros/callback_queue.h>
+#include <ros/ros.h>
+#include <std_srvs/Empty.h>
+
 #include <gazebo/common/Plugin.hh>
 #include <gazebo/physics/physics.hh>
 
@@ -31,6 +35,9 @@ namespace gazebo {
  * adds a gvm_multicamera sensor to a given model.
  * @note Expects the following configuration:
  *   - sensor: configuration of a gvm_multicamera sensor
+ *   - initService (optional): if one of the many reasons that make gazebo
+ *     crash gives you trouble, you can configure a service to call to control
+ *     when the gvm sensor gets created and initialized
  *   - sensorReference: reference model configuration for the multicamera
  *     sensor. It contains the name of the model (normally ground_plane) with
  *     which to associate the sensor, and the name of the link to which to
@@ -44,10 +51,20 @@ class GvmMulticameraBootstrapperPlugin : public WorldPlugin {
   virtual void Init() override;
 
  private:
+  bool initServiceCallback(std_srvs::EmptyRequest &req,
+                           std_srvs::EmptyResponse &res);
+
   std::string logger_prefix_;
   sdf::ElementPtr sdf_;
   physics::WorldPtr world_;
   physics::LinkPtr link_;
+
+  ros::NodeHandlePtr nh_;
+  ros::CallbackQueue callback_queue_;
+  ros::AsyncSpinner spinner_;
+
+  bool inited_;
+  ros::ServiceServer init_service_server_;
 };
 
 }  // namespace gazebo

gazebo_video_monitor_plugins/src/gvm_multicamera_bootstrapper_plugin.cpp

@@ -23,13 +23,19 @@
 namespace gazebo {
 
 GvmMulticameraBootstrapperPlugin::GvmMulticameraBootstrapperPlugin()
-    : logger_prefix_(getClassName<GvmMulticameraBootstrapperPlugin>() + ": ") {
+    : logger_prefix_(getClassName<GvmMulticameraBootstrapperPlugin>() + ": "),
+      spinner_(1, &callback_queue_),
+      inited_(false) {
   sensors::SensorFactory::RegisterSensor(
       "gvm_multicamera", reinterpret_cast<sensors::SensorFactoryFn>(
                              &sensors::GvmMulticameraSensor::newSensor));
 }
 
 GvmMulticameraBootstrapperPlugin::~GvmMulticameraBootstrapperPlugin() {
+  callback_queue_.clear();
+  callback_queue_.disable();
+  nh_->shutdown();
+
   if (not link_) return;
   std::string scoped_sensor_name =
       world_->Name() + "::" + link_->GetScopedName() +
@@ -51,8 +57,7 @@ void GvmMulticameraBootstrapperPlugin::Load(physics::WorldPtr _world,
   // Load sensor model configuration
   if (not _sdf->HasElement("sensorReference"))
     gzthrow(logger_prefix_ + "Failed to get sensorReference");
-  auto model_config =
-      parseRefModelConfig(_sdf->GetElement("sensorReference"));
+  auto model_config = parseRefModelConfig(_sdf->GetElement("sensorReference"));
 
   // Get sensor model
   auto model = world_->ModelByName(model_config->model_name);
@@ -64,13 +69,34 @@ void GvmMulticameraBootstrapperPlugin::Load(physics::WorldPtr _world,
   if (not link_)
     gzthrow(logger_prefix_ + "Failed to get link " + model_config->link_name +
             " in model " + model_config->model_name);
+
+  nh_ = boost::make_shared<ros::NodeHandle>();
+  nh_->setCallbackQueue(&callback_queue_);
+  spinner_.start();
+
+  if (_sdf->HasElement("initService"))
+    init_service_server_ = nh_->advertiseService(
+        _sdf->Get<std::string>("initService"),
+        &GvmMulticameraBootstrapperPlugin::initServiceCallback, this);
 }
 
 void GvmMulticameraBootstrapperPlugin::Init() {
+  // If the subscriber is enabled, it will be responsible for initialization
+  if (not init_service_server_.getService().empty()) return;
   event::Events::createSensor(sdf_->GetElement("sensor"), world_->Name(),
                               link_->GetScopedName(), link_->GetId());
 }
 
+bool GvmMulticameraBootstrapperPlugin::initServiceCallback(
+    std_srvs::EmptyRequest &req, std_srvs::EmptyResponse &res) {
+  if (not inited_) {
+    event::Events::createSensor(sdf_->GetElement("sensor"), world_->Name(),
+                                link_->GetScopedName(), link_->GetId());
+    inited_ = true;
+  }
+  return true;
+}
+
 GZ_REGISTER_WORLD_PLUGIN(GvmMulticameraBootstrapperPlugin)
 
 }  // namespace gazebo