Add Plugin XML Attributes to Mission XML Log

docs/source/tutorials/mission-to-mission-xml.rst

@@ -0,0 +1,75 @@
+==================================
+Mission to Mission XML Generation
+==================================
+
+This tutorial covers the process of capturing the end states of entities in
+an XML file, which can be used as starting points for future simulations. The 
+following must be completed before the mission to mission capabilities can be
+utilized:
+
+1. In the mission XML file, the following tag must be included with the value ``true``::
+
+        <mission_to_mission>true</mission_to_mission>
+
+2. In the mission XML file's entity block, if the block should be included in the output
+   mission XML file - meaning future simulations will require the block, the following tag
+   must be included with the value ``true``. If it is not included, the entity block will be removed
+   from the output mission XML file. ::
+
+        <remove_block>true</remove_block>
+
+3. If plugin specific XML tag attributes (applicable to motion, sensor, autonomy, and controller plugins) 
+   are expected to be updated while the simulation is running, the following function declaration will need to 
+   be added to the plugin's header file::
+
+        std::map<std::string,std::string> mission_xml_get() override;
+
+   The mission_xml_get function must insert plugin specific xml tags as strings to a map. Depending on the variable
+   type of the xml tag, extra formatting might be needed - for example: converting  a bool to a string results in 
+   "0" or "1," which will need to be converted to "true" or "false."
+
+   Here is an example of the mission_xml_get, implemented in the SimpleAircraft.cpp file::
+
+        std::map<std::string,std::string> SimpleAircraft::mission_xml_get() {
+            std::map<std::string,std::string> mission_xml;
+
+            mission_xml.insert({"Name","SimpleAircraft"});
+            mission_xml.insert({"min_velocity",std::to_string(min_velocity_)});
+            mission_xml.insert({"max_velocity",std::to_string(max_velocity_)});
+            mission_xml.insert({"max_roll",std::to_string(max_roll_)});
+            mission_xml.insert({"max_roll_rate",std::to_string(max_roll_rate_)});
+            mission_xml.insert({"max_pitch",std::to_string(max_pitch_)});
+            mission_xml.insert({"max_pitch_rate",std::to_string(max_pitch_rate_)});
+            mission_xml.insert({"speed_target",std::to_string(speedTarget_)});
+            mission_xml.insert({"radius_slope_per_speed",std::to_string(lengthSlopePerSpeed_)});
+            mission_xml.insert({"turning_radius",std::to_string(length_)});
+
+            return mission_xml;
+        }
+
+   Note that the plugin name must be specified as the first entry in the map, with the key ``Name`` and the 
+   value ``<plugin_name>``. In the map, each xml specific tag attribute name must be the key of the map and 
+   the attribute value must be the value of the map.
+
+There will be 2 output files, which can be found in the simulation's log directory: 
+``~/.scrimmage/logs/<scrimmage_run_log_directory>``.
+
+1. The ``mission_to_mission.xml`` file captures the final entity states and formats the data to be used as an
+   input file to future SCRIMMAGE simulations.
+
+2. The ``final_ent_states.txt`` file captures the final state information for each entity. These values can be
+   used as reference to verify end states and capture values like velocity, which do not have entity specific tags.
+   The following entity end states are included in the output text file:
+
+    * Team_ID
+    * X_Pos
+    * Y_Pos
+    * Z_Pos
+    * Vel_X
+    * Vel_Y
+    * Vel_Z
+    * Heading
+    * Pitch
+    * Roll
+    * Altitude
+    * Health

docs/source/tutorials/tutorials.rst

@@ -34,3 +34,4 @@ swarm behaviors.
    utilities.rst
    simcontrol.rst
    capture-the-flag.rst
+   mission-to-mission-xml.rst

include/scrimmage/autonomy/Autonomy.h

@@ -60,6 +60,13 @@ class Autonomy : public EntityPlugin {
     StatePtr &desired_state();
     void set_desired_state(StatePtr desired_state);
 
+    /// @brief Plugin specific xml tags are inserted to a map and returned. The map
+    /// is used to update the mission to mission xml file, which captures entity end states
+    /// to be used as starting points in future simulations.
+    /// @return A map of keys and values of type string, representing plugin specific
+    /// xml tag attribute names and associated values
+    virtual std::map<std::string,std::string> mission_xml_get();
+
     ContactMapPtr &get_contacts();
     ContactMap &get_contacts_raw();
     virtual void set_contacts(ContactMapPtr &contacts);

include/scrimmage/entity/Entity.h

@@ -138,6 +138,21 @@ class Entity : public std::enable_shared_from_this<Entity> {
     MotionModelPtr &motion();
     std::vector<ControllerPtr> &controllers();
 
+    /// @brief Loops through each motion model for the given entity and retrieves a map of plugin 
+    /// specific xml tags
+    /// @return A map of keys and values of type string, representing plugin specific xml tag 
+    /// attribute names and associated values
+    std::map<std::string,std::string> set_motion_xml_map();
+
+    /// @brief Loops through each sensor, autonomy, or controller plugin for the given entity and 
+    /// retrieves a map of plugin specific xml tags. Each map corresponds to a given plugin and 
+    /// is pushed into a vector.
+    /// @return A vector of maps of keys and values of type string, representing plugin specific 
+    /// xml tag attribute names and associated values
+    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();
+
     void set_id(ID &id);
     ID &id();
 

include/scrimmage/motion/Controller.h

@@ -52,6 +52,17 @@ class Controller : public EntityPlugin {
         desired_state_ = nullptr;
     }
 
+    /// @brief Plugin specific xml tags are inserted to a map and returned. The map
+    /// is used to update the mission to mission xml file, which captures entity end states
+    /// to be used as starting points in future simulations.
+    /// @return A map of keys and values of type string, representing plugin specific
+    /// xml tag attribute names and associated values
+    virtual std::map<std::string,std::string> mission_xml_get() {
+        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,14 @@ class MotionModel : public EntityPlugin {
                       std::map<std::string, std::string> &params);
 
     virtual bool step(double time, double dt);
+
+    /// @brief Plugin specific xml tags are inserted to a map and returned. The map
+    /// is used to update the mission to mission xml file, which captures entity end states
+    /// to be used as starting points in future simulations.
+    /// @return A map of keys and values of type string, representing plugin specific
+    /// xml tag attribute names and associated values
+    virtual std::map<std::string,std::string> mission_xml_get();
+
     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,19 @@ class MissionParse {
     bool create_log_dir();
     void set_overrides(const std::string &overrides);
     bool parse(const std::string &filename);
+
+    /// @brief Adds all plugin specific xml attributes to the plugin_spec_attrs map
+    /// @param node_name 
+    /// @param node_value 
+    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 +174,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 mission_to_mission_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/sensor/Sensor.h

@@ -51,6 +51,16 @@ class Sensor : public EntityPlugin {
 
     virtual bool step() {return true;}
 
+    /// @brief Plugin specific xml tags are inserted to a map and returned. The map
+    /// is used to update the mission to mission xml file, which captures entity end states
+    /// to be used as starting points in future simulations.
+    /// @return A map of keys and values of type string, representing plugin specific
+    /// xml tag attribute names and associated values
+    virtual std::map<std::string,std::string> mission_xml_get() {
+        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

@@ -321,6 +321,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
@@ -365,6 +391,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 mission_to_mission = 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_get() { 
+    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,54 @@ 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
+
+std::map<std::string,std::string> Entity::set_motion_xml_map(){
+    std::map<std::string,std::string> cur_motion_xml;
+    if (motion_model_ && motion_model_->name() != "BLANK") {
+        cur_motion_xml = motion_model_->mission_xml_get();
+    }
+
+    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_get();
+        all_sensor_xml.push_back(cur_sensor_xml);
+    }
+
+    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_get();
+        all_autonomy_xml.push_back(cur_autonomy_xml);
+    }
+
+    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_get();
+        all_controller_xml.push_back(cur_controller_xml);
+    }
+
+    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) {