Add Plugin XML Attributes to Mission XML Log

include/scrimmage/autonomy/Autonomy.h

@@ -56,6 +56,9 @@ class Autonomy : public EntityPlugin {
     void close(double t) override;
     bool need_reset();
 
+    // Mission to mission function
+    virtual std::map<std::string,std::string> mission_xml_set();
+
     // getters/setters
     StatePtr &desired_state();
     void set_desired_state(StatePtr desired_state);

include/scrimmage/entity/Entity.h

@@ -138,6 +138,13 @@ class Entity : public std::enable_shared_from_this<Entity> {
     MotionModelPtr &motion();
     std::vector<ControllerPtr> &controllers();
 
+    // Mission to mission pull request
+    std::map<std::string,std::string> set_motion_xml_vect();
+    std::vector<std::map<std::string,std::string>> set_sensor_xml_vect();
+    std::vector<std::map<std::string,std::string>> set_autonomy_xml_vect();
+    std::vector<std::map<std::string,std::string>> set_controller_xml_vect();
+    // End of mission to mission
+
     void set_id(ID &id);
     ID &id();
 

include/scrimmage/motion/Controller.h

@@ -52,6 +52,13 @@ class Controller : public EntityPlugin {
         desired_state_ = nullptr;
     }
 
+    // Mission to mission function
+    virtual std::map<std::string,std::string> mission_xml_set() {
+        std::map<std::string,std::string> mission_xml; 
+        return mission_xml;
+    };
+
+
  protected:
     StatePtr state_;
     StatePtr desired_state_;

include/scrimmage/motion/MotionModel.h

@@ -56,6 +56,10 @@ class MotionModel : public EntityPlugin {
                       std::map<std::string, std::string> &params);
 
     virtual bool step(double time, double dt);
+
+    // Mission to mission function
+    virtual std::map<std::string,std::string> mission_xml_set();
+
     virtual bool posthumous(double t);
     virtual StatePtr &state();
     virtual void set_state(StatePtr &state);

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>
@@ -84,6 +86,15 @@ class MissionParse {
     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();
@@ -159,9 +170,18 @@ class MissionParse {
     bool output_type_required(const std::string& output_type);
 
  protected:
+    rapidxml::xml_document<> doc;
+
     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;
+
     double t0_ = 0;
     double tend_ = 50;
     double dt_ = 0.00833333;

include/scrimmage/plugins/autonomy/Straight/Straight.h

@@ -52,6 +52,7 @@ class Straight : public scrimmage::Autonomy{
  public:
     void init(std::map<std::string, std::string> &params) override;
     bool step_autonomy(double t, double dt) override;
+    std::map<std::string,std::string> mission_xml_set() override;
 
  protected:
     double speed_;

include/scrimmage/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.h

@@ -54,6 +54,8 @@ class SimpleAircraftControllerPID : public Controller {
  public:
     virtual void init(std::map<std::string, std::string> &params);
     virtual bool step(double t, double dt);
+    std::map<std::string,std::string> mission_xml_set() override;
+
 
  protected:
     scrimmage::PID heading_pid_;

include/scrimmage/plugins/motion/SimpleAircraft/SimpleAircraft.h

@@ -61,6 +61,8 @@ class SimpleAircraft : public scrimmage::MotionModel{
 
     void teleport(scrimmage::StatePtr &state) override;
 
+    std::map<std::string,std::string> mission_xml_set() override;
+
  protected:
     scrimmage::PID heading_pid_;
     scrimmage::PID alt_pid_;

include/scrimmage/sensor/Sensor.h

@@ -51,6 +51,12 @@ class Sensor : public EntityPlugin {
 
     virtual bool step() {return true;}
 
+    // Mission to mission function
+    virtual std::map<std::string,std::string> mission_xml_set() {
+        std::map<std::string,std::string> mission_xml; 
+        return mission_xml;
+    };
+
     virtual scrimmage::MessageBasePtr sensor_msg(double t);
 
     /*! \brief version when T = MessageBase (calls sensor_msg without casting) */

include/scrimmage/simcontrol/SimControl.h

@@ -320,6 +320,32 @@ 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;
+
+      std::map<std::string,std::string> motion_xml_tags;
+      std::vector<std::map<std::string,std::string>> autonomy_xml_tags;
+      std::vector<std::map<std::string,std::string>> controller_xml_tags;
+      std::vector<std::map<std::string,std::string>> sensor_xml_tags;
+
+   } end_state;
+
+   std::list<ent_end_state> all_end_states;
+
  protected:
     // Key: Entity ID
     // Value: Team ID
@@ -364,6 +390,9 @@ class SimControl {
     bool finished_ = false;
     bool exit_ = false;
 
+    /// @brief Holds the mission_to_mission tag. If true, will create an output XML file of final entity states
+    bool miss2miss = false;
+
     std::mutex finished_mutex_;
     std::mutex contacts_mutex_;
     std::mutex exit_mutex_;

src/autonomy/Autonomy.cpp

@@ -78,6 +78,11 @@ void Autonomy::init() {}
 void Autonomy::init(std::map<std::string, std::string> &/*params*/) {}
 bool Autonomy::need_reset() {return need_reset_;}
 
+std::map<std::string,std::string> Autonomy::mission_xml_set() { 
+    std::map<std::string,std::string> mission_xml; 
+    return mission_xml;
+}
+
 StatePtr &Autonomy::desired_state() {return desired_state_;}
 
 void Autonomy::set_desired_state(StatePtr desired_state) {desired_state_ = desired_state;}

src/entity/Entity.cpp

@@ -726,8 +726,99 @@ 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;
     }
 }
-}  // namespace scrimmage
+
+// Function that creates some kind of data structure based off the sensors, autonomies, 
+// controllers, and motion models.
+// The plugins can call the function to pass their plugin specific xml values, which will
+// be added to the corresponding plugin stored in the data structure
+// Might be good to have a function for each plugin type, instead of combining them like in the
+// print plugins function
+// In the case the xml tag is a vector of PID, could do a type check in the mission parse file and
+// update accordingly
+
+// Note: all data passed from the mission_xml_set function should be formatted as a map of strings
+// to strings. This will assist with the Rapid XML formatting. Additionally, the first entry of the map
+// must be the name of the given plugin - for example: "Straight" for an autonomy plugin.
+
+std::map<std::string,std::string> Entity::set_motion_xml_vect(){
+    std::map<std::string,std::string> cur_motion_xml;
+    if (motion_model_ && motion_model_->name() != "BLANK") {
+        cur_motion_xml = motion_model_->mission_xml_set();
+
+        // Verifies the motion model has the expected xml specific tags. This can be uncommented for debugging.
+        // for (auto itr = cur_motion_xml.begin(); itr != cur_motion_xml.end(); ++itr) {
+        //     std::cout << "Motion Model: 1. " << itr->first << " 2. " << itr->second << std::endl;
+        // }
+    }
+
+    return cur_motion_xml;
+}
+
+std::vector<std::map<std::string,std::string>> Entity::set_sensor_xml_vect(){
+    std::vector<std::map<std::string,std::string>> all_sensor_xml;
+    std::map<std::string,std::string> cur_sensor_xml;
+
+    for (auto &kv : sensors_) {
+        cur_sensor_xml = kv.second->mission_xml_set();
+        all_sensor_xml.push_back(cur_sensor_xml);
+    }
+
+    // Verifies each sensor has the expected xml specific tags. This can be uncommented for debugging.
+    // for (int i = 0; i < all_sensor_xml.size(); i++) {
+    //     std::cout << "Sensor vector #: " << i << std::endl;
+
+    //     // Traverse the map
+    //     for (auto itr : all_sensor_xml[i])
+    //         std::cout << "Sensor: 1. " << itr.first << " 2. " << itr.second << std::endl;
+    // }
+
+    return all_sensor_xml;
+}
+
+std::vector<std::map<std::string,std::string>> Entity::set_autonomy_xml_vect(){
+    std::vector<std::map<std::string,std::string>> all_autonomy_xml;
+    std::map<std::string,std::string> cur_autonomy_xml;
+
+    for (AutonomyPtr a : autonomies_) {
+        cur_autonomy_xml = a->mission_xml_set();
+        all_autonomy_xml.push_back(cur_autonomy_xml);
+    }
+
+    // Verifies each autonomy has the expected xml specific tags. This can be uncommented for debugging.
+    // for (int i = 0; i < all_autonomy_xml.size(); i++) {
+    //     std::cout << "Autonomy vector #: " << i << std::endl;
+
+    //     // Traverse the map
+    //     for (auto itr : all_autonomy_xml[i])
+    //         std::cout << "Autonomy: 1. " << itr.first << " 2. " << itr.second << std::endl;
+    // }
+
+    return all_autonomy_xml;
+}
+
+std::vector<std::map<std::string,std::string>> Entity::set_controller_xml_vect(){
+    std::vector<std::map<std::string,std::string>> all_controller_xml;
+    std::map<std::string,std::string> cur_controller_xml;
+
+    for (ControllerPtr c : controllers_) {
+        cur_controller_xml = c->mission_xml_set();
+        all_controller_xml.push_back(cur_controller_xml);
+    }
+
+    // Verifies each controller has the expected xml specific tags. This can be uncommented for debugging.
+    // for (int i = 0; i < all_controller_xml.size(); i++) {
+    //     std::cout << "Controller vector #: " << i << std::endl;
+
+    //     // Traverse the map
+    //     for (auto itr : all_controller_xml[i])
+    //         std::cout << "Controller: 1. " << itr.first << " 2. " << itr.second << std::endl;
+    // }
+
+    return all_controller_xml;
+}
+
+}  // namespace scrimmage

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) {