Merged in issue_08 (pull request #68)

Fix issue #8

include/swarm/RobotPlugin.hh

@@ -30,6 +30,7 @@
 #include <gazebo/common/Console.hh>
 #include <gazebo/common/Events.hh>
 #include <gazebo/common/Plugin.hh>
+#include <gazebo/common/Time.hh>
 #include <gazebo/common/UpdateInfo.hh>
 #include <gazebo/physics/PhysicsTypes.hh>
 #include <gazebo/sensors/sensors.hh>
@@ -58,6 +59,16 @@ namespace swarm
     public: ObjPose_M objects;
   };
 
+  /// \brief A class that stores information about a false positive.
+  class IGNITION_VISIBLE FalsePositiveData
+  {
+    /// \brief When will the last false positive period finish?
+    public: gazebo::common::Time enabledUntil;
+
+    /// \brief The name of model used in a false positive.
+    public: std::string falsePositiveModel;
+  };
+
   /// \brief A Model plugin that is the base class for all agent plugins
   /// in a swarm.
   /// This plugin exposes the following functionality to the derived plugins:
@@ -596,6 +607,22 @@ namespace swarm
     // Documentation inherited.
     private: virtual void OnLog(msgs::LogEntry &_logEntry) const;
 
+    /// \brief For the observed model we decide to create a false positive
+    /// based on distance a percentage of the time. If a false positive is
+    /// created, we also randomly choose a duration for it. In the future and
+    /// if we observe the same model '_model' and the false positive is still
+    /// active, we'll replace '_model' with the same false positive.
+    /// \param[in] _model Observed model.
+    /// \param[in] _p Position in the camera frame where the model was seen.
+    /// \param[in] _normalizedDist Distance at which the object is perceived
+    /// (normalized with camera frustum). The probability of creating a false
+    /// positive is proportional to the distance.
+    /// \param[in] _curTime Current simulation time.
+    private: void UpdateFalsePositives(const std::string &_model,
+                                       const ignition::math::Pose3d &_p,
+                                       const double _normalizedDist,
+                                       const gazebo::common::Time &_curTime);
+
     /// \def Callback_t
     /// \brief The callback specified by the user when new data is available.
     /// This callback contains two parameters: the source address of the agent
@@ -783,9 +810,20 @@ namespace swarm
     /// \brief Max random number used to compute a false positive
     private: double cameraFalsePositiveProbMax = 0.8;
 
+    /// \brief Minimum length of a false positive (secs).
+    private: double cameraFalsePositiveDurationMin = 0.03;
+
+    /// \brief Maximum length of a false positive (secs).
+    private: double cameraFalsePositiveDurationMax = 0.08;
+
     /// \brief Max position error in objects detected by the camera
     private: double cameraMaxPositionError = 5.0;
 
+    /// \brief The key is the real model perceived by the camera and the value
+    /// is an object that contains information about this false positive
+    /// (the duration and the model that will replace the real model observed).
+    public: std::map<std::string, FalsePositiveData> camFalsePositiveModels;
+
     /// \brief Array of all the models
     private: std::vector<std::string> modelNames;
 

src/RobotPlugin.cc

@@ -128,6 +128,8 @@ bool RobotPlugin::SetAngularVelocity(const double _x, const double _y,
 //////////////////////////////////////////////////
 void RobotPlugin::UpdateSensors()
 {
+  gazebo::common::Time curTime = this->world->GetSimTime();
+
   if (this->gps)
   {
     this->observedLatitude = this->gps->Latitude().Degree();
@@ -205,19 +207,9 @@ void RobotPlugin::UpdateSensors()
       p.Pos().Y() += ignition::math::Rand::DblUniform(-posError, posError);
       p.Pos().Z() += ignition::math::Rand::DblUniform(-posError, posError);
 
-      // A percentage of the time we get a false positive for the lost person,
-      if (ignition::math::Rand::DblUniform(
-            this->cameraFalsePositiveProbMin,
-            this->cameraFalsePositiveProbMax) < distSquaredNormalized)
-      {
-        // Randomly choose a model name
-        this->img.objects[this->modelNames[
-          ignition::math::Rand::IntUniform(0, this->modelNames.size()-1)]] = p;
-      }
-      else
-      {
-        this->img.objects[imgModel.name()] = p;
-      }
+      // Handle false positives.
+      this->UpdateFalsePositives(imgModel.name(), p, distSquaredNormalized,
+          curTime);
     }
   }
 }
@@ -1369,4 +1361,57 @@ void RobotPlugin::Reset()
 
   // Set camera starting pitch and yaw
   this->SetCameraOrientation(this->cameraStartPitch, this->cameraStartYaw);
+
+  // Clear information about false positives.
+  this->camFalsePositiveModels.clear();
+}
+
+//////////////////////////////////////////////////
+void RobotPlugin::UpdateFalsePositives(const std::string &_model,
+    const ignition::math::Pose3d &_p, const double _normalizedDist,
+    const gazebo::common::Time &_curTime)
+{
+  // Check if we are currently on a false positive period for this model.
+  if (this->camFalsePositiveModels.find(_model) !=
+      this->camFalsePositiveModels.end())
+  {
+    auto &falsePositiveInfo = this->camFalsePositiveModels[_model];
+
+    // Check if the false positive should finish.
+    if (_curTime >= falsePositiveInfo.enabledUntil)
+    {
+      this->camFalsePositiveModels.erase(_model);
+      this->img.objects[_model] = _p;
+    }
+    else
+      this->img.objects[falsePositiveInfo.falsePositiveModel] = _p;
+  }
+  else
+  {
+    // A percentage of the time we get a false positive for the lost person.
+    if (ignition::math::Rand::DblUniform(
+          this->cameraFalsePositiveProbMin,
+          this->cameraFalsePositiveProbMax) < _normalizedDist)
+    {
+      // Randomly choose a model name.
+      auto cameraFalsePositiveModelName = this->modelNames[
+        ignition::math::Rand::IntUniform(0, this->modelNames.size()-1)];
+
+      FalsePositiveData fpData;
+
+      // Set the duration of the false positive.
+      fpData.enabledUntil = _curTime + ignition::math::Rand::DblUniform(
+          this->cameraFalsePositiveDurationMin,
+          this->cameraFalsePositiveDurationMax);
+
+      // Set the model that will replace the real observed model.
+      fpData.falsePositiveModel = cameraFalsePositiveModelName;
+
+      this->camFalsePositiveModels[_model] = fpData;
+
+      this->img.objects[cameraFalsePositiveModelName] = _p;
+    }
+    else
+      this->img.objects[_model] = _p;
+  }
 }

test/integration/logical_camera.cc

@@ -56,10 +56,24 @@ TEST_F(LogicalCameraTest, FalsePositive)
   world->Step(1000);
 }
 
+/////////////////////////////////////////////////
+TEST_F(LogicalCameraTest, FalsePositiveDuration)
+{
+  Load("logical_camera_02.world", true);
+
+  gazebo::physics::WorldPtr world = gazebo::physics::get_world("default");
+  ASSERT_TRUE(world != NULL);
+
+  // Step the world so that the test library experiences update events.
+  world->Step(1000);
+}
+
 int main(int argc, char **argv)
 {
   // Set a specific seed to avoid occasional test failures due to
   // statistically unlikely, but possible results.
+  ignition::math::Rand::Seed(13458);
+
   ::testing::InitGoogleTest(&argc, argv);
   return RUN_ALL_TESTS();
 }

test/integration/logical_camera_plugin.cc

@@ -47,6 +47,9 @@ void LogicalCameraPlugin::Update(const gazebo::common::UpdateInfo & /*_info*/)
     case 1:
       this->Update1();
       break;
+    case 2:
+      this->Update2();
+      break;
   }
 }
 
@@ -70,10 +73,13 @@ void LogicalCameraPlugin::Update0()
   {
     // Make sure the lost person is not seen 100% of the time. This number,
     // and the number in the following test is tied to
-    // RobotPlugin::cameraFalseNegativeProbMin and
-    // RobotPlugin::cameraFalseNegativeProbMax.
-    EXPECT_LT(count, 950);
-    EXPECT_GT(count, 890);
+    // RobotPlugin::cameraFalseNegativeProbMin,
+    // RobotPlugin::cameraFalseNegativeProbMax,
+    // RobotPlugin::cameraFalsePositiveProbMin, cameraFalsePositiveProbMax,
+    // RobotPlugin::cameraFalsePositiveDurationMin and
+    // RobotPlugin::cameraFalsePositiveDurationMax.
+    EXPECT_LT(count, 900);
+    EXPECT_GT(count, 800);
   }
   ++iteration;
 }
@@ -101,9 +107,60 @@ void LogicalCameraPlugin::Update1()
   {
     // Make sure the lost person is seen some of the time. This number,
     // and the number in the following test are tied to
-    // RobotPlugin::cameraFalsePositiveProbMin and cameraFalsePositiveProbMax.
-    EXPECT_LT(count, 30);
-    EXPECT_GT(count, 2);
+    // RobotPlugin::cameraFalsePositiveProbMin, cameraFalsePositiveProbMax,
+    // RobotPlugin::cameraFalsePositiveDurationMin and
+    // RobotPlugin::cameraFalsePositiveDurationMax.
+    EXPECT_LT(count, 70);
+    EXPECT_GT(count, 30);
+  }
+  ++iteration;
+}
+
+/////////////////////////////////////////////////
+void LogicalCameraPlugin::Update2()
+{
+  if (this->Host() != "192.168.1.1")
+    return;
+
+  static int iteration = 0;
+  static int consecutives = 0;
+  static int total = 0;
+  static int counter = 0;
+
+  swarm::ImageData logImg;
+  EXPECT_TRUE(this->Image(logImg));
+
+  // Keep track of the number of times the lost_person is observed
+  if (logImg.objects.find("lost_person") != logImg.objects.end())
+  {
+    ++consecutives;
+  }
+  else
+  {
+    // I had a false positive in the previous iteration.
+    if (consecutives > 0)
+    {
+      total += consecutives;
+      ++counter;
+    }
+
+    consecutives = 0;
+  }
+
+  // We assume the test runs for 1000 iterations.
+  if (iteration == 999)
+  {
+    // We calculate the mean number of consecutive times that we saw the lost
+    // person.
+    int consecutiveMean = total / counter;
+
+    // Make sure the lost person is seen some of the time. This number,
+    // and the number in the following test are tied to
+    // RobotPlugin::cameraFalsePositiveProbMin, cameraFalsePositiveProbMax,
+    // RobotPlugin::cameraFalsePositiveDurationMin and
+    // RobotPlugin::cameraFalsePositiveDurationMax.
+    EXPECT_LE(consecutiveMean, 8);
+    EXPECT_GE(consecutiveMean, 2);
   }
   ++iteration;
 }

test/integration/logical_camera_plugin.hh

@@ -47,6 +47,9 @@ namespace swarm
     /// \brief Update for test case 1
     private: void Update1();
 
+    /// \brief Update for test case 2
+    private: void Update2();
+
     /// \brief Pointer to the world
     private: gazebo::physics::WorldPtr world;
 

test/worlds/CMakeLists.txt

@@ -32,6 +32,7 @@ set (world_erb_files
   comms_12.world.erb
   logical_camera_00.world.erb
   logical_camera_01.world.erb
+  logical_camera_02.world.erb
   map_00.world.erb
   map_01.world.erb
   map_02.world.erb