Add settings for MONR in params.yaml

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);

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;

atos/modules/ObjectControl/src/objectcontrol.cpp

@@ -53,10 +53,14 @@ ObjectControl::ObjectControl(std::shared_ptr<rclcpp::executors::MultiThreadedExe
 	traj_client_cb_group_	= this->create_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<atos_interfaces::srv::GetObjectIds>(
-	   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<atos_interfaces::srv::GetTestOrigin>(
 	  ServiceNames::getTestOrigin, rmw_qos_profile_services_default, origin_client_cb_group_);
 	trajectoryClient = create_client<atos_interfaces::srv::GetObjectTrajectory>(
@@ -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<bool> trajLoaded;
 			auto trajectoryCallback =
@@ -335,63 +341,6 @@ bool ObjectControl::loadScenario() {
 	return false;
 }
 
-void ObjectControl::loadObjectFiles() {
-	char path[MAX_FILE_PATH];
-	std::vector<std::invalid_argument> 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<TestObject> object = std::make_shared<TestObject>(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<TestObject> obj, std::shared_future<void>& 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 {

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: