From 58c3fedd91fbc11a7a8cc6b59830f15c7213552a Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Fri, 31 Mar 2023 16:01:22 -0400 Subject: [PATCH 01/46] Initial commit --- missions/straight.xml | 2 +- src/parse/MissionParse.cpp | 100 +++++++++++++++++++++++++++++++++++++ 2 files changed, 101 insertions(+), 1 deletion(-) diff --git a/missions/straight.xml b/missions/straight.xml index 9c368c098..95043d85e 100644 --- a/missions/straight.xml +++ b/missions/straight.xml @@ -5,7 +5,7 @@ next_sibling()) { std::string nm = node->name(); + + ///////////////////////////////////////////////// + ///////////////////////////////////////////////// + // Plugin type: nm + // Plugin name: node->value() + // Plugin attribute name: attr->name() + // Plugin attribute value: attr->value() + // + // Need to open the plugin specific xml file if it is one of the following + // plugins: + // - Autonomy + // - Controller + // - Motion model + // - ? + // + // Starting with just the autonomy plugin, let's open that file... + if (nm == "autonomy"){ + cout << "Natalie - in the autonomy file opener" << endl; + std::string plugin_file = node->value() + std::string(".xml"); + std::string plugin_filename_ = expand_user(plugin_file); + + // First, explicitly search for the mission file. + if (!fs::exists(plugin_filename_)) { + // If the file doesn't exist, search for the mission file under the + // SCRIMMAGE_MISSION_PATH. + FileSearch file_search; + std::string result = ""; + + std::string pluginxml_path = "/home/ndavis64/scrimmage/scrimmage/include/scrimmage/plugins/" + nm + "/" + node->value(); + + bool status = file_search.find_file(plugin_filename_, "xml", + pluginxml_path, + result, false); + if (!status) { + // The mission file wasn't found. Exit. + cout << "SCRIMMAGE mission file not found: " << plugin_filename_ << endl; + return false; + } + // The mission file was found, save its path. + plugin_filename_ = result; + } + + std::ifstream file(plugin_filename_.c_str()); + if (!file.is_open()) { + std::cout << "Failed to open mission file: " << plugin_filename_ << endl; + return false; + } + + std::stringstream buffer; + buffer << file.rdbuf(); + file.close(); + mission_file_content_ = buffer.str(); + + // Search and replace any overrides of the form ${key=value} in the mission + // file + for (auto &kv : overrides_map_) { + std::regex reg("\\$\\{" + kv.first + "=(.+?)\\}"); + mission_file_content_ = std::regex_replace(mission_file_content_, reg, + kv.second); + } + + // Replace our xml variables of the form ${var=default} with the default + // value + std::string fmt{"$1"}; + std::regex reg("\\$\\{.+?=(.+?)\\}"); + mission_file_content_ = std::regex_replace(mission_file_content_, reg, fmt); + + // Parse the xml tree. + rapidxml::xml_document<> doc; + // doc.parse requires a null terminated string that it can modify. + std::vector mission_file_content_vec(mission_file_content_.size() + 1); // allocation done here + mission_file_content_vec.assign(mission_file_content_.begin(), mission_file_content_.end()); // copy + mission_file_content_vec.push_back('\0'); // shouldn't reallocate + 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()); + } catch (...) { + cout << "scrimmage::MissionParse::parse: Exception during rapidxml::xml_document<>.parse<>()." << endl; + return false; + } + + rapidxml::xml_node<> *params_node = doc.first_node("params"); + if (params_node == 0) { + cout << "Missing params tag." << endl; + return false; + } + + for (rapidxml::xml_node<> *node = params_node->first_node(); node != 0; + node = node->next_sibling()){ + cout << "Natalie node name: " << node->name() << " and the value: " << node->value() << endl; + } + + } + ///////////////////////////////////////////////// + ///////////////////////////////////////////////// + if (nm == "autonomy") { nm += std::to_string(autonomy_order++); } else if (nm == "controller") { @@ -536,6 +635,7 @@ bool MissionParse::parse(const std::string &filename) { attr; attr = attr->next_attribute()) { const std::string attr_name = attr->name(); + if (attr_name == "param_common") { for (auto &kv : param_common[attr->value()]) { entity_attributes_[ent_desc_id][nm][kv.first] = kv.second; From 84bd801e1dcd832be2a24388c42852c81ff28a47 Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Mon, 10 Apr 2023 11:33:03 -0400 Subject: [PATCH 02/46] Working verison, adds attribute and value and then prints the doc updated. Need to update the string stream that is written to the mission.xml now so that the changes are saved to the logs --- include/scrimmage/parse/MissionParse.h | 6 +++- src/parse/MissionParse.cpp | 50 +++++++++++++++++++------- 2 files changed, 43 insertions(+), 13 deletions(-) diff --git a/include/scrimmage/parse/MissionParse.h b/include/scrimmage/parse/MissionParse.h index f4ec4ff4d..0190339d6 100644 --- a/include/scrimmage/parse/MissionParse.h +++ b/include/scrimmage/parse/MissionParse.h @@ -47,6 +47,8 @@ #include #include +#include + namespace sp = scrimmage_proto; namespace scrimmage { @@ -161,7 +163,9 @@ class MissionParse { protected: std::string mission_filename_ = ""; std::string mission_file_content_ = ""; - + std::string plugin_file_content_ = ""; + rapidxml::xml_document<> doc; + double t0_ = 0; double tend_ = 50; double dt_ = 0.00833333; diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index a82bd06c1..6326488a2 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -53,6 +53,7 @@ #undef BOOST_NO_CXX11_SCOPED_ENUMS #include +#include using std::cout; using std::endl; @@ -129,7 +130,7 @@ bool MissionParse::parse(const std::string &filename) { mission_file_content_ = std::regex_replace(mission_file_content_, reg, fmt); // Parse the xml tree. - rapidxml::xml_document<> doc; + //rapidxml::xml_document<> doc; // doc.parse requires a null terminated string that it can modify. std::vector mission_file_content_vec(mission_file_content_.size() + 1); // allocation done here mission_file_content_vec.assign(mission_file_content_.begin(), mission_file_content_.end()); // copy @@ -143,6 +144,11 @@ bool MissionParse::parse(const std::string &filename) { return false; } + // Natalie - this works for printing the document + std::string s; + rapidxml::print(std::back_inserter(s), doc, 0); + cout << s << endl; + rapidxml::xml_node<> *runscript_node = doc.first_node("runscript"); if (runscript_node == 0) { cout << "Missing runscript tag." << endl; @@ -521,6 +527,7 @@ bool MissionParse::parse(const std::string &filename) { std::string nm = node->name(); + std::map plugin_spec_attrs; ///////////////////////////////////////////////// ///////////////////////////////////////////////// // Plugin type: nm @@ -571,13 +578,13 @@ bool MissionParse::parse(const std::string &filename) { std::stringstream buffer; buffer << file.rdbuf(); file.close(); - mission_file_content_ = buffer.str(); + plugin_file_content_ = buffer.str(); // Search and replace any overrides of the form ${key=value} in the mission // file for (auto &kv : overrides_map_) { std::regex reg("\\$\\{" + kv.first + "=(.+?)\\}"); - mission_file_content_ = std::regex_replace(mission_file_content_, reg, + plugin_file_content_ = std::regex_replace(plugin_file_content_, reg, kv.second); } @@ -585,24 +592,24 @@ bool MissionParse::parse(const std::string &filename) { // value std::string fmt{"$1"}; std::regex reg("\\$\\{.+?=(.+?)\\}"); - mission_file_content_ = std::regex_replace(mission_file_content_, reg, fmt); + plugin_file_content_ = std::regex_replace(plugin_file_content_, reg, fmt); // Parse the xml tree. - rapidxml::xml_document<> doc; + rapidxml::xml_document<> plugin_doc; // doc.parse requires a null terminated string that it can modify. - std::vector mission_file_content_vec(mission_file_content_.size() + 1); // allocation done here - mission_file_content_vec.assign(mission_file_content_.begin(), mission_file_content_.end()); // copy - mission_file_content_vec.push_back('\0'); // shouldn't reallocate + std::vector plugin_file_content_vec(plugin_file_content_.size() + 1); // allocation done here + plugin_file_content_vec.assign(plugin_file_content_.begin(), plugin_file_content_.end()); // copy + plugin_file_content_vec.push_back('\0'); // shouldn't reallocate 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()); + plugin_doc.parse<0>(plugin_file_content_vec.data()); } catch (...) { cout << "scrimmage::MissionParse::parse: Exception during rapidxml::xml_document<>.parse<>()." << endl; return false; } - rapidxml::xml_node<> *params_node = doc.first_node("params"); + rapidxml::xml_node<> *params_node = plugin_doc.first_node("params"); if (params_node == 0) { cout << "Missing params tag." << endl; return false; @@ -610,7 +617,8 @@ bool MissionParse::parse(const std::string &filename) { for (rapidxml::xml_node<> *node = params_node->first_node(); node != 0; node = node->next_sibling()){ - cout << "Natalie node name: " << node->name() << " and the value: " << node->value() << endl; + plugin_spec_attrs.insert({node->name(), node->value()}); + cout << "Natalie node name: " << typeid(node->name()).name() << " and the value: " << node->value() << endl; } } @@ -641,11 +649,29 @@ bool MissionParse::parse(const std::string &filename) { entity_attributes_[ent_desc_id][nm][kv.first] = kv.second; } } else { + if (plugin_spec_attrs[attr->name()]!=""){ + cout << "The key is specified in the mission xml file, so delete it from the map." << endl; + plugin_spec_attrs.erase(attr->name()); + } entity_attributes_[ent_desc_id][nm][attr_name] = attr->value(); } } - } + for(std::map::iterator mapitr=plugin_spec_attrs.begin(); mapitr!=plugin_spec_attrs.end(); ++mapitr){ + cout << "Natalie adding new lines to plugin: 1. " << mapitr->first << " 2. " << mapitr->second << endl; + //rapidxml::xml_attribute <> *tempattr = doc.allocate_attribute((mapitr->first).c_str(), (mapitr->second).c_str()); + char *attribute_name = doc.allocate_string(mapitr->first.c_str()); + char *attribute_value = doc.allocate_string(mapitr->second.c_str()); + rapidxml::xml_attribute <> *tempattr = doc.allocate_attribute(attribute_name, attribute_value); + node->append_attribute(tempattr); + } + + // Natalie - this works for printing the document + std::string s; + rapidxml::print(std::back_inserter(s), doc, 0); + cout << s << endl; + } + // For each entity, if the lat/lon are defined, use these values to // overwrite the "x" and "y" values // Search for lon lat alt. From 68dc6c300e511a3671ff4002bcdc790f1154c8a7 Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Mon, 10 Apr 2023 15:01:15 -0400 Subject: [PATCH 03/46] Working for all plugins --- include/scrimmage/parse/MissionParse.h | 1 - src/parse/MissionParse.cpp | 176 ++++++++++++++++++++----- 2 files changed, 142 insertions(+), 35 deletions(-) diff --git a/include/scrimmage/parse/MissionParse.h b/include/scrimmage/parse/MissionParse.h index 0190339d6..48f9d2528 100644 --- a/include/scrimmage/parse/MissionParse.h +++ b/include/scrimmage/parse/MissionParse.h @@ -163,7 +163,6 @@ class MissionParse { protected: std::string mission_filename_ = ""; std::string mission_file_content_ = ""; - std::string plugin_file_content_ = ""; rapidxml::xml_document<> doc; double t0_ = 0; diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index 6326488a2..5cfc8db86 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -84,8 +84,6 @@ void MissionParse::set_overrides(const std::string &overrides) { bool MissionParse::parse(const std::string &filename) { mission_filename_ = expand_user(filename); - //cout << "Natalie, filename: " << filename << " and the mission file name: " << mission_filename_ << endl; - // First, explicitly search for the mission file. if (!fs::exists(mission_filename_)) { // If the file doesn't exist, search for the mission file under the @@ -130,7 +128,6 @@ bool MissionParse::parse(const std::string &filename) { mission_file_content_ = std::regex_replace(mission_file_content_, reg, fmt); // Parse the xml tree. - //rapidxml::xml_document<> doc; // doc.parse requires a null terminated string that it can modify. std::vector mission_file_content_vec(mission_file_content_.size() + 1); // allocation done here mission_file_content_vec.assign(mission_file_content_.begin(), mission_file_content_.end()); // copy @@ -144,11 +141,6 @@ bool MissionParse::parse(const std::string &filename) { return false; } - // Natalie - this works for printing the document - std::string s; - rapidxml::print(std::back_inserter(s), doc, 0); - cout << s << endl; - rapidxml::xml_node<> *runscript_node = doc.first_node("runscript"); if (runscript_node == 0) { cout << "Missing runscript tag." << endl; @@ -247,6 +239,8 @@ bool MissionParse::parse(const std::string &filename) { std::string nm = node->name(); if (nm != "entity" && nm != "base" && nm != "entity_common" && nm != "param_common") { + cout << "Node name: " << nm << endl; + params_[nm] = node->value(); rapidxml::xml_attribute<> *attr = node->first_attribute("name"); @@ -260,6 +254,98 @@ bool MissionParse::parse(const std::string &filename) { attributes_[nm3]["ORIGINAL_PLUGIN_NAME"] = node->value(); attributes_[nm4]["ORIGINAL_PLUGIN_NAME"] = node->value(); + // Ideally - need to make the following conditional a separate function since it is reused code + + // Add plugin specific xml attributes and values to the plugin_spec_attrs map. + // These plugin specific values will be checked against the mission xml file's plugin + // attributes - adding them to the mission xml log file if they are not already included. + // + // Plugins that are not entity specific to check for: + // - entity_interaction + // - metrics + // - network + std::map plugin_spec_attrs; + if (nm == "metrics" || nm == "entity_interaction" || nm == "network"){ + std::string plugin_file = node->value() + std::string(".xml"); + std::string plugin_filename_ = expand_user(plugin_file); + + // First, explicitly search for the mission file. + if (!fs::exists(plugin_filename_)) { + // If the file doesn't exist, search for the mission file under the + // SCRIMMAGE_MISSION_PATH. + FileSearch file_search; + std::string result = ""; + + std::string pluginxml_path = "/home/ndavis64/scrimmage/scrimmage/include/scrimmage/plugins/" + nm + "/" + node->value(); //This will need to be updated to the environment variable path + if(nm=="entity_interaction"){ + std::string temp = node->value(); + pluginxml_path = "/home/ndavis64/scrimmage/scrimmage/include/scrimmage/plugins/interaction/" + temp; //This will need to be updated to the environment variable path + } + + bool status = file_search.find_file(plugin_filename_, "xml", + pluginxml_path, + result, false); + if (!status) { + // The mission file wasn't found. Exit. + cout << "SCRIMMAGE mission file not found: " << plugin_filename_ << endl; + return false; + } + // The mission file was found, save its path. + plugin_filename_ = result; + } + + std::ifstream file(plugin_filename_.c_str()); + if (!file.is_open()) { + std::cout << "Failed to open mission file: " << plugin_filename_ << endl; + return false; + } + + std::stringstream buffer; + buffer << file.rdbuf(); + file.close(); + std::string plugin_file_content_ = buffer.str(); + + // Search and replace any overrides of the form ${key=value} in the mission + // file + for (auto &kv : overrides_map_) { + std::regex reg("\\$\\{" + kv.first + "=(.+?)\\}"); + plugin_file_content_ = std::regex_replace(plugin_file_content_, reg, + kv.second); + } + + // Replace our xml variables of the form ${var=default} with the default + // value + std::string fmt{"$1"}; + std::regex reg("\\$\\{.+?=(.+?)\\}"); + plugin_file_content_ = std::regex_replace(plugin_file_content_, reg, fmt); + + // Parse the xml tree. + rapidxml::xml_document<> plugin_doc; + // doc.parse requires a null terminated string that it can modify. + std::vector plugin_file_content_vec(plugin_file_content_.size() + 1); // allocation done here + plugin_file_content_vec.assign(plugin_file_content_.begin(), plugin_file_content_.end()); // copy + plugin_file_content_vec.push_back('\0'); // shouldn't reallocate + try { + // Note: This parse function can hard fail (seg fault, no exception) on + // badly formatted xml data. Sometimes it'll except, sometimes not. + plugin_doc.parse<0>(plugin_file_content_vec.data()); + } catch (...) { + cout << "scrimmage::MissionParse::parse: Exception during rapidxml::xml_document<>.parse<>()." << endl; + return false; + } + + rapidxml::xml_node<> *params_node = plugin_doc.first_node("params"); + if (params_node == 0) { + cout << "Missing params tag." << endl; + return false; + } + + for (rapidxml::xml_node<> *node = params_node->first_node(); node != 0; node = node->next_sibling()){ + plugin_spec_attrs.insert({node->name(), node->value()}); + } + } + + // Loop through each node's attributes: for (rapidxml::xml_attribute<> *attr = node->first_attribute(); attr; attr = attr->next_attribute()) { @@ -275,10 +361,35 @@ bool MissionParse::parse(const std::string &filename) { attributes_[nm2][attr->name()] = attr->value(); attributes_[nm3][attr->name()] = attr->value(); attributes_[nm4][attr->name()] = attr->value(); + + // If the plugin xml attribute already exists in the mission xml, + // remove it from the map + if (plugin_spec_attrs[attr->name()]!=""){ + plugin_spec_attrs.erase(attr->name()); + } } + + // Natalie - loops through the map of plugin specific params and allocate them to the doc } + + // Add plugin specific xml attributes and values to the mission xml + for(std::map::iterator mapitr=plugin_spec_attrs.begin(); mapitr!=plugin_spec_attrs.end(); ++mapitr){ + char *attribute_name = doc.allocate_string(mapitr->first.c_str()); + char *attribute_value = doc.allocate_string(mapitr->second.c_str()); + rapidxml::xml_attribute <> *tempattr = doc.allocate_attribute(attribute_name, attribute_value); + node->append_attribute(tempattr); + } + + // Update the mission plugin file string with the new doc } } + + //I don't think this is needed....because it is done later... check this + // Save doc with new allocated attributes to the mission_file_content_ string to be saved to mission.xml logs + // Note - this only updates entity specific plugins... need to add for non-entity specific plugins next + std::string s; + rapidxml::print(std::back_inserter(s), doc, 0); + mission_file_content_ = s; // Save background color: bool bg_color_result = false; @@ -527,24 +638,22 @@ bool MissionParse::parse(const std::string &filename) { std::string nm = node->name(); - std::map plugin_spec_attrs; - ///////////////////////////////////////////////// - ///////////////////////////////////////////////// - // Plugin type: nm - // Plugin name: node->value() - // Plugin attribute name: attr->name() - // Plugin attribute value: attr->value() - // + // Add plugin specific xml attributes and values to the plugin_spec_attrs map. + // These plugin specific values will be checked against the mission xml file's plugin + // attributes - adding them to the mission xml log file if they are not already included. // Need to open the plugin specific xml file if it is one of the following // plugins: // - Autonomy // - Controller // - Motion model - // - ? + // - Sensor // - // Starting with just the autonomy plugin, let's open that file... - if (nm == "autonomy"){ - cout << "Natalie - in the autonomy file opener" << endl; + // plugins that are not entity specific to check for: + // - entity_interaction + // - metrics + // - network + std::map plugin_spec_attrs; + if (nm == "autonomy" || nm == "controller" || nm == "motion_model" || nm == "sensor"){ std::string plugin_file = node->value() + std::string(".xml"); std::string plugin_filename_ = expand_user(plugin_file); @@ -555,7 +664,11 @@ bool MissionParse::parse(const std::string &filename) { FileSearch file_search; std::string result = ""; - std::string pluginxml_path = "/home/ndavis64/scrimmage/scrimmage/include/scrimmage/plugins/" + nm + "/" + node->value(); + std::string pluginxml_path = "/home/ndavis64/scrimmage/scrimmage/include/scrimmage/plugins/" + nm + "/" + node->value(); //This will need to be updated to the environment variable path + if(nm=="motion_model"){ + std::string temp = node->value(); + pluginxml_path = "/home/ndavis64/scrimmage/scrimmage/include/scrimmage/plugins/motion/" + temp; //This will need to be updated to the environment variable path + } bool status = file_search.find_file(plugin_filename_, "xml", pluginxml_path, @@ -578,7 +691,7 @@ bool MissionParse::parse(const std::string &filename) { std::stringstream buffer; buffer << file.rdbuf(); file.close(); - plugin_file_content_ = buffer.str(); + std::string plugin_file_content_ = buffer.str(); // Search and replace any overrides of the form ${key=value} in the mission // file @@ -615,15 +728,10 @@ bool MissionParse::parse(const std::string &filename) { return false; } - for (rapidxml::xml_node<> *node = params_node->first_node(); node != 0; - node = node->next_sibling()){ + for (rapidxml::xml_node<> *node = params_node->first_node(); node != 0; node = node->next_sibling()){ plugin_spec_attrs.insert({node->name(), node->value()}); - cout << "Natalie node name: " << typeid(node->name()).name() << " and the value: " << node->value() << endl; } - } - ///////////////////////////////////////////////// - ///////////////////////////////////////////////// if (nm == "autonomy") { nm += std::to_string(autonomy_order++); @@ -643,33 +751,33 @@ bool MissionParse::parse(const std::string &filename) { attr; attr = attr->next_attribute()) { const std::string attr_name = attr->name(); - if (attr_name == "param_common") { for (auto &kv : param_common[attr->value()]) { entity_attributes_[ent_desc_id][nm][kv.first] = kv.second; } } else { + // If the plugin xml attribute already exists in the mission xml, + // remove it from the map if (plugin_spec_attrs[attr->name()]!=""){ - cout << "The key is specified in the mission xml file, so delete it from the map." << endl; plugin_spec_attrs.erase(attr->name()); } entity_attributes_[ent_desc_id][nm][attr_name] = attr->value(); } } + // Add plugin specific xml attributes and values to the mission xml for(std::map::iterator mapitr=plugin_spec_attrs.begin(); mapitr!=plugin_spec_attrs.end(); ++mapitr){ - cout << "Natalie adding new lines to plugin: 1. " << mapitr->first << " 2. " << mapitr->second << endl; - //rapidxml::xml_attribute <> *tempattr = doc.allocate_attribute((mapitr->first).c_str(), (mapitr->second).c_str()); char *attribute_name = doc.allocate_string(mapitr->first.c_str()); char *attribute_value = doc.allocate_string(mapitr->second.c_str()); rapidxml::xml_attribute <> *tempattr = doc.allocate_attribute(attribute_name, attribute_value); node->append_attribute(tempattr); } - // Natalie - this works for printing the document + // Save doc with new allocated attributes to the mission_file_content_ string to be saved to mission.xml logs + // Note - this only updates entity specific plugins... need to add for non-entity specific plugins next std::string s; rapidxml::print(std::back_inserter(s), doc, 0); - cout << s << endl; + mission_file_content_ = s; } // For each entity, if the lat/lon are defined, use these values to From 21c7b98f55cec74664e2256cff10301d5643f255 Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Tue, 11 Apr 2023 11:37:51 -0400 Subject: [PATCH 04/46] Created function to parse the plugin specific xml file. This is working code. --- include/scrimmage/parse/MissionParse.h | 2 + src/parse/MissionParse.cpp | 259 +++++++++---------------- 2 files changed, 91 insertions(+), 170 deletions(-) diff --git a/include/scrimmage/parse/MissionParse.h b/include/scrimmage/parse/MissionParse.h index 48f9d2528..6085d1140 100644 --- a/include/scrimmage/parse/MissionParse.h +++ b/include/scrimmage/parse/MissionParse.h @@ -86,6 +86,7 @@ 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); bool write(const std::string &filename); double t0(); @@ -163,6 +164,7 @@ class MissionParse { protected: std::string mission_filename_ = ""; std::string mission_file_content_ = ""; + std::map plugin_spec_attrs; rapidxml::xml_document<> doc; double t0_ = 0; diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index 5cfc8db86..19433a112 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -237,10 +237,10 @@ bool MissionParse::parse(const std::string &filename) { for (rapidxml::xml_node<> *node = runscript_node->first_node(); node != 0; node = node->next_sibling()) { + // Clear the map so other plugins do not utilize the values + plugin_spec_attrs.clear(); std::string nm = node->name(); - if (nm != "entity" && nm != "base" && nm != "entity_common" && nm != "param_common") { - cout << "Node name: " << nm << endl; - + if (nm != "entity" && nm != "base" && nm != "entity_common" && nm != "param_common") { params_[nm] = node->value(); rapidxml::xml_attribute<> *attr = node->first_attribute("name"); @@ -254,8 +254,6 @@ bool MissionParse::parse(const std::string &filename) { attributes_[nm3]["ORIGINAL_PLUGIN_NAME"] = node->value(); attributes_[nm4]["ORIGINAL_PLUGIN_NAME"] = node->value(); - // Ideally - need to make the following conditional a separate function since it is reused code - // Add plugin specific xml attributes and values to the plugin_spec_attrs map. // These plugin specific values will be checked against the mission xml file's plugin // attributes - adding them to the mission xml log file if they are not already included. @@ -264,86 +262,9 @@ bool MissionParse::parse(const std::string &filename) { // - entity_interaction // - metrics // - network - std::map plugin_spec_attrs; if (nm == "metrics" || nm == "entity_interaction" || nm == "network"){ - std::string plugin_file = node->value() + std::string(".xml"); - std::string plugin_filename_ = expand_user(plugin_file); - - // First, explicitly search for the mission file. - if (!fs::exists(plugin_filename_)) { - // If the file doesn't exist, search for the mission file under the - // SCRIMMAGE_MISSION_PATH. - FileSearch file_search; - std::string result = ""; - - std::string pluginxml_path = "/home/ndavis64/scrimmage/scrimmage/include/scrimmage/plugins/" + nm + "/" + node->value(); //This will need to be updated to the environment variable path - if(nm=="entity_interaction"){ - std::string temp = node->value(); - pluginxml_path = "/home/ndavis64/scrimmage/scrimmage/include/scrimmage/plugins/interaction/" + temp; //This will need to be updated to the environment variable path - } - - bool status = file_search.find_file(plugin_filename_, "xml", - pluginxml_path, - result, false); - if (!status) { - // The mission file wasn't found. Exit. - cout << "SCRIMMAGE mission file not found: " << plugin_filename_ << endl; - return false; - } - // The mission file was found, save its path. - plugin_filename_ = result; - } - - std::ifstream file(plugin_filename_.c_str()); - if (!file.is_open()) { - std::cout << "Failed to open mission file: " << plugin_filename_ << endl; - return false; - } - - std::stringstream buffer; - buffer << file.rdbuf(); - file.close(); - std::string plugin_file_content_ = buffer.str(); - - // Search and replace any overrides of the form ${key=value} in the mission - // file - for (auto &kv : overrides_map_) { - std::regex reg("\\$\\{" + kv.first + "=(.+?)\\}"); - plugin_file_content_ = std::regex_replace(plugin_file_content_, reg, - kv.second); - } - - // Replace our xml variables of the form ${var=default} with the default - // value - std::string fmt{"$1"}; - std::regex reg("\\$\\{.+?=(.+?)\\}"); - plugin_file_content_ = std::regex_replace(plugin_file_content_, reg, fmt); - - // Parse the xml tree. - rapidxml::xml_document<> plugin_doc; - // doc.parse requires a null terminated string that it can modify. - std::vector plugin_file_content_vec(plugin_file_content_.size() + 1); // allocation done here - plugin_file_content_vec.assign(plugin_file_content_.begin(), plugin_file_content_.end()); // copy - plugin_file_content_vec.push_back('\0'); // shouldn't reallocate - try { - // Note: This parse function can hard fail (seg fault, no exception) on - // badly formatted xml data. Sometimes it'll except, sometimes not. - plugin_doc.parse<0>(plugin_file_content_vec.data()); - } catch (...) { - cout << "scrimmage::MissionParse::parse: Exception during rapidxml::xml_document<>.parse<>()." << endl; - return false; - } - - rapidxml::xml_node<> *params_node = plugin_doc.first_node("params"); - if (params_node == 0) { - cout << "Missing params tag." << endl; - return false; - } - - for (rapidxml::xml_node<> *node = params_node->first_node(); node != 0; node = node->next_sibling()){ - plugin_spec_attrs.insert({node->name(), node->value()}); - } - } + get_plugin_params(nm, node->value()); + } // Loop through each node's attributes: @@ -379,8 +300,6 @@ bool MissionParse::parse(const std::string &filename) { rapidxml::xml_attribute <> *tempattr = doc.allocate_attribute(attribute_name, attribute_value); node->append_attribute(tempattr); } - - // Update the mission plugin file string with the new doc } } @@ -636,6 +555,8 @@ bool MissionParse::parse(const std::string &filename) { for (rapidxml::xml_node<> *node = script_node->first_node(); node != 0; node = node->next_sibling()) { + // Clear the map so other plugins do not utilize the values + plugin_spec_attrs.clear(); std::string nm = node->name(); // Add plugin specific xml attributes and values to the plugin_spec_attrs map. @@ -647,91 +568,9 @@ bool MissionParse::parse(const std::string &filename) { // - Controller // - Motion model // - Sensor - // - // plugins that are not entity specific to check for: - // - entity_interaction - // - metrics - // - network - std::map plugin_spec_attrs; if (nm == "autonomy" || nm == "controller" || nm == "motion_model" || nm == "sensor"){ - std::string plugin_file = node->value() + std::string(".xml"); - std::string plugin_filename_ = expand_user(plugin_file); - - // First, explicitly search for the mission file. - if (!fs::exists(plugin_filename_)) { - // If the file doesn't exist, search for the mission file under the - // SCRIMMAGE_MISSION_PATH. - FileSearch file_search; - std::string result = ""; - - std::string pluginxml_path = "/home/ndavis64/scrimmage/scrimmage/include/scrimmage/plugins/" + nm + "/" + node->value(); //This will need to be updated to the environment variable path - if(nm=="motion_model"){ - std::string temp = node->value(); - pluginxml_path = "/home/ndavis64/scrimmage/scrimmage/include/scrimmage/plugins/motion/" + temp; //This will need to be updated to the environment variable path - } - - bool status = file_search.find_file(plugin_filename_, "xml", - pluginxml_path, - result, false); - if (!status) { - // The mission file wasn't found. Exit. - cout << "SCRIMMAGE mission file not found: " << plugin_filename_ << endl; - return false; - } - // The mission file was found, save its path. - plugin_filename_ = result; - } - - std::ifstream file(plugin_filename_.c_str()); - if (!file.is_open()) { - std::cout << "Failed to open mission file: " << plugin_filename_ << endl; - return false; - } - - std::stringstream buffer; - buffer << file.rdbuf(); - file.close(); - std::string plugin_file_content_ = buffer.str(); - - // Search and replace any overrides of the form ${key=value} in the mission - // file - for (auto &kv : overrides_map_) { - std::regex reg("\\$\\{" + kv.first + "=(.+?)\\}"); - plugin_file_content_ = std::regex_replace(plugin_file_content_, reg, - kv.second); - } - - // Replace our xml variables of the form ${var=default} with the default - // value - std::string fmt{"$1"}; - std::regex reg("\\$\\{.+?=(.+?)\\}"); - plugin_file_content_ = std::regex_replace(plugin_file_content_, reg, fmt); - - // Parse the xml tree. - rapidxml::xml_document<> plugin_doc; - // doc.parse requires a null terminated string that it can modify. - std::vector plugin_file_content_vec(plugin_file_content_.size() + 1); // allocation done here - plugin_file_content_vec.assign(plugin_file_content_.begin(), plugin_file_content_.end()); // copy - plugin_file_content_vec.push_back('\0'); // shouldn't reallocate - try { - // Note: This parse function can hard fail (seg fault, no exception) on - // badly formatted xml data. Sometimes it'll except, sometimes not. - plugin_doc.parse<0>(plugin_file_content_vec.data()); - } catch (...) { - cout << "scrimmage::MissionParse::parse: Exception during rapidxml::xml_document<>.parse<>()." << endl; - return false; - } - - rapidxml::xml_node<> *params_node = plugin_doc.first_node("params"); - if (params_node == 0) { - cout << "Missing params tag." << endl; - return false; - } - - for (rapidxml::xml_node<> *node = params_node->first_node(); node != 0; node = node->next_sibling()){ - plugin_spec_attrs.insert({node->name(), node->value()}); - } - } + get_plugin_params(nm, node->value()); + } if (nm == "autonomy") { nm += std::to_string(autonomy_order++); @@ -931,6 +770,86 @@ bool MissionParse::parse(const std::string &filename) { return true; } +void MissionParse::get_plugin_params(std::string node_name, std::string node_value) { + std::string plugin_file = node_value + std::string(".xml"); + std::string plugin_filename_ = expand_user(plugin_file); + + // First, explicitly search for the mission file. + if (!fs::exists(plugin_filename_)) { + // If the file doesn't exist, search for the mission file under the + // SCRIMMAGE_MISSION_PATH. + FileSearch file_search; + std::string result = ""; + std::string pluginxml_path = "/home/ndavis64/scrimmage/scrimmage/include/scrimmage/plugins/" + node_name + "/" + node_value; //This will need to be updated to the environment variable path + + if(node_name=="entity_interaction"){ + std::string temp = node_value; + pluginxml_path = "/home/ndavis64/scrimmage/scrimmage/include/scrimmage/plugins/interaction/" + temp; //This will need to be updated to the environment variable path + } else if (node_name=="motion_model"){ + std::string temp = node_value; + pluginxml_path = "/home/ndavis64/scrimmage/scrimmage/include/scrimmage/plugins/motion/" + temp; //This will need to be updated to the environment variable path + } + + bool status = file_search.find_file(plugin_filename_, "xml", + pluginxml_path, + result, false); + if (!status) { + // The mission file wasn't found. Exit. + cout << "SCRIMMAGE mission file not found: " << plugin_filename_ << endl; + //return false; //Need to somehow return bad result to parse if this doesnt work...maybe just an empty list? + } + // The mission file was found, save its path. + plugin_filename_ = result; + } + + std::ifstream file(plugin_filename_.c_str()); + if (!file.is_open()) { + std::cout << "Failed to open mission file: " << plugin_filename_ << endl; + //return false; //Need to somehow return bad result to parse if this doesnt work...maybe just an empty list? + } + + std::stringstream buffer; + buffer << file.rdbuf(); + file.close(); + std::string plugin_file_content_ = buffer.str(); + + // Search and replace any overrides of the form ${key=value} in the mission file + for (auto &kv : overrides_map_) { + std::regex reg("\\$\\{" + kv.first + "=(.+?)\\}"); + plugin_file_content_ = std::regex_replace(plugin_file_content_, reg, kv.second); + } + + // Replace our xml variables of the form ${var=default} with the default value + std::string fmt{"$1"}; + std::regex reg("\\$\\{.+?=(.+?)\\}"); + plugin_file_content_ = std::regex_replace(plugin_file_content_, reg, fmt); + + // Parse the xml tree. + rapidxml::xml_document<> plugin_doc; + // doc.parse requires a null terminated string that it can modify. + std::vector plugin_file_content_vec(plugin_file_content_.size() + 1); // allocation done here + plugin_file_content_vec.assign(plugin_file_content_.begin(), plugin_file_content_.end()); // copy + plugin_file_content_vec.push_back('\0'); // shouldn't reallocate + try { + // Note: This parse function can hard fail (seg fault, no exception) on + // badly formatted xml data. Sometimes it'll except, sometimes not. + plugin_doc.parse<0>(plugin_file_content_vec.data()); + } catch (...) { + cout << "scrimmage::MissionParse::parse: Exception during rapidxml::xml_document<>.parse<>()." << endl; + //return false; //Need to somehow return bad result to parse if this doesnt work...maybe just an empty list? + } + + rapidxml::xml_node<> *params_node = plugin_doc.first_node("params"); + if (params_node == 0) { + cout << "Missing params tag." << endl; + //return false; //Need to somehow return bad result to parse if this doesnt work + } + + for (rapidxml::xml_node<> *node = params_node->first_node(); node != 0; node = node->next_sibling()){ + plugin_spec_attrs.insert({node->name(), node->value()}); + } +} + bool MissionParse::create_log_dir() { // Create the root_log_dir_ if it doesn't exist: if (not fs::exists(fs::path(root_log_dir_)) && From c95c68a524f003e4131f13ff4bf5bee4d77a78e6 Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Tue, 11 Apr 2023 11:46:39 -0400 Subject: [PATCH 05/46] Code cleanup --- src/parse/MissionParse.cpp | 29 ++++++++++------------------- 1 file changed, 10 insertions(+), 19 deletions(-) diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index 19433a112..32f5ecffe 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -240,7 +240,7 @@ bool MissionParse::parse(const std::string &filename) { // Clear the map so other plugins do not utilize the values plugin_spec_attrs.clear(); std::string nm = node->name(); - if (nm != "entity" && nm != "base" && nm != "entity_common" && nm != "param_common") { + if (nm != "entity" && nm != "base" && nm != "entity_common" && nm != "param_common") { params_[nm] = node->value(); rapidxml::xml_attribute<> *attr = node->first_attribute("name"); @@ -283,14 +283,11 @@ bool MissionParse::parse(const std::string &filename) { attributes_[nm3][attr->name()] = attr->value(); attributes_[nm4][attr->name()] = attr->value(); - // If the plugin xml attribute already exists in the mission xml, - // remove it from the map + // If the plugin xml attribute already exists in the mission xml, remove it from the map if (plugin_spec_attrs[attr->name()]!=""){ plugin_spec_attrs.erase(attr->name()); } } - - // Natalie - loops through the map of plugin specific params and allocate them to the doc } // Add plugin specific xml attributes and values to the mission xml @@ -302,13 +299,6 @@ bool MissionParse::parse(const std::string &filename) { } } } - - //I don't think this is needed....because it is done later... check this - // Save doc with new allocated attributes to the mission_file_content_ string to be saved to mission.xml logs - // Note - this only updates entity specific plugins... need to add for non-entity specific plugins next - std::string s; - rapidxml::print(std::back_inserter(s), doc, 0); - mission_file_content_ = s; // Save background color: bool bg_color_result = false; @@ -611,14 +601,13 @@ bool MissionParse::parse(const std::string &filename) { rapidxml::xml_attribute <> *tempattr = doc.allocate_attribute(attribute_name, attribute_value); node->append_attribute(tempattr); } - - // Save doc with new allocated attributes to the mission_file_content_ string to be saved to mission.xml logs - // Note - this only updates entity specific plugins... need to add for non-entity specific plugins next - std::string s; - rapidxml::print(std::back_inserter(s), doc, 0); - mission_file_content_ = s; } + // Save doc with new allocated attributes to the mission_file_content_ string to be saved to mission.xml logs + std::string s; + rapidxml::print(std::back_inserter(s), doc, 0); + mission_file_content_ = s; + // For each entity, if the lat/lon are defined, use these values to // overwrite the "x" and "y" values // Search for lon lat alt. @@ -653,7 +642,9 @@ bool MissionParse::parse(const std::string &filename) { if (script_info.count("x") > 0) { script_info["x0"] = script_info["x"]; } else { - cout << "Entity missing 'x' tag." << endl; + cout << "Entity missing 'x' tag." << endl; // std::string s; + // rapidxml::print(std::back_inserter(s), doc, 0); + // mission_file_content_ = s; } if (script_info.count("y") > 0) { From 45508c53e0fc3926951624c50a20f401308d7a9d Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Tue, 11 Apr 2023 12:06:46 -0400 Subject: [PATCH 06/46] Removed mission file change --- missions/straight.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/missions/straight.xml b/missions/straight.xml index 95043d85e..9c368c098 100644 --- a/missions/straight.xml +++ b/missions/straight.xml @@ -5,7 +5,7 @@ Date: Tue, 11 Apr 2023 13:01:01 -0400 Subject: [PATCH 07/46] minor error message changes, added extra digit to time UI --- src/entity/Entity.cpp | 2 +- src/parse/ConfigParse.cpp | 14 +++++++++++++- src/parse/MissionParse.cpp | 16 ++++++++++++++-- src/viewer/Updater.cpp | 2 +- 4 files changed, 29 insertions(+), 5 deletions(-) diff --git a/src/entity/Entity.cpp b/src/entity/Entity.cpp index 1ae63f00c..92a8e05ea 100644 --- a/src/entity/Entity.cpp +++ b/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; } } diff --git a/src/parse/ConfigParse.cpp b/src/parse/ConfigParse.cpp index cfa839ddf..0ee3f21cf 100644 --- a/src/parse/ConfigParse.cpp +++ b/src/parse/ConfigParse.cpp @@ -126,7 +126,19 @@ bool ConfigParse::parse(const std::map &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) { diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index 159aee452..1b78eda25 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -135,11 +135,23 @@ bool MissionParse::parse(const std::string &filename) { 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()); - } catch (...) { + // doc.parse<0>(mission_file_content_vec.data()); + doc.parse(mission_file_content_vec.data()); + } + catch (const rapidxml::parse_error& e) + { + std::cout << e.what() << std::endl; cout << "scrimmage::MissionParse::parse: Exception during rapidxml::xml_document<>.parse<>()." << endl; return false; } + catch (const std::exception& e) + { + std::cerr << "Error was: " << e.what() << std::endl; + } + catch (...) + { + std::cerr << "An unknown error occurred." << std::endl; + } rapidxml::xml_node<> *runscript_node = doc.first_node("runscript"); if (runscript_node == 0) { diff --git a/src/viewer/Updater.cpp b/src/viewer/Updater.cpp index 72bbfa2b2..31c62b72c 100644 --- a/src/viewer/Updater.cpp +++ b/src/viewer/Updater.cpp @@ -685,7 +685,7 @@ bool Updater::update_text_display() { } // Update the time (text) display - const int num_digits = std::abs(log10(dt_)); + const int num_digits = std::ceil(std::abs(log10(dt_))); std::stringstream ss; ss << std::setprecision(num_digits) << std::fixed << frame_time_ << " s"; time_actor_->SetInput(ss.str().c_str()); From cc27377ce5a2e5bfd4ff08db6fad2fe8dd3e0756 Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Tue, 11 Apr 2023 14:21:41 -0400 Subject: [PATCH 08/46] Added returns in case of error with finding params files --- src/parse/MissionParse.cpp | 32 ++++++++++---------------------- 1 file changed, 10 insertions(+), 22 deletions(-) diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index 2a7174358..3db46af4c 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -135,23 +135,11 @@ bool MissionParse::parse(const std::string &filename) { 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(mission_file_content_vec.data()); - } - catch (const rapidxml::parse_error& e) - { - std::cout << e.what() << std::endl; + doc.parse<0>(mission_file_content_vec.data()); + } catch (...) { cout << "scrimmage::MissionParse::parse: Exception during rapidxml::xml_document<>.parse<>()." << endl; return false; } - catch (const std::exception& e) - { - std::cerr << "Error was: " << e.what() << std::endl; - } - catch (...) - { - std::cerr << "An unknown error occurred." << std::endl; - } rapidxml::xml_node<> *runscript_node = doc.first_node("runscript"); if (runscript_node == 0) { @@ -618,11 +606,13 @@ bool MissionParse::parse(const std::string &filename) { // Save doc with new allocated attributes to the mission_file_content_ string to be saved to mission.xml logs std::string s; rapidxml::print(std::back_inserter(s), doc, 0); + // Note - save this to a different string and create a combined specific mission xml log mission_file_content_ = s; // For each entity, if the lat/lon are defined, use these values to // overwrite the "x" and "y" values - // Search for lon lat alt. + // Search for lon lat alt.2. search could be just the name of the plugin (e.g., QuadTest_plugin) + // a. Is the file located in the plugin path environment variab Eigen::Vector3d pos_LLA(-1, -1, -1); Eigen::Vector3d pos_xyz(-1, -1, -1); bool lon_valid = false, lat_valid = false, alt_valid = false; @@ -654,9 +644,7 @@ bool MissionParse::parse(const std::string &filename) { if (script_info.count("x") > 0) { script_info["x0"] = script_info["x"]; } else { - cout << "Entity missing 'x' tag." << endl; // std::string s; - // rapidxml::print(std::back_inserter(s), doc, 0); - // mission_file_content_ = s; + cout << "Entity missing 'x' tag." << endl; } if (script_info.count("y") > 0) { @@ -799,7 +787,7 @@ void MissionParse::get_plugin_params(std::string node_name, std::string node_val if (!status) { // The mission file wasn't found. Exit. cout << "SCRIMMAGE mission file not found: " << plugin_filename_ << endl; - //return false; //Need to somehow return bad result to parse if this doesnt work...maybe just an empty list? + return; } // The mission file was found, save its path. plugin_filename_ = result; @@ -808,7 +796,7 @@ void MissionParse::get_plugin_params(std::string node_name, std::string node_val std::ifstream file(plugin_filename_.c_str()); if (!file.is_open()) { std::cout << "Failed to open mission file: " << plugin_filename_ << endl; - //return false; //Need to somehow return bad result to parse if this doesnt work...maybe just an empty list? + return; } std::stringstream buffer; @@ -839,13 +827,13 @@ void MissionParse::get_plugin_params(std::string node_name, std::string node_val plugin_doc.parse<0>(plugin_file_content_vec.data()); } catch (...) { cout << "scrimmage::MissionParse::parse: Exception during rapidxml::xml_document<>.parse<>()." << endl; - //return false; //Need to somehow return bad result to parse if this doesnt work...maybe just an empty list? + return false; } rapidxml::xml_node<> *params_node = plugin_doc.first_node("params"); if (params_node == 0) { cout << "Missing params tag." << endl; - //return false; //Need to somehow return bad result to parse if this doesnt work + return; } for (rapidxml::xml_node<> *node = params_node->first_node(); node != 0; node = node->next_sibling()){ From afd20a8c56796b5443c4546b68f8ece7993bcac3 Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Tue, 18 Apr 2023 12:44:51 -0400 Subject: [PATCH 09/46] Working with scrimmage_plugin_path environment variable --- include/scrimmage/parse/MissionParse.h | 1 + src/parse/MissionParse.cpp | 29 +++++++++++++++++++++----- 2 files changed, 25 insertions(+), 5 deletions(-) diff --git a/include/scrimmage/parse/MissionParse.h b/include/scrimmage/parse/MissionParse.h index 6085d1140..1e9ac328b 100644 --- a/include/scrimmage/parse/MissionParse.h +++ b/include/scrimmage/parse/MissionParse.h @@ -164,6 +164,7 @@ class MissionParse { protected: std::string mission_filename_ = ""; std::string mission_file_content_ = ""; + std::string scrimmage_plugin_path = ""; std::map plugin_spec_attrs; rapidxml::xml_document<> doc; diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index 3db46af4c..696bb9133 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -84,6 +84,25 @@ void MissionParse::set_overrides(const std::string &overrides) { bool MissionParse::parse(const std::string &filename) { mission_filename_ = expand_user(filename); + // Extracts the appropriate Scrimmage plugin path for plugin specific + // xml files. This path is utilized for formatting the mission.plugin.xml file + std::string plugin_env_string = getenv("SCRIMMAGE_PLUGIN_PATH"); + std::vector plugin_substrings; + std::stringstream plugin_ss(plugin_env_string); + std::string plugin_path; + + while(std::getline(plugin_ss, plugin_path, ':')){ + if(!plugin_path.empty()){ + plugin_substrings.push_back(plugin_path); + } + } + + for(const auto& plugin_string : plugin_substrings){ + if (plugin_string.find("/include/scrimmage/plugins") != std::string::npos){ + scrimmage_plugin_path = plugin_string; + } + } + // First, explicitly search for the mission file. if (!fs::exists(mission_filename_)) { // If the file doesn't exist, search for the mission file under the @@ -771,14 +790,14 @@ void MissionParse::get_plugin_params(std::string node_name, std::string node_val // SCRIMMAGE_MISSION_PATH. FileSearch file_search; std::string result = ""; - std::string pluginxml_path = "/home/ndavis64/scrimmage/scrimmage/include/scrimmage/plugins/" + node_name + "/" + node_value; //This will need to be updated to the environment variable path - + std::string pluginxml_path = scrimmage_plugin_path + "/" + node_name + "/" + node_value; //This will need to be updated to the environment variable path + if(node_name=="entity_interaction"){ std::string temp = node_value; - pluginxml_path = "/home/ndavis64/scrimmage/scrimmage/include/scrimmage/plugins/interaction/" + temp; //This will need to be updated to the environment variable path + pluginxml_path = scrimmage_plugin_path + "/" + "interaction/" + temp; //This will need to be updated to the environment variable path } else if (node_name=="motion_model"){ std::string temp = node_value; - pluginxml_path = "/home/ndavis64/scrimmage/scrimmage/include/scrimmage/plugins/motion/" + temp; //This will need to be updated to the environment variable path + pluginxml_path = scrimmage_plugin_path + "/" + "motion/" + temp; //This will need to be updated to the environment variable path } bool status = file_search.find_file(plugin_filename_, "xml", @@ -827,7 +846,7 @@ void MissionParse::get_plugin_params(std::string node_name, std::string node_val plugin_doc.parse<0>(plugin_file_content_vec.data()); } catch (...) { cout << "scrimmage::MissionParse::parse: Exception during rapidxml::xml_document<>.parse<>()." << endl; - return false; + return; } rapidxml::xml_node<> *params_node = plugin_doc.first_node("params"); From ad6b58fd3d18f26202502cfc81231173255e17b7 Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Tue, 18 Apr 2023 13:43:38 -0400 Subject: [PATCH 10/46] Mission.plugin.xml log file added --- include/scrimmage/parse/MissionParse.h | 1 + src/parse/MissionParse.cpp | 16 +++++++++------- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/include/scrimmage/parse/MissionParse.h b/include/scrimmage/parse/MissionParse.h index 1e9ac328b..7854a30cf 100644 --- a/include/scrimmage/parse/MissionParse.h +++ b/include/scrimmage/parse/MissionParse.h @@ -164,6 +164,7 @@ class MissionParse { protected: std::string mission_filename_ = ""; std::string mission_file_content_ = ""; + std::string mission_plugin_file_content = ""; std::string scrimmage_plugin_path = ""; std::map plugin_spec_attrs; rapidxml::xml_document<> doc; diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index 696bb9133..1aa26e2e7 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -622,16 +622,14 @@ bool MissionParse::parse(const std::string &filename) { } } - // Save doc with new allocated attributes to the mission_file_content_ string to be saved to mission.xml logs - std::string s; - rapidxml::print(std::back_inserter(s), doc, 0); - // Note - save this to a different string and create a combined specific mission xml log - mission_file_content_ = s; + // Save doc with new allocated attributes to the mission_plugin_file_content string to be saved to mission.plugin.xml logs + std::string rapidxml_plugin_doc; + rapidxml::print(std::back_inserter(rapidxml_plugin_doc), doc, 0); + mission_plugin_file_content = rapidxml_plugin_doc; // For each entity, if the lat/lon are defined, use these values to // overwrite the "x" and "y" values - // Search for lon lat alt.2. search could be just the name of the plugin (e.g., QuadTest_plugin) - // a. Is the file located in the plugin path environment variab + // Search for lon lat alt.2. Eigen::Vector3d pos_LLA(-1, -1, -1); Eigen::Vector3d pos_xyz(-1, -1, -1); bool lon_valid = false, lat_valid = false, alt_valid = false; @@ -904,6 +902,10 @@ bool MissionParse::create_log_dir() { content_out << mission_file_content_; content_out.close(); + std::ofstream mission_plugin_content_out(log_dir_+"/mission.plugin.xml"); + mission_plugin_content_out << mission_plugin_file_content; + mission_plugin_content_out.close(); + // Create the latest log directory by default. Don't create the latest // directory if the tag is defined in the mission file and it is set to // false. From a9fb57dcf783285c093c9d76d84e23f3b3ea7a92 Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Tue, 18 Apr 2023 14:07:24 -0400 Subject: [PATCH 11/46] Code cleanup --- src/parse/MissionParse.cpp | 52 ++++++++++++++++++++------------------ 1 file changed, 27 insertions(+), 25 deletions(-) diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index 1aa26e2e7..bd69a0d85 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -84,25 +84,6 @@ void MissionParse::set_overrides(const std::string &overrides) { bool MissionParse::parse(const std::string &filename) { mission_filename_ = expand_user(filename); - // Extracts the appropriate Scrimmage plugin path for plugin specific - // xml files. This path is utilized for formatting the mission.plugin.xml file - std::string plugin_env_string = getenv("SCRIMMAGE_PLUGIN_PATH"); - std::vector plugin_substrings; - std::stringstream plugin_ss(plugin_env_string); - std::string plugin_path; - - while(std::getline(plugin_ss, plugin_path, ':')){ - if(!plugin_path.empty()){ - plugin_substrings.push_back(plugin_path); - } - } - - for(const auto& plugin_string : plugin_substrings){ - if (plugin_string.find("/include/scrimmage/plugins") != std::string::npos){ - scrimmage_plugin_path = plugin_string; - } - } - // First, explicitly search for the mission file. if (!fs::exists(mission_filename_)) { // If the file doesn't exist, search for the mission file under the @@ -250,6 +231,28 @@ bool MissionParse::parse(const std::string &filename) { } } + // Extracts the appropriate Scrimmage plugin path for plugin specific + // xml files. This path is utilized for formatting the mission.plugin.xml file + std::string plugin_env_string = getenv("SCRIMMAGE_PLUGIN_PATH"); + std::vector plugin_substrings; + std::stringstream plugin_ss(plugin_env_string); + std::string plugin_path; + + // Create substrings of the plugin_env_string, using the ':' character as + // a delimiter + while(std::getline(plugin_ss, plugin_path, ':')){ + if(!plugin_path.empty()){ + plugin_substrings.push_back(plugin_path); + } + } + + // Update the scrimmage_plugin_path string with the path that can access plugin + // specific xml files + for(const auto& plugin_string : plugin_substrings){ + if (plugin_string.find("/include/scrimmage/plugins") != std::string::npos){ + scrimmage_plugin_path = plugin_string; + } + } // Loop through each node under "runscript" that isn't an entity or base attributes_.clear(); @@ -629,7 +632,7 @@ bool MissionParse::parse(const std::string &filename) { // For each entity, if the lat/lon are defined, use these values to // overwrite the "x" and "y" values - // Search for lon lat alt.2. + // Search for lon lat alt. Eigen::Vector3d pos_LLA(-1, -1, -1); Eigen::Vector3d pos_xyz(-1, -1, -1); bool lon_valid = false, lat_valid = false, alt_valid = false; @@ -784,18 +787,16 @@ void MissionParse::get_plugin_params(std::string node_name, std::string node_val // First, explicitly search for the mission file. if (!fs::exists(plugin_filename_)) { - // If the file doesn't exist, search for the mission file under the - // SCRIMMAGE_MISSION_PATH. FileSearch file_search; std::string result = ""; - std::string pluginxml_path = scrimmage_plugin_path + "/" + node_name + "/" + node_value; //This will need to be updated to the environment variable path + std::string pluginxml_path = scrimmage_plugin_path + "/" + node_name + "/" + node_value; if(node_name=="entity_interaction"){ std::string temp = node_value; - pluginxml_path = scrimmage_plugin_path + "/" + "interaction/" + temp; //This will need to be updated to the environment variable path + pluginxml_path = scrimmage_plugin_path + "/" + "interaction/" + temp; } else if (node_name=="motion_model"){ std::string temp = node_value; - pluginxml_path = scrimmage_plugin_path + "/" + "motion/" + temp; //This will need to be updated to the environment variable path + pluginxml_path = scrimmage_plugin_path + "/" + "motion/" + temp; } bool status = file_search.find_file(plugin_filename_, "xml", @@ -853,6 +854,7 @@ void MissionParse::get_plugin_params(std::string node_name, std::string node_val return; } + // Add all plugin specific xml attributes to the map for (rapidxml::xml_node<> *node = params_node->first_node(); node != 0; node = node->next_sibling()){ plugin_spec_attrs.insert({node->name(), node->value()}); } From 2d04f5ae88ff8a564e2db43bfb19603765220bf2 Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Wed, 24 May 2023 14:36:53 -0400 Subject: [PATCH 12/46] Initial commit of mission to mission xml pull request. Currently, able to copy entire entity nodes to the xml file --- include/scrimmage/parse/MissionParse.h | 7 ++- src/parse/MissionParse.cpp | 51 +++++++++++++++++++++ src/simcontrol/SimControl.cpp | 62 ++++++++++++++++++++++++++ 3 files changed, 119 insertions(+), 1 deletion(-) diff --git a/include/scrimmage/parse/MissionParse.h b/include/scrimmage/parse/MissionParse.h index 7854a30cf..93edf57e9 100644 --- a/include/scrimmage/parse/MissionParse.h +++ b/include/scrimmage/parse/MissionParse.h @@ -87,6 +87,10 @@ class MissionParse { 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); + + //Natalie + void final_state_xml(); + bool write(const std::string &filename); double t0(); @@ -164,11 +168,12 @@ class MissionParse { protected: std::string mission_filename_ = ""; std::string mission_file_content_ = ""; + std::string miss2miss_file_content = ""; std::string mission_plugin_file_content = ""; std::string scrimmage_plugin_path = ""; std::map plugin_spec_attrs; rapidxml::xml_document<> doc; - + double t0_ = 0; double tend_ = 50; double dt_ = 0.00833333; diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index bd69a0d85..85b7aa7bd 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -448,6 +448,11 @@ bool MissionParse::parse(const std::string &filename) { entity_common[nm] = script_info; } + // Natalie - for the mission to mission pull request, will want to save the format of the entity blocks + // I think what should be done is call a function that parses through the mission_file_content and will add new + // entity nodes based on the count of each team, duplicating entries that are common and updating entity specific + // values based on the final state of the entity + // Loop through each "entity" node for (rapidxml::xml_node<> *script_node = runscript_node->first_node("entity"); script_node != 0; @@ -781,6 +786,52 @@ bool MissionParse::parse(const std::string &filename) { return true; } +// Natalie test function +void MissionParse::final_state_xml(){ + // Parse the xml tree. + // doc.parse requires a null terminated string that it can modify. + std::vector mission_file_content_vec(mission_file_content_.size() + 1); // allocation done here + mission_file_content_vec.assign(mission_file_content_.begin(), mission_file_content_.end()); // copy + mission_file_content_vec.push_back('\0'); // shouldn't reallocate + 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()); + } catch (...) { + cout << "scrimmage::MissionParse::parse: Exception during rapidxml::xml_document<>.parse<>()." << endl; + return; + } + + int loop_test = 0; + rapidxml::xml_node<> *runscript_node = doc.first_node("runscript"); + if (runscript_node == 0) { + cout << "Missing runscript tag." << endl; + return; + } + // Loop through each "entity" node + for (rapidxml::xml_node<> *script_node = runscript_node->first_node("entity"); + script_node != 0; + script_node = script_node->next_sibling("entity")) { + + // This successfully adds 4 entities of the first entity node to the end of the XML file! + while(loop_test < 4){ + rapidxml::xml_node<> *test_ent = doc.clone_node(script_node); + doc.first_node("runscript")->append_node(test_ent); + loop_test ++; + } + } + + // Save doc with new allocated attributes to the miss2miss_file_content string to be saved to mission.plugin.xml logs + std::string rapidxml_miss2miss_doc; + rapidxml::print(std::back_inserter(rapidxml_miss2miss_doc), doc, 0); + miss2miss_file_content = rapidxml_miss2miss_doc; + + std::ofstream miss2miss_content_out(log_dir_+"/miss2miss.xml"); + miss2miss_content_out << miss2miss_file_content; + miss2miss_content_out.close(); + +} + void MissionParse::get_plugin_params(std::string node_name, std::string node_value) { std::string plugin_file = node_value + std::string(".xml"); std::string plugin_filename_ = expand_user(plugin_file); diff --git a/src/simcontrol/SimControl.cpp b/src/simcontrol/SimControl.cpp index 072eb0669..b95b8526c 100644 --- a/src/simcontrol/SimControl.cpp +++ b/src/simcontrol/SimControl.cpp @@ -1063,6 +1063,68 @@ bool SimControl::shutdown(const bool& shutdown_python) { // Close all plugins for (EntityPtr &ent : ents_) { + + // Natalie - need to store all state information before the GUI shuts down. Grab state information that can be captured in the entity block + // in the mission xml file. Need to then send the information to the mission parse file to create a new xml file when shutting down that + // can then be used by another simulation + // + // Need to pass the team ID and need to determine the number of entities that are part of a given team... might be able to handle this with + // looping + // + // Need to attempt to specify the following for the mission xml: + // Name - can multiple blocks have the same name? + + // team_id + cout << "Team id: " << ent->id().team_id() << endl; + + // color - probably going to need to get this from the mission parse file + // count (should be 1), + int block_count = 1; + cout << "Block count: " << block_count << endl; + + // autonomy, autonomy loop rate, + + + // health + // Note: Entities that have collisions are removed. Health points could still be used for other mission xml output, because they might have more than 1 as a + // starting point, being able to endure multiple collisions + cout << "Health points: " << ent->health_points() << endl; // Need to check if the value is lower than a certain number, the entity should be created or not + + // x, y, z + //double x, y, z = ent->state()->pos(); + cout << "Position values, x: " << ent->state()->pos()[0] << " y: " << ent->state()->pos()[1] << " z: " << ent->state()->pos()[2] << endl; + + // latitude + // longitude + + // altitude + // altitude is the same as the z position + cout << "Altitude: " << ent->state()->pos()[2] << endl; + + // heading (shoud be able to retrieve this from the quaternion) + cout << "Yaw of the quaternion: " << ent->state()->quat().yaw() << endl; + cout << "Roll of the quaternion: " << ent->state()->quat().roll() << endl; + cout << "Pitch of the quaternion: " << ent->state()->quat().pitch() << endl; + + // motion model + // visual model + // controller, controller loop rate, + // sensor, sensor loop rate, + // base - x,y,z, latitude, longitude, altitude, and radius of the base + + // Velocity - no known tag for the entity block, may need to be an entry for controller + //double vx, vy, vz = ent->state()->vel(); + cout << "Velocity values, vx: " << ent->state()->vel()[0] << " vy: " << ent->state()->vel()[1] << " vz: " << ent->state()->vel()[2] << endl; + + // Data that could come from the mission xml file + + + // Do not include: + // generate_rate, start time, count, and time variance, + // variance x y and z + // use variance all ents, + + mp_->final_state_xml(); ent->close(t()); } From 36187fae1df715a5b05ee9d1a71d493d2a1a825c Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Thu, 25 May 2023 11:58:23 -0400 Subject: [PATCH 13/46] Struct is working between mission parse and sim control files. Can retrieve end state data in mission parse from sim control --- include/scrimmage/parse/EntEndStates.h | 12 ++++++++++++ include/scrimmage/parse/MissionParse.h | 5 ++++- include/scrimmage/simcontrol/SimControl.h | 10 ++++++++++ src/parse/MissionParse.cpp | 15 +++++++++++++-- src/simcontrol/SimControl.cpp | 15 +++++++++++++-- 5 files changed, 52 insertions(+), 5 deletions(-) create mode 100644 include/scrimmage/parse/EntEndStates.h diff --git a/include/scrimmage/parse/EntEndStates.h b/include/scrimmage/parse/EntEndStates.h new file mode 100644 index 000000000..262cdb6d9 --- /dev/null +++ b/include/scrimmage/parse/EntEndStates.h @@ -0,0 +1,12 @@ +#ifndef INCLUDE_SCRIMMAGE_PARSE_ENTENDSTATES_H_ +#define INCLUDE_SCRIMMAGE_PARSE_ENTENDSTATES_H_ + + + struct ent_end_state { + int team_id; + double x_pos; + double y_pos; + double z_pos; + }; + +#endif \ No newline at end of file diff --git a/include/scrimmage/parse/MissionParse.h b/include/scrimmage/parse/MissionParse.h index 93edf57e9..3af230d61 100644 --- a/include/scrimmage/parse/MissionParse.h +++ b/include/scrimmage/parse/MissionParse.h @@ -39,6 +39,9 @@ #include #include +#include + +#include #include #include @@ -89,7 +92,7 @@ class MissionParse { void get_plugin_params(std::string node_name, std::string node_value); //Natalie - void final_state_xml(); + void final_state_xml(std::vector & all_end_states); bool write(const std::string &filename); diff --git a/include/scrimmage/simcontrol/SimControl.h b/include/scrimmage/simcontrol/SimControl.h index ff7d5d027..a0d5b33c3 100644 --- a/include/scrimmage/simcontrol/SimControl.h +++ b/include/scrimmage/simcontrol/SimControl.h @@ -320,6 +320,16 @@ class SimControl { void set_running_in_thread(bool running_in_thread); + // Natalie adding struct information + // struct ent_end_state { + // int team_id; + // double x_pos; + // double y_pos; + // double z_pos; + // } end_state; + + //std::vector all_end_states; + protected: // Key: Entity ID // Value: Team ID diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index 85b7aa7bd..0a4c40ecb 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -787,7 +787,12 @@ bool MissionParse::parse(const std::string &filename) { } // Natalie test function -void MissionParse::final_state_xml(){ +void MissionParse::final_state_xml(std::vector & all_end_states){ + + for(auto& a : all_end_states){ + cout << "1. " << a.team_id << " 2. " << a.x_pos << " 3. " << a.y_pos << " 4. " << a.z_pos << endl; + } + // Parse the xml tree. // doc.parse requires a null terminated string that it can modify. std::vector mission_file_content_vec(mission_file_content_.size() + 1); // allocation done here @@ -802,17 +807,23 @@ void MissionParse::final_state_xml(){ return; } - int loop_test = 0; + int loop_test = 0; // remove once passing actual sim control ent information + rapidxml::xml_node<> *runscript_node = doc.first_node("runscript"); if (runscript_node == 0) { cout << "Missing runscript tag." << endl; return; } + // Loop through each "entity" node for (rapidxml::xml_node<> *script_node = runscript_node->first_node("entity"); script_node != 0; script_node = script_node->next_sibling("entity")) { + // Get the team id number of the current node + rapidxml::xml_node<> *team_id_node = script_node->first_node("team_id"); + cout << "Team id for the given node: " << team_id_node->value() << endl; + // This successfully adds 4 entities of the first entity node to the end of the XML file! while(loop_test < 4){ rapidxml::xml_node<> *test_ent = doc.clone_node(script_node); diff --git a/src/simcontrol/SimControl.cpp b/src/simcontrol/SimControl.cpp index b95b8526c..900fa4051 100644 --- a/src/simcontrol/SimControl.cpp +++ b/src/simcontrol/SimControl.cpp @@ -70,6 +70,8 @@ #include +#include + #include #include #include @@ -1060,6 +1062,8 @@ bool SimControl::finalize() { bool SimControl::shutdown(const bool& shutdown_python) { finalize(); + + std::vector all_end_states; // Close all plugins for (EntityPtr &ent : ents_) { @@ -1101,7 +1105,7 @@ bool SimControl::shutdown(const bool& shutdown_python) { // altitude is the same as the z position cout << "Altitude: " << ent->state()->pos()[2] << endl; - // heading (shoud be able to retrieve this from the quaternion) + // heading - same thing as yaw cout << "Yaw of the quaternion: " << ent->state()->quat().yaw() << endl; cout << "Roll of the quaternion: " << ent->state()->quat().roll() << endl; cout << "Pitch of the quaternion: " << ent->state()->quat().pitch() << endl; @@ -1124,10 +1128,17 @@ bool SimControl::shutdown(const bool& shutdown_python) { // variance x y and z // use variance all ents, - mp_->final_state_xml(); + //mp_->final_state_xml(); + + // Struct saving and vector + ent_end_state end_state = {ent->id().team_id(), ent->state()->pos()[0], ent->state()->pos()[1], ent->state()->pos()[2]}; + all_end_states.push_back(end_state); + ent->close(t()); } + mp_->final_state_xml(all_end_states); + for (EntityInteractionPtr ent_inter : ent_inters_) { ent_inter->close_plugin(t()); } From 87f2ed77d988683f10aaa4a5409cb8823e402b85 Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Fri, 26 May 2023 15:00:51 -0400 Subject: [PATCH 14/46] Working for updating x, y, and z pos of new entities --- include/scrimmage/parse/MissionParse.h | 3 +- src/parse/MissionParse.cpp | 74 +++++++++++++++++++------- src/simcontrol/SimControl.cpp | 2 +- 3 files changed, 57 insertions(+), 22 deletions(-) diff --git a/include/scrimmage/parse/MissionParse.h b/include/scrimmage/parse/MissionParse.h index 3af230d61..ed67b9076 100644 --- a/include/scrimmage/parse/MissionParse.h +++ b/include/scrimmage/parse/MissionParse.h @@ -92,7 +92,8 @@ class MissionParse { void get_plugin_params(std::string node_name, std::string node_value); //Natalie - void final_state_xml(std::vector & all_end_states); + void final_state_xml(std::list & all_end_states); + int num_ents = 0; bool write(const std::string &filename); diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index 0a4c40ecb..2e58f7ded 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -458,6 +458,9 @@ bool MissionParse::parse(const std::string &filename) { script_node != 0; script_node = script_node->next_sibling("entity")) { + // Tracks the number of entities in the original XML file + num_ents++; + std::map script_info; rapidxml::xml_attribute<> *nm_attr = script_node->first_attribute("entity_common"); @@ -787,12 +790,8 @@ bool MissionParse::parse(const std::string &filename) { } // Natalie test function -void MissionParse::final_state_xml(std::vector & all_end_states){ - - for(auto& a : all_end_states){ - cout << "1. " << a.team_id << " 2. " << a.x_pos << " 3. " << a.y_pos << " 4. " << a.z_pos << endl; - } - +void MissionParse::final_state_xml(std::list & all_end_states){ + // Parse the xml tree. // doc.parse requires a null terminated string that it can modify. std::vector mission_file_content_vec(mission_file_content_.size() + 1); // allocation done here @@ -815,22 +814,57 @@ void MissionParse::final_state_xml(std::vector & all_end_states){ return; } - // Loop through each "entity" node - for (rapidxml::xml_node<> *script_node = runscript_node->first_node("entity"); - script_node != 0; - script_node = script_node->next_sibling("entity")) { + for(auto a = all_end_states.begin(); a != all_end_states.end(); ++a){ + const auto& cur_ent = *a; + cout << "1. " << cur_ent.team_id << " 2. " << cur_ent.x_pos << " 3. " << cur_ent.y_pos << " 4. " << cur_ent.z_pos << endl; - // Get the team id number of the current node - rapidxml::xml_node<> *team_id_node = script_node->first_node("team_id"); - cout << "Team id for the given node: " << team_id_node->value() << endl; + // Loop through each "entity" node + for (rapidxml::xml_node<> *script_node = runscript_node->first_node("entity"); + loop_test < num_ents; //Do not need this condiiton... will not have to worry about added nodes because of the break statement + script_node = script_node->next_sibling("entity")) { + + // Get the team id number of the current node + rapidxml::xml_node<> *team_id_node = script_node->first_node("team_id"); + + if(strcmp(std::to_string(cur_ent.team_id).c_str(),team_id_node->value()) == 0){ + cout << "Creating new entity instance... " << "1. " << cur_ent.team_id + << " 2. " << cur_ent.x_pos << " 3. " << cur_ent.y_pos << " 4. " << cur_ent.z_pos << endl; + + // Creates a clone of the current entity node + rapidxml::xml_node<> *new_ent = doc.clone_node(script_node); + + char *xpos_value = doc.allocate_string(std::to_string(cur_ent.x_pos).c_str()); // If this way of converting to char * is not used, the code will error with repeated values or random ascii + rapidxml::xml_node<> *x_pos = doc.allocate_node(rapidxml::node_element, "x", xpos_value); + new_ent->insert_node(new_ent->first_node("x"),x_pos); + new_ent->remove_node(new_ent->first_node("x")->next_sibling()); + + + char *ypos_value = doc.allocate_string(std::to_string(cur_ent.y_pos).c_str()); // If this way of converting to char * is not used, the code will error with repeated values or random ascii + rapidxml::xml_node<> *y_pos = doc.allocate_node(rapidxml::node_element, "y", ypos_value); + new_ent->insert_node(new_ent->first_node("y"),y_pos); + new_ent->remove_node(new_ent->first_node("y")->next_sibling()); + + char *zpos_value = doc.allocate_string(std::to_string(cur_ent.z_pos).c_str()); // If this way of converting to char * is not used, the code will error with repeated values or random ascii + rapidxml::xml_node<> *z_pos = doc.allocate_node(rapidxml::node_element, "z", zpos_value); + new_ent->insert_node(new_ent->first_node("z"),z_pos); + new_ent->remove_node(new_ent->first_node("z")->next_sibling()); + + + // Adds the new entity node to the main XML tree + doc.first_node("runscript")->append_node(new_ent); + + // If a new node is added, break to the next entity in the list of structs + break; + } + + loop_test++; + } + + + + loop_test = 0; + } - // This successfully adds 4 entities of the first entity node to the end of the XML file! - while(loop_test < 4){ - rapidxml::xml_node<> *test_ent = doc.clone_node(script_node); - doc.first_node("runscript")->append_node(test_ent); - loop_test ++; - } - } // Save doc with new allocated attributes to the miss2miss_file_content string to be saved to mission.plugin.xml logs std::string rapidxml_miss2miss_doc; diff --git a/src/simcontrol/SimControl.cpp b/src/simcontrol/SimControl.cpp index 900fa4051..639fd0b7d 100644 --- a/src/simcontrol/SimControl.cpp +++ b/src/simcontrol/SimControl.cpp @@ -1063,7 +1063,7 @@ bool SimControl::finalize() { bool SimControl::shutdown(const bool& shutdown_python) { finalize(); - std::vector all_end_states; + std::list all_end_states; // Close all plugins for (EntityPtr &ent : ents_) { From 811875e9373120a31ea6a280a1e15859ffe93340 Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Fri, 26 May 2023 15:50:05 -0400 Subject: [PATCH 15/46] Removes original entity blocks --- src/parse/MissionParse.cpp | 31 ++++++++++++++++++++++++++++--- 1 file changed, 28 insertions(+), 3 deletions(-) diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index 2e58f7ded..769306962 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -814,6 +814,8 @@ void MissionParse::final_state_xml(std::list & all_end_states){ return; } + cout << "Number of entity blocks: " << num_ents << endl; + for(auto a = all_end_states.begin(); a != all_end_states.end(); ++a){ const auto& cur_ent = *a; cout << "1. " << cur_ent.team_id << " 2. " << cur_ent.x_pos << " 3. " << cur_ent.y_pos << " 4. " << cur_ent.z_pos << endl; @@ -849,7 +851,28 @@ void MissionParse::final_state_xml(std::list & all_end_states){ new_ent->insert_node(new_ent->first_node("z"),z_pos); new_ent->remove_node(new_ent->first_node("z")->next_sibling()); + // Make the count value 1 + if(new_ent->first_node("count")){ + rapidxml::xml_node<> *ent_count = doc.allocate_node(rapidxml::node_element, "count", "1"); + new_ent->insert_node(new_ent->first_node("count"),ent_count); + new_ent->remove_node(new_ent->first_node("count")->next_sibling()); + } + + // Remove instances of variance + if(new_ent->first_node("variance_x")){ + new_ent->remove_node(new_ent->first_node("variance_x")); + } + if(new_ent->first_node("variance_y")){ + new_ent->remove_node(new_ent->first_node("variance_y")); + } + if(new_ent->first_node("variance_z")){ + new_ent->remove_node(new_ent->first_node("variance_z")); + } + // Update the heading + if(new_ent->first_node("heading")){ + cout << "In the heading node if statement" << endl; + } // Adds the new entity node to the main XML tree doc.first_node("runscript")->append_node(new_ent); @@ -858,13 +881,15 @@ void MissionParse::final_state_xml(std::list & all_end_states){ } loop_test++; - } - - + } loop_test = 0; } + // Remove original entity nodes + for (int i = 0; iremove_node(runscript_node->first_node("entity")); + } // Save doc with new allocated attributes to the miss2miss_file_content string to be saved to mission.plugin.xml logs std::string rapidxml_miss2miss_doc; From 2f9d201836343d7c34f79a85671563644ee46bab Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Wed, 31 May 2023 16:30:15 -0400 Subject: [PATCH 16/46] Additional checks/removals --- include/scrimmage/parse/EntEndStates.h | 7 ++ src/parse/MissionParse.cpp | 123 +++++++++++++++++-------- src/simcontrol/SimControl.cpp | 34 +++---- 3 files changed, 107 insertions(+), 57 deletions(-) diff --git a/include/scrimmage/parse/EntEndStates.h b/include/scrimmage/parse/EntEndStates.h index 262cdb6d9..a035b9177 100644 --- a/include/scrimmage/parse/EntEndStates.h +++ b/include/scrimmage/parse/EntEndStates.h @@ -4,9 +4,16 @@ 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; }; #endif \ No newline at end of file diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index 769306962..5919d666d 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -448,11 +448,6 @@ bool MissionParse::parse(const std::string &filename) { entity_common[nm] = script_info; } - // Natalie - for the mission to mission pull request, will want to save the format of the entity blocks - // I think what should be done is call a function that parses through the mission_file_content and will add new - // entity nodes based on the count of each team, duplicating entries that are common and updating entity specific - // values based on the final state of the entity - // Loop through each "entity" node for (rapidxml::xml_node<> *script_node = runscript_node->first_node("entity"); script_node != 0; @@ -806,59 +801,111 @@ void MissionParse::final_state_xml(std::list & all_end_states){ return; } - int loop_test = 0; // remove once passing actual sim control ent information - rapidxml::xml_node<> *runscript_node = doc.first_node("runscript"); if (runscript_node == 0) { cout << "Missing runscript tag." << endl; return; } - cout << "Number of entity blocks: " << num_ents << endl; - for(auto a = all_end_states.begin(); a != all_end_states.end(); ++a){ const auto& cur_ent = *a; - cout << "1. " << cur_ent.team_id << " 2. " << cur_ent.x_pos << " 3. " << cur_ent.y_pos << " 4. " << cur_ent.z_pos << endl; // Loop through each "entity" node for (rapidxml::xml_node<> *script_node = runscript_node->first_node("entity"); - loop_test < num_ents; //Do not need this condiiton... will not have to worry about added nodes because of the break statement + script_node != 0; script_node = script_node->next_sibling("entity")) { // Get the team id number of the current node rapidxml::xml_node<> *team_id_node = script_node->first_node("team_id"); if(strcmp(std::to_string(cur_ent.team_id).c_str(),team_id_node->value()) == 0){ - cout << "Creating new entity instance... " << "1. " << cur_ent.team_id - << " 2. " << cur_ent.x_pos << " 3. " << cur_ent.y_pos << " 4. " << cur_ent.z_pos << endl; - - // Creates a clone of the current entity node + // Creates a clone of the entity node that matches the struct's team id rapidxml::xml_node<> *new_ent = doc.clone_node(script_node); - char *xpos_value = doc.allocate_string(std::to_string(cur_ent.x_pos).c_str()); // If this way of converting to char * is not used, the code will error with repeated values or random ascii + // Update the entity block with the final state values for the given entity + char *xpos_value = doc.allocate_string(std::to_string(cur_ent.x_pos).c_str()); // Must convert to char * this way; otherwise, the code will error with repeated values or random ascii rapidxml::xml_node<> *x_pos = doc.allocate_node(rapidxml::node_element, "x", xpos_value); - new_ent->insert_node(new_ent->first_node("x"),x_pos); - new_ent->remove_node(new_ent->first_node("x")->next_sibling()); + if(new_ent->first_node("x")){ + new_ent->insert_node(new_ent->first_node("x"),x_pos); + new_ent->remove_node(new_ent->first_node("x")->next_sibling()); + } else{ + new_ent->insert_node(new_ent->first_node(),x_pos); + } - - char *ypos_value = doc.allocate_string(std::to_string(cur_ent.y_pos).c_str()); // If this way of converting to char * is not used, the code will error with repeated values or random ascii + char *ypos_value = doc.allocate_string(std::to_string(cur_ent.y_pos).c_str()); rapidxml::xml_node<> *y_pos = doc.allocate_node(rapidxml::node_element, "y", ypos_value); - new_ent->insert_node(new_ent->first_node("y"),y_pos); - new_ent->remove_node(new_ent->first_node("y")->next_sibling()); + if(new_ent->first_node("y")){ + new_ent->insert_node(new_ent->first_node("y"),y_pos); + new_ent->remove_node(new_ent->first_node("y")->next_sibling()); + } else{ + new_ent->insert_node(new_ent->first_node(),y_pos); + } - char *zpos_value = doc.allocate_string(std::to_string(cur_ent.z_pos).c_str()); // If this way of converting to char * is not used, the code will error with repeated values or random ascii + char *zpos_value = doc.allocate_string(std::to_string(cur_ent.z_pos).c_str()); rapidxml::xml_node<> *z_pos = doc.allocate_node(rapidxml::node_element, "z", zpos_value); - new_ent->insert_node(new_ent->first_node("z"),z_pos); - new_ent->remove_node(new_ent->first_node("z")->next_sibling()); + if(new_ent->first_node("z")){ + new_ent->insert_node(new_ent->first_node("z"),z_pos); + new_ent->remove_node(new_ent->first_node("z")->next_sibling()); + } else{ + new_ent->insert_node(new_ent->first_node(),z_pos); + } + + char *heading_value = doc.allocate_string(std::to_string(cur_ent.yaw).c_str()); + rapidxml::xml_node<> * heading = doc.allocate_node(rapidxml::node_element, "heading", heading_value); + if(new_ent->first_node("heading")){ + new_ent->insert_node(new_ent->first_node("heading"),heading); + new_ent->remove_node(new_ent->first_node("heading")->next_sibling()); + cout << "In heading if." << endl; + } else{ + new_ent->insert_node(new_ent->first_node("z")->next_sibling(),heading); + cout << "In heading else." << endl; + } - // Make the count value 1 + char *pitch_value = doc.allocate_string(std::to_string(cur_ent.pitch).c_str()); + rapidxml::xml_node<> * pitch = doc.allocate_node(rapidxml::node_element, "pitch", pitch_value); + if(new_ent->first_node("pitch")){ + new_ent->insert_node(new_ent->first_node("pitch"),pitch); + new_ent->remove_node(new_ent->first_node("pitch")->next_sibling()); + } else{ + new_ent->insert_node(new_ent->first_node("heading")->next_sibling(),pitch); + } + + char *roll_value = doc.allocate_string(std::to_string(cur_ent.roll).c_str()); + rapidxml::xml_node<> * roll = doc.allocate_node(rapidxml::node_element, "roll", roll_value); + if(new_ent->first_node("roll")){ + new_ent->insert_node(new_ent->first_node("roll"),roll); + new_ent->remove_node(new_ent->first_node("roll")->next_sibling()); + } else{ + new_ent->insert_node(new_ent->first_node("pitch")->next_sibling(),roll); + } + + char *altitude_value = doc.allocate_string(std::to_string(cur_ent.z_pos).c_str()); + rapidxml::xml_node<> * altitude = doc.allocate_node(rapidxml::node_element, "altitude", altitude_value); + if(new_ent->first_node("altitude")){ + new_ent->insert_node(new_ent->first_node("altitude"),altitude); + new_ent->remove_node(new_ent->first_node("altitude")->next_sibling()); + } else{ + new_ent->insert_node(new_ent->first_node("roll")->next_sibling(),altitude); + } + + rapidxml::xml_node<> *ent_count = doc.allocate_node(rapidxml::node_element, "count", "1"); if(new_ent->first_node("count")){ - rapidxml::xml_node<> *ent_count = doc.allocate_node(rapidxml::node_element, "count", "1"); new_ent->insert_node(new_ent->first_node("count"),ent_count); new_ent->remove_node(new_ent->first_node("count")->next_sibling()); + } else{ + new_ent->insert_node(new_ent->first_node("team_id")->next_sibling(),ent_count); } - // Remove instances of variance + char *health_value = doc.allocate_string(std::to_string(cur_ent.health_points).c_str()); + rapidxml::xml_node<> *health = doc.allocate_node(rapidxml::node_element, "health", health_value); + if(new_ent->first_node("health")){ + new_ent->insert_node(new_ent->first_node("health"),health); + new_ent->remove_node(new_ent->first_node("health")->next_sibling()); + } else{ + new_ent->insert_node(new_ent->first_node("count"),health); + } + + // Remove tags that are not needed for single entity blocks if(new_ent->first_node("variance_x")){ new_ent->remove_node(new_ent->first_node("variance_x")); } @@ -868,22 +915,26 @@ void MissionParse::final_state_xml(std::list & all_end_states){ if(new_ent->first_node("variance_z")){ new_ent->remove_node(new_ent->first_node("variance_z")); } - - // Update the heading - if(new_ent->first_node("heading")){ - cout << "In the heading node if statement" << endl; + if(new_ent->first_node("generate_rate")){ + new_ent->remove_node(new_ent->first_node("generate_rate")); } + if(new_ent->first_node("generate_count")){ + new_ent->remove_node(new_ent->first_node("generate_count")); + } + if(new_ent->first_node("generate_start_time")){ + new_ent->remove_node(new_ent->first_node("generate_start_time")); + } + if(new_ent->first_node("generate_time_variance")){ + new_ent->remove_node(new_ent->first_node("generate_time_variance")); + } + // Adds the new entity node to the main XML tree doc.first_node("runscript")->append_node(new_ent); // If a new node is added, break to the next entity in the list of structs break; } - - loop_test++; } - - loop_test = 0; } // Remove original entity nodes diff --git a/src/simcontrol/SimControl.cpp b/src/simcontrol/SimControl.cpp index 639fd0b7d..94c1b6981 100644 --- a/src/simcontrol/SimControl.cpp +++ b/src/simcontrol/SimControl.cpp @@ -1081,14 +1081,6 @@ bool SimControl::shutdown(const bool& shutdown_python) { // team_id cout << "Team id: " << ent->id().team_id() << endl; - // color - probably going to need to get this from the mission parse file - // count (should be 1), - int block_count = 1; - cout << "Block count: " << block_count << endl; - - // autonomy, autonomy loop rate, - - // health // Note: Entities that have collisions are removed. Health points could still be used for other mission xml output, because they might have more than 1 as a // starting point, being able to endure multiple collisions @@ -1107,36 +1099,36 @@ bool SimControl::shutdown(const bool& shutdown_python) { // heading - same thing as yaw cout << "Yaw of the quaternion: " << ent->state()->quat().yaw() << endl; - cout << "Roll of the quaternion: " << ent->state()->quat().roll() << endl; cout << "Pitch of the quaternion: " << ent->state()->quat().pitch() << endl; - - // motion model - // visual model - // controller, controller loop rate, - // sensor, sensor loop rate, - // base - x,y,z, latitude, longitude, altitude, and radius of the base + cout << "Roll of the quaternion: " << ent->state()->quat().roll() << endl; // Velocity - no known tag for the entity block, may need to be an entry for controller //double vx, vy, vz = ent->state()->vel(); cout << "Velocity values, vx: " << ent->state()->vel()[0] << " vy: " << ent->state()->vel()[1] << " vz: " << ent->state()->vel()[2] << endl; - // Data that could come from the mission xml file - - + // I wonder if code about generating new ents, etc. should be removed, because it would + // be duplicated many times + // Will probably also want to remove the original calls of the ent blocks so that + // more are not created in the 2nd mission file... // Do not include: // generate_rate, start time, count, and time variance, // variance x y and z // use variance all ents, - //mp_->final_state_xml(); - // Struct saving and vector - ent_end_state end_state = {ent->id().team_id(), ent->state()->pos()[0], ent->state()->pos()[1], ent->state()->pos()[2]}; + ent_end_state end_state = {ent->id().team_id(), + ent->state()->pos()[0], ent->state()->pos()[1], ent->state()->pos()[2], + ent->state()->quat().yaw(), ent->state()->quat().pitch(), ent->state()->quat().roll(), + ent->health_points()}; all_end_states.push_back(end_state); ent->close(t()); } + // Add a tag to not remove certain entity blocks + // Add a tag to determine if the mission file should be created, at the same level as the run tag + + //only call this and the above if a flag is set in the mission xml to create an output state file mp_->final_state_xml(all_end_states); for (EntityInteractionPtr ent_inter : ent_inters_) { From b2936d3f84b09aa5f8dc4cbd218904b7e9b4d625 Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Thu, 1 Jun 2023 10:18:52 -0400 Subject: [PATCH 17/46] Added tag to determine if mission 2 mission output file should be created --- include/scrimmage/simcontrol/SimControl.h | 1 + src/simcontrol/SimControl.cpp | 117 +++++++++++----------- 2 files changed, 57 insertions(+), 61 deletions(-) diff --git a/include/scrimmage/simcontrol/SimControl.h b/include/scrimmage/simcontrol/SimControl.h index a0d5b33c3..b9a754742 100644 --- a/include/scrimmage/simcontrol/SimControl.h +++ b/include/scrimmage/simcontrol/SimControl.h @@ -373,6 +373,7 @@ class SimControl { bool finished_ = false; bool exit_ = false; + bool miss2miss = false; std::mutex finished_mutex_; std::mutex contacts_mutex_; diff --git a/src/simcontrol/SimControl.cpp b/src/simcontrol/SimControl.cpp index 94c1b6981..f7919adb6 100644 --- a/src/simcontrol/SimControl.cpp +++ b/src/simcontrol/SimControl.cpp @@ -892,6 +892,10 @@ bool SimControl::start() { contacts_->reserve(max_num_entities+1); contacts_mutex_.unlock(); + if (get("mission_to_mission", mp_->params(), true)) { + miss2miss = true; + } + if (get("show_plugins", mp_->params(), false)) { plugin_manager_->print_returned_plugins(); } @@ -1066,70 +1070,61 @@ bool SimControl::shutdown(const bool& shutdown_python) { std::list all_end_states; // Close all plugins - for (EntityPtr &ent : ents_) { - - // Natalie - need to store all state information before the GUI shuts down. Grab state information that can be captured in the entity block - // in the mission xml file. Need to then send the information to the mission parse file to create a new xml file when shutting down that - // can then be used by another simulation - // - // Need to pass the team ID and need to determine the number of entities that are part of a given team... might be able to handle this with - // looping - // - // Need to attempt to specify the following for the mission xml: - // Name - can multiple blocks have the same name? - - // team_id - cout << "Team id: " << ent->id().team_id() << endl; - - // health - // Note: Entities that have collisions are removed. Health points could still be used for other mission xml output, because they might have more than 1 as a - // starting point, being able to endure multiple collisions - cout << "Health points: " << ent->health_points() << endl; // Need to check if the value is lower than a certain number, the entity should be created or not - - // x, y, z - //double x, y, z = ent->state()->pos(); - cout << "Position values, x: " << ent->state()->pos()[0] << " y: " << ent->state()->pos()[1] << " z: " << ent->state()->pos()[2] << endl; - - // latitude - // longitude - - // altitude - // altitude is the same as the z position - cout << "Altitude: " << ent->state()->pos()[2] << endl; - - // heading - same thing as yaw - cout << "Yaw of the quaternion: " << ent->state()->quat().yaw() << endl; - cout << "Pitch of the quaternion: " << ent->state()->quat().pitch() << endl; - cout << "Roll of the quaternion: " << ent->state()->quat().roll() << endl; - - // Velocity - no known tag for the entity block, may need to be an entry for controller - //double vx, vy, vz = ent->state()->vel(); - cout << "Velocity values, vx: " << ent->state()->vel()[0] << " vy: " << ent->state()->vel()[1] << " vz: " << ent->state()->vel()[2] << endl; - - // I wonder if code about generating new ents, etc. should be removed, because it would - // be duplicated many times - // Will probably also want to remove the original calls of the ent blocks so that - // more are not created in the 2nd mission file... - // Do not include: - // generate_rate, start time, count, and time variance, - // variance x y and z - // use variance all ents, - - // Struct saving and vector - ent_end_state end_state = {ent->id().team_id(), - ent->state()->pos()[0], ent->state()->pos()[1], ent->state()->pos()[2], - ent->state()->quat().yaw(), ent->state()->quat().pitch(), ent->state()->quat().roll(), - ent->health_points()}; - all_end_states.push_back(end_state); - - ent->close(t()); + if(miss2miss){ + for (EntityPtr &ent : ents_) { + // Natalie - need to store all state information before the GUI shuts down. Grab state information that can be captured in the entity block + // in the mission xml file. Need to then send the information to the mission parse file to create a new xml file when shutting down that + // can then be used by another simulation + // + // Need to pass the team ID and need to determine the number of entities that are part of a given team... might be able to handle this with + // looping + // + // Need to attempt to specify the following for the mission xml: + // Name - can multiple blocks have the same name? + + // team_id + cout << "Team id: " << ent->id().team_id() << endl; + + // health + // Note: Entities that have collisions are removed. Health points could still be used for other mission xml output, because they might have more than 1 as a + // starting point, being able to endure multiple collisions + cout << "Health points: " << ent->health_points() << endl; // Need to check if the value is lower than a certain number, the entity should be created or not + + // x, y, z + //double x, y, z = ent->state()->pos(); + cout << "Position values, x: " << ent->state()->pos()[0] << " y: " << ent->state()->pos()[1] << " z: " << ent->state()->pos()[2] << endl; + + // heading - same thing as yaw + cout << "Yaw of the quaternion: " << ent->state()->quat().yaw() << endl; + cout << "Pitch of the quaternion: " << ent->state()->quat().pitch() << endl; + cout << "Roll of the quaternion: " << ent->state()->quat().roll() << endl; + + // Velocity - no known tag for the entity block, may need to be an entry for controller + //double vx, vy, vz = ent->state()->vel(); + cout << "Velocity values, vx: " << ent->state()->vel()[0] << " vy: " << ent->state()->vel()[1] << " vz: " << ent->state()->vel()[2] << endl; + + // Struct saving and vector + ent_end_state end_state = {ent->id().team_id(), + ent->state()->pos()[0], ent->state()->pos()[1], ent->state()->pos()[2], + ent->state()->quat().yaw(), ent->state()->quat().pitch(), ent->state()->quat().roll(), + ent->health_points()}; + all_end_states.push_back(end_state); + + ent->close(t()); + } + } else { + for (EntityPtr &ent : ents_) { + ent->close(t()); + } } - + // Add a tag to not remove certain entity blocks - // Add a tag to determine if the mission file should be created, at the same level as the run tag + // Add information that cannot be utilized in the mission block explicitly to a separate output file, like + // the velocity values - //only call this and the above if a flag is set in the mission xml to create an output state file - mp_->final_state_xml(all_end_states); + if(miss2miss){ + mp_->final_state_xml(all_end_states); + } for (EntityInteractionPtr ent_inter : ent_inters_) { ent_inter->close_plugin(t()); From a5d6fce2d9c4304dc0c68137defa38f0a441e63a Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Mon, 5 Jun 2023 12:12:47 -0400 Subject: [PATCH 18/46] Remove block tag working for original entities being added/removed from the miss2miss file --- src/parse/MissionParse.cpp | 27 +++++++++++++++++++++++---- 1 file changed, 23 insertions(+), 4 deletions(-) diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index 5919d666d..69d0cf217 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -855,10 +855,8 @@ void MissionParse::final_state_xml(std::list & all_end_states){ if(new_ent->first_node("heading")){ new_ent->insert_node(new_ent->first_node("heading"),heading); new_ent->remove_node(new_ent->first_node("heading")->next_sibling()); - cout << "In heading if." << endl; } else{ new_ent->insert_node(new_ent->first_node("z")->next_sibling(),heading); - cout << "In heading else." << endl; } char *pitch_value = doc.allocate_string(std::to_string(cur_ent.pitch).c_str()); @@ -938,8 +936,29 @@ void MissionParse::final_state_xml(std::list & all_end_states){ } // Remove original entity nodes - for (int i = 0; iremove_node(runscript_node->first_node("entity")); + int i = 0; + for (rapidxml::xml_node<> *script_node = runscript_node->first_node("entity"); + i *remove_node = script_node; + + // If the remove_block is set to true, do not remove the entity block from the miss2miss file + cout << "Value of remove block for ent " << i << ": " << script_node->first_node("remove_block")->value() + << "strcmp value: " << strcmp("true",script_node->first_node("remove_block")->value()) << endl; + + std::string node_value = script_node->first_node("remove_block")->value(); + + cout << "Node string value: " << node_value << " Compare value: " << node_value.compare("true") + << "Team id value: " << script_node->first_node("team_id")->value() << endl; + + if(node_value.compare("false") == 0){ + cout << "Not removing the entity block" << endl; + continue; + } + + script_node = script_node->next_sibling("entity"); + doc.first_node("runscript")->remove_node(remove_node); } // Save doc with new allocated attributes to the miss2miss_file_content string to be saved to mission.plugin.xml logs From 4fce2a753fd31d89718cd8cac1d37e3d5c23a667 Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Mon, 5 Jun 2023 15:37:02 -0400 Subject: [PATCH 19/46] New final entity state file is created to view the final states of each entity --- include/scrimmage/parse/EntEndStates.h | 4 +++ include/scrimmage/parse/MissionParse.h | 1 + src/parse/MissionParse.cpp | 38 ++++++++++++++++---------- src/simcontrol/SimControl.cpp | 7 +++-- 4 files changed, 32 insertions(+), 18 deletions(-) diff --git a/include/scrimmage/parse/EntEndStates.h b/include/scrimmage/parse/EntEndStates.h index a035b9177..0a9feca2c 100644 --- a/include/scrimmage/parse/EntEndStates.h +++ b/include/scrimmage/parse/EntEndStates.h @@ -14,6 +14,10 @@ double roll; int health_points; + + double vel_x; + double vel_y; + double vel_z; }; #endif \ No newline at end of file diff --git a/include/scrimmage/parse/MissionParse.h b/include/scrimmage/parse/MissionParse.h index ed67b9076..845873836 100644 --- a/include/scrimmage/parse/MissionParse.h +++ b/include/scrimmage/parse/MissionParse.h @@ -173,6 +173,7 @@ class MissionParse { 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 plugin_spec_attrs; diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index 69d0cf217..a801b5da5 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -816,7 +816,7 @@ void MissionParse::final_state_xml(std::list & all_end_states){ script_node = script_node->next_sibling("entity")) { // Get the team id number of the current node - rapidxml::xml_node<> *team_id_node = script_node->first_node("team_id"); + rapidxml::xml_node<> *team_id_node = script_node->first_node("team_id"); // is this needed? if(strcmp(std::to_string(cur_ent.team_id).c_str(),team_id_node->value()) == 0){ // Creates a clone of the entity node that matches the struct's team id @@ -903,6 +903,20 @@ void MissionParse::final_state_xml(std::list & all_end_states){ new_ent->insert_node(new_ent->first_node("count"),health); } + ent_state_file_content << "--- New Entity Entry ---" << endl + << "Team_ID: " << team_id_node->value() << endl + << "X_Pos: " << xpos_value << endl + << "Y_Pos: " << ypos_value << endl + << "Z_Pos: " << zpos_value << endl + << "Vel_X: " << cur_ent.vel_x << endl + << "Vel_Y: " << cur_ent.vel_y << endl + << "Vel_Z: " << cur_ent.vel_z << endl + << "Heading: " << heading_value << endl + << "Pitch: " << pitch_value << endl + << "Roll: " << roll_value << endl + << "Altitude: " << altitude_value << endl + << "Health: " << health_value << endl << endl; + // Remove tags that are not needed for single entity blocks if(new_ent->first_node("variance_x")){ new_ent->remove_node(new_ent->first_node("variance_x")); @@ -931,32 +945,21 @@ void MissionParse::final_state_xml(std::list & all_end_states){ // If a new node is added, break to the next entity in the list of structs break; - } + } } } // Remove original entity nodes int i = 0; - for (rapidxml::xml_node<> *script_node = runscript_node->first_node("entity"); - i *remove_node = script_node; - - // If the remove_block is set to true, do not remove the entity block from the miss2miss file - cout << "Value of remove block for ent " << i << ": " << script_node->first_node("remove_block")->value() - << "strcmp value: " << strcmp("true",script_node->first_node("remove_block")->value()) << endl; - + for (rapidxml::xml_node<> *script_node = runscript_node->first_node("entity"); ifirst_node("remove_block")->value(); - cout << "Node string value: " << node_value << " Compare value: " << node_value.compare("true") - << "Team id value: " << script_node->first_node("team_id")->value() << endl; - if(node_value.compare("false") == 0){ cout << "Not removing the entity block" << endl; continue; } + rapidxml::xml_node<> *remove_node = script_node; script_node = script_node->next_sibling("entity"); doc.first_node("runscript")->remove_node(remove_node); } @@ -966,10 +969,15 @@ void MissionParse::final_state_xml(std::list & all_end_states){ rapidxml::print(std::back_inserter(rapidxml_miss2miss_doc), doc, 0); miss2miss_file_content = rapidxml_miss2miss_doc; + // Create the new mission to mission xml file std::ofstream miss2miss_content_out(log_dir_+"/miss2miss.xml"); miss2miss_content_out << miss2miss_file_content; miss2miss_content_out.close(); + // Create the entity end state file + std::ofstream ent_state_content_out(log_dir_+"/final_ent_states.xml"); + ent_state_content_out << ent_state_file_content.rdbuf(); + ent_state_content_out.close(); } void MissionParse::get_plugin_params(std::string node_name, std::string node_value) { diff --git a/src/simcontrol/SimControl.cpp b/src/simcontrol/SimControl.cpp index f7919adb6..1d48613d6 100644 --- a/src/simcontrol/SimControl.cpp +++ b/src/simcontrol/SimControl.cpp @@ -1103,11 +1103,13 @@ bool SimControl::shutdown(const bool& shutdown_python) { //double vx, vy, vz = ent->state()->vel(); cout << "Velocity values, vx: " << ent->state()->vel()[0] << " vy: " << ent->state()->vel()[1] << " vz: " << ent->state()->vel()[2] << endl; - // Struct saving and vector + // Create the struct for the entity end states + // x_pos, y_pos, z_pos, yaw, pitch, roll, health_points, vel_x, vel_y, vel_z ent_end_state end_state = {ent->id().team_id(), ent->state()->pos()[0], ent->state()->pos()[1], ent->state()->pos()[2], ent->state()->quat().yaw(), ent->state()->quat().pitch(), ent->state()->quat().roll(), - ent->health_points()}; + ent->health_points(), + ent->state()->vel()[0], ent->state()->vel()[1], ent->state()->vel()[2]}; all_end_states.push_back(end_state); ent->close(t()); @@ -1118,7 +1120,6 @@ bool SimControl::shutdown(const bool& shutdown_python) { } } - // Add a tag to not remove certain entity blocks // Add information that cannot be utilized in the mission block explicitly to a separate output file, like // the velocity values From b8eaacbc661e5b0553de1e26235f4974968e9c1c Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Mon, 5 Jun 2023 16:50:59 -0400 Subject: [PATCH 20/46] Code cleanup --- include/scrimmage/parse/MissionParse.h | 7 ++--- src/parse/MissionParse.cpp | 13 +++------ src/simcontrol/SimControl.cpp | 37 ++------------------------ 3 files changed, 8 insertions(+), 49 deletions(-) diff --git a/include/scrimmage/parse/MissionParse.h b/include/scrimmage/parse/MissionParse.h index 845873836..4ff966a0d 100644 --- a/include/scrimmage/parse/MissionParse.h +++ b/include/scrimmage/parse/MissionParse.h @@ -40,8 +40,8 @@ #include #include - #include +#include #include #include @@ -50,8 +50,6 @@ #include #include -#include - namespace sp = scrimmage_proto; namespace scrimmage { @@ -90,9 +88,8 @@ class MissionParse { 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); - - //Natalie void final_state_xml(std::list & all_end_states); + // Track the number of entity blocks in the input Mission XML file int num_ents = 0; bool write(const std::string &filename); diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index a801b5da5..da3c97147 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -784,7 +784,6 @@ bool MissionParse::parse(const std::string &filename) { return true; } -// Natalie test function void MissionParse::final_state_xml(std::list & all_end_states){ // Parse the xml tree. @@ -814,16 +813,12 @@ void MissionParse::final_state_xml(std::list & all_end_states){ for (rapidxml::xml_node<> *script_node = runscript_node->first_node("entity"); script_node != 0; script_node = script_node->next_sibling("entity")) { - - // Get the team id number of the current node - rapidxml::xml_node<> *team_id_node = script_node->first_node("team_id"); // is this needed? - - if(strcmp(std::to_string(cur_ent.team_id).c_str(),team_id_node->value()) == 0){ + if(strcmp(std::to_string(cur_ent.team_id).c_str(),script_node->first_node("team_id")->value()) == 0){ // Creates a clone of the entity node that matches the struct's team id rapidxml::xml_node<> *new_ent = doc.clone_node(script_node); // Update the entity block with the final state values for the given entity - char *xpos_value = doc.allocate_string(std::to_string(cur_ent.x_pos).c_str()); // Must convert to char * this way; otherwise, the code will error with repeated values or random ascii + char *xpos_value = doc.allocate_string(std::to_string(cur_ent.x_pos).c_str()); rapidxml::xml_node<> *x_pos = doc.allocate_node(rapidxml::node_element, "x", xpos_value); if(new_ent->first_node("x")){ new_ent->insert_node(new_ent->first_node("x"),x_pos); @@ -949,12 +944,12 @@ void MissionParse::final_state_xml(std::list & all_end_states){ } } - // Remove original entity nodes + // Remove original entity nodes based on the bool value of the remove_block entity tag int i = 0; for (rapidxml::xml_node<> *script_node = runscript_node->first_node("entity"); ifirst_node("remove_block")->value(); - if(node_value.compare("false") == 0){ + if(node_value=="false"){ cout << "Not removing the entity block" << endl; continue; } diff --git a/src/simcontrol/SimControl.cpp b/src/simcontrol/SimControl.cpp index 1d48613d6..db64a2306 100644 --- a/src/simcontrol/SimControl.cpp +++ b/src/simcontrol/SimControl.cpp @@ -1070,39 +1070,9 @@ bool SimControl::shutdown(const bool& shutdown_python) { std::list all_end_states; // Close all plugins + // If the mission_to_mission tag is true, store final states of each entity in a vector of structs if(miss2miss){ - for (EntityPtr &ent : ents_) { - // Natalie - need to store all state information before the GUI shuts down. Grab state information that can be captured in the entity block - // in the mission xml file. Need to then send the information to the mission parse file to create a new xml file when shutting down that - // can then be used by another simulation - // - // Need to pass the team ID and need to determine the number of entities that are part of a given team... might be able to handle this with - // looping - // - // Need to attempt to specify the following for the mission xml: - // Name - can multiple blocks have the same name? - - // team_id - cout << "Team id: " << ent->id().team_id() << endl; - - // health - // Note: Entities that have collisions are removed. Health points could still be used for other mission xml output, because they might have more than 1 as a - // starting point, being able to endure multiple collisions - cout << "Health points: " << ent->health_points() << endl; // Need to check if the value is lower than a certain number, the entity should be created or not - - // x, y, z - //double x, y, z = ent->state()->pos(); - cout << "Position values, x: " << ent->state()->pos()[0] << " y: " << ent->state()->pos()[1] << " z: " << ent->state()->pos()[2] << endl; - - // heading - same thing as yaw - cout << "Yaw of the quaternion: " << ent->state()->quat().yaw() << endl; - cout << "Pitch of the quaternion: " << ent->state()->quat().pitch() << endl; - cout << "Roll of the quaternion: " << ent->state()->quat().roll() << endl; - - // Velocity - no known tag for the entity block, may need to be an entry for controller - //double vx, vy, vz = ent->state()->vel(); - cout << "Velocity values, vx: " << ent->state()->vel()[0] << " vy: " << ent->state()->vel()[1] << " vz: " << ent->state()->vel()[2] << endl; - + for (EntityPtr &ent : ents_) { // Create the struct for the entity end states // x_pos, y_pos, z_pos, yaw, pitch, roll, health_points, vel_x, vel_y, vel_z ent_end_state end_state = {ent->id().team_id(), @@ -1119,9 +1089,6 @@ bool SimControl::shutdown(const bool& shutdown_python) { ent->close(t()); } } - - // Add information that cannot be utilized in the mission block explicitly to a separate output file, like - // the velocity values if(miss2miss){ mp_->final_state_xml(all_end_states); From 1e0fa944a8395f00662a45e657added0e40684a3 Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Wed, 7 Jun 2023 10:20:24 -0400 Subject: [PATCH 21/46] Code cleanup --- include/scrimmage/parse/MissionParse.h | 2 +- src/parse/MissionParse.cpp | 18 ++++++------------ 2 files changed, 7 insertions(+), 13 deletions(-) diff --git a/include/scrimmage/parse/MissionParse.h b/include/scrimmage/parse/MissionParse.h index 4ff966a0d..6b18cf512 100644 --- a/include/scrimmage/parse/MissionParse.h +++ b/include/scrimmage/parse/MissionParse.h @@ -39,7 +39,7 @@ #include #include -#include +//#include #include #include diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index da3c97147..f6cce0c42 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -239,18 +239,12 @@ bool MissionParse::parse(const std::string &filename) { std::string plugin_path; // Create substrings of the plugin_env_string, using the ':' character as - // a delimiter + // a delimiter. Update the scrimmage_plugin_path string with the path that + // can access plugin specific xml files while(std::getline(plugin_ss, plugin_path, ':')){ - if(!plugin_path.empty()){ - plugin_substrings.push_back(plugin_path); - } - } - - // Update the scrimmage_plugin_path string with the path that can access plugin - // specific xml files - for(const auto& plugin_string : plugin_substrings){ - if (plugin_string.find("/include/scrimmage/plugins") != std::string::npos){ - scrimmage_plugin_path = plugin_string; + if (!plugin_path.empty() && (plugin_path.find("/include/scrimmage/plugins") != std::string::npos)){ + scrimmage_plugin_path = plugin_path; + break; } } @@ -899,7 +893,7 @@ void MissionParse::final_state_xml(std::list & all_end_states){ } ent_state_file_content << "--- New Entity Entry ---" << endl - << "Team_ID: " << team_id_node->value() << endl + << "Team_ID: " << script_node->first_node("team_id")->value() << endl << "X_Pos: " << xpos_value << endl << "Y_Pos: " << ypos_value << endl << "Z_Pos: " << zpos_value << endl From 794e01864506014a17fe1daedc7e312d9c4eeee3 Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Wed, 7 Jun 2023 13:15:13 -0400 Subject: [PATCH 22/46] Working before trying to put struct with fwd decl file --- src/parse/MissionParse.cpp | 20 +++++++++----------- 1 file changed, 9 insertions(+), 11 deletions(-) diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index f6cce0c42..5ea50d520 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -282,7 +282,6 @@ bool MissionParse::parse(const std::string &filename) { get_plugin_params(nm, node->value()); } - // Loop through each node's attributes: for (rapidxml::xml_attribute<> *attr = node->first_attribute(); attr; attr = attr->next_attribute()) { @@ -622,7 +621,8 @@ bool MissionParse::parse(const std::string &filename) { } } - // Save doc with new allocated attributes to the mission_plugin_file_content string to be saved to mission.plugin.xml logs + // Save doc with new allocated attributes to the mission_plugin_file_content string, which will be output to the + // mission.plugin.xml file in the log directory std::string rapidxml_plugin_doc; rapidxml::print(std::back_inserter(rapidxml_plugin_doc), doc, 0); mission_plugin_file_content = rapidxml_plugin_doc; @@ -779,9 +779,7 @@ bool MissionParse::parse(const std::string &filename) { } void MissionParse::final_state_xml(std::list & all_end_states){ - - // Parse the xml tree. - // doc.parse requires a null terminated string that it can modify. + // Parse the xml tree. doc.parse requires a null terminated string that it can modify. std::vector mission_file_content_vec(mission_file_content_.size() + 1); // allocation done here mission_file_content_vec.assign(mission_file_content_.begin(), mission_file_content_.end()); // copy mission_file_content_vec.push_back('\0'); // shouldn't reallocate @@ -939,11 +937,11 @@ void MissionParse::final_state_xml(std::list & all_end_states){ } // Remove original entity nodes based on the bool value of the remove_block entity tag - int i = 0; - for (rapidxml::xml_node<> *script_node = runscript_node->first_node("entity"); i *script_node = runscript_node->first_node("entity"); + for (int i = 0; ifirst_node("remove_block")->value(); - if(node_value=="false"){ + if(script_node->first_node("remove_block") && (node_value=="false")){ cout << "Not removing the entity block" << endl; continue; } @@ -953,7 +951,8 @@ void MissionParse::final_state_xml(std::list & all_end_states){ doc.first_node("runscript")->remove_node(remove_node); } - // Save doc with new allocated attributes to the miss2miss_file_content string to be saved to mission.plugin.xml logs + // Save doc with new allocated attributes to the miss2miss_file_content string, which will be output to the + // miss2miss.xml file in the log directory std::string rapidxml_miss2miss_doc; rapidxml::print(std::back_inserter(rapidxml_miss2miss_doc), doc, 0); miss2miss_file_content = rapidxml_miss2miss_doc; @@ -1021,9 +1020,8 @@ void MissionParse::get_plugin_params(std::string node_name, std::string node_val std::regex reg("\\$\\{.+?=(.+?)\\}"); plugin_file_content_ = std::regex_replace(plugin_file_content_, reg, fmt); - // Parse the xml tree. + // Parse the xml tree. doc.parse requires a null terminated string that it can modify. rapidxml::xml_document<> plugin_doc; - // doc.parse requires a null terminated string that it can modify. std::vector plugin_file_content_vec(plugin_file_content_.size() + 1); // allocation done here plugin_file_content_vec.assign(plugin_file_content_.begin(), plugin_file_content_.end()); // copy plugin_file_content_vec.push_back('\0'); // shouldn't reallocate From 7902c32263c5bcefcba47535688eaba69f1a848d Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Wed, 7 Jun 2023 13:38:51 -0400 Subject: [PATCH 23/46] Struct in header is now working... do not need fwd decl or the new header file --- include/scrimmage/parse/MissionParse.h | 6 +++--- include/scrimmage/simcontrol/SimControl.h | 22 ++++++++++++++++------ src/parse/MissionParse.cpp | 2 +- src/simcontrol/SimControl.cpp | 5 +++-- 4 files changed, 23 insertions(+), 12 deletions(-) diff --git a/include/scrimmage/parse/MissionParse.h b/include/scrimmage/parse/MissionParse.h index 6b18cf512..b29cef0ed 100644 --- a/include/scrimmage/parse/MissionParse.h +++ b/include/scrimmage/parse/MissionParse.h @@ -39,8 +39,8 @@ #include #include -//#include -#include +#include +//#include #include #include @@ -88,7 +88,7 @@ class MissionParse { 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); - void final_state_xml(std::list & all_end_states); + void final_state_xml(std::list & all_end_states); // Track the number of entity blocks in the input Mission XML file int num_ents = 0; diff --git a/include/scrimmage/simcontrol/SimControl.h b/include/scrimmage/simcontrol/SimControl.h index b9a754742..7589953b9 100644 --- a/include/scrimmage/simcontrol/SimControl.h +++ b/include/scrimmage/simcontrol/SimControl.h @@ -321,12 +321,22 @@ class SimControl { void set_running_in_thread(bool running_in_thread); // Natalie adding struct information - // struct ent_end_state { - // int team_id; - // double x_pos; - // double y_pos; - // double z_pos; - // } end_state; + 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::vector all_end_states; diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index 5ea50d520..74d9fd7eb 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -778,7 +778,7 @@ bool MissionParse::parse(const std::string &filename) { return true; } -void MissionParse::final_state_xml(std::list & all_end_states){ +void MissionParse::final_state_xml(std::list & all_end_states){ // Parse the xml tree. doc.parse requires a null terminated string that it can modify. std::vector mission_file_content_vec(mission_file_content_.size() + 1); // allocation done here mission_file_content_vec.assign(mission_file_content_.begin(), mission_file_content_.end()); // copy diff --git a/src/simcontrol/SimControl.cpp b/src/simcontrol/SimControl.cpp index db64a2306..d219710b5 100644 --- a/src/simcontrol/SimControl.cpp +++ b/src/simcontrol/SimControl.cpp @@ -70,7 +70,7 @@ #include -#include +//#include #include #include @@ -1075,7 +1075,8 @@ bool SimControl::shutdown(const bool& shutdown_python) { for (EntityPtr &ent : ents_) { // Create the struct for the entity end states // x_pos, y_pos, z_pos, yaw, pitch, roll, health_points, vel_x, vel_y, vel_z - ent_end_state end_state = {ent->id().team_id(), + //ent_end_state end_state = {ent->id().team_id(), + end_state = {ent->id().team_id(), ent->state()->pos()[0], ent->state()->pos()[1], ent->state()->pos()[2], ent->state()->quat().yaw(), ent->state()->quat().pitch(), ent->state()->quat().roll(), ent->health_points(), From b119eab1ab5325e0854b3624b0b07360a7f36f19 Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Wed, 7 Jun 2023 13:40:36 -0400 Subject: [PATCH 24/46] Delete EntEndStates.h --- include/scrimmage/parse/EntEndStates.h | 23 ----------------------- 1 file changed, 23 deletions(-) delete mode 100644 include/scrimmage/parse/EntEndStates.h diff --git a/include/scrimmage/parse/EntEndStates.h b/include/scrimmage/parse/EntEndStates.h deleted file mode 100644 index 0a9feca2c..000000000 --- a/include/scrimmage/parse/EntEndStates.h +++ /dev/null @@ -1,23 +0,0 @@ -#ifndef INCLUDE_SCRIMMAGE_PARSE_ENTENDSTATES_H_ -#define INCLUDE_SCRIMMAGE_PARSE_ENTENDSTATES_H_ - - - 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; - }; - -#endif \ No newline at end of file From 7e6d3cb5db249f842e077b441ab45f21255ac240 Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Wed, 7 Jun 2023 13:56:41 -0400 Subject: [PATCH 25/46] Cleanup and support for struct in simcontrol header --- include/scrimmage/parse/MissionParse.h | 1 - include/scrimmage/simcontrol/SimControl.h | 4 ++-- src/simcontrol/SimControl.cpp | 3 --- 3 files changed, 2 insertions(+), 6 deletions(-) diff --git a/include/scrimmage/parse/MissionParse.h b/include/scrimmage/parse/MissionParse.h index b29cef0ed..c8300558c 100644 --- a/include/scrimmage/parse/MissionParse.h +++ b/include/scrimmage/parse/MissionParse.h @@ -40,7 +40,6 @@ #include #include -//#include #include #include diff --git a/include/scrimmage/simcontrol/SimControl.h b/include/scrimmage/simcontrol/SimControl.h index 7589953b9..370ef8400 100644 --- a/include/scrimmage/simcontrol/SimControl.h +++ b/include/scrimmage/simcontrol/SimControl.h @@ -320,7 +320,7 @@ class SimControl { void set_running_in_thread(bool running_in_thread); - // Natalie adding struct information + /// @brief Contains the final state values of each entity struct ent_end_state { int team_id; double x_pos; @@ -338,7 +338,7 @@ class SimControl { double vel_z; } end_state; - //std::vector all_end_states; + std::list all_end_states; protected: // Key: Entity ID diff --git a/src/simcontrol/SimControl.cpp b/src/simcontrol/SimControl.cpp index d219710b5..6679518f2 100644 --- a/src/simcontrol/SimControl.cpp +++ b/src/simcontrol/SimControl.cpp @@ -1067,15 +1067,12 @@ bool SimControl::finalize() { bool SimControl::shutdown(const bool& shutdown_python) { finalize(); - std::list all_end_states; - // Close all plugins // If the mission_to_mission tag is true, store final states of each entity in a vector of structs if(miss2miss){ for (EntityPtr &ent : ents_) { // Create the struct for the entity end states // x_pos, y_pos, z_pos, yaw, pitch, roll, health_points, vel_x, vel_y, vel_z - //ent_end_state end_state = {ent->id().team_id(), end_state = {ent->id().team_id(), ent->state()->pos()[0], ent->state()->pos()[1], ent->state()->pos()[2], ent->state()->quat().yaw(), ent->state()->quat().pitch(), ent->state()->quat().roll(), From 1b53c6f09057fdddee18e4908ea59660bef28b6e Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Wed, 7 Jun 2023 14:03:27 -0400 Subject: [PATCH 26/46] Updating header comments --- include/scrimmage/parse/MissionParse.h | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/include/scrimmage/parse/MissionParse.h b/include/scrimmage/parse/MissionParse.h index c8300558c..56592a578 100644 --- a/include/scrimmage/parse/MissionParse.h +++ b/include/scrimmage/parse/MissionParse.h @@ -87,8 +87,12 @@ class MissionParse { 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 & all_end_states); - // Track the number of entity blocks in the input Mission XML file + + /// @brief Track the number of entity blocks in the input Mission XML file int num_ents = 0; bool write(const std::string &filename); From 683ef141015b9b68977b40e85bc9662258af6e48 Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Wed, 14 Jun 2023 11:16:07 -0400 Subject: [PATCH 27/46] Test commit --- include/scrimmage/parse/MissionParse.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/scrimmage/parse/MissionParse.h b/include/scrimmage/parse/MissionParse.h index 56592a578..6d80f6f23 100644 --- a/include/scrimmage/parse/MissionParse.h +++ b/include/scrimmage/parse/MissionParse.h @@ -80,7 +80,7 @@ struct GenerateInfo { bool first_in_group; double time_variance; }; - +// test class MissionParse { public: bool create_log_dir(); From d09062ec16df9b102661ba62ba5d36d2a16291ed Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Wed, 21 Jun 2023 11:50:47 -0400 Subject: [PATCH 28/46] Code clean up --- include/scrimmage/parse/MissionParse.h | 7 +++++-- include/scrimmage/simcontrol/SimControl.h | 2 ++ src/parse/MissionParse.cpp | 5 ++--- src/simcontrol/SimControl.cpp | 4 +--- 4 files changed, 10 insertions(+), 8 deletions(-) diff --git a/include/scrimmage/parse/MissionParse.h b/include/scrimmage/parse/MissionParse.h index 6d80f6f23..027b2958b 100644 --- a/include/scrimmage/parse/MissionParse.h +++ b/include/scrimmage/parse/MissionParse.h @@ -80,7 +80,7 @@ struct GenerateInfo { bool first_in_group; double time_variance; }; -// test + class MissionParse { public: bool create_log_dir(); @@ -170,14 +170,17 @@ 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 plugin_spec_attrs; - rapidxml::xml_document<> doc; double t0_ = 0; double tend_ = 50; diff --git a/include/scrimmage/simcontrol/SimControl.h b/include/scrimmage/simcontrol/SimControl.h index 370ef8400..c99c4e395 100644 --- a/include/scrimmage/simcontrol/SimControl.h +++ b/include/scrimmage/simcontrol/SimControl.h @@ -383,6 +383,8 @@ 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_; diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index 74d9fd7eb..06cf1669e 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -446,7 +446,6 @@ bool MissionParse::parse(const std::string &filename) { script_node != 0; script_node = script_node->next_sibling("entity")) { - // Tracks the number of entities in the original XML file num_ents++; std::map script_info; @@ -962,8 +961,8 @@ void MissionParse::final_state_xml(std::list & all_en miss2miss_content_out << miss2miss_file_content; miss2miss_content_out.close(); - // Create the entity end state file - std::ofstream ent_state_content_out(log_dir_+"/final_ent_states.xml"); + // Create the entity end state txt file + std::ofstream ent_state_content_out(log_dir_+"/final_ent_states.txt"); ent_state_content_out << ent_state_file_content.rdbuf(); ent_state_content_out.close(); } diff --git a/src/simcontrol/SimControl.cpp b/src/simcontrol/SimControl.cpp index 6679518f2..031931871 100644 --- a/src/simcontrol/SimControl.cpp +++ b/src/simcontrol/SimControl.cpp @@ -70,8 +70,6 @@ #include -//#include - #include #include #include @@ -1066,7 +1064,7 @@ bool SimControl::finalize() { bool SimControl::shutdown(const bool& shutdown_python) { finalize(); - + // Close all plugins // If the mission_to_mission tag is true, store final states of each entity in a vector of structs if(miss2miss){ From 495c54ac7d50b7a6f92590dd9b1a2b1d5760196a Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Fri, 23 Jun 2023 16:22:48 -0400 Subject: [PATCH 29/46] Fixing a seg fault case based on mission xml tags --- src/parse/MissionParse.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index 06cf1669e..4b9cb7863 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -779,6 +779,7 @@ bool MissionParse::parse(const std::string &filename) { void MissionParse::final_state_xml(std::list & all_end_states){ // Parse the xml tree. doc.parse requires a null terminated string that it can modify. + cout << "In the final state function" << endl; std::vector mission_file_content_vec(mission_file_content_.size() + 1); // allocation done here mission_file_content_vec.assign(mission_file_content_.begin(), mission_file_content_.end()); // copy mission_file_content_vec.push_back('\0'); // shouldn't reallocate @@ -938,11 +939,12 @@ void MissionParse::final_state_xml(std::list & all_en // Remove original entity nodes based on the bool value of the remove_block entity tag rapidxml::xml_node<> *script_node = runscript_node->first_node("entity"); for (int i = 0; ifirst_node("remove_block")->value(); - - if(script_node->first_node("remove_block") && (node_value=="false")){ - cout << "Not removing the entity block" << endl; - continue; + if(script_node->first_node("remove_block")){ + std::string node_value = script_node->first_node("remove_block")->value(); // Placed in the if statement, otherwise it will seg fault + if(node_value == "false"){ + cout << "Not removing the entity block" << endl; + continue; + } } rapidxml::xml_node<> *remove_node = script_node; From 50e62695c5d3922b383c6c002895a96118634e7b Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Mon, 26 Jun 2023 17:07:59 -0400 Subject: [PATCH 30/46] Working on getting the plugin specific xml tag values. Currently am able to get the motion model tag values through the entity class. Need to now add a similar interface for other plugins. The final step will be checking these values in the mission parse function and adding them to the mission to mission xml file --- include/scrimmage/entity/Entity.h | 8 +++++ include/scrimmage/motion/MotionModel.h | 4 +++ .../motion/SimpleAircraft/SimpleAircraft.h | 2 ++ src/entity/Entity.cpp | 34 ++++++++++++++++++- src/plugin_manager/MotionModel.cpp | 5 +++ .../motion/SimpleAircraft/SimpleAircraft.cpp | 20 +++++++++++ src/simcontrol/SimControl.cpp | 2 ++ 7 files changed, 74 insertions(+), 1 deletion(-) diff --git a/include/scrimmage/entity/Entity.h b/include/scrimmage/entity/Entity.h index caa37a9b9..7016cd5dc 100644 --- a/include/scrimmage/entity/Entity.h +++ b/include/scrimmage/entity/Entity.h @@ -138,6 +138,14 @@ class Entity : public std::enable_shared_from_this { MotionModelPtr &motion(); std::vector &controllers(); + // Mission to mission pull request + //std::vector motion_xml; + std::vector> all_motion_xml; + void set_motion_xml_vect(); + void set_sensor_xml_vect(); + void set_autonomy_xml_vect(); + void set_controller_xml_vect(); + void set_id(ID &id); ID &id(); diff --git a/include/scrimmage/motion/MotionModel.h b/include/scrimmage/motion/MotionModel.h index 1fdf2ce45..30db7e26f 100644 --- a/include/scrimmage/motion/MotionModel.h +++ b/include/scrimmage/motion/MotionModel.h @@ -56,6 +56,10 @@ class MotionModel : public EntityPlugin { std::map ¶ms); virtual bool step(double time, double dt); + + // Mission to mission function + virtual std::map mission_xml_set(); + virtual bool posthumous(double t); virtual StatePtr &state(); virtual void set_state(StatePtr &state); diff --git a/include/scrimmage/plugins/motion/SimpleAircraft/SimpleAircraft.h b/include/scrimmage/plugins/motion/SimpleAircraft/SimpleAircraft.h index 6156d7b45..317b699f0 100644 --- a/include/scrimmage/plugins/motion/SimpleAircraft/SimpleAircraft.h +++ b/include/scrimmage/plugins/motion/SimpleAircraft/SimpleAircraft.h @@ -61,6 +61,8 @@ class SimpleAircraft : public scrimmage::MotionModel{ void teleport(scrimmage::StatePtr &state) override; + std::map mission_xml_set() override; + protected: scrimmage::PID heading_pid_; scrimmage::PID alt_pid_; diff --git a/src/entity/Entity.cpp b/src/entity/Entity.cpp index 92a8e05ea..7438bbd18 100644 --- a/src/entity/Entity.cpp +++ b/src/entity/Entity.cpp @@ -730,4 +730,36 @@ void Entity::print_plugins(std::ostream &out) const { 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 + +void Entity::set_motion_xml_vect(){ + std::map cur_mm_xml; + if (motion_model_ && motion_model_->name() != "BLANK") { + cur_mm_xml = motion_model_->mission_xml_set(); + } + //all_motion_xml.push_back(motion_xml); + + for (auto itr = cur_mm_xml.begin(); itr != cur_mm_xml.end(); ++itr) { + std::cout << "1. " << itr->first << " 2. " << itr->second << std::endl; + } +} + +void Entity::set_sensor_xml_vect(){ + return; +} + +void Entity::set_autonomy_xml_vect(){ + return; +} + +void Entity::set_controller_xml_vect(){ + return; +} + +} // namespace scrimmage \ No newline at end of file diff --git a/src/plugin_manager/MotionModel.cpp b/src/plugin_manager/MotionModel.cpp index 7710ec946..bca1b42d3 100644 --- a/src/plugin_manager/MotionModel.cpp +++ b/src/plugin_manager/MotionModel.cpp @@ -48,6 +48,11 @@ bool MotionModel::init(std::map &info, std::map MotionModel::mission_xml_set() { + std::map mission_xml; + return mission_xml; +} + bool MotionModel::posthumous(double t) { return true; } StatePtr &MotionModel::state() {return state_;} diff --git a/src/plugins/motion/SimpleAircraft/SimpleAircraft.cpp b/src/plugins/motion/SimpleAircraft/SimpleAircraft.cpp index 4f6da8154..bed3f3836 100644 --- a/src/plugins/motion/SimpleAircraft/SimpleAircraft.cpp +++ b/src/plugins/motion/SimpleAircraft/SimpleAircraft.cpp @@ -106,6 +106,26 @@ bool SimpleAircraft::init(std::map &info, return true; } +// Misison to mission testing +std::map SimpleAircraft::mission_xml_set() { + std::map mission_xml; + + // I actually think it would make more sense to do a vector of maps, + // so that I can relate the xml tag name to the value duhhhhhhh + 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; +} +// End of mission to mission testing + bool SimpleAircraft::step(double time, double dt) { // Need to saturate state variables before model runs x_[ROLL] = clamp(x_[ROLL], -max_roll_, max_roll_); diff --git a/src/simcontrol/SimControl.cpp b/src/simcontrol/SimControl.cpp index 031931871..828e7d501 100644 --- a/src/simcontrol/SimControl.cpp +++ b/src/simcontrol/SimControl.cpp @@ -1078,6 +1078,8 @@ bool SimControl::shutdown(const bool& shutdown_python) { ent->state()->vel()[0], ent->state()->vel()[1], ent->state()->vel()[2]}; all_end_states.push_back(end_state); + //ent->print_plugins(std::cout); + ent->set_motion_xml_vect(); ent->close(t()); } } else { From 300567f9cf16cd3c678f374d18dafd6017aad1af Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Mon, 26 Jun 2023 17:57:35 -0400 Subject: [PATCH 31/46] Function working to get the xml tag values from the autonomy plugin --- include/scrimmage/autonomy/Autonomy.h | 3 +++ include/scrimmage/entity/Entity.h | 3 +-- .../plugins/autonomy/Straight/Straight.h | 1 + src/autonomy/Autonomy.cpp | 5 +++++ src/entity/Entity.cpp | 21 +++++++++++++------ src/plugins/autonomy/Straight/Straight.cpp | 18 ++++++++++++++++ .../motion/SimpleAircraft/SimpleAircraft.cpp | 1 + src/simcontrol/SimControl.cpp | 1 + 8 files changed, 45 insertions(+), 8 deletions(-) diff --git a/include/scrimmage/autonomy/Autonomy.h b/include/scrimmage/autonomy/Autonomy.h index 8760bfe45..1a467c3d8 100644 --- a/include/scrimmage/autonomy/Autonomy.h +++ b/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 mission_xml_set(); + // getters/setters StatePtr &desired_state(); void set_desired_state(StatePtr desired_state); diff --git a/include/scrimmage/entity/Entity.h b/include/scrimmage/entity/Entity.h index 7016cd5dc..645959241 100644 --- a/include/scrimmage/entity/Entity.h +++ b/include/scrimmage/entity/Entity.h @@ -139,12 +139,11 @@ class Entity : public std::enable_shared_from_this { std::vector &controllers(); // Mission to mission pull request - //std::vector motion_xml; - std::vector> all_motion_xml; void set_motion_xml_vect(); void set_sensor_xml_vect(); void set_autonomy_xml_vect(); void set_controller_xml_vect(); + // End of mission to mission void set_id(ID &id); ID &id(); diff --git a/include/scrimmage/plugins/autonomy/Straight/Straight.h b/include/scrimmage/plugins/autonomy/Straight/Straight.h index 656ec0ca3..2bba03cb8 100644 --- a/include/scrimmage/plugins/autonomy/Straight/Straight.h +++ b/include/scrimmage/plugins/autonomy/Straight/Straight.h @@ -52,6 +52,7 @@ class Straight : public scrimmage::Autonomy{ public: void init(std::map ¶ms) override; bool step_autonomy(double t, double dt) override; + std::map mission_xml_set() override; protected: double speed_; diff --git a/src/autonomy/Autonomy.cpp b/src/autonomy/Autonomy.cpp index 6af0b9375..eef37d570 100644 --- a/src/autonomy/Autonomy.cpp +++ b/src/autonomy/Autonomy.cpp @@ -78,6 +78,11 @@ void Autonomy::init() {} void Autonomy::init(std::map &/*params*/) {} bool Autonomy::need_reset() {return need_reset_;} +std::map Autonomy::mission_xml_set() { + std::map mission_xml; + return mission_xml; +} + StatePtr &Autonomy::desired_state() {return desired_state_;} void Autonomy::set_desired_state(StatePtr desired_state) {desired_state_ = desired_state;} diff --git a/src/entity/Entity.cpp b/src/entity/Entity.cpp index 7438bbd18..24c632329 100644 --- a/src/entity/Entity.cpp +++ b/src/entity/Entity.cpp @@ -742,11 +742,10 @@ void Entity::set_motion_xml_vect(){ std::map cur_mm_xml; if (motion_model_ && motion_model_->name() != "BLANK") { cur_mm_xml = motion_model_->mission_xml_set(); - } - //all_motion_xml.push_back(motion_xml); - - for (auto itr = cur_mm_xml.begin(); itr != cur_mm_xml.end(); ++itr) { - std::cout << "1. " << itr->first << " 2. " << itr->second << std::endl; + + for (auto itr = cur_mm_xml.begin(); itr != cur_mm_xml.end(); ++itr) { + std::cout << "Motion Model: 1. " << itr->first << " 2. " << itr->second << std::endl; + } } } @@ -755,7 +754,17 @@ void Entity::set_sensor_xml_vect(){ } void Entity::set_autonomy_xml_vect(){ - return; + std::vector> all_autonomy_xml; + std::map cur_aut_xml; + + for (AutonomyPtr a : autonomies_) { + cur_aut_xml = a->mission_xml_set(); + all_autonomy_xml.push_back(cur_aut_xml); + + for (auto itr = cur_aut_xml.begin(); itr != cur_aut_xml.end(); ++itr) { + std::cout << "Autonomy: 1. " << itr->first << " 2. " << itr->second << std::endl; + } + } } void Entity::set_controller_xml_vect(){ diff --git a/src/plugins/autonomy/Straight/Straight.cpp b/src/plugins/autonomy/Straight/Straight.cpp index 8d714d3be..b04a508f8 100644 --- a/src/plugins/autonomy/Straight/Straight.cpp +++ b/src/plugins/autonomy/Straight/Straight.cpp @@ -213,6 +213,24 @@ void Straight::init(std::map ¶ms) { desired_heading_idx_ = vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::Out); } +// Misison to mission testing +std::map Straight::mission_xml_set() { + std::map mission_xml; + + // I actually think it would make more sense to do a vector of maps, + // so that I can relate the xml tag name to the value duhhhhhhh + mission_xml.insert({"Autonomy Plugin Name","Straight"}); + mission_xml.insert({"speed",std::to_string(speed_)}); + mission_xml.insert({"show_camera_images",std::to_string(show_camera_images_)}); + mission_xml.insert({"save_camera_images",std::to_string(save_camera_images_)}); + mission_xml.insert({"show_text_label",std::to_string(show_text_label_)}); + mission_xml.insert({"enable_boundary_control",std::to_string(enable_boundary_control_)}); + mission_xml.insert({"generate_entities",std::to_string(gen_ents_)}); + + return mission_xml; +} +// End of mission to mission testing + bool Straight::step_autonomy(double t, double dt) { if (gen_ents_) { if (time_->t() > (prev_gen_time_ + 2.0)) { diff --git a/src/plugins/motion/SimpleAircraft/SimpleAircraft.cpp b/src/plugins/motion/SimpleAircraft/SimpleAircraft.cpp index bed3f3836..08613468f 100644 --- a/src/plugins/motion/SimpleAircraft/SimpleAircraft.cpp +++ b/src/plugins/motion/SimpleAircraft/SimpleAircraft.cpp @@ -112,6 +112,7 @@ std::map SimpleAircraft::mission_xml_set() { // I actually think it would make more sense to do a vector of maps, // so that I can relate the xml tag name to the value duhhhhhhh + mission_xml.insert({"Motion Model Plugin 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_)}); diff --git a/src/simcontrol/SimControl.cpp b/src/simcontrol/SimControl.cpp index 828e7d501..b3521a662 100644 --- a/src/simcontrol/SimControl.cpp +++ b/src/simcontrol/SimControl.cpp @@ -1080,6 +1080,7 @@ bool SimControl::shutdown(const bool& shutdown_python) { //ent->print_plugins(std::cout); ent->set_motion_xml_vect(); + ent->set_autonomy_xml_vect(); ent->close(t()); } } else { From 5115e3b6550a0751f2aeb6d5947bb391c77b3dc3 Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Wed, 28 Jun 2023 17:11:01 -0400 Subject: [PATCH 32/46] Rapid xml is working, writing plugin specific tags to the motion model node. Need to add autonomy, sensor, and controller plugin tags now. --- include/scrimmage/entity/Entity.h | 8 +- include/scrimmage/motion/Controller.h | 7 ++ .../SimpleAircraftControllerPID.h | 2 + include/scrimmage/sensor/Sensor.h | 6 ++ include/scrimmage/simcontrol/SimControl.h | 6 ++ src/entity/Entity.cpp | 86 +++++++++++++++---- src/parse/MissionParse.cpp | 29 ++++++- src/plugins/autonomy/Straight/Straight.cpp | 2 +- .../SimpleAircraftControllerPID.cpp | 18 ++++ src/simcontrol/SimControl.cpp | 14 ++- 10 files changed, 149 insertions(+), 29 deletions(-) diff --git a/include/scrimmage/entity/Entity.h b/include/scrimmage/entity/Entity.h index 645959241..9e7f23ac5 100644 --- a/include/scrimmage/entity/Entity.h +++ b/include/scrimmage/entity/Entity.h @@ -139,10 +139,10 @@ class Entity : public std::enable_shared_from_this { std::vector &controllers(); // Mission to mission pull request - void set_motion_xml_vect(); - void set_sensor_xml_vect(); - void set_autonomy_xml_vect(); - void set_controller_xml_vect(); + std::map set_motion_xml_vect(); + std::vector> set_sensor_xml_vect(); + std::vector> set_autonomy_xml_vect(); + std::vector> set_controller_xml_vect(); // End of mission to mission void set_id(ID &id); diff --git a/include/scrimmage/motion/Controller.h b/include/scrimmage/motion/Controller.h index d97a3da21..61bd384d6 100644 --- a/include/scrimmage/motion/Controller.h +++ b/include/scrimmage/motion/Controller.h @@ -52,6 +52,13 @@ class Controller : public EntityPlugin { desired_state_ = nullptr; } + // Mission to mission function + virtual std::map mission_xml_set() { + std::map mission_xml; + return mission_xml; + }; + + protected: StatePtr state_; StatePtr desired_state_; diff --git a/include/scrimmage/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.h b/include/scrimmage/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.h index df928bca6..36746a88f 100644 --- a/include/scrimmage/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.h +++ b/include/scrimmage/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.h @@ -54,6 +54,8 @@ class SimpleAircraftControllerPID : public Controller { public: virtual void init(std::map ¶ms); virtual bool step(double t, double dt); + std::map mission_xml_set() override; + protected: scrimmage::PID heading_pid_; diff --git a/include/scrimmage/sensor/Sensor.h b/include/scrimmage/sensor/Sensor.h index f56eede6e..e2f6eb038 100644 --- a/include/scrimmage/sensor/Sensor.h +++ b/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 mission_xml_set() { + std::map mission_xml; + return mission_xml; + }; + virtual scrimmage::MessageBasePtr sensor_msg(double t); /*! \brief version when T = MessageBase (calls sensor_msg without casting) */ diff --git a/include/scrimmage/simcontrol/SimControl.h b/include/scrimmage/simcontrol/SimControl.h index c99c4e395..c13fd3678 100644 --- a/include/scrimmage/simcontrol/SimControl.h +++ b/include/scrimmage/simcontrol/SimControl.h @@ -336,6 +336,12 @@ class SimControl { double vel_x; double vel_y; double vel_z; + + std::map motion_xml_tags; + std::vector> autonomy_xml_tags; + std::vector> controller_xml_tags; + std::vector> sensor_xml_tags; + } end_state; std::list all_end_states; diff --git a/src/entity/Entity.cpp b/src/entity/Entity.cpp index 24c632329..8ff5838cf 100644 --- a/src/entity/Entity.cpp +++ b/src/entity/Entity.cpp @@ -737,38 +737,88 @@ void Entity::print_plugins(std::ostream &out) const { // 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 -void Entity::set_motion_xml_vect(){ - std::map cur_mm_xml; +// 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 Entity::set_motion_xml_vect(){ + std::map cur_motion_xml; if (motion_model_ && motion_model_->name() != "BLANK") { - cur_mm_xml = motion_model_->mission_xml_set(); + cur_motion_xml = motion_model_->mission_xml_set(); - for (auto itr = cur_mm_xml.begin(); itr != cur_mm_xml.end(); ++itr) { - std::cout << "Motion Model: 1. " << itr->first << " 2. " << itr->second << std::endl; - } + // 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; } -void Entity::set_sensor_xml_vect(){ - return; +std::vector> Entity::set_sensor_xml_vect(){ + std::vector> all_sensor_xml; + std::map 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 << "Autonomy vector #: " << i << std::endl; + + // // Traverse the map + // for (auto itr : all_sensor_xml[i]) + // std::cout << "Autonomy: 1. " << itr.first << " 2. " << itr.second << std::endl; + // } + + return all_sensor_xml; } -void Entity::set_autonomy_xml_vect(){ +std::vector> Entity::set_autonomy_xml_vect(){ std::vector> all_autonomy_xml; - std::map cur_aut_xml; + std::map cur_autonomy_xml; for (AutonomyPtr a : autonomies_) { - cur_aut_xml = a->mission_xml_set(); - all_autonomy_xml.push_back(cur_aut_xml); - - for (auto itr = cur_aut_xml.begin(); itr != cur_aut_xml.end(); ++itr) { - std::cout << "Autonomy: 1. " << itr->first << " 2. " << itr->second << std::endl; - } + 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; } -void Entity::set_controller_xml_vect(){ - return; +std::vector> Entity::set_controller_xml_vect(){ + std::vector> all_controller_xml; + std::map 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 << "Autonomy vector #: " << i << std::endl; + + // // Traverse the map + // for (auto itr : all_controller_xml[i]) + // std::cout << "Autonomy: 1. " << itr.first << " 2. " << itr.second << std::endl; + // } + + return all_controller_xml; } } // namespace scrimmage \ No newline at end of file diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index 4b9cb7863..36e5d10d7 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -926,8 +926,33 @@ void MissionParse::final_state_xml(std::list & all_en if(new_ent->first_node("generate_time_variance")){ new_ent->remove_node(new_ent->first_node("generate_time_variance")); } - - // Adds the new entity node to the main XML tree + + // Handle plugin specific xml tags + for (rapidxml::xml_node<> *node = new_ent->first_node(); node != 0; node = node->next_sibling()) { + std::string nm = node->name(); + + cout << "Node name: " << nm << " Node value: " << node->value() << endl; + + // Will need to check the name of the given autonomy, controller, or sensor. Do not need to check name for the + // motion model, since there is just one. To do this, compare the node->value() to the name in the struct's map + if (nm == "autonomy"){ + + } else if (nm == "motion_model"){ + for(auto itr = cur_ent.motion_xml_tags.begin(); itr != cur_ent.motion_xml_tags.end(); ++itr){ + char *attribute_name = doc.allocate_string(itr->first.c_str()); + char *attribute_value = doc.allocate_string(itr->second.c_str()); + rapidxml::xml_attribute <> *tempattr = doc.allocate_attribute(attribute_name, attribute_value); + node->append_attribute(tempattr); + } + } else if (nm == "controller"){ + + } else if (nm == "sensor"){ + + } + + } + + // Adds the new entity node to the main xml tree doc.first_node("runscript")->append_node(new_ent); // If a new node is added, break to the next entity in the list of structs diff --git a/src/plugins/autonomy/Straight/Straight.cpp b/src/plugins/autonomy/Straight/Straight.cpp index b04a508f8..05d9bd23d 100644 --- a/src/plugins/autonomy/Straight/Straight.cpp +++ b/src/plugins/autonomy/Straight/Straight.cpp @@ -218,7 +218,7 @@ std::map Straight::mission_xml_set() { std::map mission_xml; // I actually think it would make more sense to do a vector of maps, - // so that I can relate the xml tag name to the value duhhhhhhh + // so that I can relate the xml tag name to the value mission_xml.insert({"Autonomy Plugin Name","Straight"}); mission_xml.insert({"speed",std::to_string(speed_)}); mission_xml.insert({"show_camera_images",std::to_string(show_camera_images_)}); diff --git a/src/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.cpp b/src/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.cpp index ef20409d9..23d07f9cd 100644 --- a/src/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.cpp +++ b/src/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.cpp @@ -77,6 +77,24 @@ void SimpleAircraftControllerPID::init(std::map ¶m output_pitch_rate_idx_ = vars_.declare(VariableIO::Type::pitch_rate, VariableIO::Direction::Out); } +// Misison to mission testing +std::map SimpleAircraftControllerPID::mission_xml_set() { + std::map mission_xml; + + // I actually think it would make more sense to do a vector of maps, + // so that I can relate the xml tag name to the value + mission_xml.insert({"Controller Plugin Name","SimpleAircraftControllerPID"}); + // mission_xml.insert({"heading_pid",std::to_string()}); + // mission_xml.insert({"show_camera_images",std::to_string(show_camera_images_)}); + // mission_xml.insert({"save_camera_images",std::to_string(save_camera_images_)}); + // mission_xml.insert({"show_text_label",std::to_string(show_text_label_)}); + // mission_xml.insert({"enable_boundary_control",std::to_string(enable_boundary_control_)}); + // mission_xml.insert({"generate_entities",std::to_string(gen_ents_)}); + + return mission_xml; +} +// End of mission to mission testing + bool SimpleAircraftControllerPID::step(double t, double dt) { heading_pid_.set_setpoint(vars_.input(input_roll_or_heading_idx_)); double u_roll_rate = use_roll_ ? diff --git a/src/simcontrol/SimControl.cpp b/src/simcontrol/SimControl.cpp index b3521a662..ca6fe7175 100644 --- a/src/simcontrol/SimControl.cpp +++ b/src/simcontrol/SimControl.cpp @@ -1069,18 +1069,24 @@ bool SimControl::shutdown(const bool& shutdown_python) { // If the mission_to_mission tag is true, store final states of each entity in a vector of structs if(miss2miss){ for (EntityPtr &ent : ents_) { + + // Get the vectors of all entity specific plugin xml tags + std::map motion_xml_vect = ent->set_motion_xml_vect(); + std::vector> autonomy_xml_vect = ent->set_autonomy_xml_vect(); + std::vector> controller_xml_vect = ent->set_controller_xml_vect(); + std::vector> sensor_xml_vect = ent->set_sensor_xml_vect(); + // Create the struct for the entity end states // x_pos, y_pos, z_pos, yaw, pitch, roll, health_points, vel_x, vel_y, vel_z end_state = {ent->id().team_id(), ent->state()->pos()[0], ent->state()->pos()[1], ent->state()->pos()[2], ent->state()->quat().yaw(), ent->state()->quat().pitch(), ent->state()->quat().roll(), ent->health_points(), - ent->state()->vel()[0], ent->state()->vel()[1], ent->state()->vel()[2]}; + ent->state()->vel()[0], ent->state()->vel()[1], ent->state()->vel()[2], + motion_xml_vect, autonomy_xml_vect, controller_xml_vect, sensor_xml_vect}; + all_end_states.push_back(end_state); - //ent->print_plugins(std::cout); - ent->set_motion_xml_vect(); - ent->set_autonomy_xml_vect(); ent->close(t()); } } else { From 44a8fd7a037a45a09430e6423b8bec5bb9b8d57d Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Fri, 30 Jun 2023 16:27:30 -0400 Subject: [PATCH 33/46] Fully working for plugin specific xml tags populating the mission to mission file --- src/entity/Entity.cpp | 8 +- src/parse/MissionParse.cpp | 86 +++++++++++++++++-- src/plugins/autonomy/Straight/Straight.cpp | 2 +- .../SimpleAircraftControllerPID.cpp | 2 +- .../motion/SimpleAircraft/SimpleAircraft.cpp | 2 +- 5 files changed, 85 insertions(+), 15 deletions(-) diff --git a/src/entity/Entity.cpp b/src/entity/Entity.cpp index 8ff5838cf..c7f6ebf87 100644 --- a/src/entity/Entity.cpp +++ b/src/entity/Entity.cpp @@ -769,11 +769,11 @@ std::vector> Entity::set_sensor_xml_vect(){ // 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 << "Autonomy vector #: " << i << std::endl; + // std::cout << "Sensor vector #: " << i << std::endl; // // Traverse the map // for (auto itr : all_sensor_xml[i]) - // std::cout << "Autonomy: 1. " << itr.first << " 2. " << itr.second << std::endl; + // std::cout << "Sensor: 1. " << itr.first << " 2. " << itr.second << std::endl; // } return all_sensor_xml; @@ -811,11 +811,11 @@ std::vector> Entity::set_controller_xml_vect() // 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 << "Autonomy vector #: " << i << std::endl; + // std::cout << "Controller vector #: " << i << std::endl; // // Traverse the map // for (auto itr : all_controller_xml[i]) - // std::cout << "Autonomy: 1. " << itr.first << " 2. " << itr.second << std::endl; + // std::cout << "Controller: 1. " << itr.first << " 2. " << itr.second << std::endl; // } return all_controller_xml; diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index 36e5d10d7..0274945ae 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -930,24 +930,95 @@ void MissionParse::final_state_xml(std::list & all_en // Handle plugin specific xml tags for (rapidxml::xml_node<> *node = new_ent->first_node(); node != 0; node = node->next_sibling()) { std::string nm = node->name(); + std::string nv = node->value(); - cout << "Node name: " << nm << " Node value: " << node->value() << endl; - - // Will need to check the name of the given autonomy, controller, or sensor. Do not need to check name for the - // motion model, since there is just one. To do this, compare the node->value() to the name in the struct's map if (nm == "autonomy"){ - + for (int i = 0; i < cur_ent.autonomy_xml_tags.size(); i++) { + // Traverse the map + for (auto itr : cur_ent.autonomy_xml_tags[i]){ + if(itr.first == "Name"){ + if(nv != itr.second){ + break; + } + else{ + continue; + } + } + + if(node->first_attribute(itr.first.c_str())){ + continue; + } + + char *attribute_name = doc.allocate_string(itr.first.c_str()); + char *attribute_value = doc.allocate_string(itr.second.c_str()); + rapidxml::xml_attribute <> *tempattr = doc.allocate_attribute(attribute_name, attribute_value); + node->append_attribute(tempattr); + } + + } } else if (nm == "motion_model"){ for(auto itr = cur_ent.motion_xml_tags.begin(); itr != cur_ent.motion_xml_tags.end(); ++itr){ + if(itr->first == "Name"){ + continue; + } + + if(node->first_attribute(itr->first.c_str())){ + continue; + } + char *attribute_name = doc.allocate_string(itr->first.c_str()); char *attribute_value = doc.allocate_string(itr->second.c_str()); rapidxml::xml_attribute <> *tempattr = doc.allocate_attribute(attribute_name, attribute_value); node->append_attribute(tempattr); } } else if (nm == "controller"){ - + for (int i = 0; i < cur_ent.controller_xml_tags.size(); i++) { + // Traverse the map + for (auto itr : cur_ent.controller_xml_tags[i]){ + if(itr.first == "Name"){ + if(nv != itr.second){ + break; + } + else{ + continue; + } + } + + if(node->first_attribute(itr.first.c_str())){ + continue; + } + + char *attribute_name = doc.allocate_string(itr.first.c_str()); + char *attribute_value = doc.allocate_string(itr.second.c_str()); + rapidxml::xml_attribute <> *tempattr = doc.allocate_attribute(attribute_name, attribute_value); + node->append_attribute(tempattr); + } + + } } else if (nm == "sensor"){ - + for (int i = 0; i < cur_ent.sensor_xml_tags.size(); i++) { + // Traverse the map + for (auto itr : cur_ent.sensor_xml_tags[i]){ + if(itr.first == "Name"){ + if(nv != itr.second){ + break; + } + else{ + continue; + } + } + + if(node->first_attribute(itr.first.c_str())){ + continue; + } + + char *attribute_name = doc.allocate_string(itr.first.c_str()); + char *attribute_value = doc.allocate_string(itr.second.c_str()); + rapidxml::xml_attribute <> *tempattr = doc.allocate_attribute(attribute_name, attribute_value); + node->append_attribute(tempattr); + } + + } } } @@ -967,7 +1038,6 @@ void MissionParse::final_state_xml(std::list & all_en if(script_node->first_node("remove_block")){ std::string node_value = script_node->first_node("remove_block")->value(); // Placed in the if statement, otherwise it will seg fault if(node_value == "false"){ - cout << "Not removing the entity block" << endl; continue; } } diff --git a/src/plugins/autonomy/Straight/Straight.cpp b/src/plugins/autonomy/Straight/Straight.cpp index 05d9bd23d..a20e9b9fa 100644 --- a/src/plugins/autonomy/Straight/Straight.cpp +++ b/src/plugins/autonomy/Straight/Straight.cpp @@ -219,7 +219,7 @@ std::map Straight::mission_xml_set() { // I actually think it would make more sense to do a vector of maps, // so that I can relate the xml tag name to the value - mission_xml.insert({"Autonomy Plugin Name","Straight"}); + mission_xml.insert({"Name","Straight"}); mission_xml.insert({"speed",std::to_string(speed_)}); mission_xml.insert({"show_camera_images",std::to_string(show_camera_images_)}); mission_xml.insert({"save_camera_images",std::to_string(save_camera_images_)}); diff --git a/src/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.cpp b/src/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.cpp index 23d07f9cd..d39910f35 100644 --- a/src/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.cpp +++ b/src/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.cpp @@ -83,7 +83,7 @@ std::map SimpleAircraftControllerPID::mission_xml_set() // I actually think it would make more sense to do a vector of maps, // so that I can relate the xml tag name to the value - mission_xml.insert({"Controller Plugin Name","SimpleAircraftControllerPID"}); + mission_xml.insert({"Name","SimpleAircraftControllerPID"}); // mission_xml.insert({"heading_pid",std::to_string()}); // mission_xml.insert({"show_camera_images",std::to_string(show_camera_images_)}); // mission_xml.insert({"save_camera_images",std::to_string(save_camera_images_)}); diff --git a/src/plugins/motion/SimpleAircraft/SimpleAircraft.cpp b/src/plugins/motion/SimpleAircraft/SimpleAircraft.cpp index 08613468f..6463f41d9 100644 --- a/src/plugins/motion/SimpleAircraft/SimpleAircraft.cpp +++ b/src/plugins/motion/SimpleAircraft/SimpleAircraft.cpp @@ -112,7 +112,7 @@ std::map SimpleAircraft::mission_xml_set() { // I actually think it would make more sense to do a vector of maps, // so that I can relate the xml tag name to the value duhhhhhhh - mission_xml.insert({"Motion Model Plugin Name","SimpleAircraft"}); + 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_)}); From 5ae9f77990f52d724e6f7ec193141658300b7272 Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Fri, 30 Jun 2023 16:45:54 -0400 Subject: [PATCH 34/46] Fixed so that the plugin xml tags that are from the function overwrite the originals of the mission xml file --- src/parse/MissionParse.cpp | 45 +++++++++++++++++++++----------------- 1 file changed, 25 insertions(+), 20 deletions(-) diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index 0274945ae..6e096ca6f 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -944,15 +944,17 @@ void MissionParse::final_state_xml(std::list & all_en continue; } } - - if(node->first_attribute(itr.first.c_str())){ - continue; - } char *attribute_name = doc.allocate_string(itr.first.c_str()); char *attribute_value = doc.allocate_string(itr.second.c_str()); rapidxml::xml_attribute <> *tempattr = doc.allocate_attribute(attribute_name, attribute_value); - node->append_attribute(tempattr); + //node->append_attribute(tempattr); + if(node->first_attribute(itr.first.c_str())){ + node->insert_attribute(node->first_attribute(itr.first.c_str()),tempattr); + node->remove_attribute(node->first_attribute(itr.first.c_str())->next_attribute()); + } else { + node->append_attribute(tempattr); + } } } @@ -962,14 +964,15 @@ void MissionParse::final_state_xml(std::list & all_en continue; } - if(node->first_attribute(itr->first.c_str())){ - continue; - } - char *attribute_name = doc.allocate_string(itr->first.c_str()); char *attribute_value = doc.allocate_string(itr->second.c_str()); rapidxml::xml_attribute <> *tempattr = doc.allocate_attribute(attribute_name, attribute_value); - node->append_attribute(tempattr); + if(node->first_attribute(itr->first.c_str())){ + node->insert_attribute(node->first_attribute(itr->first.c_str()),tempattr); + node->remove_attribute(node->first_attribute(itr->first.c_str())->next_attribute()); + } else { + node->append_attribute(tempattr); + } } } else if (nm == "controller"){ for (int i = 0; i < cur_ent.controller_xml_tags.size(); i++) { @@ -984,14 +987,15 @@ void MissionParse::final_state_xml(std::list & all_en } } - if(node->first_attribute(itr.first.c_str())){ - continue; - } - char *attribute_name = doc.allocate_string(itr.first.c_str()); char *attribute_value = doc.allocate_string(itr.second.c_str()); rapidxml::xml_attribute <> *tempattr = doc.allocate_attribute(attribute_name, attribute_value); - node->append_attribute(tempattr); + if(node->first_attribute(itr.first.c_str())){ + node->insert_attribute(node->first_attribute(itr.first.c_str()),tempattr); + node->remove_attribute(node->first_attribute(itr.first.c_str())->next_attribute()); + } else { + node->append_attribute(tempattr); + } } } @@ -1008,14 +1012,15 @@ void MissionParse::final_state_xml(std::list & all_en } } - if(node->first_attribute(itr.first.c_str())){ - continue; - } - char *attribute_name = doc.allocate_string(itr.first.c_str()); char *attribute_value = doc.allocate_string(itr.second.c_str()); rapidxml::xml_attribute <> *tempattr = doc.allocate_attribute(attribute_name, attribute_value); - node->append_attribute(tempattr); + if(node->first_attribute(itr.first.c_str())){ + node->insert_attribute(node->first_attribute(itr.first.c_str()),tempattr); + node->remove_attribute(node->first_attribute(itr.first.c_str())->next_attribute()); + } else { + node->append_attribute(tempattr); + } } } From 35035aecdb442c5a1d19eb446020c974c14cf47c Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Wed, 5 Jul 2023 12:21:59 -0400 Subject: [PATCH 35/46] Fixed remove block tag logic --- src/parse/MissionParse.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index 6e096ca6f..802b3710b 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -948,7 +948,6 @@ void MissionParse::final_state_xml(std::list & all_en char *attribute_name = doc.allocate_string(itr.first.c_str()); char *attribute_value = doc.allocate_string(itr.second.c_str()); rapidxml::xml_attribute <> *tempattr = doc.allocate_attribute(attribute_name, attribute_value); - //node->append_attribute(tempattr); if(node->first_attribute(itr.first.c_str())){ node->insert_attribute(node->first_attribute(itr.first.c_str()),tempattr); node->remove_attribute(node->first_attribute(itr.first.c_str())->next_attribute()); @@ -1039,14 +1038,20 @@ void MissionParse::final_state_xml(std::list & all_en // Remove original entity nodes based on the bool value of the remove_block entity tag rapidxml::xml_node<> *script_node = runscript_node->first_node("entity"); - for (int i = 0; ifirst_node("remove_block")){ std::string node_value = script_node->first_node("remove_block")->value(); // Placed in the if statement, otherwise it will seg fault + cout << "Remove block is present" << endl; if(node_value == "false"){ + cout << "Continuing, remove block node value is false" << endl; + script_node = script_node->next_sibling("entity"); continue; } } + cout << "Removing the node" << endl; + rapidxml::xml_node<> *remove_node = script_node; script_node = script_node->next_sibling("entity"); doc.first_node("runscript")->remove_node(remove_node); From d54a383d19ceb736d7e2cbfbc616cdfaba7ed0d7 Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Thu, 6 Jul 2023 11:31:36 -0400 Subject: [PATCH 36/46] Code cleanup and adding briefs --- include/scrimmage/autonomy/Autonomy.h | 10 +++- include/scrimmage/entity/Entity.h | 14 ++++- include/scrimmage/motion/Controller.h | 8 ++- include/scrimmage/motion/MotionModel.h | 8 ++- .../plugins/autonomy/Straight/Straight.h | 2 +- .../SimpleAircraftControllerPID.h | 2 +- .../motion/SimpleAircraft/SimpleAircraft.h | 2 +- include/scrimmage/sensor/Sensor.h | 8 ++- missions/straight.xml | 33 +++-------- src/autonomy/Autonomy.cpp | 2 +- src/entity/Entity.cpp | 56 +++---------------- src/parse/MissionParse.cpp | 10 +--- src/plugin_manager/MotionModel.cpp | 2 +- src/plugins/autonomy/Straight/Straight.cpp | 2 +- .../SimpleAircraftControllerPID.cpp | 2 +- .../motion/SimpleAircraft/SimpleAircraft.cpp | 2 +- src/simcontrol/SimControl.cpp | 9 ++- 17 files changed, 67 insertions(+), 105 deletions(-) diff --git a/include/scrimmage/autonomy/Autonomy.h b/include/scrimmage/autonomy/Autonomy.h index 1a467c3d8..37e91c288 100644 --- a/include/scrimmage/autonomy/Autonomy.h +++ b/include/scrimmage/autonomy/Autonomy.h @@ -56,13 +56,17 @@ class Autonomy : public EntityPlugin { void close(double t) override; bool need_reset(); - // Mission to mission function - virtual std::map mission_xml_set(); - // getters/setters 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 mission_xml_get(); + ContactMapPtr &get_contacts(); ContactMap &get_contacts_raw(); virtual void set_contacts(ContactMapPtr &contacts); diff --git a/include/scrimmage/entity/Entity.h b/include/scrimmage/entity/Entity.h index 9e7f23ac5..3944a1fbc 100644 --- a/include/scrimmage/entity/Entity.h +++ b/include/scrimmage/entity/Entity.h @@ -138,12 +138,20 @@ class Entity : public std::enable_shared_from_this { MotionModelPtr &motion(); std::vector &controllers(); - // Mission to mission pull request - std::map set_motion_xml_vect(); + /// @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 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> set_sensor_xml_vect(); std::vector> set_autonomy_xml_vect(); std::vector> set_controller_xml_vect(); - // End of mission to mission void set_id(ID &id); ID &id(); diff --git a/include/scrimmage/motion/Controller.h b/include/scrimmage/motion/Controller.h index 61bd384d6..e6ceef8cd 100644 --- a/include/scrimmage/motion/Controller.h +++ b/include/scrimmage/motion/Controller.h @@ -52,8 +52,12 @@ class Controller : public EntityPlugin { desired_state_ = nullptr; } - // Mission to mission function - virtual std::map mission_xml_set() { + /// @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 mission_xml_get() { std::map mission_xml; return mission_xml; }; diff --git a/include/scrimmage/motion/MotionModel.h b/include/scrimmage/motion/MotionModel.h index 30db7e26f..049d0d8ca 100644 --- a/include/scrimmage/motion/MotionModel.h +++ b/include/scrimmage/motion/MotionModel.h @@ -57,8 +57,12 @@ class MotionModel : public EntityPlugin { virtual bool step(double time, double dt); - // Mission to mission function - virtual std::map mission_xml_set(); + /// @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 mission_xml_get(); virtual bool posthumous(double t); virtual StatePtr &state(); diff --git a/include/scrimmage/plugins/autonomy/Straight/Straight.h b/include/scrimmage/plugins/autonomy/Straight/Straight.h index 2bba03cb8..d535627c7 100644 --- a/include/scrimmage/plugins/autonomy/Straight/Straight.h +++ b/include/scrimmage/plugins/autonomy/Straight/Straight.h @@ -52,7 +52,7 @@ class Straight : public scrimmage::Autonomy{ public: void init(std::map ¶ms) override; bool step_autonomy(double t, double dt) override; - std::map mission_xml_set() override; + std::map mission_xml_get() override; protected: double speed_; diff --git a/include/scrimmage/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.h b/include/scrimmage/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.h index 36746a88f..b8505dc33 100644 --- a/include/scrimmage/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.h +++ b/include/scrimmage/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.h @@ -54,7 +54,7 @@ class SimpleAircraftControllerPID : public Controller { public: virtual void init(std::map ¶ms); virtual bool step(double t, double dt); - std::map mission_xml_set() override; + std::map mission_xml_get() override; protected: diff --git a/include/scrimmage/plugins/motion/SimpleAircraft/SimpleAircraft.h b/include/scrimmage/plugins/motion/SimpleAircraft/SimpleAircraft.h index 317b699f0..b53b824e5 100644 --- a/include/scrimmage/plugins/motion/SimpleAircraft/SimpleAircraft.h +++ b/include/scrimmage/plugins/motion/SimpleAircraft/SimpleAircraft.h @@ -61,7 +61,7 @@ class SimpleAircraft : public scrimmage::MotionModel{ void teleport(scrimmage::StatePtr &state) override; - std::map mission_xml_set() override; + std::map mission_xml_get() override; protected: scrimmage::PID heading_pid_; diff --git a/include/scrimmage/sensor/Sensor.h b/include/scrimmage/sensor/Sensor.h index e2f6eb038..73063802d 100644 --- a/include/scrimmage/sensor/Sensor.h +++ b/include/scrimmage/sensor/Sensor.h @@ -51,8 +51,12 @@ class Sensor : public EntityPlugin { virtual bool step() {return true;} - // Mission to mission function - virtual std::map mission_xml_set() { + /// @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 mission_xml_get() { std::map mission_xml; return mission_xml; }; diff --git a/missions/straight.xml b/missions/straight.xml index 9c368c098..152094af0 100644 --- a/missions/straight.xml +++ b/missions/straight.xml @@ -3,15 +3,17 @@ - + true + false 50051 localhost @@ -82,6 +84,7 @@ + true uav_entity 1 77 77 255 @@ -127,9 +130,10 @@ + false 2 255 0 0 - 30 + 2 1 2 @@ -148,6 +152,7 @@ zephyr-red Straight + Follow 35.719961 -120.767304 @@ -156,26 +161,4 @@ - - - 0 - 1 - 1 - 1 - - 20 - 20 - 10 - - 0 - 0 - 0 - 0 - - Straight - SimpleAircraftControllerPID - SimpleAircraft - zephyr-blue - - diff --git a/src/autonomy/Autonomy.cpp b/src/autonomy/Autonomy.cpp index eef37d570..83c484058 100644 --- a/src/autonomy/Autonomy.cpp +++ b/src/autonomy/Autonomy.cpp @@ -78,7 +78,7 @@ void Autonomy::init() {} void Autonomy::init(std::map &/*params*/) {} bool Autonomy::need_reset() {return need_reset_;} -std::map Autonomy::mission_xml_set() { +std::map Autonomy::mission_xml_get() { std::map mission_xml; return mission_xml; } diff --git a/src/entity/Entity.cpp b/src/entity/Entity.cpp index c7f6ebf87..15db1a3db 100644 --- a/src/entity/Entity.cpp +++ b/src/entity/Entity.cpp @@ -731,28 +731,15 @@ void Entity::print_plugins(std::ostream &out) const { } } -// 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 +// 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. +// 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 Entity::set_motion_xml_vect(){ +std::map Entity::set_motion_xml_map(){ std::map 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; - // } + cur_motion_xml = motion_model_->mission_xml_get(); } return cur_motion_xml; @@ -763,19 +750,10 @@ std::vector> Entity::set_sensor_xml_vect(){ std::map cur_sensor_xml; for (auto &kv : sensors_) { - cur_sensor_xml = kv.second->mission_xml_set(); + cur_sensor_xml = kv.second->mission_xml_get(); 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; } @@ -784,19 +762,10 @@ std::vector> Entity::set_autonomy_xml_vect(){ std::map cur_autonomy_xml; for (AutonomyPtr a : autonomies_) { - cur_autonomy_xml = a->mission_xml_set(); + cur_autonomy_xml = a->mission_xml_get(); 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; } @@ -805,19 +774,10 @@ std::vector> Entity::set_controller_xml_vect() std::map cur_controller_xml; for (ControllerPtr c : controllers_) { - cur_controller_xml = c->mission_xml_set(); + cur_controller_xml = c->mission_xml_get(); 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; } diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index 802b3710b..3da5d1409 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -934,7 +934,6 @@ void MissionParse::final_state_xml(std::list & all_en if (nm == "autonomy"){ for (int i = 0; i < cur_ent.autonomy_xml_tags.size(); i++) { - // Traverse the map for (auto itr : cur_ent.autonomy_xml_tags[i]){ if(itr.first == "Name"){ if(nv != itr.second){ @@ -975,7 +974,6 @@ void MissionParse::final_state_xml(std::list & all_en } } else if (nm == "controller"){ for (int i = 0; i < cur_ent.controller_xml_tags.size(); i++) { - // Traverse the map for (auto itr : cur_ent.controller_xml_tags[i]){ if(itr.first == "Name"){ if(nv != itr.second){ @@ -1000,7 +998,6 @@ void MissionParse::final_state_xml(std::list & all_en } } else if (nm == "sensor"){ for (int i = 0; i < cur_ent.sensor_xml_tags.size(); i++) { - // Traverse the map for (auto itr : cur_ent.sensor_xml_tags[i]){ if(itr.first == "Name"){ if(nv != itr.second){ @@ -1039,19 +1036,14 @@ void MissionParse::final_state_xml(std::list & all_en // Remove original entity nodes based on the bool value of the remove_block entity tag rapidxml::xml_node<> *script_node = runscript_node->first_node("entity"); for (int i = 0; ifirst_node("remove_block")){ - std::string node_value = script_node->first_node("remove_block")->value(); // Placed in the if statement, otherwise it will seg fault - cout << "Remove block is present" << endl; + std::string node_value = script_node->first_node("remove_block")->value(); if(node_value == "false"){ - cout << "Continuing, remove block node value is false" << endl; script_node = script_node->next_sibling("entity"); continue; } } - cout << "Removing the node" << endl; - rapidxml::xml_node<> *remove_node = script_node; script_node = script_node->next_sibling("entity"); doc.first_node("runscript")->remove_node(remove_node); diff --git a/src/plugin_manager/MotionModel.cpp b/src/plugin_manager/MotionModel.cpp index bca1b42d3..53525c4eb 100644 --- a/src/plugin_manager/MotionModel.cpp +++ b/src/plugin_manager/MotionModel.cpp @@ -48,7 +48,7 @@ bool MotionModel::init(std::map &info, std::map MotionModel::mission_xml_set() { +std::map MotionModel::mission_xml_get() { std::map mission_xml; return mission_xml; } diff --git a/src/plugins/autonomy/Straight/Straight.cpp b/src/plugins/autonomy/Straight/Straight.cpp index a20e9b9fa..16d7777e0 100644 --- a/src/plugins/autonomy/Straight/Straight.cpp +++ b/src/plugins/autonomy/Straight/Straight.cpp @@ -214,7 +214,7 @@ void Straight::init(std::map ¶ms) { } // Misison to mission testing -std::map Straight::mission_xml_set() { +std::map Straight::mission_xml_get() { std::map mission_xml; // I actually think it would make more sense to do a vector of maps, diff --git a/src/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.cpp b/src/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.cpp index d39910f35..a92c6a853 100644 --- a/src/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.cpp +++ b/src/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.cpp @@ -78,7 +78,7 @@ void SimpleAircraftControllerPID::init(std::map ¶m } // Misison to mission testing -std::map SimpleAircraftControllerPID::mission_xml_set() { +std::map SimpleAircraftControllerPID::mission_xml_get() { std::map mission_xml; // I actually think it would make more sense to do a vector of maps, diff --git a/src/plugins/motion/SimpleAircraft/SimpleAircraft.cpp b/src/plugins/motion/SimpleAircraft/SimpleAircraft.cpp index 6463f41d9..00f231946 100644 --- a/src/plugins/motion/SimpleAircraft/SimpleAircraft.cpp +++ b/src/plugins/motion/SimpleAircraft/SimpleAircraft.cpp @@ -107,7 +107,7 @@ bool SimpleAircraft::init(std::map &info, } // Misison to mission testing -std::map SimpleAircraft::mission_xml_set() { +std::map SimpleAircraft::mission_xml_get() { std::map mission_xml; // I actually think it would make more sense to do a vector of maps, diff --git a/src/simcontrol/SimControl.cpp b/src/simcontrol/SimControl.cpp index ca6fe7175..a0ad6cd95 100644 --- a/src/simcontrol/SimControl.cpp +++ b/src/simcontrol/SimControl.cpp @@ -1066,18 +1066,21 @@ bool SimControl::shutdown(const bool& shutdown_python) { finalize(); // Close all plugins - // If the mission_to_mission tag is true, store final states of each entity in a vector of structs if(miss2miss){ for (EntityPtr &ent : ents_) { // Get the vectors of all entity specific plugin xml tags - std::map motion_xml_vect = ent->set_motion_xml_vect(); + std::map motion_xml_vect = ent->set_motion_xml_map(); std::vector> autonomy_xml_vect = ent->set_autonomy_xml_vect(); std::vector> controller_xml_vect = ent->set_controller_xml_vect(); std::vector> sensor_xml_vect = ent->set_sensor_xml_vect(); // Create the struct for the entity end states - // x_pos, y_pos, z_pos, yaw, pitch, roll, health_points, vel_x, vel_y, vel_z + // x_pos, y_pos, z_pos, + // yaw, pitch, roll, + // health_points, + // vel_x, vel_y, vel_z + // motion_xml_tags, autonomy_xml_tags, controller_xml_tags, sensor_xml_tags end_state = {ent->id().team_id(), ent->state()->pos()[0], ent->state()->pos()[1], ent->state()->pos()[2], ent->state()->quat().yaw(), ent->state()->quat().pitch(), ent->state()->quat().roll(), From f5dfa48e2bcdfc6de005d9f97ef72824d10a0497 Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Thu, 6 Jul 2023 12:26:05 -0400 Subject: [PATCH 37/46] Changing var names of miss2miss --- include/scrimmage/parse/MissionParse.h | 2 +- include/scrimmage/simcontrol/SimControl.h | 2 +- src/parse/MissionParse.cpp | 16 ++++++++-------- src/simcontrol/SimControl.cpp | 6 +++--- 4 files changed, 13 insertions(+), 13 deletions(-) diff --git a/include/scrimmage/parse/MissionParse.h b/include/scrimmage/parse/MissionParse.h index 027b2958b..75940bf82 100644 --- a/include/scrimmage/parse/MissionParse.h +++ b/include/scrimmage/parse/MissionParse.h @@ -175,7 +175,7 @@ class MissionParse { std::string mission_filename_ = ""; std::string mission_file_content_ = ""; - std::string miss2miss_file_content = ""; + std::string mission_to_mission_file_content = ""; std::stringstream ent_state_file_content; std::string mission_plugin_file_content = ""; diff --git a/include/scrimmage/simcontrol/SimControl.h b/include/scrimmage/simcontrol/SimControl.h index c13fd3678..c87b90849 100644 --- a/include/scrimmage/simcontrol/SimControl.h +++ b/include/scrimmage/simcontrol/SimControl.h @@ -391,7 +391,7 @@ class SimControl { 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; + bool mission_to_mission = false; std::mutex finished_mutex_; std::mutex contacts_mutex_; diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index 3da5d1409..3daf4c0a6 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -1049,16 +1049,16 @@ void MissionParse::final_state_xml(std::list & all_en doc.first_node("runscript")->remove_node(remove_node); } - // Save doc with new allocated attributes to the miss2miss_file_content string, which will be output to the - // miss2miss.xml file in the log directory - std::string rapidxml_miss2miss_doc; - rapidxml::print(std::back_inserter(rapidxml_miss2miss_doc), doc, 0); - miss2miss_file_content = rapidxml_miss2miss_doc; + // Save doc with new allocated attributes to the mission_to_mission_file_content string, which will be output to the + // mission_to_mission.xml file in the log directory + std::string rapidxml_mission_to_mission_doc; + rapidxml::print(std::back_inserter(rapidxml_mission_to_mission_doc), doc, 0); + mission_to_mission_file_content = rapidxml_mission_to_mission_doc; // Create the new mission to mission xml file - std::ofstream miss2miss_content_out(log_dir_+"/miss2miss.xml"); - miss2miss_content_out << miss2miss_file_content; - miss2miss_content_out.close(); + std::ofstream mission_to_mission_content_out(log_dir_+"/mission_to_mission.xml"); + mission_to_mission_content_out << mission_to_mission_file_content; + mission_to_mission_content_out.close(); // Create the entity end state txt file std::ofstream ent_state_content_out(log_dir_+"/final_ent_states.txt"); diff --git a/src/simcontrol/SimControl.cpp b/src/simcontrol/SimControl.cpp index a0ad6cd95..507723fc9 100644 --- a/src/simcontrol/SimControl.cpp +++ b/src/simcontrol/SimControl.cpp @@ -891,7 +891,7 @@ bool SimControl::start() { contacts_mutex_.unlock(); if (get("mission_to_mission", mp_->params(), true)) { - miss2miss = true; + mission_to_mission = true; } if (get("show_plugins", mp_->params(), false)) { @@ -1066,7 +1066,7 @@ bool SimControl::shutdown(const bool& shutdown_python) { finalize(); // Close all plugins - if(miss2miss){ + if(mission_to_mission){ for (EntityPtr &ent : ents_) { // Get the vectors of all entity specific plugin xml tags @@ -1098,7 +1098,7 @@ bool SimControl::shutdown(const bool& shutdown_python) { } } - if(miss2miss){ + if(mission_to_mission){ mp_->final_state_xml(all_end_states); } From 09bde100cab781e337703d009462e911b07092b4 Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Thu, 6 Jul 2023 13:13:24 -0400 Subject: [PATCH 38/46] Added brief to Mission Parse header --- include/scrimmage/parse/MissionParse.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/include/scrimmage/parse/MissionParse.h b/include/scrimmage/parse/MissionParse.h index 75940bf82..fb64a2bfb 100644 --- a/include/scrimmage/parse/MissionParse.h +++ b/include/scrimmage/parse/MissionParse.h @@ -86,6 +86,10 @@ 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 From 4271d6a9fae129cb04ae7484ce03b8ab1bdc2074 Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Mon, 10 Jul 2023 14:42:01 -0400 Subject: [PATCH 39/46] RST file --- docs/source/tutorials/tutorials.rst | 1 + src/entity/Entity.cpp | 5 ----- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/docs/source/tutorials/tutorials.rst b/docs/source/tutorials/tutorials.rst index a57720d05..394b9a59c 100644 --- a/docs/source/tutorials/tutorials.rst +++ b/docs/source/tutorials/tutorials.rst @@ -34,3 +34,4 @@ swarm behaviors. utilities.rst simcontrol.rst capture-the-flag.rst + mission-to-mission-xml.rst diff --git a/src/entity/Entity.cpp b/src/entity/Entity.cpp index 15db1a3db..ed7a26e31 100644 --- a/src/entity/Entity.cpp +++ b/src/entity/Entity.cpp @@ -731,11 +731,6 @@ void Entity::print_plugins(std::ostream &out) const { } } -// 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 Entity::set_motion_xml_map(){ std::map cur_motion_xml; if (motion_model_ && motion_model_->name() != "BLANK") { From 318b4059eef80603e842e15d8b23b3e86d76b895 Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Mon, 10 Jul 2023 14:43:07 -0400 Subject: [PATCH 40/46] Forgot to add the mission-to-mission-xml rst file --- .../tutorials/mission-to-mission-xml.rst | 75 +++++++++++++++++++ 1 file changed, 75 insertions(+) create mode 100644 docs/source/tutorials/mission-to-mission-xml.rst diff --git a/docs/source/tutorials/mission-to-mission-xml.rst b/docs/source/tutorials/mission-to-mission-xml.rst new file mode 100644 index 000000000..99aaf5d0a --- /dev/null +++ b/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``:: + + true + +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. :: + + true + +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 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 SimpleAircraft::mission_xml_get() { + std::map 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 ````. 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/``. + +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 \ No newline at end of file From 050e2d5d11e97720d624d6c7718b3e8b8b835c05 Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Thu, 13 Jul 2023 11:58:40 -0400 Subject: [PATCH 41/46] Minor updates --- src/parse/MissionParse.cpp | 6 +++--- src/simcontrol/SimControl.cpp | 2 ++ 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index 3daf4c0a6..6dc0898d2 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -933,7 +933,7 @@ void MissionParse::final_state_xml(std::list & all_en std::string nv = node->value(); if (nm == "autonomy"){ - for (int i = 0; i < cur_ent.autonomy_xml_tags.size(); i++) { + for (unsigned int i = 0; i < cur_ent.autonomy_xml_tags.size(); i++) { for (auto itr : cur_ent.autonomy_xml_tags[i]){ if(itr.first == "Name"){ if(nv != itr.second){ @@ -973,7 +973,7 @@ void MissionParse::final_state_xml(std::list & all_en } } } else if (nm == "controller"){ - for (int i = 0; i < cur_ent.controller_xml_tags.size(); i++) { + for (unsigned int i = 0; i < cur_ent.controller_xml_tags.size(); i++) { for (auto itr : cur_ent.controller_xml_tags[i]){ if(itr.first == "Name"){ if(nv != itr.second){ @@ -997,7 +997,7 @@ void MissionParse::final_state_xml(std::list & all_en } } else if (nm == "sensor"){ - for (int i = 0; i < cur_ent.sensor_xml_tags.size(); i++) { + for (unsigned int i = 0; i < cur_ent.sensor_xml_tags.size(); i++) { for (auto itr : cur_ent.sensor_xml_tags[i]){ if(itr.first == "Name"){ if(nv != itr.second){ diff --git a/src/simcontrol/SimControl.cpp b/src/simcontrol/SimControl.cpp index 507723fc9..03efbdef4 100644 --- a/src/simcontrol/SimControl.cpp +++ b/src/simcontrol/SimControl.cpp @@ -1065,6 +1065,8 @@ bool SimControl::finalize() { bool SimControl::shutdown(const bool& shutdown_python) { finalize(); + cout << "Shutting down" << endl; + // Close all plugins if(mission_to_mission){ for (EntityPtr &ent : ents_) { From ebf9605e05622e2a8c1ba15f7df8acd80479b3f3 Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Wed, 2 Aug 2023 10:15:55 -0400 Subject: [PATCH 42/46] Removing files that were used only for demoing --- .../plugins/autonomy/Straight/Straight.h | 1 - .../SimpleAircraftControllerPID.h | 2 -- .../motion/SimpleAircraft/SimpleAircraft.h | 2 -- missions/straight.xml | 33 ++++++++++++++----- src/plugins/autonomy/Straight/Straight.cpp | 18 ---------- .../SimpleAircraftControllerPID.cpp | 18 ---------- .../motion/SimpleAircraft/SimpleAircraft.cpp | 21 ------------ 7 files changed, 25 insertions(+), 70 deletions(-) diff --git a/include/scrimmage/plugins/autonomy/Straight/Straight.h b/include/scrimmage/plugins/autonomy/Straight/Straight.h index d535627c7..656ec0ca3 100644 --- a/include/scrimmage/plugins/autonomy/Straight/Straight.h +++ b/include/scrimmage/plugins/autonomy/Straight/Straight.h @@ -52,7 +52,6 @@ class Straight : public scrimmage::Autonomy{ public: void init(std::map ¶ms) override; bool step_autonomy(double t, double dt) override; - std::map mission_xml_get() override; protected: double speed_; diff --git a/include/scrimmage/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.h b/include/scrimmage/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.h index b8505dc33..df928bca6 100644 --- a/include/scrimmage/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.h +++ b/include/scrimmage/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.h @@ -54,8 +54,6 @@ class SimpleAircraftControllerPID : public Controller { public: virtual void init(std::map ¶ms); virtual bool step(double t, double dt); - std::map mission_xml_get() override; - protected: scrimmage::PID heading_pid_; diff --git a/include/scrimmage/plugins/motion/SimpleAircraft/SimpleAircraft.h b/include/scrimmage/plugins/motion/SimpleAircraft/SimpleAircraft.h index b53b824e5..6156d7b45 100644 --- a/include/scrimmage/plugins/motion/SimpleAircraft/SimpleAircraft.h +++ b/include/scrimmage/plugins/motion/SimpleAircraft/SimpleAircraft.h @@ -61,8 +61,6 @@ class SimpleAircraft : public scrimmage::MotionModel{ void teleport(scrimmage::StatePtr &state) override; - std::map mission_xml_get() override; - protected: scrimmage::PID heading_pid_; scrimmage::PID alt_pid_; diff --git a/missions/straight.xml b/missions/straight.xml index 152094af0..9c368c098 100644 --- a/missions/straight.xml +++ b/missions/straight.xml @@ -3,17 +3,15 @@ - - true - false 50051 localhost @@ -84,7 +82,6 @@ - true uav_entity 1 77 77 255 @@ -130,10 +127,9 @@ - false 2 255 0 0 - 2 + 30 1 2 @@ -152,7 +148,6 @@ zephyr-red Straight - Follow 35.719961 -120.767304 @@ -161,4 +156,26 @@ + + + 0 + 1 + 1 + 1 + + 20 + 20 + 10 + + 0 + 0 + 0 + 0 + + Straight + SimpleAircraftControllerPID + SimpleAircraft + zephyr-blue + + diff --git a/src/plugins/autonomy/Straight/Straight.cpp b/src/plugins/autonomy/Straight/Straight.cpp index 16d7777e0..8d714d3be 100644 --- a/src/plugins/autonomy/Straight/Straight.cpp +++ b/src/plugins/autonomy/Straight/Straight.cpp @@ -213,24 +213,6 @@ void Straight::init(std::map ¶ms) { desired_heading_idx_ = vars_.declare(VariableIO::Type::desired_heading, VariableIO::Direction::Out); } -// Misison to mission testing -std::map Straight::mission_xml_get() { - std::map mission_xml; - - // I actually think it would make more sense to do a vector of maps, - // so that I can relate the xml tag name to the value - mission_xml.insert({"Name","Straight"}); - mission_xml.insert({"speed",std::to_string(speed_)}); - mission_xml.insert({"show_camera_images",std::to_string(show_camera_images_)}); - mission_xml.insert({"save_camera_images",std::to_string(save_camera_images_)}); - mission_xml.insert({"show_text_label",std::to_string(show_text_label_)}); - mission_xml.insert({"enable_boundary_control",std::to_string(enable_boundary_control_)}); - mission_xml.insert({"generate_entities",std::to_string(gen_ents_)}); - - return mission_xml; -} -// End of mission to mission testing - bool Straight::step_autonomy(double t, double dt) { if (gen_ents_) { if (time_->t() > (prev_gen_time_ + 2.0)) { diff --git a/src/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.cpp b/src/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.cpp index a92c6a853..ef20409d9 100644 --- a/src/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.cpp +++ b/src/plugins/controller/SimpleAircraftControllerPID/SimpleAircraftControllerPID.cpp @@ -77,24 +77,6 @@ void SimpleAircraftControllerPID::init(std::map ¶m output_pitch_rate_idx_ = vars_.declare(VariableIO::Type::pitch_rate, VariableIO::Direction::Out); } -// Misison to mission testing -std::map SimpleAircraftControllerPID::mission_xml_get() { - std::map mission_xml; - - // I actually think it would make more sense to do a vector of maps, - // so that I can relate the xml tag name to the value - mission_xml.insert({"Name","SimpleAircraftControllerPID"}); - // mission_xml.insert({"heading_pid",std::to_string()}); - // mission_xml.insert({"show_camera_images",std::to_string(show_camera_images_)}); - // mission_xml.insert({"save_camera_images",std::to_string(save_camera_images_)}); - // mission_xml.insert({"show_text_label",std::to_string(show_text_label_)}); - // mission_xml.insert({"enable_boundary_control",std::to_string(enable_boundary_control_)}); - // mission_xml.insert({"generate_entities",std::to_string(gen_ents_)}); - - return mission_xml; -} -// End of mission to mission testing - bool SimpleAircraftControllerPID::step(double t, double dt) { heading_pid_.set_setpoint(vars_.input(input_roll_or_heading_idx_)); double u_roll_rate = use_roll_ ? diff --git a/src/plugins/motion/SimpleAircraft/SimpleAircraft.cpp b/src/plugins/motion/SimpleAircraft/SimpleAircraft.cpp index 00f231946..4f6da8154 100644 --- a/src/plugins/motion/SimpleAircraft/SimpleAircraft.cpp +++ b/src/plugins/motion/SimpleAircraft/SimpleAircraft.cpp @@ -106,27 +106,6 @@ bool SimpleAircraft::init(std::map &info, return true; } -// Misison to mission testing -std::map SimpleAircraft::mission_xml_get() { - std::map mission_xml; - - // I actually think it would make more sense to do a vector of maps, - // so that I can relate the xml tag name to the value duhhhhhhh - 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; -} -// End of mission to mission testing - bool SimpleAircraft::step(double time, double dt) { // Need to saturate state variables before model runs x_[ROLL] = clamp(x_[ROLL], -max_roll_, max_roll_); From 2ddb14b8bb0178c82cd1391aa2a3a1b0a7cfe162 Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Wed, 2 Aug 2023 10:26:18 -0400 Subject: [PATCH 43/46] Reverting William's changes to see if Docker builds --- src/entity/Entity.cpp | 2 +- src/parse/ConfigParse.cpp | 14 +------------- src/viewer/Updater.cpp | 2 +- 3 files changed, 3 insertions(+), 15 deletions(-) diff --git a/src/entity/Entity.cpp b/src/entity/Entity.cpp index ed7a26e31..50574fe4b 100644 --- a/src/entity/Entity.cpp +++ b/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_ && motion_model_->name() != "BLANK") { + if (motion_model_->name() != "BLANK") { out << motion_model_->name() << endl; } } diff --git a/src/parse/ConfigParse.cpp b/src/parse/ConfigParse.cpp index 0ee3f21cf..cfa839ddf 100644 --- a/src/parse/ConfigParse.cpp +++ b/src/parse/ConfigParse.cpp @@ -126,19 +126,7 @@ bool ConfigParse::parse(const std::map &overrides, buffer << file.rdbuf(); file.close(); std::string content(buffer.str()); - 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; - } + doc.parse<0>(&content[0]); rx::xml_node<> *config_node = doc.first_node("params"); if (config_node == 0) { diff --git a/src/viewer/Updater.cpp b/src/viewer/Updater.cpp index e4f470e7f..3c16d167b 100644 --- a/src/viewer/Updater.cpp +++ b/src/viewer/Updater.cpp @@ -685,7 +685,7 @@ bool Updater::update_text_display() { } // Update the time (text) display - const int num_digits = std::ceil(std::abs(log10(dt_))); + const int num_digits = std::abs(log10(dt_)); std::stringstream ss; ss << std::setprecision(num_digits) << std::fixed << frame_time_ << " s"; time_actor_->SetInput(ss.str().c_str()); From c025b7c663d95bc9e67d20969e492a8e46aec5f2 Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Wed, 2 Aug 2023 12:04:34 -0400 Subject: [PATCH 44/46] Fixed issue where docker was seg faulting for the test_openai.py file --- src/simcontrol/SimControl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/simcontrol/SimControl.cpp b/src/simcontrol/SimControl.cpp index 9d865721e..9f24eba69 100644 --- a/src/simcontrol/SimControl.cpp +++ b/src/simcontrol/SimControl.cpp @@ -899,7 +899,7 @@ bool SimControl::start() { contacts_->reserve(max_num_entities+1); contacts_mutex_.unlock(); - if (get("mission_to_mission", mp_->params(), true)) { + if (get("mission_to_mission", mp_->params(), false)) { mission_to_mission = true; } From c0267fa0a46b61268027f92e2a8b76d70bdf8b8a Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Wed, 9 Aug 2023 13:41:09 -0400 Subject: [PATCH 45/46] Adding williams changes back --- src/entity/Entity.cpp | 2 +- src/parse/ConfigParse.cpp | 14 +++++++++++++- src/parse/MissionParse.cpp | 10 ++++++++-- src/viewer/Updater.cpp | 2 +- 4 files changed, 23 insertions(+), 5 deletions(-) diff --git a/src/entity/Entity.cpp b/src/entity/Entity.cpp index 50574fe4b..ed7a26e31 100644 --- a/src/entity/Entity.cpp +++ b/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; } } diff --git a/src/parse/ConfigParse.cpp b/src/parse/ConfigParse.cpp index cfa839ddf..0ee3f21cf 100644 --- a/src/parse/ConfigParse.cpp +++ b/src/parse/ConfigParse.cpp @@ -126,7 +126,19 @@ bool ConfigParse::parse(const std::map &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) { diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index 6dc0898d2..d0c3cc821 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -135,10 +135,16 @@ bool MissionParse::parse(const std::string &filename) { 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()); - } catch (...) { + // doc.parse<0>(mission_file_content_vec.data()); + doc.parse(mission_file_content_vec.data()); + } catch (const rapidxml::parse_error& e) { + std::cout << e.what() << std::endl; cout << "scrimmage::MissionParse::parse: Exception during rapidxml::xml_document<>.parse<>()." << endl; return false; + } catch (const std::exception& e) { + std::cerr << "Error was: " << e.what() << std::endl; + } catch (...) { + std::cerr << "An unknown error occurred." << std::endl; } rapidxml::xml_node<> *runscript_node = doc.first_node("runscript"); diff --git a/src/viewer/Updater.cpp b/src/viewer/Updater.cpp index 3c16d167b..e4f470e7f 100644 --- a/src/viewer/Updater.cpp +++ b/src/viewer/Updater.cpp @@ -685,7 +685,7 @@ bool Updater::update_text_display() { } // Update the time (text) display - const int num_digits = std::abs(log10(dt_)); + const int num_digits = std::ceil(std::abs(log10(dt_))); std::stringstream ss; ss << std::setprecision(num_digits) << std::fixed << frame_time_ << " s"; time_actor_->SetInput(ss.str().c_str()); From fd5c886df4b29a8a0d52e1ab917add9c6ed2d1b0 Mon Sep 17 00:00:00 2001 From: Natalie Davis Date: Sat, 26 Aug 2023 16:13:11 -0400 Subject: [PATCH 46/46] Fixed the team id check and wording in the tutorial file --- docs/source/tutorials/mission-to-mission-xml.rst | 4 ++-- src/parse/MissionParse.cpp | 6 +++++- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/docs/source/tutorials/mission-to-mission-xml.rst b/docs/source/tutorials/mission-to-mission-xml.rst index 99aaf5d0a..b53aecf49 100644 --- a/docs/source/tutorials/mission-to-mission-xml.rst +++ b/docs/source/tutorials/mission-to-mission-xml.rst @@ -13,10 +13,10 @@ utilized: 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 + must be included with the value ``false``. If it is not included, the entity block will be removed from the output mission XML file. :: - true + false 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 diff --git a/src/parse/MissionParse.cpp b/src/parse/MissionParse.cpp index d0c3cc821..873b22a9e 100644 --- a/src/parse/MissionParse.cpp +++ b/src/parse/MissionParse.cpp @@ -811,7 +811,11 @@ void MissionParse::final_state_xml(std::list & all_en for (rapidxml::xml_node<> *script_node = runscript_node->first_node("entity"); script_node != 0; script_node = script_node->next_sibling("entity")) { - if(strcmp(std::to_string(cur_ent.team_id).c_str(),script_node->first_node("team_id")->value()) == 0){ + if(!script_node->first_node("team_id")){ + cout << "Team id was not specified in the Mission XML file. Mission to Mission end state is not being logged for the given entity." << endl; + break; + } + else if(strcmp(std::to_string(cur_ent.team_id).c_str(),script_node->first_node("team_id")->value()) == 0){ // Creates a clone of the entity node that matches the struct's team id rapidxml::xml_node<> *new_ent = doc.clone_node(script_node);