From e17cc55f3fa4dece34b30e90ce7aa0ac91a326ec Mon Sep 17 00:00:00 2001 From: Wesley Ford Date: Wed, 18 Sep 2024 13:58:58 -0400 Subject: [PATCH 1/2] Deprecated entity state() Deprecated the Entity state() function in favor of state_truth() and state_belief() to encourage developers to be explicit about which version of the state they are accessing. --- include/scrimmage/entity/Entity.h | 54 ++-- include/scrimmage/math/State.h | 1 + .../bindings_src/src/py_openai_env.cpp | 2 +- src/entity/Entity.cpp | 230 +++++++++--------- src/entity/External.cpp | 8 +- src/math/State.cpp | 72 +++--- .../autonomy/GoToWaypoint/GoToWaypoint.cpp | 2 +- src/plugins/sensor/NoisyState/NoisyState.cpp | 5 +- .../sensor/RLSimpleSensor/RLSimpleSensor.cpp | 2 +- src/plugins/sensor/SimpleINS/SimpleINS.cpp | 6 +- src/simcontrol/SimControl.cpp | 6 +- 11 files changed, 201 insertions(+), 187 deletions(-) diff --git a/include/scrimmage/entity/Entity.h b/include/scrimmage/entity/Entity.h index caa37a9b90..6cfee7270b 100644 --- a/include/scrimmage/entity/Entity.h +++ b/include/scrimmage/entity/Entity.h @@ -33,20 +33,20 @@ #ifndef INCLUDE_SCRIMMAGE_ENTITY_ENTITY_H_ #define INCLUDE_SCRIMMAGE_ENTITY_ENTITY_H_ -#include #include #include +#include #include #include +#include +#include #include +#include #include +#include #include -#include #include -#include -#include -#include #include @@ -56,7 +56,7 @@ using ContactVisualPtr = std::shared_ptr; namespace scrimmage { -using Service = std::function; +using Service = std::function; typedef std::map> AttributeMap; @@ -72,7 +72,8 @@ class Entity : public std::enable_shared_from_this { ContactMapPtr &contacts, MissionParsePtr mp, const std::shared_ptr &proj, - int id, int ent_desc_id, + int id, + int ent_desc_id, PluginManagerPtr plugin_manager, FileSearchPtr &file_search, RTreePtr &rtree, @@ -82,8 +83,8 @@ class Entity : public std::enable_shared_from_this { const ParameterServerPtr ¶m_server, const GlobalServicePtr &global_services, const std::set &plugin_tags, - std::function&)> param_override_func, - const int& debug_level = 0); + std::function &)> param_override_func, + const int &debug_level = 0); void print_plugins(std::ostream &out) const; @@ -132,7 +133,12 @@ class Entity : public std::enable_shared_from_this { /*! \name getters/setters */ ///@{ + [[deprecated("Use state_belief() or state_truth() to query state information instead")]] StatePtr &state(); + + void set_state_belief(const StatePtr &other); + void set_state_belief(const State &other); + const std::shared_ptr state_belief() const; StatePtr &state_truth(); std::vector &autonomies(); MotionModelPtr &motion(); @@ -179,25 +185,15 @@ class Entity : public std::enable_shared_from_this { ContactMapPtr &contacts() { return contacts_; } RTreePtr &rtree() { return rtree_; } - PluginManagerPtr & plugin_manager() { - return plugin_manager_; - } + PluginManagerPtr &plugin_manager() { return plugin_manager_; } - FileSearchPtr & file_search() { - return file_search_; - } + FileSearchPtr &file_search() { return file_search_; } - PubSubPtr & pubsub() { - return pubsub_; - } + PubSubPtr &pubsub() { return pubsub_; } - PrintPtr & printer() { - return printer_; - } + PrintPtr &printer() { return printer_; } - const ParameterServerPtr& param_server() { - return param_server_; - } + const ParameterServerPtr ¶m_server() { return param_server_; } double radius() { return radius_; } void set_time_ptr(TimePtr t); @@ -208,8 +204,7 @@ class Entity : public std::enable_shared_from_this { protected: ID id_; - scrimmage_proto::ContactVisualPtr visual_ = - std::make_shared(); + scrimmage_proto::ContactVisualPtr visual_ = std::make_shared(); std::vector controllers_; MotionModelPtr motion_model_; @@ -226,7 +221,7 @@ class Entity : public std::enable_shared_from_this { RandomPtr random_; - StatePtr state_; + StatePtr state_belief_; StatePtr state_truth_; std::unordered_map properties_; std::unordered_map sensors_; @@ -248,8 +243,9 @@ class Entity : public std::enable_shared_from_this { GlobalServicePtr global_services_; ParameterServerPtr param_server_; TimePtr time_; + }; using EntityPtr = std::shared_ptr; -} // namespace scrimmage -#endif // INCLUDE_SCRIMMAGE_ENTITY_ENTITY_H_ +} // namespace scrimmage +#endif // INCLUDE_SCRIMMAGE_ENTITY_ENTITY_H_ diff --git a/include/scrimmage/math/State.h b/include/scrimmage/math/State.h index db7a51de02..bbc50e70b4 100644 --- a/include/scrimmage/math/State.h +++ b/include/scrimmage/math/State.h @@ -48,6 +48,7 @@ class State { State(); State(Eigen::Vector3d _pos, Eigen::Vector3d _vel, Eigen::Vector3d _ang_vel, Quaternion _quat); + State(const State& other); virtual ~State(); diff --git a/python/scrimmage/bindings_src/src/py_openai_env.cpp b/python/scrimmage/bindings_src/src/py_openai_env.cpp index a45c2887c6..c9ffe9a45a 100644 --- a/python/scrimmage/bindings_src/src/py_openai_env.cpp +++ b/python/scrimmage/bindings_src/src/py_openai_env.cpp @@ -237,7 +237,7 @@ void ScrimmageOpenAIEnv::scrimmage_memory_cleanup() { e->motion() = nullptr; e->controllers().clear(); e->set_mp(nullptr); - e->state() = nullptr; + e->state_truth() = nullptr; e->contacts() = nullptr; e->plugin_manager() = nullptr; e->file_search() = nullptr; diff --git a/src/entity/Entity.cpp b/src/entity/Entity.cpp index 1ae63f00cf..25485241ab 100644 --- a/src/entity/Entity.cpp +++ b/src/entity/Entity.cpp @@ -31,32 +31,32 @@ */ #include -#include #include +#include #include -#include +#include #include -#include +#include #include -#include +#include #include +#include #include #include #include #include #include -#include -#include +#include #include +#include #include -#include #include #include -#include -#include #include +#include +#include using std::cout; using std::endl; @@ -74,7 +74,8 @@ bool Entity::init(AttributeMap &overrides, ContactMapPtr &contacts, MissionParsePtr mp, const std::shared_ptr &proj, - int id, int ent_desc_id, + int id, + int ent_desc_id, PluginManagerPtr plugin_manager, FileSearchPtr &file_search, RTreePtr &rtree, @@ -84,8 +85,8 @@ bool Entity::init(AttributeMap &overrides, const ParameterServerPtr ¶m_server, const GlobalServicePtr &global_services, const std::set &plugin_tags, - std::function&)> param_override_func, - const int& debug_level) { + std::function &)> param_override_func, + const int &debug_level) { pubsub_ = pubsub; printer_ = printer; global_services_ = global_services; @@ -117,32 +118,32 @@ bool Entity::init(AttributeMap &overrides, //////////////////////////////////////////////////////////// // set state //////////////////////////////////////////////////////////// - if (!state_) { - state_ = std::make_shared(); + if (!state_belief_) { + state_belief_ = std::make_shared(); } - state_truth_ = state_; + state_truth_ = state_belief_; double x = get("x", info, 0.0); double y = get("y", info, 0.0); double z = get("z", info, 0.0); - state_->pos() << x, y, z; + state_truth_->pos() << x, y, z; double vx = get("vx", info, 0.0); double vy = get("vy", info, 0.0); double vz = get("vz", info, 0.0); - state_->vel() << vx, vy, vz; + state_truth_->vel() << vx, vy, vz; double sp = get("speed", info, 0.0); if (sp > 0 && vx == 0 && vy == 0 && vz == 0) { - Eigen::Vector3d relative_vel_vector = Eigen::Vector3d::UnitX()*sp; - Eigen::Vector3d vel_vector = state_->quat().rotate(relative_vel_vector); - state_->vel() << vel_vector[0], vel_vector[1], vel_vector[2]; + Eigen::Vector3d relative_vel_vector = Eigen::Vector3d::UnitX() * sp; + Eigen::Vector3d vel_vector = state_truth_->quat().rotate(relative_vel_vector); + state_truth_->vel() << vel_vector[0], vel_vector[1], vel_vector[2]; } double roll = Angles::deg2rad(get("roll", info, 0.0)); double pitch = Angles::deg2rad(get("pitch", info, 0.0)); double yaw = Angles::deg2rad(get("heading", info, 0.0)); - state_->quat().set(roll, pitch, yaw); + state_truth_->quat().set(roll, pitch, yaw); EntityPtr parent = shared_from_this(); @@ -156,21 +157,20 @@ bool Entity::init(AttributeMap &overrides, // The MissionParser appends the order number to the sensor (e.g., sensor0, // sensor1, etc.) int sensor_ct = 0; - std::string sensor_order_name = std::string("sensor") + - std::to_string(sensor_ct); + std::string sensor_order_name = std::string("sensor") + std::to_string(sensor_ct); while (info.count(sensor_order_name) > 0) { ConfigParse config_parse; std::string sensor_name = info[sensor_order_name]; PluginStatus status = plugin_manager->make_plugin("scrimmage::Sensor", - sensor_name, *file_search, + sensor_name, + *file_search, config_parse, overrides[sensor_order_name], plugin_tags); if (status.status == PluginStatus::cast_failed) { - std::cout << "Failed to open sensor plugin: " << sensor_name - << std::endl; + std::cout << "Failed to open sensor plugin: " << sensor_name << std::endl; return false; } else if (status.status == PluginStatus::parse_failed) { return false; @@ -191,9 +191,8 @@ bool Entity::init(AttributeMap &overrides, if (it_rpy != overrides[sensor_order_name].end()) { str2container(it_rpy->second, " ", tf_rpy, 3); } - sensor->transform()->quat().set(Angles::deg2rad(tf_rpy[0]), - Angles::deg2rad(tf_rpy[1]), - Angles::deg2rad(tf_rpy[2])); + sensor->transform()->quat().set( + Angles::deg2rad(tf_rpy[0]), Angles::deg2rad(tf_rpy[1]), Angles::deg2rad(tf_rpy[2])); sensor->set_parent(parent); sensor->set_pubsub(pubsub); @@ -206,8 +205,8 @@ bool Entity::init(AttributeMap &overrides, // get loop rate from plugin's params auto it_loop_rate = config_parse.params().find("loop_rate"); if (it_loop_rate != config_parse.params().end()) { - const double loop_rate = std::stod(it_loop_rate->second); - sensor->set_loop_rate(loop_rate); + const double loop_rate = std::stod(it_loop_rate->second); + sensor->set_loop_rate(loop_rate); } std::string given_name = sensor_name + std::to_string(sensor_ct); @@ -299,7 +298,8 @@ bool Entity::init(AttributeMap &overrides, // correctly. Last controller connects to motion model, second to last // controller connects to the last controller. for (std::list::reverse_iterator rit = controller_names.rbegin(); - rit != controller_names.rend(); ++rit) { + rit != controller_names.rend(); + ++rit) { std::string controller_name = *rit; ConfigParse config_parse; @@ -311,14 +311,13 @@ bool Entity::init(AttributeMap &overrides, overrides[controller_name], plugin_tags); if (status.status == PluginStatus::cast_failed) { - std::cout << "Failed to open controller plugin: " - << controller_name << std::endl; + std::cout << "Failed to open controller plugin: " << controller_name << std::endl; return false; } else if (status.status == PluginStatus::parse_failed) { return false; } else if (status.status == PluginStatus::loaded) { ControllerPtr controller = status.plugin; - controller->set_state(state_); + controller->set_state(state_belief_); controller->set_parent(shared_from_this()); controller->set_time(time_); @@ -332,8 +331,8 @@ bool Entity::init(AttributeMap &overrides, // get loop rate from plugin's params auto it_loop_rate = config_parse.params().find("loop_rate"); if (it_loop_rate != config_parse.params().end()) { - const double loop_rate = std::stod(it_loop_rate->second); - controller->set_loop_rate(loop_rate); + const double loop_rate = std::stod(it_loop_rate->second); + controller->set_loop_rate(loop_rate); } // Connect this controller to the motion model if it is the last @@ -358,21 +357,17 @@ bool Entity::init(AttributeMap &overrides, // Verify the VariableIO connection if (connect_to_motion_model) { if (!verify_io_connection(controller->vars(), motion_model_->vars())) { - std::cout << "VariableIO Error: " - << std::quoted(controller->name()) + std::cout << "VariableIO Error: " << std::quoted(controller->name()) << " does not provide inputs required by motion model " - << std::quoted(motion_model_->name()) - << ": "; + << std::quoted(motion_model_->name()) << ": "; print_io_error(motion_model_->name(), motion_model_->vars()); return false; } } else if (not connect_to_motion_model) { if (!verify_io_connection(controller->vars(), controllers_.back()->vars())) { - std::cout << "VariableIO Error: " - << std::quoted(controller->name()) + std::cout << "VariableIO Error: " << std::quoted(controller->name()) << " does not provide inputs required by next controller " - << std::quoted(controllers_.back()->name()) - << ": "; + << std::quoted(controllers_.back()->name()) << ": "; print_io_error(controllers_.back()->name(), controllers_.back()->vars()); return false; } @@ -390,11 +385,10 @@ bool Entity::init(AttributeMap &overrides, // If the motion model requires any inputs and there are no controllers, // this is a VariableIO error. - if (motion_model_->vars().input_variable_index().size() > 0 && - controllers_.size() == 0) { + if (motion_model_->vars().input_variable_index().size() > 0 && controllers_.size() == 0) { std::cout << "VariableIO Error: There are not any controllers that " - << "provide the inputs required by " - << std::quoted(motion_model_->name()) << std::endl; + << "provide the inputs required by " << std::quoted(motion_model_->name()) + << std::endl; print_io_error(motion_model_->name(), motion_model_->vars()); std::cout << "If you want to directly pass the outputs from the " << "autonomy to the motion_model, see the DirectController " @@ -416,12 +410,24 @@ bool Entity::init(AttributeMap &overrides, // Create the autonomy plugins from the autonomy_names list. for (auto autonomy_name : autonomy_names) { - auto autonomy = make_autonomy( - info[autonomy_name], plugin_manager, overrides[autonomy_name], - parent, state_, id_to_team_map, id_to_ent_map, proj_, contacts, - file_search, rtree, pubsub, time, param_server, plugin_tags, - param_override_func, controllers_, - debug_level); + auto autonomy = make_autonomy(info[autonomy_name], + plugin_manager, + overrides[autonomy_name], + parent, + state_belief_, + id_to_team_map, + id_to_ent_map, + proj_, + contacts, + file_search, + rtree, + pubsub, + time, + param_server, + plugin_tags, + param_override_func, + controllers_, + debug_level); if (autonomy) { autonomies_.push_back(*autonomy); @@ -438,15 +444,15 @@ bool Entity::init(AttributeMap &overrides, if (connect_entity && not controllers_.empty() && controllers_.front()->vars().input_variable_index().size() > 0) { auto verify_io = [&](auto &autonomy) { - return verify_io_connection(autonomy->vars(), - controllers_.front()->vars());}; + return verify_io_connection(autonomy->vars(), controllers_.front()->vars()); + }; if (boost::algorithm::none_of(autonomies_, verify_io)) { auto out_it = std::ostream_iterator(std::cout, ", "); std::cout << "VariableIO Error: " << "no autonomies provide inputs required by Controller " << std::quoted(controllers_.front()->name()) << ". Add VariableIO output declarations in "; - auto get_name = [&](auto &p) {return p->name();}; + auto get_name = [&](auto &p) { return p->name(); }; br::copy(autonomies_ | ba::transformed(get_name), out_it); std::cout << "as follows " << std::endl; @@ -457,7 +463,7 @@ bool Entity::init(AttributeMap &overrides, if (not controllers_.empty()) { if (autonomies_.empty()) { - controllers_.front()->set_desired_state(state_); + controllers_.front()->set_desired_state(state_belief_); } else { controllers_.front()->set_desired_state(autonomies_.front()->desired_state()); } @@ -478,15 +484,13 @@ bool Entity::parse_visual(std::map &info, return true; } - find_model_properties(it->second, cv_parse, - *file_search_, overrides, visual_, - mesh_found, texture_found); + find_model_properties( + it->second, cv_parse, *file_search_, overrides, visual_, mesh_found, texture_found); // Set the entity color. Use the team color by default std::vector color; auto it_color = info.find("color"); - if (it_color != info.end() and - str2container(it_color->second, ", ", color, 3)) { + if (it_color != info.end() and str2container(it_color->second, ", ", color, 3)) { } else { set(color, mp->team_info()[id_.team_id()].color); } @@ -495,7 +499,8 @@ bool Entity::parse_visual(std::map &info, std::string visual_model = boost::to_upper_copy(info["visual_model"]); if (mesh_found) { type_ = Contact::Type::MESH; - visual_->set_visual_mode(texture_found ? scrimmage_proto::ContactVisual::TEXTURE : scrimmage_proto::ContactVisual::COLOR); + visual_->set_visual_mode(texture_found ? scrimmage_proto::ContactVisual::TEXTURE + : scrimmage_proto::ContactVisual::COLOR); } else if (visual_model == std::string("QUADROTOR")) { type_ = Contact::Type::QUADROTOR; visual_->set_visual_mode(scrimmage_proto::ContactVisual::COLOR); @@ -518,26 +523,42 @@ bool Entity::ready() { return std::all_of(rng.begin(), rng.end(), func); }; - auto single_ready = [&](auto &plugin) {return plugin->ready();}; - auto values_single_ready = [&](auto &kv) {return kv.second->ready();}; + auto single_ready = [&](auto &plugin) { return plugin->ready(); }; + auto values_single_ready = [&](auto &kv) { return kv.second->ready(); }; - return all_ready(autonomies_, single_ready) - && all_ready(controllers_, single_ready) - && all_ready(sensors_, values_single_ready) - && motion_model_->ready(); + return all_ready(autonomies_, single_ready) && all_ready(controllers_, single_ready) && + all_ready(sensors_, values_single_ready) && motion_model_->ready(); } -StatePtr &Entity::state() {return state_;} -StatePtr &Entity::state_truth() {return state_truth_;} +StatePtr &Entity::state() { return state_belief_; } -std::vector &Entity::autonomies() {return autonomies_;} - -MotionModelPtr &Entity::motion() {return motion_model_;} +void Entity::set_state_belief(const StatePtr &other) { + if (state_belief_ == state_truth_) { + std::cout << "Decoupling State Belief and State Truth. Ensure that you have an explicit " + << "method of updating the state belief.\n"; + state_belief_ = std::make_shared(); + } + *state_belief_ = *other; +} -std::vector &Entity::controllers() { - return controllers_; +void Entity::set_state_belief(const State &other) { + if (state_belief_ == state_truth_) { + std::cout << "Decoupling State Belief and State Truth. Ensure that you have an explicit " + << "method of updating the state belief.\n"; + state_belief_ = std::make_shared(); + } + *state_belief_ = other; } +const std::shared_ptr Entity::state_belief() const { return state_belief_; } +StatePtr &Entity::state_truth() { return state_truth_; } + +std::vector &Entity::autonomies() { return autonomies_; } + +MotionModelPtr &Entity::motion() { return motion_model_; } + +std::vector &Entity::controllers() { return controllers_; } + void Entity::set_id(ID &id) { id_ = id; } ID &Entity::id() { return id_; } @@ -546,24 +567,19 @@ void Entity::collision() { health_points_ -= 1e9; } void Entity::hit() { health_points_--; } -void Entity::set_health_points(int health_points) -{ health_points_ = health_points; } +void Entity::set_health_points(int health_points) { health_points_ = health_points; } int Entity::health_points() { return health_points_; } -bool Entity::is_alive() { - return (health_points_ > 0); -} +bool Entity::is_alive() { return (health_points_ > 0); } bool Entity::posthumous(double t) { - bool any_autonomies = - std::any_of(autonomies_.begin(), autonomies_.end(), - [t](AutonomyPtr &a) {return a->posthumous(t);}); + bool any_autonomies = std::any_of( + autonomies_.begin(), autonomies_.end(), [t](AutonomyPtr &a) { return a->posthumous(t); }); return any_autonomies && motion_model_->posthumous(t); } -std::shared_ptr Entity::projection() -{ return proj_; } +std::shared_ptr Entity::projection() { return proj_; } MissionParsePtr Entity::mp() { return mp_; } @@ -574,17 +590,13 @@ RandomPtr Entity::random() { return random_; } Contact::Type Entity::type() { return type_; } -void Entity::set_visual_changed(bool visual_changed) -{ visual_changed_ = visual_changed; } +void Entity::set_visual_changed(bool visual_changed) { visual_changed_ = visual_changed; } bool Entity::visual_changed() { return visual_changed_; } -scrimmage_proto::ContactVisualPtr &Entity::contact_visual() -{ return visual_; } +scrimmage_proto::ContactVisualPtr &Entity::contact_visual() { return visual_; } -std::unordered_map &Entity::sensors() { - return sensors_; -} +std::unordered_map &Entity::sensors() { return sensors_; } std::unordered_map Entity::sensors(const std::string &sensor_name) { std::unordered_map out; @@ -608,15 +620,16 @@ bool Entity::active() { return active_; } void Entity::setup_desired_state() { if (controllers_.empty()) return; - auto it = std::find_if(autonomies_.rbegin(), autonomies_.rend(), - [&](auto autonomy) {return autonomy->get_is_controlling();}); + auto it = std::find_if(autonomies_.rbegin(), autonomies_.rend(), [&](auto autonomy) { + return autonomy->get_is_controlling(); + }); if (it != autonomies_.rend()) { controllers_.front()->set_desired_state((*it)->desired_state()); } } -std::unordered_map &Entity::services() {return services_;} +std::unordered_map &Entity::services() { return services_; } std::unordered_map &Entity::global_services() { return global_services_->services(); } @@ -626,15 +639,15 @@ void Entity::set_global_services(const GlobalServicePtr &global_services) { } bool Entity::call_service(scrimmage::MessageBasePtr req, - scrimmage::MessageBasePtr &res, const std::string &service_name) { + scrimmage::MessageBasePtr &res, + const std::string &service_name) { auto it = services_.find(service_name); if (it == services_.end()) { // First check for a global service of this name bool found = global_services_->call_service(req, res, service_name); if (!found) { - std::cout << "request for service (" - << service_name - << ") that does not exist" << std::endl; + std::cout << "request for service (" << service_name << ") that does not exist" + << std::endl; std::cout << "services are: "; for (auto &kv : services_) { std::cout << kv.first << ", "; @@ -657,9 +670,7 @@ bool Entity::call_service(scrimmage::MessageBasePtr req, } } -void Entity::print(const std::string &msg) { - std::cout << msg << std::endl; -} +void Entity::print(const std::string &msg) { std::cout << msg << std::endl; } void Entity::close(double t) { for (AutonomyPtr autonomy : autonomies_) { @@ -684,7 +695,7 @@ void Entity::close(double t) { mp_ = nullptr; proj_ = nullptr; random_ = nullptr; - state_ = nullptr; + state_belief_ = nullptr; state_truth_ = nullptr; properties_.clear(); sensors_.clear(); @@ -699,14 +710,11 @@ void Entity::close(double t) { time_ = nullptr; } -std::unordered_map &Entity::properties() { - return properties_; -} +std::unordered_map &Entity::properties() { return properties_; } -void Entity::set_time_ptr(TimePtr t) {time_ = t;} +void Entity::set_time_ptr(TimePtr t) { time_ = t; } void Entity::set_printer(PrintPtr printer) { printer_ = printer; } - // cppcheck-suppress passedByValue void Entity::set_projection(const std::shared_ptr &proj) { proj_ = proj; diff --git a/src/entity/External.cpp b/src/entity/External.cpp index ed5a264851..cfbb4f32f9 100644 --- a/src/entity/External.cpp +++ b/src/entity/External.cpp @@ -196,7 +196,7 @@ bool External::create_entity(const std::string &mission_file, entity_->set_random(random); entity_->contacts() = contacts; entity_->rtree() = rtree; - entity_->state() = std::make_shared(); + entity_->state_truth() = std::make_shared(); call_update_contacts(time_->t()); auto it = entity_->contacts()->find(entity_id); @@ -434,10 +434,10 @@ void External::update_ents() { auto ent = id.id() == entity_->id().id() ? entity_ : std::make_shared(); ent->id() = id; - if (ent->state()) { - *ent->state() = *kv.second.state(); + if (ent->state_truth()) { + *ent->state_truth() = *kv.second.state(); } else { - ent->state() = kv.second.state(); + ent->state_truth() = kv.second.state(); } ents_.push_back(ent); diff --git a/src/math/State.cpp b/src/math/State.cpp index 94605c3981..dc75672df4 100644 --- a/src/math/State.cpp +++ b/src/math/State.cpp @@ -31,40 +31,51 @@ */ #include -#include #include +#include -#include #include +#include namespace scrimmage { -State::State() : pos_(0, 0, 0), vel_(0, 0, 0), ang_vel_(0, 0, 0) {} +State::State() + : pos_(0, 0, 0), + vel_(0, 0, 0), + ang_vel_(0, 0, 0) {} + +State::State(Eigen::Vector3d _pos, Eigen::Vector3d _vel, Eigen::Vector3d _ang_vel, Quaternion _quat) + : pos_(_pos), + vel_(_vel), + ang_vel_(_ang_vel), + quat_(_quat) {} -State::State(Eigen::Vector3d _pos, Eigen::Vector3d _vel, - Eigen::Vector3d _ang_vel, Quaternion _quat) : - pos_(_pos), vel_(_vel), ang_vel_(_ang_vel), quat_(_quat) {} +State::State(const State &other) + : pos_{other.pos_}, + vel_{other.vel_}, + ang_vel_{other.ang_vel_}, + quat_{other.quat_} {} State::~State() {} -Eigen::Vector3d &State::pos() {return pos_;} -Eigen::Vector3d &State::vel() {return vel_;} -Eigen::Vector3d &State::ang_vel() {return ang_vel_;} -Quaternion &State::quat() {return quat_;} -const Eigen::Vector3d &State::pos() const {return pos_;} -const Eigen::Vector3d &State::vel() const {return vel_;} -const Eigen::Vector3d &State::ang_vel() const {return ang_vel_;} -const Quaternion &State::quat() const {return quat_;} +Eigen::Vector3d &State::pos() { return pos_; } +Eigen::Vector3d &State::vel() { return vel_; } +Eigen::Vector3d &State::ang_vel() { return ang_vel_; } +Quaternion &State::quat() { return quat_; } +const Eigen::Vector3d &State::pos() const { return pos_; } +const Eigen::Vector3d &State::vel() const { return vel_; } +const Eigen::Vector3d &State::ang_vel() const { return ang_vel_; } +const Quaternion &State::quat() const { return quat_; } // for backwards compatibility -const Eigen::Vector3d &State::pos_const() const {return pos_;} -const Eigen::Vector3d &State::vel_const() const {return vel_;} -const Eigen::Vector3d &State::ang_vel_const() const {return ang_vel_;} -const Quaternion &State::quat_const() const {return quat_;} -void State::set_pos(const Eigen::Vector3d &pos) {pos_ = pos;} -void State::set_vel(const Eigen::Vector3d &vel) {vel_ = vel;} -void State::set_ang_vel(const Eigen::Vector3d &ang_vel) {ang_vel_ = ang_vel;} -void State::set_quat(const Quaternion &quat) {quat_ = quat;} +const Eigen::Vector3d &State::pos_const() const { return pos_; } +const Eigen::Vector3d &State::vel_const() const { return vel_; } +const Eigen::Vector3d &State::ang_vel_const() const { return ang_vel_; } +const Quaternion &State::quat_const() const { return quat_; } +void State::set_pos(const Eigen::Vector3d &pos) { pos_ = pos; } +void State::set_vel(const Eigen::Vector3d &vel) { vel_ = vel; } +void State::set_ang_vel(const Eigen::Vector3d &ang_vel) { ang_vel_ = ang_vel; } +void State::set_quat(const Quaternion &quat) { quat_ = quat; } bool State::InFieldOfView(State &other, double fov_width, double fov_height) const { Eigen::Vector3d rel_pos = this->rel_pos_local_frame(other.pos()); @@ -112,17 +123,16 @@ Eigen::Matrix4d State::tf_matrix(bool enable_translate) { return mat4; } -std::ostream& operator<<(std::ostream& os, const State& s) { +std::ostream &operator<<(std::ostream &os, const State &s) { const Quaternion &q = s.quat(); - os << "(" << eigen_str(s.pos_, s.output_precision) - << "), (" << eigen_str(s.vel_, s.output_precision) - << "), (" << eigen_str(s.ang_vel_, s.output_precision) - << "), (" - << std::setprecision(s.output_precision) << q.roll() << ", " - << std::setprecision(s.output_precision) << q.pitch() << ", " - << std::setprecision(s.output_precision) << q.yaw() << ")"; + os << "(" << eigen_str(s.pos_, s.output_precision) << "), (" + << eigen_str(s.vel_, s.output_precision) << "), (" + << eigen_str(s.ang_vel_, s.output_precision) << "), (" + << std::setprecision(s.output_precision) << q.roll() << ", " + << std::setprecision(s.output_precision) << q.pitch() << ", " + << std::setprecision(s.output_precision) << q.yaw() << ")"; return os; } -} // namespace scrimmage +} // namespace scrimmage diff --git a/src/plugins/autonomy/GoToWaypoint/GoToWaypoint.cpp b/src/plugins/autonomy/GoToWaypoint/GoToWaypoint.cpp index d2a7d8a4f8..fd614603b9 100644 --- a/src/plugins/autonomy/GoToWaypoint/GoToWaypoint.cpp +++ b/src/plugins/autonomy/GoToWaypoint/GoToWaypoint.cpp @@ -64,7 +64,7 @@ void GoToWaypoint::init(std::map ¶ms) { bool GoToWaypoint::step_autonomy(double t, double dt) { if (waypoint_status_) { - sc::State ns = *(parent_->state()); + sc::State ns = *(parent_->state_belief()); double lat = (waypoint_[2] == "X") ? ns.pos()(0): stod(waypoint_[2]); double lon = (waypoint_[3] == "Y") ? ns.pos()(1): stod(waypoint_[3]); diff --git a/src/plugins/sensor/NoisyState/NoisyState.cpp b/src/plugins/sensor/NoisyState/NoisyState.cpp index f5d68e51a0..6e254287fd 100644 --- a/src/plugins/sensor/NoisyState/NoisyState.cpp +++ b/src/plugins/sensor/NoisyState/NoisyState.cpp @@ -70,8 +70,7 @@ void NoisyState::init(std::map ¶ms) { // Move the enity's state pointer to use the state instance provided by // this plugin. - parent_->state() = std::make_shared(); - *(parent_->state()) = *(parent_->state_truth()); + parent_->set_state_belief(parent_->state_truth()); return; } @@ -99,7 +98,7 @@ bool NoisyState::step() { pub_->publish(msg); // Update the entity's state. - *(parent_->state()) = static_cast(msg->data); + parent_->set_state_belief(static_cast(msg->data)); return true; } diff --git a/src/plugins/sensor/RLSimpleSensor/RLSimpleSensor.cpp b/src/plugins/sensor/RLSimpleSensor/RLSimpleSensor.cpp index 52346ce30d..c0ea9fff32 100644 --- a/src/plugins/sensor/RLSimpleSensor/RLSimpleSensor.cpp +++ b/src/plugins/sensor/RLSimpleSensor/RLSimpleSensor.cpp @@ -43,7 +43,7 @@ namespace scrimmage { namespace sensor { void RLSimpleSensor::get_observation(double *data, uint32_t beg_idx, uint32_t /*end_idx*/) { - data[beg_idx] = parent_->state()->pos()(0); + data[beg_idx] = parent_->state_belief()->pos()(0); data[beg_idx + 1] = time_->t(); } diff --git a/src/plugins/sensor/SimpleINS/SimpleINS.cpp b/src/plugins/sensor/SimpleINS/SimpleINS.cpp index dafcb8f9dd..54b8f2164a 100644 --- a/src/plugins/sensor/SimpleINS/SimpleINS.cpp +++ b/src/plugins/sensor/SimpleINS/SimpleINS.cpp @@ -88,8 +88,8 @@ void SimpleINS::init(std::map ¶ms) { surface_timer_ = sc::get("surface_timer", params, 10); - parent_->state() = std::make_shared(); - *(parent_->state()) = *(parent_->state_truth()); + + parent_->set_state_belief(parent_->state_truth()); } @@ -190,7 +190,7 @@ bool SimpleINS::step() { pub_->publish(msg); // Update the entity's state. - *(parent_->state()) = static_cast(msg->data); + parent_->set_state_belief(static_cast(msg->data)); return true; } diff --git a/src/simcontrol/SimControl.cpp b/src/simcontrol/SimControl.cpp index a7c8589e49..c13dded958 100644 --- a/src/simcontrol/SimControl.cpp +++ b/src/simcontrol/SimControl.cpp @@ -390,7 +390,7 @@ bool SimControl::generate_entity(const int &ent_desc_id, } ents_.push_back(ent); - rtree_->add(ent->state()->pos(), ent->id()); + rtree_->add(ent->state_truth()->pos(), ent->id()); contacts_mutex_.lock(); (*contacts_)[ent->id().id()] = Contact(ent->id(), ent->radius(), ent->state_truth(), @@ -421,7 +421,7 @@ void SimControl::join() { void SimControl::create_rtree(const unsigned int& additional_size) { rtree_->init(ents_.size() + additional_size); for (EntityPtr &ent: ents_) { - rtree_->add(ent->state()->pos(), ent->id()); + rtree_->add(ent->state_truth()->pos(), ent->id()); } } @@ -1599,7 +1599,7 @@ bool SimControl::run_entities() { // Check if any entity has NaN in its state for (EntityPtr &ent : ents_) { - if (ent->state()->pos().hasNaN()) { + if (ent->state_truth()->pos().hasNaN()) { cout << "WARNING: Entity with motion model, " << ent->motion()->name() << ", contains a NaN value." << endl << "Check your time step values and for NaN values coming " From 3777041ff9b37b4737009fdf37754bdb08b59f74 Mon Sep 17 00:00:00 2001 From: Wesley Ford Date: Mon, 18 Nov 2024 08:58:29 -0500 Subject: [PATCH 2/2] Updates to file formatting --- include/scrimmage/entity/Entity.h | 126 ++++++------- src/entity/Entity.cpp | 284 ++++++++++++++++-------------- src/math/State.cpp | 105 +++++++---- 3 files changed, 283 insertions(+), 232 deletions(-) diff --git a/include/scrimmage/entity/Entity.h b/include/scrimmage/entity/Entity.h index 6cfee7270b..7715142c38 100644 --- a/include/scrimmage/entity/Entity.h +++ b/include/scrimmage/entity/Entity.h @@ -39,6 +39,7 @@ #include #include +#include #include #include #include @@ -48,15 +49,13 @@ #include #include -#include - namespace scrimmage_proto { using ContactVisualPtr = std::shared_ptr; } namespace scrimmage { -using Service = std::function; +using Service = std::function; typedef std::map> AttributeMap; @@ -65,32 +64,22 @@ class Entity : public std::enable_shared_from_this { /*! \name utilities */ ///@{ - bool init(AttributeMap &overrides, - std::map &info, - std::shared_ptr> &id_to_team_map, - std::shared_ptr> &id_to_ent_map, - ContactMapPtr &contacts, - MissionParsePtr mp, - const std::shared_ptr &proj, - int id, - int ent_desc_id, - PluginManagerPtr plugin_manager, - FileSearchPtr &file_search, - RTreePtr &rtree, - PubSubPtr &pubsub, - PrintPtr &printer, - TimePtr &time, - const ParameterServerPtr ¶m_server, - const GlobalServicePtr &global_services, - const std::set &plugin_tags, - std::function &)> param_override_func, - const int &debug_level = 0); - - void print_plugins(std::ostream &out) const; - - bool parse_visual(std::map &info, - MissionParsePtr mp, - std::map &overrides); + bool init(AttributeMap& overrides, std::map& info, + std::shared_ptr>& id_to_team_map, + std::shared_ptr>& id_to_ent_map, + ContactMapPtr& contacts, MissionParsePtr mp, + const std::shared_ptr& proj, int id, int ent_desc_id, + PluginManagerPtr plugin_manager, FileSearchPtr& file_search, RTreePtr& rtree, + PubSubPtr& pubsub, PrintPtr& printer, TimePtr& time, + const ParameterServerPtr& param_server, const GlobalServicePtr& global_services, + const std::set& plugin_tags, + std::function&)> param_override_func, + const int& debug_level = 0); + + void print_plugins(std::ostream& out) const; + + bool parse_visual(std::map& info, MissionParsePtr mp, + std::map& overrides); void close(double t); void collision(); @@ -101,15 +90,15 @@ class Entity : public std::enable_shared_from_this { void setup_desired_state(); bool ready(); - bool call_service(MessageBasePtr req, MessageBasePtr &res, const std::string &service_name); + bool call_service(MessageBasePtr req, MessageBasePtr& res, const std::string& service_name); - bool call_service(MessageBasePtr &res, const std::string &service_name) { + bool call_service(MessageBasePtr& res, const std::string& service_name) { return call_service(std::make_shared(), res, service_name); } template ::value, void>::type> - bool call_service(MessageBasePtr req, T &res, const std::string &service_name) { + bool call_service(MessageBasePtr req, T& res, const std::string& service_name) { MessageBasePtr res_base; if (call_service(req, res_base, service_name)) { res = std::dynamic_pointer_cast(res_base); @@ -126,7 +115,7 @@ class Entity : public std::enable_shared_from_this { template ::value, void>::type> - bool call_service(T &res, const std::string &service_name) { + bool call_service(T& res, const std::string& service_name) { return call_service(std::make_shared(), res, service_name); } ///@} @@ -134,24 +123,24 @@ class Entity : public std::enable_shared_from_this { /*! \name getters/setters */ ///@{ [[deprecated("Use state_belief() or state_truth() to query state information instead")]] - StatePtr &state(); + StatePtr& state(); - void set_state_belief(const StatePtr &other); - void set_state_belief(const State &other); + void set_state_belief(const StatePtr& other); + void set_state_belief(const State& other); const std::shared_ptr state_belief() const; - StatePtr &state_truth(); - std::vector &autonomies(); - MotionModelPtr &motion(); - std::vector &controllers(); + StatePtr& state_truth(); + std::vector& autonomies(); + MotionModelPtr& motion(); + std::vector& controllers(); - void set_id(ID &id); - ID &id(); + void set_id(ID& id); + ID& id(); void set_health_points(int health_points); int health_points(); std::shared_ptr projection(); - void set_projection(const std::shared_ptr &proj); + void set_projection(const std::shared_ptr& proj); void set_mp(MissionParsePtr mp); MissionParsePtr mp(); @@ -164,38 +153,54 @@ class Entity : public std::enable_shared_from_this { void set_visual_changed(bool visual_changed); bool visual_changed(); - scrimmage_proto::ContactVisualPtr &contact_visual(); + scrimmage_proto::ContactVisualPtr& contact_visual(); - std::unordered_map &sensors(); - std::unordered_map sensors(const std::string &sensor_name); - SensorPtr sensor(const std::string &sensor_name); + std::unordered_map& sensors(); + std::unordered_map sensors(const std::string& sensor_name); + SensorPtr sensor(const std::string& sensor_name); // Enables creating services at the entity level - std::unordered_map &services(); + std::unordered_map& services(); // Enables creating services at the global level (especially for entity interactions, etc.) - std::unordered_map &global_services(); + std::unordered_map& global_services(); // Enables setting these for entity interactions - void set_global_services(const GlobalServicePtr &global_services); + void set_global_services(const GlobalServicePtr& global_services); - std::unordered_map &properties(); + std::unordered_map& properties(); void set_active(bool active); bool active(); - ContactMapPtr &contacts() { return contacts_; } - RTreePtr &rtree() { return rtree_; } + ContactMapPtr& contacts() { + return contacts_; + } + RTreePtr& rtree() { + return rtree_; + } - PluginManagerPtr &plugin_manager() { return plugin_manager_; } + PluginManagerPtr& plugin_manager() { + return plugin_manager_; + } - FileSearchPtr &file_search() { return file_search_; } + FileSearchPtr& file_search() { + return file_search_; + } - PubSubPtr &pubsub() { return pubsub_; } + PubSubPtr& pubsub() { + return pubsub_; + } - PrintPtr &printer() { return printer_; } + PrintPtr& printer() { + return printer_; + } - const ParameterServerPtr ¶m_server() { return param_server_; } + const ParameterServerPtr& param_server() { + return param_server_; + } - double radius() { return radius_; } + double radius() { + return radius_; + } void set_time_ptr(TimePtr t); void set_printer(PrintPtr printer); @@ -235,7 +240,7 @@ class Entity : public std::enable_shared_from_this { double radius_ = 1; - void print(const std::string &msg); + void print(const std::string& msg); PluginManagerPtr plugin_manager_; FileSearchPtr file_search_; PubSubPtr pubsub_; @@ -243,7 +248,6 @@ class Entity : public std::enable_shared_from_this { GlobalServicePtr global_services_; ParameterServerPtr param_server_; TimePtr time_; - }; using EntityPtr = std::shared_ptr; diff --git a/src/entity/Entity.cpp b/src/entity/Entity.cpp index 25485241ab..0d5ad9c61b 100644 --- a/src/entity/Entity.cpp +++ b/src/entity/Entity.cpp @@ -48,15 +48,14 @@ #include #include -#include -#include -#include - #include #include #include #include #include +#include +#include +#include using std::cout; using std::endl; @@ -67,26 +66,17 @@ namespace ba = boost::adaptors; namespace scrimmage { -bool Entity::init(AttributeMap &overrides, - std::map &info, - std::shared_ptr> &id_to_team_map, - std::shared_ptr> &id_to_ent_map, - ContactMapPtr &contacts, - MissionParsePtr mp, - const std::shared_ptr &proj, - int id, - int ent_desc_id, - PluginManagerPtr plugin_manager, - FileSearchPtr &file_search, - RTreePtr &rtree, - PubSubPtr &pubsub, - PrintPtr &printer, - TimePtr &time, - const ParameterServerPtr ¶m_server, - const GlobalServicePtr &global_services, - const std::set &plugin_tags, - std::function &)> param_override_func, - const int &debug_level) { +bool Entity::init(AttributeMap& overrides, std::map& info, + std::shared_ptr>& id_to_team_map, + std::shared_ptr>& id_to_ent_map, + ContactMapPtr& contacts, MissionParsePtr mp, + const std::shared_ptr& proj, int id, + int ent_desc_id, PluginManagerPtr plugin_manager, FileSearchPtr& file_search, + RTreePtr& rtree, PubSubPtr& pubsub, PrintPtr& printer, TimePtr& time, + const ParameterServerPtr& param_server, const GlobalServicePtr& global_services, + const std::set& plugin_tags, + std::function&)> param_override_func, + const int& debug_level) { pubsub_ = pubsub; printer_ = printer; global_services_ = global_services; @@ -162,13 +152,9 @@ bool Entity::init(AttributeMap &overrides, while (info.count(sensor_order_name) > 0) { ConfigParse config_parse; std::string sensor_name = info[sensor_order_name]; - PluginStatus status = - plugin_manager->make_plugin("scrimmage::Sensor", - sensor_name, - *file_search, - config_parse, - overrides[sensor_order_name], - plugin_tags); + PluginStatus status = plugin_manager->make_plugin( + "scrimmage::Sensor", sensor_name, *file_search, config_parse, + overrides[sensor_order_name], plugin_tags); if (status.status == PluginStatus::cast_failed) { std::cout << "Failed to open sensor plugin: " << sensor_name << std::endl; return false; @@ -191,8 +177,8 @@ bool Entity::init(AttributeMap &overrides, if (it_rpy != overrides[sensor_order_name].end()) { str2container(it_rpy->second, " ", tf_rpy, 3); } - sensor->transform()->quat().set( - Angles::deg2rad(tf_rpy[0]), Angles::deg2rad(tf_rpy[1]), Angles::deg2rad(tf_rpy[2])); + sensor->transform()->quat().set(Angles::deg2rad(tf_rpy[0]), Angles::deg2rad(tf_rpy[1]), + Angles::deg2rad(tf_rpy[2])); sensor->set_parent(parent); sensor->set_pubsub(pubsub); @@ -229,13 +215,9 @@ bool Entity::init(AttributeMap &overrides, bool init_empty_motion_model = true; if (info.count("motion_model") > 0) { ConfigParse config_parse; - PluginStatus status = - plugin_manager->make_plugin("scrimmage::MotionModel", - info["motion_model"], - *file_search, - config_parse, - overrides["motion_model"], - plugin_tags); + PluginStatus status = plugin_manager->make_plugin( + "scrimmage::MotionModel", info["motion_model"], *file_search, config_parse, + overrides["motion_model"], plugin_tags); if (status.status == PluginStatus::cast_failed) { cout << "Failed to open motion model plugin: " << info["motion_model"] << endl; return false; @@ -298,18 +280,13 @@ bool Entity::init(AttributeMap &overrides, // correctly. Last controller connects to motion model, second to last // controller connects to the last controller. for (std::list::reverse_iterator rit = controller_names.rbegin(); - rit != controller_names.rend(); - ++rit) { + rit != controller_names.rend(); ++rit) { std::string controller_name = *rit; ConfigParse config_parse; - PluginStatus status = - plugin_manager_->make_plugin("scrimmage::Controller", - info[controller_name], - *file_search, - config_parse, - overrides[controller_name], - plugin_tags); + PluginStatus status = plugin_manager_->make_plugin( + "scrimmage::Controller", info[controller_name], *file_search, config_parse, + overrides[controller_name], plugin_tags); if (status.status == PluginStatus::cast_failed) { std::cout << "Failed to open controller plugin: " << controller_name << std::endl; return false; @@ -410,24 +387,10 @@ bool Entity::init(AttributeMap &overrides, // Create the autonomy plugins from the autonomy_names list. for (auto autonomy_name : autonomy_names) { - auto autonomy = make_autonomy(info[autonomy_name], - plugin_manager, - overrides[autonomy_name], - parent, - state_belief_, - id_to_team_map, - id_to_ent_map, - proj_, - contacts, - file_search, - rtree, - pubsub, - time, - param_server, - plugin_tags, - param_override_func, - controllers_, - debug_level); + auto autonomy = make_autonomy( + info[autonomy_name], plugin_manager, overrides[autonomy_name], parent, state_belief_, + id_to_team_map, id_to_ent_map, proj_, contacts, file_search, rtree, pubsub, time, + param_server, plugin_tags, param_override_func, controllers_, debug_level); if (autonomy) { autonomies_.push_back(*autonomy); @@ -441,9 +404,9 @@ bool Entity::init(AttributeMap &overrides, // Verify that at least one autonomy provides the inputs to the first // controller if the first controller requires some VariableIO input. - if (connect_entity && not controllers_.empty() && - controllers_.front()->vars().input_variable_index().size() > 0) { - auto verify_io = [&](auto &autonomy) { + if (connect_entity && not controllers_.empty() + && controllers_.front()->vars().input_variable_index().size() > 0) { + auto verify_io = [&](auto& autonomy) { return verify_io_connection(autonomy->vars(), controllers_.front()->vars()); }; if (boost::algorithm::none_of(autonomies_, verify_io)) { @@ -452,7 +415,7 @@ bool Entity::init(AttributeMap &overrides, << "no autonomies provide inputs required by Controller " << std::quoted(controllers_.front()->name()) << ". Add VariableIO output declarations in "; - auto get_name = [&](auto &p) { return p->name(); }; + auto get_name = [&](auto& p) { return p->name(); }; br::copy(autonomies_ | ba::transformed(get_name), out_it); std::cout << "as follows " << std::endl; @@ -471,9 +434,8 @@ bool Entity::init(AttributeMap &overrides, return true; } -bool Entity::parse_visual(std::map &info, - MissionParsePtr mp, - std::map &overrides) { +bool Entity::parse_visual(std::map& info, MissionParsePtr mp, + std::map& overrides) { visual_->set_id(id_.id()); visual_->set_opacity(1.0); @@ -484,8 +446,8 @@ bool Entity::parse_visual(std::map &info, return true; } - find_model_properties( - it->second, cv_parse, *file_search_, overrides, visual_, mesh_found, texture_found); + find_model_properties(it->second, cv_parse, *file_search_, overrides, visual_, mesh_found, + texture_found); // Set the entity color. Use the team color by default std::vector color; @@ -519,20 +481,22 @@ bool Entity::parse_visual(std::map &info, } bool Entity::ready() { - auto all_ready = [&](auto &rng, auto &func) { + auto all_ready = [&](auto& rng, auto& func) { return std::all_of(rng.begin(), rng.end(), func); }; - auto single_ready = [&](auto &plugin) { return plugin->ready(); }; - auto values_single_ready = [&](auto &kv) { return kv.second->ready(); }; + auto single_ready = [&](auto& plugin) { return plugin->ready(); }; + auto values_single_ready = [&](auto& kv) { return kv.second->ready(); }; - return all_ready(autonomies_, single_ready) && all_ready(controllers_, single_ready) && - all_ready(sensors_, values_single_ready) && motion_model_->ready(); + return all_ready(autonomies_, single_ready) && all_ready(controllers_, single_ready) + && all_ready(sensors_, values_single_ready) && motion_model_->ready(); } -StatePtr &Entity::state() { return state_belief_; } +StatePtr& Entity::state() { + return state_belief_; +} -void Entity::set_state_belief(const StatePtr &other) { +void Entity::set_state_belief(const StatePtr& other) { if (state_belief_ == state_truth_) { std::cout << "Decoupling State Belief and State Truth. Ensure that you have an explicit " << "method of updating the state belief.\n"; @@ -541,7 +505,7 @@ void Entity::set_state_belief(const StatePtr &other) { *state_belief_ = *other; } -void Entity::set_state_belief(const State &other) { +void Entity::set_state_belief(const State& other) { if (state_belief_ == state_truth_) { std::cout << "Decoupling State Belief and State Truth. Ensure that you have an explicit " << "method of updating the state belief.\n"; @@ -550,57 +514,101 @@ void Entity::set_state_belief(const State &other) { *state_belief_ = other; } -const std::shared_ptr Entity::state_belief() const { return state_belief_; } -StatePtr &Entity::state_truth() { return state_truth_; } +const std::shared_ptr Entity::state_belief() const { + return state_belief_; +} +StatePtr& Entity::state_truth() { + return state_truth_; +} -std::vector &Entity::autonomies() { return autonomies_; } +std::vector& Entity::autonomies() { + return autonomies_; +} -MotionModelPtr &Entity::motion() { return motion_model_; } +MotionModelPtr& Entity::motion() { + return motion_model_; +} -std::vector &Entity::controllers() { return controllers_; } +std::vector& Entity::controllers() { + return controllers_; +} -void Entity::set_id(ID &id) { id_ = id; } +void Entity::set_id(ID& id) { + id_ = id; +} -ID &Entity::id() { return id_; } +ID& Entity::id() { + return id_; +} -void Entity::collision() { health_points_ -= 1e9; } +void Entity::collision() { + health_points_ -= 1e9; +} -void Entity::hit() { health_points_--; } +void Entity::hit() { + health_points_--; +} -void Entity::set_health_points(int health_points) { health_points_ = health_points; } +void Entity::set_health_points(int health_points) { + health_points_ = health_points; +} -int Entity::health_points() { return health_points_; } +int Entity::health_points() { + return health_points_; +} -bool Entity::is_alive() { return (health_points_ > 0); } +bool Entity::is_alive() { + return (health_points_ > 0); +} bool Entity::posthumous(double t) { - bool any_autonomies = std::any_of( - autonomies_.begin(), autonomies_.end(), [t](AutonomyPtr &a) { return a->posthumous(t); }); + bool any_autonomies = std::any_of(autonomies_.begin(), autonomies_.end(), + [t](AutonomyPtr& a) { return a->posthumous(t); }); return any_autonomies && motion_model_->posthumous(t); } -std::shared_ptr Entity::projection() { return proj_; } +std::shared_ptr Entity::projection() { + return proj_; +} -MissionParsePtr Entity::mp() { return mp_; } +MissionParsePtr Entity::mp() { + return mp_; +} -void Entity::set_mp(MissionParsePtr mp) { mp_ = mp; } -void Entity::set_random(RandomPtr random) { random_ = random; } +void Entity::set_mp(MissionParsePtr mp) { + mp_ = mp; +} +void Entity::set_random(RandomPtr random) { + random_ = random; +} -RandomPtr Entity::random() { return random_; } +RandomPtr Entity::random() { + return random_; +} -Contact::Type Entity::type() { return type_; } +Contact::Type Entity::type() { + return type_; +} -void Entity::set_visual_changed(bool visual_changed) { visual_changed_ = visual_changed; } +void Entity::set_visual_changed(bool visual_changed) { + visual_changed_ = visual_changed; +} -bool Entity::visual_changed() { return visual_changed_; } +bool Entity::visual_changed() { + return visual_changed_; +} -scrimmage_proto::ContactVisualPtr &Entity::contact_visual() { return visual_; } +scrimmage_proto::ContactVisualPtr& Entity::contact_visual() { + return visual_; +} -std::unordered_map &Entity::sensors() { return sensors_; } +std::unordered_map& Entity::sensors() { + return sensors_; +} -std::unordered_map Entity::sensors(const std::string &sensor_name) { +std::unordered_map Entity::sensors(const std::string& sensor_name) { std::unordered_map out; - for (auto &kv : sensors_) { + for (auto& kv : sensors_) { if (kv.first.find(sensor_name) != std::string::npos) { out[kv.first] = kv.second; } @@ -608,39 +616,43 @@ std::unordered_map Entity::sensors(const std::string &se return out; } -SensorPtr Entity::sensor(const std::string &sensor_name) { +SensorPtr Entity::sensor(const std::string& sensor_name) { std::unordered_map out = sensors(sensor_name); return out.empty() ? nullptr : out.begin()->second; } -void Entity::set_active(bool active) { active_ = active; } +void Entity::set_active(bool active) { + active_ = active; +} -bool Entity::active() { return active_; } +bool Entity::active() { + return active_; +} void Entity::setup_desired_state() { if (controllers_.empty()) return; - auto it = std::find_if(autonomies_.rbegin(), autonomies_.rend(), [&](auto autonomy) { - return autonomy->get_is_controlling(); - }); + auto it = std::find_if(autonomies_.rbegin(), autonomies_.rend(), + [&](auto autonomy) { return autonomy->get_is_controlling(); }); if (it != autonomies_.rend()) { controllers_.front()->set_desired_state((*it)->desired_state()); } } -std::unordered_map &Entity::services() { return services_; } -std::unordered_map &Entity::global_services() { +std::unordered_map& Entity::services() { + return services_; +} +std::unordered_map& Entity::global_services() { return global_services_->services(); } -void Entity::set_global_services(const GlobalServicePtr &global_services) { +void Entity::set_global_services(const GlobalServicePtr& global_services) { global_services_ = global_services; } -bool Entity::call_service(scrimmage::MessageBasePtr req, - scrimmage::MessageBasePtr &res, - const std::string &service_name) { +bool Entity::call_service(scrimmage::MessageBasePtr req, scrimmage::MessageBasePtr& res, + const std::string& service_name) { auto it = services_.find(service_name); if (it == services_.end()) { // First check for a global service of this name @@ -649,7 +661,7 @@ bool Entity::call_service(scrimmage::MessageBasePtr req, std::cout << "request for service (" << service_name << ") that does not exist" << std::endl; std::cout << "services are: "; - for (auto &kv : services_) { + for (auto& kv : services_) { std::cout << kv.first << ", "; } std::cout << std::endl; @@ -659,7 +671,7 @@ bool Entity::call_service(scrimmage::MessageBasePtr req, } } - Service &service = it->second; + Service& service = it->second; bool success = service(req, res); if (!success) { @@ -670,14 +682,16 @@ bool Entity::call_service(scrimmage::MessageBasePtr req, } } -void Entity::print(const std::string &msg) { std::cout << msg << std::endl; } +void Entity::print(const std::string& msg) { + std::cout << msg << std::endl; +} void Entity::close(double t) { for (AutonomyPtr autonomy : autonomies_) { autonomy->close_plugin(t); } - for (auto &kv : sensors_) { + for (auto& kv : sensors_) { kv.second->close_plugin(t); } @@ -710,19 +724,25 @@ void Entity::close(double t) { time_ = nullptr; } -std::unordered_map &Entity::properties() { return properties_; } +std::unordered_map& Entity::properties() { + return properties_; +} -void Entity::set_time_ptr(TimePtr t) { time_ = t; } -void Entity::set_printer(PrintPtr printer) { printer_ = printer; } +void Entity::set_time_ptr(TimePtr t) { + time_ = t; +} +void Entity::set_printer(PrintPtr printer) { + printer_ = printer; +} // cppcheck-suppress passedByValue -void Entity::set_projection(const std::shared_ptr &proj) { +void Entity::set_projection(const std::shared_ptr& proj) { proj_ = proj; } -void Entity::print_plugins(std::ostream &out) const { +void Entity::print_plugins(std::ostream& out) const { out << "----------- Sensor -------------" << endl; - for (auto &kv : sensors_) { + for (auto& kv : sensors_) { out << kv.second->name() << endl; } out << "---------- Autonomy ------------" << endl; diff --git a/src/math/State.cpp b/src/math/State.cpp index dc75672df4..2b9c6a13c2 100644 --- a/src/math/State.cpp +++ b/src/math/State.cpp @@ -39,45 +39,72 @@ namespace scrimmage { -State::State() - : pos_(0, 0, 0), - vel_(0, 0, 0), - ang_vel_(0, 0, 0) {} +State::State() : pos_(0, 0, 0), vel_(0, 0, 0), ang_vel_(0, 0, 0) { +} State::State(Eigen::Vector3d _pos, Eigen::Vector3d _vel, Eigen::Vector3d _ang_vel, Quaternion _quat) - : pos_(_pos), - vel_(_vel), - ang_vel_(_ang_vel), - quat_(_quat) {} - -State::State(const State &other) - : pos_{other.pos_}, - vel_{other.vel_}, - ang_vel_{other.ang_vel_}, - quat_{other.quat_} {} - -State::~State() {} - -Eigen::Vector3d &State::pos() { return pos_; } -Eigen::Vector3d &State::vel() { return vel_; } -Eigen::Vector3d &State::ang_vel() { return ang_vel_; } -Quaternion &State::quat() { return quat_; } -const Eigen::Vector3d &State::pos() const { return pos_; } -const Eigen::Vector3d &State::vel() const { return vel_; } -const Eigen::Vector3d &State::ang_vel() const { return ang_vel_; } -const Quaternion &State::quat() const { return quat_; } + : pos_(_pos), vel_(_vel), ang_vel_(_ang_vel), quat_(_quat) { +} + +State::State(const State& other) + : pos_{other.pos_}, vel_{other.vel_}, ang_vel_{other.ang_vel_}, quat_{other.quat_} { +} + +State::~State() { +} + +Eigen::Vector3d& State::pos() { + return pos_; +} +Eigen::Vector3d& State::vel() { + return vel_; +} +Eigen::Vector3d& State::ang_vel() { + return ang_vel_; +} +Quaternion& State::quat() { + return quat_; +} +const Eigen::Vector3d& State::pos() const { + return pos_; +} +const Eigen::Vector3d& State::vel() const { + return vel_; +} +const Eigen::Vector3d& State::ang_vel() const { + return ang_vel_; +} +const Quaternion& State::quat() const { + return quat_; +} // for backwards compatibility -const Eigen::Vector3d &State::pos_const() const { return pos_; } -const Eigen::Vector3d &State::vel_const() const { return vel_; } -const Eigen::Vector3d &State::ang_vel_const() const { return ang_vel_; } -const Quaternion &State::quat_const() const { return quat_; } -void State::set_pos(const Eigen::Vector3d &pos) { pos_ = pos; } -void State::set_vel(const Eigen::Vector3d &vel) { vel_ = vel; } -void State::set_ang_vel(const Eigen::Vector3d &ang_vel) { ang_vel_ = ang_vel; } -void State::set_quat(const Quaternion &quat) { quat_ = quat; } - -bool State::InFieldOfView(State &other, double fov_width, double fov_height) const { +const Eigen::Vector3d& State::pos_const() const { + return pos_; +} +const Eigen::Vector3d& State::vel_const() const { + return vel_; +} +const Eigen::Vector3d& State::ang_vel_const() const { + return ang_vel_; +} +const Quaternion& State::quat_const() const { + return quat_; +} +void State::set_pos(const Eigen::Vector3d& pos) { + pos_ = pos; +} +void State::set_vel(const Eigen::Vector3d& vel) { + vel_ = vel; +} +void State::set_ang_vel(const Eigen::Vector3d& ang_vel) { + ang_vel_ = ang_vel; +} +void State::set_quat(const Quaternion& quat) { + quat_ = quat; +} + +bool State::InFieldOfView(State& other, double fov_width, double fov_height) const { Eigen::Vector3d rel_pos = this->rel_pos_local_frame(other.pos()); double az = atan2(rel_pos(1), rel_pos(0)); double norm_xy = sqrt(pow(rel_pos(0), 2) + pow(rel_pos(1), 2)); @@ -85,7 +112,7 @@ bool State::InFieldOfView(State &other, double fov_width, double fov_height) con return std::abs(az) < fov_width / 2 && std::abs(el) < fov_height / 2; } -Eigen::Vector3d State::rel_pos_local_frame(Eigen::Vector3d &other) const { +Eigen::Vector3d State::rel_pos_local_frame(Eigen::Vector3d& other) const { return quat_.rotate_reverse(other - pos_); } @@ -101,7 +128,7 @@ Eigen::Vector3d State::orient_global_frame() const { return quat_.rotate(Eigen::Vector3d::UnitX()); } -double State::rel_az(const Eigen::Vector3d &other) const { +double State::rel_az(const Eigen::Vector3d& other) const { Eigen::Vector3d diff = other - pos_; double az = atan2(diff(1), diff(0)); return Angles::angle_diff_rad(az, quat_.yaw()); @@ -123,8 +150,8 @@ Eigen::Matrix4d State::tf_matrix(bool enable_translate) { return mat4; } -std::ostream &operator<<(std::ostream &os, const State &s) { - const Quaternion &q = s.quat(); +std::ostream& operator<<(std::ostream& os, const State& s) { + const Quaternion& q = s.quat(); os << "(" << eigen_str(s.pos_, s.output_precision) << "), (" << eigen_str(s.vel_, s.output_precision) << "), ("