Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

AP_DDS: Parameters service #28298

Open
wants to merge 5 commits into
base: master
Choose a base branch
from
Open

AP_DDS: Parameters service #28298

wants to merge 5 commits into from
+600 −2

Conversation

PQuill33
Copy link

@PQuill33 PQuill33 commented Oct 3, 2024 •
edited by Ryanf55
Loading

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_list

  • 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:
param_success

service call window:
single_param_success

  • Service call to get parameter
ros2 service call /ap/get_parameters rcl_interfaces/srv/GetParameters "{names: ['LOIT_SPEED']}"

Expected output
mavproxy window:
get_param_success

service call window:
get_param_response

@PQuill33 PQuill33 mentioned this pull request Oct 3, 2024
Open
@Ryanf55 Ryanf55 self-requested a review October 7, 2024 22:18
@Ryanf55 Ryanf55 added the ROS label Oct 7, 2024
@PQuill33 PQuill33 marked this pull request as ready for review October 10, 2024 16:00
@tizianofiorenzani
Copy link
Contributor

tizianofiorenzani commented Oct 11, 2024 •
edited
Loading

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
Copy link
Author

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
Copy link
Contributor

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

@Ryanf55
Copy link
Collaborator

Ryanf55 commented Oct 19, 2024 •
edited
Loading

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
Ryanf55 reviewed Oct 19, 2024
View reviewed changes
libraries/AP_DDS/AP_DDS_Client.cpp
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;
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It might be good to add a GSC_SEND_TEXT warning here. I don't think people will be calling this in a loop.

libraries/AP_DDS/AP_DDS_Client.cpp
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.
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It could be useful to comment which param types we don't handle.

libraries/AP_DDS/AP_DDS_Client.cpp
break;
}
default: {
break;
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Do we silently discard other parameter types? What's the expected user behavior in this case?

libraries/AP_DDS/AP_DDS_Client.cpp
Comment on lines +781 to +782
// the user can set BRD_OPTIONS to enable set of internal
// parameters, for developer testing or unusual use cases
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
// the user can set BRD_OPTIONS to enable set of internal
// parameters, for developer testing or unusual use cases
// The user can set BRD_OPTIONS to enable set of internal
// parameters, for developer testing or unusual use cases.

Please end multi-line comments in periods which are easier to detect issues in merge conflicts.

libraries/AP_DDS/AP_DDS_Client.cpp
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);
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Did you look at using set_and_save_by_name_ifchanged?

libraries/AP_DDS/AP_DDS_Client.cpp

// set parameter
AP_Param *vp;
char param_key[AP_MAX_NAME_SIZE+1];
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
char param_key[AP_MAX_NAME_SIZE+1];
char param_key[AP_MAX_NAME_SIZE + 1];

Style nit - astyle doesn't catch these.

libraries/AP_DDS/AP_DDS_Client.cpp
enum ap_var_type var_type;

AP_Param *vp;
char param_key[AP_MAX_NAME_SIZE+1];
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
char param_key[AP_MAX_NAME_SIZE+1];
char param_key[AP_MAX_NAME_SIZE + 1];

Style nit

libraries/AP_DDS/AP_DDS_Client.cpp
continue;
}

float param_value = vp->cast_to_float(var_type);
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
float param_value = vp->cast_to_float(var_type);
const float param_value = vp->cast_to_float(var_type);

Use const for locals when possible

libraries/AP_DDS/AP_DDS_Client.cpp

uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub));

GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Get Parameters Request : %s", msg_prefix, successful_read ? "SUCCESSFUL" : "ONE OR MORE PARAM NOT FOUND");
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Get Parameters Request : %s", msg_prefix, successful_read ? "SUCCESSFUL" : "ONE OR MORE PARAM NOT FOUND");
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");

I like this trick to make the log level different dependent on the result code.

@Ryanf55
AP_DDS: Use ap as node name
4ce5403 
* This makes params work in the CLI

Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com>
@Ryanf55
Copy link
Collaborator

Ryanf55 commented Oct 19, 2024

Needs a rebase on master.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@Ryanf55 Ryanf55 Ryanf55 left review comments

Assignees
No one assigned
Labels
ROS
Projects
DDS/ROS2
Status: 👀 In review
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.
3 participants
@PQuill33 @tizianofiorenzani @Ryanf55