diff --git a/atos/modules/ObjectControl/inc/objectcontrol.hpp b/atos/modules/ObjectControl/inc/objectcontrol.hpp index 72fe68f07..88347aee3 100644 --- a/atos/modules/ObjectControl/inc/objectcontrol.hpp +++ b/atos/modules/ObjectControl/inc/objectcontrol.hpp @@ -270,8 +270,6 @@ class ObjectControl : public Module { //! \brief Read the configured object and trajectory files and load related data //! into the ScenarioHandler. bool loadScenario(); - //! \brief Read all object files and fill the list of TestObjects. - void loadObjectFiles(); //! \brief Transform the scenario trajectories relative to the trajectory of the //! specified object. void transformScenarioRelativeTo(const uint32_t objectID); diff --git a/atos/modules/ObjectControl/inc/testobject.hpp b/atos/modules/ObjectControl/inc/testobject.hpp index 6886ca3cd..16adeb8e5 100644 --- a/atos/modules/ObjectControl/inc/testobject.hpp +++ b/atos/modules/ObjectControl/inc/testobject.hpp @@ -136,6 +136,9 @@ class TestObject : public rclcpp::Node { virtual std::chrono::milliseconds getMaxAllowedMonitorPeriod() const { return this->maxAllowedMonitorPeriod; } + virtual void setMaxAllowedMonitorPeriod(const std::chrono::milliseconds maxAllowedMonitorPeriod) { + this->maxAllowedMonitorPeriod = maxAllowedMonitorPeriod; + } virtual MonitorMessage readMonitorMessage() { MonitorMessage retval; this->comms.mntr >> retval; diff --git a/atos/modules/ObjectControl/src/objectcontrol.cpp b/atos/modules/ObjectControl/src/objectcontrol.cpp index 9b3f01fce..2bf6fea29 100644 --- a/atos/modules/ObjectControl/src/objectcontrol.cpp +++ b/atos/modules/ObjectControl/src/objectcontrol.cpp @@ -53,10 +53,14 @@ ObjectControl::ObjectControl(std::shared_ptrcreate_callback_group(rclcpp::CallbackGroupType::Reentrant); ip_client_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); origin_client_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + this->declare_parameter("max_missing_heartbeats", 100); + this->declare_parameter("max_missing_monr", 100); + this->declare_parameter("monr_timeout_period_ms", 1000); + objectsConnectedTimer = create_wall_timer(1000ms, std::bind(&ObjectControl::publishObjectIds, this)); idClient = create_client( - ServiceNames::getObjectIds, rmw_qos_profile_services_default, id_client_cb_group_); + ServiceNames::getObjectIds, rmw_qos_profile_services_default, id_client_cb_group_); originClient = create_client( ServiceNames::getTestOrigin, rmw_qos_profile_services_default, origin_client_cb_group_); trajectoryClient = create_client( @@ -203,6 +207,8 @@ bool ObjectControl::loadScenario() { exec->add_node(object); objects.emplace(id, object); objects.at(id)->setTransmitterID(id); + objects.at(id)->setMaxAllowedMonitorPeriod( + std::chrono::milliseconds(this->get_parameter("monr_timeout_period_ms").as_int())); std::promise trajLoaded; auto trajectoryCallback = @@ -335,63 +341,6 @@ bool ObjectControl::loadScenario() { return false; } -void ObjectControl::loadObjectFiles() { - char path[MAX_FILE_PATH]; - std::vector errors; - - UtilGetObjectDirectoryPath(path, sizeof(path)); - fs::path objectDir(path); - if (!fs::exists(objectDir)) { - throw std::ios_base::failure("Object directory does not exist"); - } - - for (const auto& entry : fs::directory_iterator(objectDir)) { - if (fs::is_regular_file(entry.status())) { - const auto inputFile = entry.path(); - ObjectConfig conf(get_logger()); - try { - conf.parseConfigurationFile(inputFile); - uint32_t id = conf.getTransmitterID(); - RCLCPP_INFO(get_logger(), "Loaded configuration: %s", conf.toString().c_str()); - // Check preexisting - auto foundObject = objects.find(id); - if (foundObject == objects.end()) { - std::shared_ptr object = std::make_shared(id); - object->parseConfigurationFile(inputFile); - objects.emplace(id, object); - } else { - auto badID = conf.getTransmitterID(); - std::string errMsg = "Duplicate object ID " + std::to_string(badID) + " detected in files " + - objects.at(badID)->getTrajectoryFileName() + " and " + - conf.getTrajectoryFileName(); - throw std::invalid_argument(errMsg); - } - } catch (std::invalid_argument& e) { - RCLCPP_ERROR(get_logger(), e.what()); - errors.push_back(e); - } - } - } - - // Fix injector ID maps - reverse their direction - for (const auto& id : getVehicleIDs()) { - auto injMap = objects.at(id)->getObjectConfig().getInjectionMap(); - for (const auto& sourceID : injMap.sourceIDs) { - auto conf = objects.at(sourceID)->getObjectConfig(); - conf.addInjectionTarget(id); - objects.at(sourceID)->setObjectConfig(conf); - } - } - - if (!errors.empty()) { - objects.clear(); - std::ostringstream ostr; - auto append = [&ostr](const std::invalid_argument& e) { ostr << e.what() << std::endl; }; - std::for_each(errors.begin(), errors.end(), append); - throw std::invalid_argument("Failed to parse object file(s):\n" + ostr.str()); - } -} - uint32_t ObjectControl::getAnchorObjectID() const { for (auto& object : objects) { if (object.second->isAnchor()) { @@ -587,8 +536,8 @@ void ObjectControl::notifyObjectsConnected() { } void ObjectControl::connectToObject(std::shared_ptr obj, std::shared_future& connStopReq) { - const int maxConnHeabs = this->get_parameter("max_missing_heartbeats").as_int(); - constexpr int maxConnMonrs = 100; + const int maxConnHeabs = this->get_parameter("max_missing_heartbeats").as_int(); + const int maxConnMonrs = this->get_parameter("max_missing_monr").as_int(); try { if (!obj->isConnected()) { try { diff --git a/conf/conf/params.yaml b/conf/conf/params.yaml index 0d99a5bc2..8f3874f85 100644 --- a/conf/conf/params.yaml +++ b/conf/conf/params.yaml @@ -13,6 +13,8 @@ atos: object_control: ros__parameters: max_missing_heartbeats: 100 + max_missing_monr: 100 + monr_timeout_period_ms: 1000 transmitter_id: 15 osi_adapter: ros__parameters: