From f76d763a2ed5e08f0c2148e04ae9297d6f349d26 Mon Sep 17 00:00:00 2001 From: "paul.quillen" Date: Thu, 3 Oct 2024 14:57:57 -0500 Subject: [PATCH] AP_DDS: Add set parameters service. --- libraries/AP_DDS/AP_DDS_Client.cpp | 120 ++++++++++++++++++ libraries/AP_DDS/AP_DDS_Service_Table.h | 13 +- .../Idl/rcl_interfaces/msg/Parameter.idl | 23 ++++ .../Idl/rcl_interfaces/msg/ParameterType.idl | 28 ++++ .../Idl/rcl_interfaces/msg/ParameterValue.idl | 58 +++++++++ .../msg/SetParametersResult.idl | 20 +++ .../Idl/rcl_interfaces/srv/SetParameters.idl | 21 +++ 7 files changed, 282 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_DDS/Idl/rcl_interfaces/msg/Parameter.idl create mode 100644 libraries/AP_DDS/Idl/rcl_interfaces/msg/ParameterType.idl create mode 100644 libraries/AP_DDS/Idl/rcl_interfaces/msg/ParameterValue.idl create mode 100644 libraries/AP_DDS/Idl/rcl_interfaces/msg/SetParametersResult.idl create mode 100644 libraries/AP_DDS/Idl/rcl_interfaces/srv/SetParameters.idl diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index d224a22551326..c591a9b0082bd 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -19,6 +19,12 @@ #include "ardupilot_msgs/srv/ArmMotors.h" #include "ardupilot_msgs/srv/ModeSwitch.h" +#include "rcl_interfaces/srv/SetParameters.h" +#include "rcl_interfaces/msg/Parameter.h" +#include "rcl_interfaces/msg/SetParametersResult.h" +#include "rcl_interfaces/msg/ParameterValue.h" +#include "rcl_interfaces/msg/ParameterType.h" + #if AP_EXTERNAL_CONTROL_ENABLED #include "AP_DDS_ExternalControl.h" #endif @@ -711,6 +717,120 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Request for Mode Switch : %s", msg_prefix, mode_switch_response.status ? "SUCCESS" : "FAIL"); break; } + case services[to_underlying(ServiceIndex::SET_PARAMETERS)].rep_id: { + rcl_interfaces_srv_SetParameters_Request set_parameter_request; + rcl_interfaces_srv_SetParameters_Response set_parameter_response; + const bool deserialize_success = rcl_interfaces_srv_SetParameters_Request_deserialize_topic(ub, &set_parameter_request); + if (deserialize_success == false) { + break; + } + + if (set_parameter_request.parameters_size > 8U) { + break; + } + + // Set parameters and responses for each one requested + set_parameter_response.results_size = set_parameter_request.parameters_size; + for (size_t i = 0; i < set_parameter_request.parameters_size; i++) { + rcl_interfaces_msg_Parameter param = set_parameter_request.parameters[i]; + + enum ap_var_type var_type; + + // set parameter + AP_Param *vp; + char param_key[AP_MAX_NAME_SIZE+1]; + strncpy(param_key, (char *)param.name, AP_MAX_NAME_SIZE); + param_key[AP_MAX_NAME_SIZE] = 0; + + // Only worried about integer and float types for parameter setting. + bool param_isnan = true; + bool param_isinf = true; + float param_value; + switch (param.value.type) { + case PARAMETER_INTEGER: { + param_isnan = isnan(param.value.integer_value); + param_isinf = isinf(param.value.integer_value); + param_value = float(param.value.integer_value); + break; + } + case PARAMETER_DOUBLE: { + param_isnan = isnan(param.value.double_value); + param_isinf = isinf(param.value.double_value); + param_value = float(param.value.double_value); + break; + } + default: { + break; + } + } + + // find existing param to get the old value + uint16_t parameter_flags = 0; + vp = AP_Param::find(param_key, &var_type, ¶meter_flags); + if (vp == nullptr || param_isnan || param_isinf) { + set_parameter_response.results[i].successful = false; + strncpy(set_parameter_response.results[i].reason, "Parameter not found", sizeof(set_parameter_response.results[i].reason)); + continue; + } + + float old_value = vp->cast_to_float(var_type); + + // Add existing parameter checks used in GCS_Param.cpp + if (parameter_flags & AP_PARAM_FLAG_INTERNAL_USE_ONLY) { + // the user can set BRD_OPTIONS to enable set of internal + // parameters, for developer testing or unusual use cases + if (AP_BoardConfig::allow_set_internal_parameters()) { + parameter_flags &= ~AP_PARAM_FLAG_INTERNAL_USE_ONLY; + } + } + + if ((parameter_flags & AP_PARAM_FLAG_INTERNAL_USE_ONLY) || vp->is_read_only()) { + set_parameter_response.results[i].successful = false; + strncpy(set_parameter_response.results[i].reason, "Parameter is read only",sizeof(set_parameter_response.results[i].reason)); + continue; + } + + // set the value + vp->set_float(param_value, var_type); + + // Force the save if the value is not equal to the old one + bool force_save = !is_equal(param_value, old_value); + + // save the change + vp->save(force_save); + + if (force_save && (parameter_flags & AP_PARAM_FLAG_ENABLE)) { + AP_Param::invalidate_count(); + } + + set_parameter_response.results[i].successful = true; + strncpy(set_parameter_response.results[i].reason, "Parameter accepted", sizeof(set_parameter_response.results[i].reason)); + } + + const uxrObjectId replier_id = { + .id = services[to_underlying(ServiceIndex::SET_PARAMETERS)].rep_id, + .type = UXR_REPLIER_ID + }; + + uint32_t reply_size = rcl_interfaces_srv_SetParameters_Response_size_of_topic(&set_parameter_response, 0U); + uint8_t reply_buffer[reply_size] {}; + ucdrBuffer reply_ub; + + ucdr_init_buffer(&reply_ub, reply_buffer, reply_size); + const bool serialize_success = rcl_interfaces_srv_SetParameters_Response_serialize_topic(&reply_ub, &set_parameter_response); + if (serialize_success == false) { + break; + } + + uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub)); + bool successful_params = true; + for (size_t i = 0; i < set_parameter_response.results_size; i++) { + // Check that all the parameters were set successfully + successful_params &= set_parameter_response.results[i].successful; + } + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Set Parameters Request : %s", msg_prefix, successful_params ? "SUCCESSFUL" : "ONE OR MORE PARAMS FAILED" ); + break; + } } } diff --git a/libraries/AP_DDS/AP_DDS_Service_Table.h b/libraries/AP_DDS/AP_DDS_Service_Table.h index 22f6716beffd0..5d657eb715b04 100644 --- a/libraries/AP_DDS/AP_DDS_Service_Table.h +++ b/libraries/AP_DDS/AP_DDS_Service_Table.h @@ -2,7 +2,8 @@ enum class ServiceIndex: uint8_t { ARMING_MOTORS, - MODE_SWITCH + MODE_SWITCH, + SET_PARAMETERS }; static inline constexpr uint8_t to_underlying(const ServiceIndex index) @@ -38,4 +39,14 @@ constexpr struct AP_DDS_Client::Service_table AP_DDS_Client::services[] = { .request_topic_name = "rq/ap/mode_switchRequest", .reply_topic_name = "rr/ap/mode_switchReply", }, + { + .req_id = to_underlying(ServiceIndex::SET_PARAMETERS), + .rep_id = to_underlying(ServiceIndex::SET_PARAMETERS), + .service_rr = Service_rr::Replier, + .service_name = "rs/ap/set_parametersService", + .request_type = "rcl_interfaces::srv::dds_::SetParameters_Request_", + .reply_type = "rcl_interfaces::srv::dds_::SetParameters_Response_", + .request_topic_name = "rq/ap/set_parametersRequest", + .reply_topic_name = "rr/ap/set_parametersReply", + } }; diff --git a/libraries/AP_DDS/Idl/rcl_interfaces/msg/Parameter.idl b/libraries/AP_DDS/Idl/rcl_interfaces/msg/Parameter.idl new file mode 100644 index 0000000000000..992213a287458 --- /dev/null +++ b/libraries/AP_DDS/Idl/rcl_interfaces/msg/Parameter.idl @@ -0,0 +1,23 @@ +// generated from rosidl_adapter/resource/msg.idl.em +// with input from rcl_interfaces/msg/Parameter.msg +// generated code does not contain a copyright notice + +#include "rcl_interfaces/msg/ParameterValue.idl" + +module rcl_interfaces { + module msg { + @verbatim (language="comment", text= + "This is the message to communicate a parameter. It is an open struct with an enum in" "\n" + "the descriptor to select which value is active.") + struct Parameter { + @verbatim (language="comment", text= + "The full name of the parameter.") + string name; + + @verbatim (language="comment", text= + "The parameter's value which can be one of several types, see" "\n" + "`ParameterValue.msg` and `ParameterType.msg`.") + rcl_interfaces::msg::ParameterValue value; + }; + }; +}; diff --git a/libraries/AP_DDS/Idl/rcl_interfaces/msg/ParameterType.idl b/libraries/AP_DDS/Idl/rcl_interfaces/msg/ParameterType.idl new file mode 100644 index 0000000000000..e6a6bfcca7da7 --- /dev/null +++ b/libraries/AP_DDS/Idl/rcl_interfaces/msg/ParameterType.idl @@ -0,0 +1,28 @@ +// generated from rosidl_adapter/resource/msg.idl.em +// with input from rcl_interfaces/msg/ParameterType.msg +// generated code does not contain a copyright notice + + +module rcl_interfaces { + module msg { + module ParameterType_Constants { + @verbatim (language="comment", text= + "Default value, which implies this is not a valid parameter.") + const uint8 PARAMETER_NOT_SET = 0; + const uint8 PARAMETER_BOOL = 1; + const uint8 PARAMETER_INTEGER = 2; + const uint8 PARAMETER_DOUBLE = 3; + const uint8 PARAMETER_STRING = 4; + const uint8 PARAMETER_BYTE_ARRAY = 5; + const uint8 PARAMETER_BOOL_ARRAY = 6; + const uint8 PARAMETER_INTEGER_ARRAY = 7; + const uint8 PARAMETER_DOUBLE_ARRAY = 8; + const uint8 PARAMETER_STRING_ARRAY = 9; + }; + @verbatim (language="comment", text= + "These types correspond to the value that is set in the ParameterValue message.") + struct ParameterType { + uint8 structure_needs_at_least_one_member; + }; + }; +}; diff --git a/libraries/AP_DDS/Idl/rcl_interfaces/msg/ParameterValue.idl b/libraries/AP_DDS/Idl/rcl_interfaces/msg/ParameterValue.idl new file mode 100644 index 0000000000000..3adbc5233cac2 --- /dev/null +++ b/libraries/AP_DDS/Idl/rcl_interfaces/msg/ParameterValue.idl @@ -0,0 +1,58 @@ +// generated from rosidl_adapter/resource/msg.idl.em +// with input from rcl_interfaces/msg/ParameterValue.msg +// generated code does not contain a copyright notice + + +module rcl_interfaces { + module msg { + @verbatim (language="comment", text= + "Used to determine which of the next *_value fields are set." "\n" + "ParameterType.PARAMETER_NOT_SET indicates that the parameter was not set" "\n" + "(if gotten) or is uninitialized." "\n" + "Values are enumerated in `ParameterType.msg`.") + struct ParameterValue { + @verbatim (language="comment", text= + "The type of this parameter, which corresponds to the appropriate field below.") + uint8 type; + + @verbatim (language="comment", text= + "\"Variant\" style storage of the parameter value. Only the value corresponding" "\n" + "the type field will have valid information." "\n" + "Boolean value, can be either true or false.") + boolean bool_value; + + @verbatim (language="comment", text= + "Integer value ranging from -9,223,372,036,854,775,808 to" "\n" + "9,223,372,036,854,775,807.") + int64 integer_value; + + @verbatim (language="comment", text= + "A double precision floating point value following IEEE 754.") + double double_value; + + @verbatim (language="comment", text= + "A textual value with no practical length limit.") + string string_value; + + @verbatim (language="comment", text= + "An array of bytes, used for non-textual information.") + sequence byte_array_value; + + @verbatim (language="comment", text= + "An array of boolean values.") + sequence bool_array_value; + + @verbatim (language="comment", text= + "An array of 64-bit integer values.") + sequence integer_array_value; + + @verbatim (language="comment", text= + "An array of 64-bit floating point values.") + sequence double_array_value; + + @verbatim (language="comment", text= + "An array of string values.") + sequence string_array_value; + }; + }; +}; diff --git a/libraries/AP_DDS/Idl/rcl_interfaces/msg/SetParametersResult.idl b/libraries/AP_DDS/Idl/rcl_interfaces/msg/SetParametersResult.idl new file mode 100644 index 0000000000000..d1c68fe1200f1 --- /dev/null +++ b/libraries/AP_DDS/Idl/rcl_interfaces/msg/SetParametersResult.idl @@ -0,0 +1,20 @@ +// generated from rosidl_adapter/resource/msg.idl.em +// with input from rcl_interfaces/msg/SetParametersResult.msg +// generated code does not contain a copyright notice + + +module rcl_interfaces { + module msg { + @verbatim (language="comment", text= + "A true value of the same index indicates that the parameter was set" "\n" + "successfully. A false value indicates the change was rejected.") + struct SetParametersResult { + boolean successful; + + @verbatim (language="comment", text= + "Reason why the setting was either successful or a failure. This should only be" "\n" + "used for logging and user interfaces.") + string reason; + }; + }; +}; diff --git a/libraries/AP_DDS/Idl/rcl_interfaces/srv/SetParameters.idl b/libraries/AP_DDS/Idl/rcl_interfaces/srv/SetParameters.idl new file mode 100644 index 0000000000000..ade6df7994030 --- /dev/null +++ b/libraries/AP_DDS/Idl/rcl_interfaces/srv/SetParameters.idl @@ -0,0 +1,21 @@ +// generated from rosidl_adapter/resource/srv.idl.em +// with input from rcl_interfaces/srv/SetParameters.srv +// generated code does not contain a copyright notice + +#include "rcl_interfaces/msg/Parameter.idl" +#include "rcl_interfaces/msg/SetParametersResult.idl" + +module rcl_interfaces { + module srv { + @verbatim (language="comment", text= + "A list of parameters to set.") + struct SetParameters_Request { + sequence parameters; + }; + @verbatim (language="comment", text= + "Indicates whether setting each parameter succeeded or not and why.") + struct SetParameters_Response { + sequence results; + }; + }; +};