diff --git a/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_parameter_service.py b/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_parameter_service.py new file mode 100644 index 0000000000000..8e6e95601c971 --- /dev/null +++ b/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_parameter_service.py @@ -0,0 +1,197 @@ +# Copyright 2023 ArduPilot.org. +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +""" +Bring up ArduPilot SITL and check that the get/set_parameters services are up and running. + +Checks whether a parameter is changed using service call. + +colcon test --packages-select ardupilot_dds_tests \ +--event-handlers=console_cohesion+ --pytest-args -k test_parameter_service + +""" + +import launch_pytest +import pytest +import rclpy +import rclpy.node +import threading +import time + +from launch import LaunchDescription + +from launch_pytest.tools import process as process_tools + +from rclpy.qos import QoSProfile +from rclpy.qos import QoSReliabilityPolicy +from rclpy.qos import QoSHistoryPolicy + +from rcl_interfaces.srv import GetParameters +from rcl_interfaces.srv import SetParameters +from rcl_interfaces.msg import Parameter + +# Enums for parameter type +PARAMETER_NOT_SET = 0 +PARAMETER_INTEGER = 2 +PARAMETER_DOUBLE = 3 + + +class ParameterClient(rclpy.node.Node): + """Send GetParameters and SetParameters Requests.""" + + def __init__(self): + """Initialize the node.""" + super().__init__('parameter_client') + self.get_param_event_object = threading.Event() + self.set_param_event_object = threading.Event() + self.set_correct_object = threading.Event() + + def start_client(self): + """Start the parameter client.""" + qos_profile = QoSProfile( + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ) + + # Define clients for getting and setting parameter + self.get_cli = self.create_client(GetParameters, 'ap/get_parameters') + while not self.get_cli.wait_for_service(timeout_sec=1.0): + self.get_logger().info('GetParameters service not available, waiting again...') + + self.set_cli = self.create_client(SetParameters, 'ap/set_parameters') + while not self.set_cli.wait_for_service(timeout_sec=1.0): + self.get_logger().info('SetParameters service not availabel, waiting again...') + + # Add a spin thread + self.ros_spin_thread = threading.Thread(target=lambda node: rclpy.spin(node), args=(self,)) + self.ros_spin_thread.start() + + def send_get_param_req(self, param_name): + self.get_req = GetParameters.Request() + self.get_req.names.append(param_name) + + self.get_future = self.get_cli.call_async(self.get_req) + while not self.get_future.done(): + self.get_logger().info("Waiting for GetParameters service response...") + time.sleep(0.1) + + resp = self.get_future.result() + value = resp.values[0] + + if value.type != PARAMETER_NOT_SET: + self.get_param_event_object.set() + + def send_set_param_req(self, param_name, param_value, param_type): + self.set_req = SetParameters.Request() + param_update = Parameter() + param_update.name = param_name + param_update.value.type = param_type + if param_type == PARAMETER_INTEGER: + param_update.value.integer_value = int(param_value) + else: + param_update.value.double_value = float(param_value) + + self.set_req.parameters.append(param_update) + + self.desired_value = param_value + get_response = self.get_future.result() + initial_value = get_response.values[0] + + if initial_value.type == PARAMETER_INTEGER: + self.param_value = initial_value.integer_value + elif initial_value.type == PARAMETER_DOUBLE: + self.param_value = initial_value.double_value + else: + self.param_value = 'nan' + + self.set_future = self.set_cli.call_async(self.set_req) + + while not self.set_future.done(): + self.get_logger().info("Waiting for SetParameters service response...") + time.sleep(0.1) + + if self.set_future.done(): + response = self.set_future.result() + if response.results[0].successful: + self.set_param_event_object.set() + + def check_param_change(self): + param_name = self.set_req.parameters[0].name + + self.send_get_param_req(param_name) + + get_response = self.get_future.result() + new_value = get_response.values[0] + + if new_value.type == PARAMETER_INTEGER: + updated_value = new_value.integer_value + elif new_value.type == PARAMETER_DOUBLE: + updated_value = new_value.double_value + else: + updated_value = 'nan' + + if updated_value == self.desired_value: + self.set_correct_object.set() + + +@launch_pytest.fixture +def launch_sitl_copter_dds_udp(sitl_copter_dds_udp): + """Fixture to create the launch description.""" + sitl_ld, sitl_actions = sitl_copter_dds_udp + + ld = LaunchDescription( + [ + sitl_ld, + launch_pytest.actions.ReadyToTest(), + ] + ) + actions = sitl_actions + yield ld, actions + + +@pytest.mark.launch(fixture=launch_sitl_copter_dds_udp) +def test_dds_udp_parameter_services(launch_context, launch_sitl_copter_dds_udp): + """Test Get/Set parameter services broadcast by AP_DDS.""" + _, actions = launch_sitl_copter_dds_udp + micro_ros_agent = actions["micro_ros_agent"].action + mavproxy = actions["mavproxy"].action + sitl = actions["sitl"].action + + # Wait for process to start. + process_tools.wait_for_start_sync(launch_context, micro_ros_agent, timeout=2) + process_tools.wait_for_start_sync(launch_context, mavproxy, timeout=2) + process_tools.wait_for_start_sync(launch_context, sitl, timeout=2) + + rclpy.init() + try: + node = ParameterClient() + node.start_client() + parameter_name = "LOIT_SPEED" + param_change_value = 1250 + param_type = PARAMETER_DOUBLE + node.send_get_param_req(parameter_name) + get_param_received_flag = node.get_param_event_object.wait(timeout=10.0) + assert get_param_received_flag, f"Did not get '{parameter_name}' param." + node.send_set_param_req(parameter_name, param_change_value, param_type) + set_param_received_flag = node.set_param_event_object.wait(timeout=10.0) + assert set_param_received_flag, f"Could not set '{parameter_name}' to '{param_change_value}'" + node.check_param_change() + set_param_changed_flag = node.set_correct_object.wait(timeout=10.0) + assert set_param_changed_flag, f"Did not confirm '{parameter_name}' value change" + + finally: + rclpy.shutdown() + yield diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 36d8d1bf714f6..35225282f2373 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -81,6 +81,17 @@ geometry_msgs_msg_TwistStamped AP_DDS_Client::rx_velocity_control_topic {}; ardupilot_msgs_msg_GlobalPosition AP_DDS_Client::rx_global_position_control_topic {}; #endif // AP_DDS_GLOBAL_POS_CTRL_ENABLED +// Define the parameter server data members, which are static class scope. +// If these are created on the stack, then the AP_DDS_Client::on_request +// frame size is exceeded. +#if AP_DDS_PARAMETER_SERVER_ENABLED +rcl_interfaces_srv_SetParameters_Request AP_DDS_Client::set_parameter_request {}; +rcl_interfaces_srv_SetParameters_Response AP_DDS_Client::set_parameter_response {}; +rcl_interfaces_srv_GetParameters_Request AP_DDS_Client::get_parameters_request {}; +rcl_interfaces_srv_GetParameters_Response AP_DDS_Client::get_parameters_response {}; +rcl_interfaces_msg_Parameter AP_DDS_Client::param {}; +#endif + const AP_Param::GroupInfo AP_DDS_Client::var_info[] { // @Param: _ENABLE @@ -801,6 +812,196 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u break; } #endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED +#if AP_DDS_PARAMETER_SERVER_ENABLED + case services[to_underlying(ServiceIndex::SET_PARAMETERS)].rep_id: { + const bool deserialize_success = rcl_interfaces_srv_SetParameters_Request_deserialize_topic(ub, &set_parameter_request); + if (deserialize_success == false) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s Set Parameters Request : Failed to deserialize request.", msg_prefix); + 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++) { + 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; + + // Currently only integer and double value types can be set. + // The following parameter value types are not handled: + // bool, string, byte_array, bool_array, integer_array, double_array and string_array + 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; + } + + // 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 and save the value if it changed + bool force_save = vp->set_and_save_by_name_ifchanged(param_key, param_value); + + 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(successful_params ? MAV_SEVERITY_INFO : MAV_SEVERITY_WARNING, "%s Set Parameters Request : %s", msg_prefix, successful_params ? "SUCCESSFUL" : "ONE OR MORE PARAMS FAILED" ); + break; + } + case services[to_underlying(ServiceIndex::GET_PARAMETERS)].rep_id: { + const bool deserialize_success = rcl_interfaces_srv_GetParameters_Request_deserialize_topic(ub, &get_parameters_request); + if (deserialize_success == false) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s Get Parameters Request : Failed to deserialize request.", msg_prefix); + break; + } + + if (get_parameters_request.names_size > 8U) { + break; + } + + bool successful_read = true; + get_parameters_response.values_size = get_parameters_request.names_size; + for (size_t i = 0; i < get_parameters_request.names_size; i++) { + enum ap_var_type var_type; + + AP_Param *vp; + char param_key[AP_MAX_NAME_SIZE + 1]; + strncpy(param_key, (char *)get_parameters_request.names[i], AP_MAX_NAME_SIZE); + param_key[AP_MAX_NAME_SIZE] = 0; + + vp = AP_Param::find(param_key, &var_type); + if (vp == nullptr) { + get_parameters_response.values[i].type = PARAMETER_NOT_SET; + successful_read &= false; + continue; + } + + switch (var_type) { + case AP_PARAM_INT8: { + get_parameters_response.values[i].type = PARAMETER_INTEGER; + get_parameters_response.values[i].integer_value = ((AP_Int8 *)vp)->get(); + successful_read &= true; + break; + } + case AP_PARAM_INT16: { + get_parameters_response.values[i].type = PARAMETER_INTEGER; + get_parameters_response.values[i].integer_value = ((AP_Int16 *)vp)->get(); + successful_read &= true; + break; + } + case AP_PARAM_INT32: { + get_parameters_response.values[i].type = PARAMETER_INTEGER; + get_parameters_response.values[i].integer_value = ((AP_Int32 *)vp)->get(); + successful_read &= true; + break; + } + case AP_PARAM_FLOAT: { + get_parameters_response.values[i].type = PARAMETER_DOUBLE; + get_parameters_response.values[i].double_value = vp->cast_to_float(var_type); + successful_read &= true; + break; + } + default: { + get_parameters_response.values[i].type = PARAMETER_NOT_SET; + successful_read &= false; + break; + } + } + } + + const uxrObjectId replier_id = { + .id = services[to_underlying(ServiceIndex::GET_PARAMETERS)].rep_id, + .type = UXR_REPLIER_ID + }; + + uint32_t reply_size = rcl_interfaces_srv_GetParameters_Response_size_of_topic(&get_parameters_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_GetParameters_Response_serialize_topic(&reply_ub, &get_parameters_response); + if (serialize_success == false) { + break; + } + + uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub)); + + GCS_SEND_TEXT(successful_read ? MAV_SEVERITY_INFO : MAV_SEVERITY_WARNING, "%s Get Parameters Request : %s", msg_prefix, successful_read ? "SUCCESSFUL" : "ONE OR MORE PARAM NOT FOUND"); + break; + } +#endif // AP_DDS_PARAMETER_SERVER_ENABLED } } @@ -947,7 +1148,7 @@ bool AP_DDS_Client::create() .id = 0x01, .type = UXR_PARTICIPANT_ID }; - const char* participant_name = "ardupilot_dds"; + const char* participant_name = AP_DDS_PARTICIPANT_NAME; const auto participant_req_id = uxr_buffer_create_participant_bin(&session, reliable_out, participant_id, static_cast(domain_id), participant_name, UXR_REPLACE); diff --git a/libraries/AP_DDS/AP_DDS_Client.h b/libraries/AP_DDS/AP_DDS_Client.h index 92bdbf2a4bd30..ded3451f99228 100644 --- a/libraries/AP_DDS/AP_DDS_Client.h +++ b/libraries/AP_DDS/AP_DDS_Client.h @@ -46,6 +46,14 @@ #if AP_DDS_CLOCK_PUB_ENABLED #include "rosgraph_msgs/msg/Clock.h" #endif // AP_DDS_CLOCK_PUB_ENABLED +#if AP_DDS_PARAMETER_SERVER_ENABLED +#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" +#include "rcl_interfaces/srv/GetParameters.h" +#endif //AP_DDS_PARAMETER_SERVER_ENABLED #include #include @@ -201,6 +209,14 @@ class AP_DDS_Client #endif // AP_DDS_DYNAMIC_TF_SUB_ENABLED HAL_Semaphore csem; +#if AP_DDS_PARAMETER_SERVER_ENABLED + static rcl_interfaces_srv_SetParameters_Request set_parameter_request; + static rcl_interfaces_srv_SetParameters_Response set_parameter_response; + static rcl_interfaces_srv_GetParameters_Request get_parameters_request; + static rcl_interfaces_srv_GetParameters_Response get_parameters_response; + static rcl_interfaces_msg_Parameter param; +#endif + // connection parametrics bool status_ok{false}; bool connected{false}; diff --git a/libraries/AP_DDS/AP_DDS_Service_Table.h b/libraries/AP_DDS/AP_DDS_Service_Table.h index 0e86496dd5777..667149d6aa756 100644 --- a/libraries/AP_DDS/AP_DDS_Service_Table.h +++ b/libraries/AP_DDS/AP_DDS_Service_Table.h @@ -6,8 +6,12 @@ enum class ServiceIndex: uint8_t { ARMING_MOTORS, #endif // #if AP_DDS_ARM_SERVER_ENABLED #if AP_DDS_MODE_SWITCH_SERVER_ENABLED - MODE_SWITCH + MODE_SWITCH, #endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED +#if AP_DDS_PARAMETER_SERVER_ENABLED + SET_PARAMETERS, + GET_PARAMETERS +#endif // AP_DDS_PARAMETER_SERVER_ENABLED }; static inline constexpr uint8_t to_underlying(const ServiceIndex index) @@ -47,4 +51,26 @@ constexpr struct AP_DDS_Client::Service_table AP_DDS_Client::services[] = { .reply_topic_name = "rr/ap/mode_switchReply", }, #endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED +#if AP_DDS_PARAMETER_SERVER_ENABLED + { + .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", + }, + { + .req_id = to_underlying(ServiceIndex::GET_PARAMETERS), + .rep_id = to_underlying(ServiceIndex::GET_PARAMETERS), + .service_rr = Service_rr::Replier, + .service_name = "rs/ap/get_parameterService", + .request_type = "rcl_interfaces::srv::dds_::GetParameters_Request_", + .reply_type = "rcl_interfaces::srv::dds_::GetParameters_Response_", + .request_topic_name = "rq/ap/get_parametersRequest", + .reply_topic_name = "rr/ap/get_parametersReply", + }, +#endif // AP_DDS_PARAMETER_SERVER_ENABLED }; diff --git a/libraries/AP_DDS/AP_DDS_config.h b/libraries/AP_DDS/AP_DDS_config.h index b8f31e368c8a4..9bc3a5512656d 100644 --- a/libraries/AP_DDS/AP_DDS_config.h +++ b/libraries/AP_DDS/AP_DDS_config.h @@ -126,6 +126,10 @@ #define AP_DDS_MODE_SWITCH_SERVER_ENABLED 1 #endif +#ifndef AP_DDS_PARAMETER_SERVER_ENABLED +#define AP_DDS_PARAMETER_SERVER_ENABLED 1 +#endif + // Whether to include Twist support #define AP_DDS_NEEDS_TWIST AP_DDS_VEL_CTRL_ENABLED || AP_DDS_LOCAL_VEL_PUB_ENABLED @@ -139,3 +143,7 @@ #define AP_DDS_DEFAULT_UDP_IP_ADDR "127.0.0.1" #endif #endif + +#ifndef AP_DDS_PARTICIPANT_NAME +#define AP_DDS_PARTICIPANT_NAME "ap" +#endif 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/GetParameters.idl b/libraries/AP_DDS/Idl/rcl_interfaces/srv/GetParameters.idl new file mode 100644 index 0000000000000..d6579f51b2c12 --- /dev/null +++ b/libraries/AP_DDS/Idl/rcl_interfaces/srv/GetParameters.idl @@ -0,0 +1,29 @@ +// generated from rosidl_adapter/resource/srv.idl.em +// with input from rcl_interfaces/srv/GetParameters.srv +// generated code does not contain a copyright notice + +#include "rcl_interfaces/msg/ParameterValue.idl" + +module rcl_interfaces { + module srv { + @verbatim (language="comment", text= + "TODO(wjwwood): Decide on the rules for grouping, nodes, and parameter \"names\"" "\n" + "in general, then link to that." "\n" + "" "\n" + "For more information about parameters and naming rules, see:" "\n" + "https://design.ros2.org/articles/ros_parameters.html" "\n" + "https://github.com/ros2/design/pull/241") + struct GetParameters_Request { + @verbatim (language="comment", text= + "A list of parameter names to get.") + sequence names; + }; + @verbatim (language="comment", text= + "List of values which is the same length and order as the provided names. If a" "\n" + "parameter was not yet set, the value will have PARAMETER_NOT_SET as the" "\n" + "type.") + struct GetParameters_Response { + sequence values; + }; + }; +}; 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; + }; + }; +};