Add Plugin XML Attributes to Mission XML Log

include/scrimmage/parse/MissionParse.h

@@ -39,6 +39,8 @@
 #include <scrimmage/proto/Visual.pb.h>
 
 #include <scrimmage/proto/Color.pb.h>
+#include <scrimmage/simcontrol/SimControl.h>
+#include <rapidxml/rapidxml.hpp>
 
 #include <list>
 #include <vector>
@@ -78,12 +80,21 @@ struct GenerateInfo {
     bool first_in_group;
     double time_variance;
 };
-
+// test
 class MissionParse {
  public:
     bool create_log_dir();
     void set_overrides(const std::string &overrides);
     bool parse(const std::string &filename);
+    void get_plugin_params(std::string node_name, std::string node_value);
+
+    /// @brief Generate the mission to mission xml file for all final states of entities
+    /// @param all_end_states 
+    void final_state_xml(std::list<SimControl::ent_end_state> & all_end_states);
+
+    /// @brief Track the number of entity blocks in the input Mission XML file
+    int num_ents = 0;
+
     bool write(const std::string &filename);
 
     double t0();
@@ -161,6 +172,12 @@ class MissionParse {
  protected:
     std::string mission_filename_ = "";
     std::string mission_file_content_ = "";
+    std::string miss2miss_file_content = "";
+    std::stringstream ent_state_file_content;
+    std::string mission_plugin_file_content = "";
+    std::string scrimmage_plugin_path = "";
+    std::map<std::string, std::string> plugin_spec_attrs;
+    rapidxml::xml_document<> doc;
 
     double t0_ = 0;
     double tend_ = 50;

include/scrimmage/simcontrol/SimControl.h

@@ -320,6 +320,26 @@ class SimControl {
 
     void set_running_in_thread(bool running_in_thread);
 
+   /// @brief Contains the final state values of each entity
+   struct ent_end_state {
+      int team_id;
+      double x_pos;
+      double y_pos;
+      double z_pos;
+
+      double yaw;
+      double pitch;
+      double roll;
+
+      int health_points;
+
+      double vel_x;
+      double vel_y;
+      double vel_z;
+   } end_state;
+
+   std::list<ent_end_state> all_end_states;
+
  protected:
     // Key: Entity ID
     // Value: Team ID
@@ -363,6 +383,7 @@ class SimControl {
 
     bool finished_ = false;
     bool exit_ = false;
+    bool miss2miss = false;
 
     std::mutex finished_mutex_;
     std::mutex contacts_mutex_;

src/entity/Entity.cpp

@@ -726,7 +726,7 @@ void Entity::print_plugins(std::ostream &out) const {
         out << c->name() << endl;
     }
     out << "----------- Motion -------------" << endl;
-    if (motion_model_->name() != "BLANK") {
+    if (motion_model_ && motion_model_->name() != "BLANK") {
         out << motion_model_->name() << endl;
     }
 }

src/parse/ConfigParse.cpp

@@ -126,7 +126,19 @@ bool ConfigParse::parse(const std::map<std::string, std::string> &overrides,
     buffer << file.rdbuf();
     file.close();
     std::string content(buffer.str());
-    doc.parse<0>(&content[0]);
+    try {
+        // Note: This parse function can hard fail (seg fault, no exception) on
+        //       badly formatted xml data. Sometimes it'll except, sometimes not.
+        // doc.parse<0>(mission_file_content_vec.data());
+        doc.parse<0>(&content[0]);
+    }
+    catch (const rapidxml::parse_error& e)
+    {
+        std::cout << e.what() << std::endl;
+        throw std::runtime_error("Error parsing config file " + filename);
+        // cout << "scrimmage::MissionParse::parse: Exception during rapidxml::xml_document<>.parse<>()." << endl;
+        return false;
+    }
 
     rx::xml_node<> *config_node = doc.first_node("params");
     if (config_node == 0) {