path_tracer: Support rendering of breadcrumbs.

subt_ign/src/path_tracer.cc

@@ -139,6 +139,19 @@ Processor::Processor(const std::string &_path, const std::string &_configPath)
                   {0.0, 0.2, 0.2, 0.5});
   }
 
+  // Color of breadcrumbs
+  if (cfg && cfg["breadcrumb_color"])
+  {
+    this->breadcrumbColor = MarkerColor(cfg["breadcrumb_color"]);
+  }
+  else
+  {
+    this->breadcrumbColor =
+      MarkerColor({1.0, 1.0, 0.0, 0.5},
+                  {1.0, 1.0, 0.0, 0.5},
+                  {1.0, 1.0, 0.0, 0.5});
+  }
+
   // Color of robot paths
   if (cfg && cfg["robot_colors"])
   {
@@ -258,6 +271,8 @@ Processor::Processor(const std::string &_path, const std::string &_configPath)
       }
     }
 
+    size_t numReports = 0;
+    size_t numBreadcrumbs = 0;
     for (std::size_t i = 0; i < events.size(); ++i)
     {
       if (events[i]["type"].as<std::string>() == "artifact_report_attempt")
@@ -276,8 +291,22 @@ Processor::Processor(const std::string &_path, const std::string &_configPath)
         data->score = events[i]["points_scored"].as<int>();
 
         this->logData[sec].push_back(std::move(data));
+        numReports++;
+      }
+      else if (events[i]["type"].as<std::string>() == "breadcrumb_deploy")
+      {
+        int sec = events[i]["time_sec"].as<int>();
+        std::unique_ptr<BreadcrumbData> data = std::make_unique<BreadcrumbData>();
+        data->type = BREADCRUMB;
+        data->robot = events[i]["robot"].as<std::string>();
+        data->sec = sec;
+
+        this->logData[sec].push_back(std::move(data));
+        numBreadcrumbs++;
       }
     }
+    std::cout << "Parsed " << numReports << " artifact report attempt events." << std::endl;
+    std::cout << "Parsed " << numBreadcrumbs << " breadcrumb deploy events." << std::endl;
   }
   else
   {
@@ -547,6 +576,37 @@ void ReportData::Render(Processor *_p)
   }
 }
 
+/////////////////////////////////////////////////
+void BreadcrumbData::Render(Processor *_p)
+{
+  for (int sec = this->sec; sec >= 0; --sec)
+  {
+    if (_p->logData.find(sec) != _p->logData.end())
+    {
+      for (const auto& data : _p->logData[sec])
+      {
+        if (data->type == ROBOT)
+        {
+          const auto& poses = dynamic_cast<RobotPoseData*>(data.get())->poses;
+          if (poses.find(this->robot) == poses.end())
+            continue;
+
+          const auto& pose = poses.at(this->robot).back();
+
+          _p->SpawnMarker(_p->breadcrumbColor,
+                          pose.Pos() + ignition::math::Vector3d(0, 0, 0.5),
+                          ignition::msgs::Marker::BOX,
+                          ignition::math::Vector3d(6, 6, 20));
+
+          return;
+        }
+      }
+    }
+  }
+
+  std::cerr << "Could not find position for breadcrumb at time " << this->sec << std::endl;
+}
+
 /////////////////////////////////////////////////
 int main(int _argc, char **_argv)
 {

subt_ign/src/path_tracer.hh

@@ -90,6 +90,22 @@
 //     g: 0.2
 //     b: 0.2
 //     a: 0.5
+// breadcrumb_color:
+//   ambient:
+//     r: 1.0
+//     g: 1.0
+//     b: 0.0
+//     a: 0.5
+//   diffuse:
+//     r: 1.0
+//     g: 1.0
+//     b: 0.0
+//     a: 0.5
+//   emissive:
+//     r: 0.2
+//     g: 0.2
+//     b: 0.0
+//     a: 0.5
 // robot_colors:
 //   - color:
 //     ambient:
@@ -132,7 +148,8 @@ class Processor;
 enum DataType
 {
   ROBOT  = 0,
-  REPORT = 1
+  REPORT = 1,
+  BREADCRUMB = 2
 };
 
 /// \brief Color properties for a marker.
@@ -202,6 +219,19 @@ class ReportData : public Data
   public: void Render(Processor *_p);
 };
 
+/// \brief Breadcrumb deployment data
+class BreadcrumbData : public Data
+{
+  /// \brief Name of the robot that deployed the breadcrumb.
+  public: std::string robot;
+
+  /// \brief The time when the breadcrumb was deployed.
+  public: int sec;
+
+  /// \brief The render function for breadcrumb data.
+  public: void Render(Processor *_p);
+};
+
 /// \brief The log file processor.
 class Processor
 {
@@ -256,6 +286,9 @@ class Processor
   /// \brief The colors used to represent each robot.
   public: std::vector<MarkerColor> robotColors;
 
+  /// \brief Color of deployed breadcrumbs.
+  public: MarkerColor breadcrumbColor;
+
   /// \brief Last pose of a robot. This is used to reduce the number of markers.
   private: std::map<std::string, ignition::math::Pose3d> prevPose;
 
@@ -275,7 +308,7 @@ class Processor
   private: std::condition_variable cv;
 
   /// \brief All of the pose data.
-  private: std::map<int, std::vector<std::unique_ptr<Data>>> logData;
+  public: std::map<int, std::vector<std::unique_ptr<Data>>> logData;
 
   /// \brief Realtime factor for playback.
   private: double rtf = 1.0;