AP_DDS: Add set parameters service.

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, &parameter_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;
+    }
     }
 }
 

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",
+    }
 };

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;
+    };
+  };
+};

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;
+    };
+  };
+};

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<octet> byte_array_value;
+
+      @verbatim (language="comment", text=
+        "An array of boolean values.")
+      sequence<boolean> bool_array_value;
+
+      @verbatim (language="comment", text=
+        "An array of 64-bit integer values.")
+      sequence<int64> integer_array_value;
+
+      @verbatim (language="comment", text=
+        "An array of 64-bit floating point values.")
+      sequence<double> double_array_value;
+
+      @verbatim (language="comment", text=
+        "An array of string values.")
+      sequence<string> string_array_value;
+    };
+  };
+};

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;
+    };
+  };
+};

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<rcl_interfaces::msg::Parameter> parameters;
+    };
+    @verbatim (language="comment", text=
+      "Indicates whether setting each parameter succeeded or not and why.")
+    struct SetParameters_Response {
+      sequence<rcl_interfaces::msg::SetParametersResult> results;
+    };
+  };
+};