AP_DDS: Parameters service

[PQuill33]

ISSUE

#28292

Changes

-Add set parameters service call to AP_DDS library

• service is /ap/set_parameters
• Add get parameters service call to AP_DDS library
• service is /ap/get_parameters

Test

Using ros2 CLI, build and launch ardupilot sitl

colcon build --packages-up-to ardupilot_sitl
source install/setup.bash
ros2 launch ardupilot_sitl sitl_dds_udp.launch.py transport:=udp4 synthetic_clock:=True wipe:=False model:=quad speedup:=1 slave:=0 instance:=0 defaults:=$(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/default_params/copter.parm,$(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/default_params/dds_udp.parm sim_address:=127.0.0.1 master:=tcp:127.0.0.1:5760 sitl:=127.0.0.1:5501

• Check that parameter service is up

ros2 service list

Expected output

• Service call to set parameter

ros2 service call /ap/set_parameters rcl_interfaces/srv/SetParameters "{parameters: [{name: LOIT_SPEED, value: {type: 2, integer_value: 1250}}]}"

Expected output
mavproxy window:

service call window:

• Service call to get parameter

ros2 service call /ap/get_parameters rcl_interfaces/srv/GetParameters "{names: ['LOIT_SPEED']}"

Expected output
mavproxy window:

service call window:

[tizianofiorenzani]

I have tested with the Plane SITL, trying to ask for a wrong parameter. I mispelled AIRSPEED_CRUISE, which should be 22.0, and I expected to get some error. The only thing that tells me the response is wrong is the type = 0.

ros2 service call /ap/get_parameters rcl_interfaces/srv/GetParameters "names: [AIRSPEED_CRUIS2E]"
waiting for service to become available...
requester: making request: rcl_interfaces.srv.GetParameters_Request(names=['AIRSPEED_CRUIS2E'])

response:
rcl_interfaces.srv.GetParameters_Response(values=[rcl_interfaces.msg.ParameterValue(type=0, bool_value=True, integer_value=19513347822207839, double_value=22.0, string_value='', byte_array_value=[], bool_array_value=[], integer_array_value=[], double_array_value=[], string_array_value=[])])

Instead, if you ask for the correct parameter I have:

ros2 service call /ap/get_parameters rcl_interfaces/srv/GetParameters "names: [AIRSPEED_CRUISE]"
waiting for service to become available...
requester: making request: rcl_interfaces.srv.GetParameters_Request(names=['AIRSPEED_CRUISE'])

response:
rcl_interfaces.srv.GetParameters_Response(values=[rcl_interfaces.msg.ParameterValue(type=3, bool_value=True, integer_value=19513347822207839, double_value=24.0, string_value='', byte_array_value=[], bool_array_value=[], integer_array_value=[], double_array_value=[], string_array_value=[])])

Overall seems correct. I have tried to set and get multiple parameters, changed the airspeed in flight, made sure it was effective.

[PQuill33]

I have tested with the Plane SITL, trying to ask for a wrong parameter. I mispelled AIRSPEED_CRUISE, which should be 22.0, and I expected to get some error. The only thing that tells me the response is wrong is the type = 0.

ros2 service call /ap/get_parameters rcl_interfaces/srv/GetParameters "names: [AIRSPEED_CRUIS2E]"
waiting for service to become available...
requester: making request: rcl_interfaces.srv.GetParameters_Request(names=['AIRSPEED_CRUIS2E'])

response:
rcl_interfaces.srv.GetParameters_Response(values=[rcl_interfaces.msg.ParameterValue(type=0, bool_value=True, integer_value=19513347822207839, double_value=22.0, string_value='', byte_array_value=[], bool_array_value=[], integer_array_value=[], double_array_value=[], string_array_value=[])])

Instead, if you ask for the correct parameter I have:

ros2 service call /ap/get_parameters rcl_interfaces/srv/GetParameters "names: [AIRSPEED_CRUISE]"
waiting for service to become available...
requester: making request: rcl_interfaces.srv.GetParameters_Request(names=['AIRSPEED_CRUISE'])

response:
rcl_interfaces.srv.GetParameters_Response(values=[rcl_interfaces.msg.ParameterValue(type=3, bool_value=True, integer_value=19513347822207839, double_value=24.0, string_value='', byte_array_value=[], bool_array_value=[], integer_array_value=[], double_array_value=[], string_array_value=[])])

Overall seems correct. I have tried to set and get multiple parameters, changed the airspeed in flight, made sure it was effective.

The error gets reported as a GCS message. It should read AP: DDS: Get Parameters Request : ONE OR MORE PARAM NOT FOUND.

I was thinking of populating the fields in the response message with bogus entries if that sounds like an acceptable error response?

[tizianofiorenzani]

I think it's fair to just use the type field to check whether a parameter is valid.

[Ryanf55]

Overall, this is looking great. Do you know what additional services would be required to get this call to work?

$ ros2 param set /ardupilot_dds LOIT_SPEED 1249
Wait for service timed out

Seems like maybe just a topic name, you are already using the right interface:
https://github.com/ros2/ros2cli/blob/8e46bf2608d04e81a3d088ccc5087dbde9f3e32f/ros2param/ros2param/verb/set.py#L52-L64

Consider making the topic this: /ardupilot_dds/set_parameters

We could also just change the node name to ap (I pushed that to your branch, you can change the name at compile time).

If you did one of those, then we get this awesome ability to use the ROS 2 CLI like so!

ryan@ryan-thinkpad:~$ ros2 node info /ap
/ap
  Subscribers:
    /ap/cmd_gps_pose: ardupilot_msgs/msg/GlobalPosition
    /ap/cmd_vel: geometry_msgs/msg/TwistStamped
    /ap/joy: sensor_msgs/msg/Joy
    /ap/tf: tf2_msgs/msg/TFMessage
  Publishers:
    /ap/battery/battery0: sensor_msgs/msg/BatteryState
    /ap/clock: rosgraph_msgs/msg/Clock
    /ap/geopose/filtered: geographic_msgs/msg/GeoPoseStamped
    /ap/gps_global_origin/filtered: geographic_msgs/msg/GeoPointStamped
    /ap/imu/experimental/data: sensor_msgs/msg/Imu
    /ap/navsat/navsat0: sensor_msgs/msg/NavSatFix
    /ap/pose/filtered: geometry_msgs/msg/PoseStamped
    /ap/tf_static: tf2_msgs/msg/TFMessage
    /ap/time: builtin_interfaces/msg/Time
    /ap/twist/filtered: geometry_msgs/msg/TwistStamped
  Service Servers:
    /ap/arm_motors: ardupilot_msgs/srv/ArmMotors
    /ap/get_parameters: rcl_interfaces/srv/GetParameters
    /ap/mode_switch: ardupilot_msgs/srv/ModeSwitch
    /ap/set_parameters: rcl_interfaces/srv/SetParameters
  Service Clients:

  Action Servers:

  Action Clients:

ryan@ryan-thinkpad:~$ ros2 param get ap LOIT_SPEED
Double value is: 1250.0

[Ryanf55]

Needs a rebase on master.

[PQuill33]

I resolved the suggested changes and rebased onto master.

I used @Ryanf55 suggested change to get the ros2 cli to work with the parameter management, now it can also perform the following:

$ ros2 param set ap LOIT_SPEED 1350
Set parameter successful: Parameter accepted
$ ros2 param get ap LOIT_SPEED
Double value is: 1350.0

[Ryanf55]

This could be a callback in the IO thread because this could take a few mS on hardware.

[tridge]

a followup PR for debug verbosity and precision of int32 vars would be nice

[tridge]

needs fix for stack frame size on build

[Ryanf55]

needs fix for stack frame size on build

../../libraries/AP_DDS/AP_DDS_Client.cpp: In member function 'void AP_DDS_Client::on_request(uxrSession*, uxrObjectId, uint16_t, SampleIdentity*, ucdrBuffer*, uint16_t)':
../../libraries/AP_DDS/AP_DDS_Client.cpp:995:1: error: the frame size of 2800 bytes is larger than 1300 bytes [-Werror=frame-larger-than=]
  995 | }
      | ^
compilation terminated due to -Wfatal-errors.
cc1plus: all warnings being treated as errors

I can help reproduce this if you need.

[PQuill33]

needs fix for stack frame size on build

../../libraries/AP_DDS/AP_DDS_Client.cpp: In member function 'void AP_DDS_Client::on_request(uxrSession*, uxrObjectId, uint16_t, SampleIdentity*, ucdrBuffer*, uint16_t)':
../../libraries/AP_DDS/AP_DDS_Client.cpp:995:1: error: the frame size of 2800 bytes is larger than 1300 bytes [-Werror=frame-larger-than=]
  995 | }
      | ^
compilation terminated due to -Wfatal-errors.
cc1plus: all warnings being treated as errors

I can help reproduce this if you need.

I've been able to reproduce it. The last commit I pushed brought the frame size down quite a bit. Just need to bring it down a little more.

I tried changing this file with container_prealloc_size = 8 changed to 2 and it can build fine. I tried to build Durandal and pixracer with that change and it worked.

I also tried running the dds tests and nothing failed, but I wasn't sure if this is an appropriate path to take.

[Ryanf55]

I tried changing this file with container_prealloc_size = 8 changed to 2 and it can build fine. I tried to build Durandal and pixracer with that change and it worked.

I also tried running the dds tests and nothing failed, but I wasn't sure if this is an appropriate path to take.

Please leave that to 8, we need arrays larger than two for our dynamic transforms when doing SLAM.
There's probably another place to increase this, perhaps here, on the 8192 value:

ardupilot/libraries/AP_DDS/AP_DDS_Client.cpp

Line 621 in 12b761c

8192, AP_HAL::Scheduler::PRIORITY_IO, 1)) {

It's the stack size in this function:

ardupilot/libraries/AP_HAL/Scheduler.h

Line 124 in e232ccd

virtual bool thread_create(AP_HAL::MemberProc proc, const char *name,

[PQuill33]

I fixed the stack frame size issue on build and reworked the integer precision in the get_parameter service and rebased on the latest master. All checks passed now.

Per the dev call yesterday, I plan to put together a smaller follow on PR to move the GCS_SEND_TEXT statements to a DDS debug statement and will address the verbosity in there as well.