Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -26,11 +26,25 @@ namespace ros2_medkit_gateway {

using json = nlohmann::json;

/// Error codes for parameter operations
enum class ParameterErrorCode {
NONE = 0, ///< No error (success)
NOT_FOUND, ///< Parameter does not exist
READ_ONLY, ///< Parameter is read-only and cannot be modified
SERVICE_UNAVAILABLE, ///< Parameter service not available (node unreachable)
TIMEOUT, ///< Operation timed out
TYPE_MISMATCH, ///< Value type doesn't match parameter type
INVALID_VALUE, ///< Invalid value for parameter
NO_DEFAULTS_CACHED, ///< No default values cached for reset operation
INTERNAL_ERROR ///< Internal/unexpected error
};

/// Result of a parameter operation
struct ParameterResult {
bool success;
json data;
std::string error_message;
ParameterErrorCode error_code{ParameterErrorCode::NONE}; ///< Structured error code for programmatic handling
};

/// Manager for ROS2 node parameters
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -82,6 +82,37 @@ struct AggregatedOperations {
}
};

/**
* @brief Node info for configuration aggregation
*/
struct NodeConfigInfo {
std::string node_fqn; ///< Fully qualified node name
std::string app_id; ///< Source app ID
std::string entity_id; ///< Owning entity ID (app/component/area)
};

/**
* @brief Aggregated configurations (node FQNs) result from entity hierarchy
*
* Unlike data/operations which store actual values, configurations stores
* the list of ROS 2 nodes whose parameters should be queried. This is because
* parameters are owned by nodes, and aggregated entities need to iterate
* over all child nodes.
*/
struct AggregatedConfigurations {
std::vector<NodeConfigInfo> nodes; ///< Nodes to query for parameters
std::vector<std::string> source_ids; ///< Entity IDs that contributed
std::string aggregation_level; ///< "app" | "component" | "area" | "function"
bool is_aggregated{false}; ///< true if collected from sub-entities

bool empty() const {
return nodes.empty();
}
size_t node_count() const {
return nodes.size();
}
};

/**
* @brief Cache statistics
*/
Expand Down Expand Up @@ -297,6 +328,48 @@ class ThreadSafeEntityCache {
*/
AggregatedData get_function_data(const std::string & function_id) const;

// =========================================================================
// Configuration aggregation methods (collects node FQNs for parameter access)
// =========================================================================

/**
* @brief Aggregate configurations (node FQNs) for any entity by ID
*
* Unified method that works for all entity types.
* For Apps, returns the single bound node.
* For Components/Areas/Functions, aggregates all child app nodes.
*
* @param entity_id Entity ID to get configurations for
* @return AggregatedConfigurations with node FQNs, or empty if entity not found
*/
AggregatedConfigurations get_entity_configurations(const std::string & entity_id) const;

/**
* @brief Get configurations for an App (returns its single bound node)
*/
AggregatedConfigurations get_app_configurations(const std::string & app_id) const;

/**
* @brief Aggregate configurations for a Component
*
* Returns: All node FQNs from hosted Apps.
*/
AggregatedConfigurations get_component_configurations(const std::string & component_id) const;

/**
* @brief Aggregate configurations for an Area
*
* Returns: All node FQNs from all Components in the Area.
*/
AggregatedConfigurations get_area_configurations(const std::string & area_id) const;

/**
* @brief Aggregate configurations for a Function
*
* Returns: All node FQNs from all Apps implementing this Function.
*/
AggregatedConfigurations get_function_configurations(const std::string & function_id) const;

// =========================================================================
// Operation lookup (O(1) via operation index)
// =========================================================================
Expand Down
31 changes: 31 additions & 0 deletions src/ros2_medkit_gateway/src/configuration_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -74,6 +74,7 @@ ParameterResult ConfigurationManager::list_parameters(const std::string & node_n
if (!client->wait_for_service(get_service_timeout())) {
result.success = false;
result.error_message = "Parameter service not available for node: " + node_name;
result.error_code = ParameterErrorCode::SERVICE_UNAVAILABLE;
RCLCPP_WARN(node_->get_logger(), "Parameter service not available for node: '%s'", node_name.c_str());
return result;
}
Expand Down Expand Up @@ -129,6 +130,7 @@ ParameterResult ConfigurationManager::list_parameters(const std::string & node_n
} catch (const std::exception & e) {
result.success = false;
result.error_message = std::string("Failed to list parameters: ") + e.what();
result.error_code = ParameterErrorCode::INTERNAL_ERROR;
RCLCPP_ERROR(node_->get_logger(), "Exception in list_parameters for node '%s': %s", node_name.c_str(), e.what());
}

Expand All @@ -144,6 +146,7 @@ ParameterResult ConfigurationManager::get_parameter(const std::string & node_nam
if (!client->wait_for_service(get_service_timeout())) {
result.success = false;
result.error_message = "Parameter service not available for node: " + node_name;
result.error_code = ParameterErrorCode::SERVICE_UNAVAILABLE;
return result;
}

Expand All @@ -152,6 +155,7 @@ ParameterResult ConfigurationManager::get_parameter(const std::string & node_nam
if (param_names.names.empty()) {
result.success = false;
result.error_message = "Parameter not found: " + param_name;
result.error_code = ParameterErrorCode::NOT_FOUND;
return result;
}

Expand All @@ -160,6 +164,7 @@ ParameterResult ConfigurationManager::get_parameter(const std::string & node_nam
if (parameters.empty()) {
result.success = false;
result.error_message = "Failed to get parameter: " + param_name;
result.error_code = ParameterErrorCode::INTERNAL_ERROR;
return result;
}

Expand All @@ -183,6 +188,7 @@ ParameterResult ConfigurationManager::get_parameter(const std::string & node_nam
} catch (const std::exception & e) {
result.success = false;
result.error_message = std::string("Failed to get parameter: ") + e.what();
result.error_code = ParameterErrorCode::INTERNAL_ERROR;
}

return result;
Expand All @@ -198,6 +204,7 @@ ParameterResult ConfigurationManager::set_parameter(const std::string & node_nam
if (!client->wait_for_service(get_service_timeout())) {
result.success = false;
result.error_message = "Parameter service not available for node: " + node_name;
result.error_code = ParameterErrorCode::SERVICE_UNAVAILABLE;
return result;
}

Expand All @@ -217,6 +224,20 @@ ParameterResult ConfigurationManager::set_parameter(const std::string & node_nam
if (results.empty() || !results[0].successful) {
result.success = false;
result.error_message = results.empty() ? "Failed to set parameter" : results[0].reason;
// Classify error based on reason
if (!results.empty()) {
const auto & reason = results[0].reason;
if (reason.find("read-only") != std::string::npos || reason.find("read only") != std::string::npos ||
reason.find("is read_only") != std::string::npos) {
result.error_code = ParameterErrorCode::READ_ONLY;
} else if (reason.find("type") != std::string::npos) {
result.error_code = ParameterErrorCode::TYPE_MISMATCH;
} else {
result.error_code = ParameterErrorCode::INVALID_VALUE;
}
} else {
result.error_code = ParameterErrorCode::INTERNAL_ERROR;
}
return result;
}

Expand All @@ -231,6 +252,7 @@ ParameterResult ConfigurationManager::set_parameter(const std::string & node_nam
} catch (const std::exception & e) {
result.success = false;
result.error_message = std::string("Failed to set parameter: ") + e.what();
result.error_code = ParameterErrorCode::INTERNAL_ERROR;
}

return result;
Expand Down Expand Up @@ -468,13 +490,15 @@ ParameterResult ConfigurationManager::reset_parameter(const std::string & node_n
if (node_it == default_values_.end()) {
result.success = false;
result.error_message = "No default values cached for node: " + node_name;
result.error_code = ParameterErrorCode::NO_DEFAULTS_CACHED;
return result;
}

auto param_it = node_it->second.find(param_name);
if (param_it == node_it->second.end()) {
result.success = false;
result.error_message = "No default value for parameter: " + param_name;
result.error_code = ParameterErrorCode::NOT_FOUND;
return result;
}

Expand All @@ -485,13 +509,15 @@ ParameterResult ConfigurationManager::reset_parameter(const std::string & node_n
if (!client->wait_for_service(get_service_timeout())) {
result.success = false;
result.error_message = "Parameter service not available for node: " + node_name;
result.error_code = ParameterErrorCode::SERVICE_UNAVAILABLE;
return result;
}

auto results = client->set_parameters({default_param});
if (results.empty() || !results[0].successful) {
result.success = false;
result.error_message = results.empty() ? "Failed to reset parameter" : results[0].reason;
result.error_code = ParameterErrorCode::INTERNAL_ERROR;
return result;
}

Expand All @@ -510,6 +536,7 @@ ParameterResult ConfigurationManager::reset_parameter(const std::string & node_n
} catch (const std::exception & e) {
result.success = false;
result.error_message = std::string("Failed to reset parameter: ") + e.what();
result.error_code = ParameterErrorCode::INTERNAL_ERROR;
}

return result;
Expand All @@ -530,13 +557,15 @@ ParameterResult ConfigurationManager::reset_all_parameters(const std::string & n
if (node_it == default_values_.end()) {
result.success = false;
result.error_message = "No default values cached for node: " + node_name;
result.error_code = ParameterErrorCode::NO_DEFAULTS_CACHED;
return result;
}

auto client = get_param_client(node_name);
if (!client->wait_for_service(get_service_timeout())) {
result.success = false;
result.error_message = "Parameter service not available for node: " + node_name;
result.error_code = ParameterErrorCode::SERVICE_UNAVAILABLE;
return result;
}

Expand Down Expand Up @@ -580,13 +609,15 @@ ParameterResult ConfigurationManager::reset_all_parameters(const std::string & n

if (failed_count > 0) {
result.error_message = "Some parameters could not be reset";
result.error_code = ParameterErrorCode::INTERNAL_ERROR; // Partial failure
}

RCLCPP_INFO(node_->get_logger(), "Reset %zu parameters on node '%s' (%zu failed)", reset_count, node_name.c_str(),
failed_count);
} catch (const std::exception & e) {
result.success = false;
result.error_message = std::string("Failed to reset parameters: ") + e.what();
result.error_code = ParameterErrorCode::INTERNAL_ERROR;
}

return result;
Expand Down
Loading
Loading