Commits

Nathan Koenig committed 703771a

Initial threaded sensors

  • Participants
  • Parent commits 53c1d73
  • Branches threaded_sensors

Comments (0)

Files changed (3)

   // Make sure the sensors are updated once before running the world.
   // This makes sure plugins get loaded properly.
   sensors::run_once(true);
+  sensors::run();
 
   // Run each world. Each world starts a new thread
   physics::run_worlds();

gazebo/sensors/SensorManager.cc

 SensorManager::SensorManager()
   : stop(true), initialized(false), runThread(NULL)
 {
+  this->sensorLists.push_back(&this->sensors);
+  this->sensorLists.push_back(&this->imageSensors);
 }
 
 //////////////////////////////////////////////////
 SensorManager::~SensorManager()
 {
   this->sensors.clear();
+  this->imageSensors.clear();
   this->initSensors.clear();
 }
 
 void SensorManager::Run()
 {
   this->stop = false;
-  this->runThread = new boost::thread(
-      boost::bind(&SensorManager::RunLoop, this));
+  // this->runThread = new boost::thread(
+  //    boost::bind(&SensorManager::RunLoop, this));
+
+  printf("RUN baby\n");
+  this->rayThread = new boost::thread(
+      boost::bind(&SensorManager::RunRay, this));
 }
 
 //////////////////////////////////////////////////
     delete this->runThread;
     this->runThread = NULL;
   }
+
+  if (this->rayThread)
+  {
+    this->rayThread->join();
+    delete this->rayThread;
+    this->rayThread = NULL;
+  }
 }
 
 //////////////////////////////////////////////////
 }
 
 //////////////////////////////////////////////////
+void SensorManager::RunRay()
+{
+  Sensor_V::iterator iter;
+  Sensor_V::iterator end;
+
+  while (!this->stop)
+  {
+    {
+      //boost::recursive_mutex::scoped_lock lock(this->mutex);
+      end = this->sensors.end();
+      for (iter = this->sensors.begin(); iter != end; ++iter)
+      {
+        (*iter)->Update(false);
+      }
+    }
+  }
+}
+
+//////////////////////////////////////////////////
 void SensorManager::Update(bool _force)
 {
   Sensor_V::iterator iter;
     for (iter = this->initSensors.begin(); iter != end; ++iter)
     {
       (*iter)->Init();
-      this->sensors.push_back((*iter));
+      if ((*iter)->GetType() != "camera" &&
+          (*iter)->GetType() != "multicamera" &&
+          (*iter)->GetType() != "depth")
+      {
+        this->sensors.push_back(*iter);
+      }
+      else
+        this->imageSensors.push_back(*iter);
     }
     this->initSensors.clear();
   }
   event::Events::postRender();
 
   {
-    boost::recursive_mutex::scoped_lock lock(this->mutex);
+    //boost::recursive_mutex::scoped_lock lock(this->mutex);
     // in case things are spawn, sensors length changes
-    end = this->sensors.end();
-    for (iter = this->sensors.begin(); iter != end; ++iter)
+    end = this->imageSensors.end();
+    for (iter = this->imageSensors.begin(); iter != end; ++iter)
     {
       (*iter)->Update(_force);
     }
 {
   boost::recursive_mutex::scoped_lock lock(this->mutex);
   Sensor_V::iterator iter;
-  for (iter = this->sensors.begin(); iter != this->sensors.end(); ++iter)
-    (*iter)->Init();
+
+  for (std::vector<Sensor_V*>::iterator viter = this->sensorLists.begin();
+      viter != this->sensorLists.end(); ++viter)
+  {
+    for (iter = (*viter)->begin(); iter != (*viter)->end(); ++iter)
+    {
+      (*iter)->Init();
+    }
+  }
+
   this->initialized = true;
 }
 
 
   this->initialized = false;
   Sensor_V::iterator iter;
-  for (iter = this->sensors.begin(); iter != this->sensors.end(); ++iter)
-    (*iter)->Fini();
-  this->sensors.clear();
+
+  for (std::vector<Sensor_V*>::iterator viter = this->sensorLists.begin();
+      viter != this->sensorLists.end(); ++viter)
+  {
+    for (iter = (*viter)->begin(); iter != (*viter)->end(); ++iter)
+    {
+      (*iter)->Fini();
+    }
+
+    (*viter)->clear();
+  }
 }
 
 //////////////////////////////////////////////////
 
   if (!this->initialized)
   {
-    this->sensors.push_back(sensor);
+    if (sensor->GetType() != "camera" && sensor->GetType() != "multicamera" &&
+        sensor->GetType() != "depth")
+      this->sensors.push_back(sensor);
+    else
+      this->imageSensors.push_back(sensor);
   }
   else
   {
 
   SensorPtr result;
   Sensor_V::iterator iter;
-  for (iter = this->sensors.begin(); iter != this->sensors.end(); ++iter)
+
+  for (iter = this->sensors.begin();
+       iter != this->sensors.end() && !result; ++iter)
+  {
+    if ((*iter)->GetScopedName() == _name)
+      result = (*iter);
+  }
+
+  for (iter = this->imageSensors.begin();
+       iter != this->imageSensors.end() && !result; ++iter)
   {
     if ((*iter)->GetScopedName() == _name)
       result = (*iter);
   // because we don't know which sensor is correct.
   if (!result)
   {
-    for (iter = this->sensors.begin(); iter != this->sensors.end(); ++iter)
+    for (std::vector<Sensor_V*>::iterator viter = this->sensorLists.begin();
+         viter != this->sensorLists.end(); ++viter)
     {
-      if ((*iter)->GetName() != _name)
-        continue;
+      for (iter = (*viter)->begin(); iter != (*viter)->end(); ++iter)
+      {
+        if ((*iter)->GetName() != _name)
+          continue;
 
-      if (!result)
-        result = (*iter);
-      else
-      {
-        gzerr << "Unable to get a sensor, multiple sensors with the same "
-              << "name[" << _name << "]. Use a scoped name instead, "
-              << "world_name::model_name::link_name::sensor_name.\n";
-        result.reset();
+        if (!result)
+          result = (*iter);
+        else
+        {
+          gzerr << "Unable to get a sensor, multiple sensors with the same "
+            << "name[" << _name << "]. Use a scoped name instead, "
+            << "world_name::model_name::link_name::sensor_name.\n";
+          result.reset();
+        }
       }
     }
   }
   return result;
 }
 
+//////////////////////////////////////////////////
 Sensor_V SensorManager::GetSensors() const
 {
-  return this->sensors;
+  Sensor_V result;
+
+  std::copy(this->sensors.begin(), this->sensors.end(),
+            std::back_inserter(result));
+
+  std::copy(this->imageSensors.begin(), this->imageSensors.end(),
+            std::back_inserter(result));
+
+  return result;
 }
 
 //////////////////////////////////////////////////
     boost::recursive_mutex::scoped_lock lock(this->mutex);
 
     Sensor_V::iterator iter;
-    for (iter = this->sensors.begin(); iter != this->sensors.end(); ++iter)
-      if ((*iter)->GetScopedName() == sensor->GetScopedName())
-        break;
 
-    if (iter != this->sensors.end())
+    bool removed = false;
+    for (std::vector<Sensor_V*>::iterator viter = this->sensorLists.begin();
+        viter != this->sensorLists.end() && !removed; ++viter)
     {
-      (*iter)->Fini();
-      this->sensors.erase(iter);
+      for (iter = (*viter)->begin(); iter != (*viter)->end(); ++iter)
+      {
+        if ((*iter)->GetScopedName() == sensor->GetScopedName())
+        {
+          (*iter)->Fini();
+          (*viter)->erase(iter);
+          removed = true;
+        }
+      }
     }
-    else
+
+    if (!removed)
       gzerr << "RemoveSensor failed. The SensorManager's list of sensors "
         << "changed during sensor removal. This is bad, and should "
         << "never happen.\n";
   for (iter = this->sensors.begin(); iter != this->sensors.end(); ++iter)
     (*iter)->Fini();
 
+  for (iter = this->imageSensors.begin();
+       iter != this->imageSensors.end(); ++iter)
+  {
+    (*iter)->Fini();
+  }
+
   this->sensors.clear();
+  this->imageSensors.clear();
   this->initSensors.clear();
 }

gazebo/sensors/SensorManager.hh

       /// \brief Update loop
       private: void RunLoop();
 
+      private: void RunRay();
+
       /// \brief If True the sensor manager stop processing sensors.
       private: bool stop;
 
       /// \brief Mutex used when adding and removing sensors.
       private: boost::recursive_mutex mutex;
 
-      /// \brief The list of initialized sensors.
+      /// \brief The list of initialized non-image sensors.
       private: Sensor_V sensors;
 
+      /// \brief The list of initialized image sensors.
+      private: Sensor_V imageSensors;
+
       /// \brief List of sensors that require initialization.
       private: Sensor_V initSensors;
 
+      private: std::vector<Sensor_V*> sensorLists;
+
+      /// \brief Thread for ray sensors.
+      private: boost::thread *rayThread;
+
       /// \brief This is a singleton class.
       private: friend class SingletonT<SensorManager>;
     };