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 0000000000..b53aecf49e --- /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 ``false``. If it is not included, the entity block will be removed + from the output mission XML file. :: + + 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 + 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 diff --git a/docs/source/tutorials/tutorials.rst b/docs/source/tutorials/tutorials.rst index a57720d05e..394b9a59cd 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/include/scrimmage/autonomy/Autonomy.h b/include/scrimmage/autonomy/Autonomy.h index 8760bfe45f..37e91c2881 100644 --- a/include/scrimmage/autonomy/Autonomy.h +++ b/include/scrimmage/autonomy/Autonomy.h @@ -60,6 +60,13 @@ class Autonomy : public EntityPlugin { StatePtr &desired_state(); void set_desired_state(StatePtr desired_state); + /// @brief Plugin specific xml tags are inserted to a map and returned. The map + /// is used to update the mission to mission xml file, which captures entity end states + /// to be used as starting points in future simulations. + /// @return A map of keys and values of type string, representing plugin specific + /// xml tag attribute names and associated values + virtual std::map 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 caa37a9b90..3944a1fbc2 100644 --- a/include/scrimmage/entity/Entity.h +++ b/include/scrimmage/entity/Entity.h @@ -138,6 +138,21 @@ class Entity : public std::enable_shared_from_this { MotionModelPtr &motion(); std::vector &controllers(); + /// @brief Loops through each motion model for the given entity and retrieves a map of plugin + /// specific xml tags + /// @return A map of keys and values of type string, representing plugin specific xml tag + /// attribute names and associated values + std::map 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(); + void set_id(ID &id); ID &id(); diff --git a/include/scrimmage/motion/Controller.h b/include/scrimmage/motion/Controller.h index d97a3da219..e6ceef8cd9 100644 --- a/include/scrimmage/motion/Controller.h +++ b/include/scrimmage/motion/Controller.h @@ -52,6 +52,17 @@ class Controller : public EntityPlugin { desired_state_ = nullptr; } + /// @brief Plugin specific xml tags are inserted to a map and returned. The map + /// is used to update the mission to mission xml file, which captures entity end states + /// to be used as starting points in future simulations. + /// @return A map of keys and values of type string, representing plugin specific + /// xml tag attribute names and associated values + virtual std::map mission_xml_get() { + std::map mission_xml; + return mission_xml; + }; + + protected: StatePtr state_; StatePtr desired_state_; diff --git a/include/scrimmage/motion/MotionModel.h b/include/scrimmage/motion/MotionModel.h index 1fdf2ce45d..049d0d8cae 100644 --- a/include/scrimmage/motion/MotionModel.h +++ b/include/scrimmage/motion/MotionModel.h @@ -56,6 +56,14 @@ class MotionModel : public EntityPlugin { std::map ¶ms); virtual bool step(double time, double dt); + + /// @brief Plugin specific xml tags are inserted to a map and returned. The map + /// is used to update the mission to mission xml file, which captures entity end states + /// to be used as starting points in future simulations. + /// @return A map of keys and values of type string, representing plugin specific + /// xml tag attribute names and associated values + virtual std::map mission_xml_get(); + virtual bool posthumous(double t); virtual StatePtr &state(); virtual void set_state(StatePtr &state); diff --git a/include/scrimmage/parse/MissionParse.h b/include/scrimmage/parse/MissionParse.h index f4ec4ff4dd..fb64a2bfb9 100644 --- a/include/scrimmage/parse/MissionParse.h +++ b/include/scrimmage/parse/MissionParse.h @@ -39,6 +39,8 @@ #include #include +#include +#include #include #include @@ -84,6 +86,19 @@ class MissionParse { bool create_log_dir(); void set_overrides(const std::string &overrides); bool parse(const std::string &filename); + + /// @brief Adds all plugin specific xml attributes to the plugin_spec_attrs map + /// @param node_name + /// @param node_value + void get_plugin_params(std::string node_name, std::string node_value); + + /// @brief Generate the mission to mission xml file for all final states of entities + /// @param all_end_states + void final_state_xml(std::list & all_end_states); + + /// @brief Track the number of entity blocks in the input Mission XML file + int num_ents = 0; + bool write(const std::string &filename); double t0(); @@ -159,9 +174,18 @@ class MissionParse { bool output_type_required(const std::string& output_type); protected: + rapidxml::xml_document<> doc; + std::string mission_filename_ = ""; std::string mission_file_content_ = ""; + std::string mission_to_mission_file_content = ""; + std::stringstream ent_state_file_content; + + std::string mission_plugin_file_content = ""; + std::string scrimmage_plugin_path = ""; + std::map plugin_spec_attrs; + double t0_ = 0; double tend_ = 50; double dt_ = 0.00833333; diff --git a/include/scrimmage/sensor/Sensor.h b/include/scrimmage/sensor/Sensor.h index f56eede6e2..73063802de 100644 --- a/include/scrimmage/sensor/Sensor.h +++ b/include/scrimmage/sensor/Sensor.h @@ -51,6 +51,16 @@ class Sensor : public EntityPlugin { virtual bool step() {return true;} + /// @brief Plugin specific xml tags are inserted to a map and returned. The map + /// is used to update the mission to mission xml file, which captures entity end states + /// to be used as starting points in future simulations. + /// @return A map of keys and values of type string, representing plugin specific + /// xml tag attribute names and associated values + virtual std::map mission_xml_get() { + 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 d1ccd14e97..164cc9db7f 100644 --- a/include/scrimmage/simcontrol/SimControl.h +++ b/include/scrimmage/simcontrol/SimControl.h @@ -321,6 +321,32 @@ class SimControl { void set_running_in_thread(bool running_in_thread); + /// @brief Contains the final state values of each entity + struct ent_end_state { + int team_id; + double x_pos; + double y_pos; + double z_pos; + + double yaw; + double pitch; + double roll; + + int health_points; + + double vel_x; + double vel_y; + double vel_z; + + std::map 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; + protected: // Key: Entity ID // Value: Team ID @@ -365,6 +391,9 @@ class SimControl { bool finished_ = false; bool exit_ = false; + /// @brief Holds the mission_to_mission tag. If true, will create an output XML file of final entity states + bool mission_to_mission = false; + std::mutex finished_mutex_; std::mutex contacts_mutex_; std::mutex exit_mutex_; diff --git a/src/autonomy/Autonomy.cpp b/src/autonomy/Autonomy.cpp index 6af0b9375d..83c484058d 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_get() { + 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 1ae63f00cf..ed7a26e31e 100644 --- a/src/entity/Entity.cpp +++ b/src/entity/Entity.cpp @@ -726,8 +726,54 @@ void Entity::print_plugins(std::ostream &out) const { out << c->name() << endl; } out << "----------- Motion -------------" << endl; - if (motion_model_->name() != "BLANK") { + if (motion_model_ && motion_model_->name() != "BLANK") { out << motion_model_->name() << endl; } } -} // namespace scrimmage + +std::map Entity::set_motion_xml_map(){ + std::map cur_motion_xml; + if (motion_model_ && motion_model_->name() != "BLANK") { + cur_motion_xml = motion_model_->mission_xml_get(); + } + + return cur_motion_xml; +} + +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_get(); + all_sensor_xml.push_back(cur_sensor_xml); + } + + return all_sensor_xml; +} + +std::vector> Entity::set_autonomy_xml_vect(){ + std::vector> all_autonomy_xml; + std::map cur_autonomy_xml; + + for (AutonomyPtr a : autonomies_) { + cur_autonomy_xml = a->mission_xml_get(); + all_autonomy_xml.push_back(cur_autonomy_xml); + } + + return all_autonomy_xml; +} + +std::vector> 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_get(); + all_controller_xml.push_back(cur_controller_xml); + } + + return all_controller_xml; +} + +} // namespace scrimmage \ No newline at end of file diff --git a/src/parse/ConfigParse.cpp b/src/parse/ConfigParse.cpp index cfa839ddf8..0ee3f21cfc 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 7c19f020f4..873b22a9e9 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; @@ -127,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 @@ -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"); @@ -231,12 +237,30 @@ 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. 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_path.find("/include/scrimmage/plugins") != std::string::npos)){ + scrimmage_plugin_path = plugin_path; + break; + } + } // Loop through each node under "runscript" that isn't an entity or base attributes_.clear(); 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") { params_[nm] = node->value(); @@ -252,6 +276,18 @@ bool MissionParse::parse(const std::string &filename) { attributes_[nm3]["ORIGINAL_PLUGIN_NAME"] = node->value(); attributes_[nm4]["ORIGINAL_PLUGIN_NAME"] = node->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. + // + // Plugins that are not entity specific to check for: + // - entity_interaction + // - metrics + // - network + if (nm == "metrics" || nm == "entity_interaction" || nm == "network"){ + 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()) { @@ -267,8 +303,21 @@ 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()); + } } } + + // 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); + } } } @@ -403,6 +452,8 @@ bool MissionParse::parse(const std::string &filename) { script_node != 0; script_node = script_node->next_sibling("entity")) { + num_ents++; + std::map script_info; rapidxml::xml_attribute<> *nm_attr = script_node->first_attribute("entity_common"); @@ -517,7 +568,23 @@ 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. + // 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 + if (nm == "autonomy" || nm == "controller" || nm == "motion_model" || nm == "sensor"){ + get_plugin_params(nm, node->value()); + } + if (nm == "autonomy") { nm += std::to_string(autonomy_order++); } else if (nm == "controller") { @@ -541,10 +608,29 @@ bool MissionParse::parse(const std::string &filename) { 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()]!=""){ + 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){ + 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); + } } + + // 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; // For each entity, if the lat/lon are defined, use these values to // overwrite the "x" and "y" values @@ -697,6 +783,377 @@ bool MissionParse::parse(const std::string &filename) { return true; } +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 + 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; + } + + rapidxml::xml_node<> *runscript_node = doc.first_node("runscript"); + if (runscript_node == 0) { + cout << "Missing runscript tag." << endl; + return; + } + + for(auto a = all_end_states.begin(); a != all_end_states.end(); ++a){ + const auto& cur_ent = *a; + + // 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")) { + 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); + + // 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()); + 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); + 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()); + rapidxml::xml_node<> *y_pos = doc.allocate_node(rapidxml::node_element, "y", ypos_value); + 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()); + rapidxml::xml_node<> *z_pos = doc.allocate_node(rapidxml::node_element, "z", zpos_value); + 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()); + } else{ + new_ent->insert_node(new_ent->first_node("z")->next_sibling(),heading); + } + + 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")){ + 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); + } + + 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); + } + + ent_state_file_content << "--- New Entity Entry ---" << 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 + << "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")); + } + 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")); + } + 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")); + } + + // 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(); + + if (nm == "autonomy"){ + 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){ + break; + } + else{ + 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); + 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 == "motion_model"){ + for(auto itr = cur_ent.motion_xml_tags.begin(); itr != cur_ent.motion_xml_tags.end(); ++itr){ + if(itr->first == "Name"){ + 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); + 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 (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){ + break; + } + else{ + 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); + 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 == "sensor"){ + 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){ + break; + } + else{ + 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); + 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); + } + } + + } + } + + } + + // 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; + } + } + } + + // 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(); + if(node_value == "false"){ + script_node = script_node->next_sibling("entity"); + continue; + } + } + + rapidxml::xml_node<> *remove_node = script_node; + script_node = script_node->next_sibling("entity"); + doc.first_node("runscript")->remove_node(remove_node); + } + + // 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 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"); + 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) { + 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_)) { + FileSearch file_search; + std::string result = ""; + 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; + } else if (node_name=="motion_model"){ + std::string temp = node_value; + pluginxml_path = scrimmage_plugin_path + "/" + "motion/" + temp; + } + + 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; + } + // 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; + } + + 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. doc.parse requires a null terminated string that it can modify. + rapidxml::xml_document<> plugin_doc; + 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; + } + + rapidxml::xml_node<> *params_node = plugin_doc.first_node("params"); + if (params_node == 0) { + cout << "Missing params tag." << endl; + 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()}); + } +} + bool MissionParse::create_log_dir() { // Create the root_log_dir_ if it doesn't exist: if (not fs::exists(fs::path(root_log_dir_)) && @@ -741,6 +1198,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. diff --git a/src/plugin_manager/MotionModel.cpp b/src/plugin_manager/MotionModel.cpp index 7710ec9467..53525c4eb2 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_get() { + std::map mission_xml; + return mission_xml; +} + bool MotionModel::posthumous(double t) { return true; } StatePtr &MotionModel::state() {return state_;} diff --git a/src/simcontrol/SimControl.cpp b/src/simcontrol/SimControl.cpp index a7c8589e49..cc330951ae 100644 --- a/src/simcontrol/SimControl.cpp +++ b/src/simcontrol/SimControl.cpp @@ -899,6 +899,10 @@ bool SimControl::start() { contacts_->reserve(max_num_entities+1); contacts_mutex_.unlock(); + if (get("mission_to_mission", mp_->params(), false)) { + mission_to_mission = true; + } + if (get("show_plugins", mp_->params(), false)) { plugin_manager_->print_returned_plugins(); } @@ -1070,9 +1074,43 @@ bool SimControl::finalize() { bool SimControl::shutdown(const bool& shutdown_python) { finalize(); + cout << "Shutting down" << endl; + // Close all plugins - for (EntityPtr &ent : ents_) { - ent->close(t()); + if(mission_to_mission){ + for (EntityPtr &ent : ents_) { + + // Get the vectors of all entity specific plugin xml tags + 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 + // 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(), + ent->health_points(), + 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->close(t()); + } + } else { + for (EntityPtr &ent : ents_) { + ent->close(t()); + } + } + + if(mission_to_mission){ + mp_->final_state_xml(all_end_states); } for (EntityInteractionPtr ent_inter : ent_inters_) { diff --git a/src/viewer/Updater.cpp b/src/viewer/Updater.cpp index 3c16d167b3..e4f470e7fe 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());