support changing video streams when reporting targets (#195)

Signed-off-by: Ian Chen <[email protected]>
osrf · Jul 29, 2022 · 5f9ec7b · 5f9ec7b
1 parent 52474ac
commit 5f9ec7b
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 4 deletions.
diff --git a/mbzirc_ign/src/BaseStation.cc b/mbzirc_ign/src/BaseStation.cc
@@ -114,9 +114,9 @@ void BaseStation::OnVideo(const ignition::msgs::Dataframe &_msg)
     double avgT = this->simTimeSum / this->kVideoStreamWindowSize;
     if ((1 / avgT) < this->kminStreamRate)
     {
-      std::cerr << "video stream is lower than "
-                << std::to_string(this->kminStreamRate)
-                << " Hz" << std::endl;
+      // std::cerr << "video stream is lower than "
+      //           << std::to_string(this->kminStreamRate)
+      //           << " Hz" << std::endl;
     }
 
     auto front = this->simTimes.front();

diff --git a/mbzirc_ign/src/GameLogicPlugin.cc b/mbzirc_ign/src/GameLogicPlugin.cc
@@ -479,6 +479,10 @@ class mbzirc::GameLogicPluginPrivate
   /// \brief Sensor entity on vehicle currently streaming video of target
   public: Entity targetStreamSensorEntity;
 
+  /// \brief Keeps track of whether the sensor providing the target stream
+  /// has changed or not.
+  public: bool targetStreamSensorEntityChanged = false;
+
   /// \brief Name of topic that contains the video stream of target
   public: std::string targetStreamTopic;
 
@@ -903,6 +907,7 @@ void GameLogicPlugin::PostUpdate(
         if (this->dataPtr->targetStreamTopic.find(topic) != std::string::npos)
         {
           this->dataPtr->targetStreamSensorEntity = sensorEntity;
+          this->dataPtr->targetStreamSensorEntityChanged = true;
         }
       }
     }
@@ -2244,7 +2249,7 @@ void GameLogicPluginPrivate::OnPostRender()
 
   // get camera currently streaming video
   std::lock_guard<std::mutex> lock(this->streamMutex);
-  if (!this->camera && this->targetStreamSensorEntity != kNullEntity)
+  if (this->targetStreamSensorEntityChanged)
   {
     for (unsigned int i = 0; i < this->scene->SensorCount(); ++i)
     {
@@ -2262,6 +2267,7 @@ void GameLogicPluginPrivate::OnPostRender()
         if (value && *value == static_cast<int>(this->targetStreamSensorEntity))
         {
           this->camera = std::dynamic_pointer_cast<rendering::Camera>(sensor);
+          this->targetStreamSensorEntityChanged = false;
           break;
         }
       }