Add Plugin XML Attributes to Mission XML Log

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,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 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/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_get() 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_get() 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_get() override;
+
  protected:
     scrimmage::PID heading_pid_;
     scrimmage::PID alt_pid_;

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

@@ -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 mission_to_mission = false;
+
     std::mutex finished_mutex_;
     std::mutex contacts_mutex_;
     std::mutex exit_mutex_;

missions/straight.xml

@@ -3,15 +3,17 @@
 <runscript xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
     name="Straight flying">
 
-  <run start="0.0" end="100" dt="0.1"
+  <run start="0.0" end="10" dt="0.1"
        time_warp="10"
-       enable_gui="true"
+       enable_gui="false"
        network_gui="false"
        start_paused="true"
        full_screen="true"
        window_width="800"
        window_height="600"/>
 
+  <mission_to_mission>true</mission_to_mission>
+
   <multi_threaded num_threads="8">false</multi_threaded>
   <stream_port>50051</stream_port>
   <stream_ip>localhost</stream_ip>
@@ -82,6 +84,7 @@
 
   <!-- ========================== TEAM 1 ========================= -->
   <entity>
+    <remove_block>true</remove_block>
     <name>uav_entity</name>
     <team_id>1</team_id>
     <color>77 77 255</color>
@@ -127,9 +130,10 @@
   </entity>
 
   <entity>
+    <remove_block>false</remove_block>
     <team_id>2</team_id>
     <color>255 0 0</color>
-    <count>30</count>
+    <count>2</count>
     <health>1</health>
     <radius>2</radius>
 
@@ -148,6 +152,7 @@
     <visual_model>zephyr-red</visual_model>
     <autonomy speed="21"
               enable_boundary_control="true">Straight</autonomy>
+    <autonomy>Follow</autonomy>
     <base>
       <latitude>35.719961</latitude>
       <longitude>-120.767304</longitude>
@@ -156,26 +161,4 @@
     </base>
   </entity>
 
-  <!-- Entity that is generated during runtime by other plugins -->
-  <entity tag="gen_straight">
-    <count>0</count>
-    <team_id>1</team_id>
-    <health>1</health>
-    <radius>1</radius>
-
-    <variance_x>20</variance_x>
-    <variance_y>20</variance_y>
-    <variance_z>10</variance_z>
-
-    <x>0</x>
-    <y>0</y>
-    <z>0</z>
-    <heading>0</heading>
-
-    <autonomy>Straight</autonomy>
-    <controller>SimpleAircraftControllerPID</controller>
-    <motion_model>SimpleAircraft</motion_model>
-    <visual_model>zephyr-blue</visual_model>
-  </entity>
-
 </runscript>

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,59 @@ 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
+
+// Note: All data passed from the mission_xml_get function must 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. The key for the
+// plugin name must be "Name".
+
+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) {