AP_DDS: Added ROS 2 service support #24145

arshPratap · 2023-06-27T08:13:35Z

This is work in progress PR that aims to provide support for service support as requested in #23822 and will work as a foundation for #23363 . Currently we focus on providing support for Arming / Disarming motors.

@Ryanf55 @srmainwaring @tridge would like to know your thoughts on this

Ryanf55

Looks good so far. Could you update the README with how to use it so I can test it out?

Are there not any new messages to add to the build?

libraries/AP_DDS/AP_DDS_Client.cpp

libraries/AP_DDS/AP_DDS_Client.h

libraries/AP_DDS/AP_DDS_Service_Table.h

libraries/AP_DDS/dds_xrce_profile.xml

arshPratap · 2023-06-27T16:32:47Z

Looks good so far. Could you update the README with how to use it so I can test it out?

Are there not any new messages to add to the build?

@Ryanf55 thanks for the suggestions , have filled in the README as requested ... I hope you guys are able to successfully setup the Integration Services ( just in case , you can also take a look at this to get yourself a bit more familiar with this aspect of the project)
Currently on my way to working on the other requested changes
Thanks a lot for your quick response

srmainwaring · 2023-06-28T13:27:33Z

Some notes for running on macOS and making this PR independent of other projects / repos.

1. Move the ArduPilot custom service definitions into `ardupilot/Tools/ros2`

Add new ROS 2 project for the ArduPilot specific messages and move definitions from https://github.com/arshPratap/ardupilot_ros2/tree/ArmingClient to ardupilot/Tools/ros2/ardupilot_msgs
Suggested approach here: Tools: add custom ROS 2 messages and service interfaces arshPratap/ardupilot#8

2. Include Integration-Services in the `ros2-ardupilot` workspace

Add the following entries to the rosdep file (this is the macOS version which needs patched repos)

repositories:
  integration_service:
    type: git
    url: https://github.com/srmainwaring/Integration-Service.git
    version: wips/wip-macos
  fastdds_sh:
    type: git
    url: https://github.com/srmainwaring/FastDDS-SH
    version: wips/wip-macos
  ros2_sh:
    type: git
    url: https://github.com/eProsima/ROS2-SH.git
    version: main

3. Install the integration service config file alongside `dds_xrce_profile.xml`

Update the ardupilot_sitl build to install the file from ./libraries/AP_DDS/Is-Config.

4. Add a launch script for the `integration-service` process

Add new launch class and script to ardupilot_sitl to run the process:

IS_PREFIX_PATH=$(pwd)/install/is-ros2/lib/is/ros2:$(pwd)/install/is-ros2-mix-generator/lib \ 
LD_LIBRARY_PATH=$(pwd)/install/is-fastdds/lib \
integration-service \
$(pwd)/src/ardupilot/libraries/AP_DDS/Is-Config/Arm_Motors_DDS_IS_config.yaml

Launch file for integration services here: Tools: add ROS 2 launch file for eProsima integration service arshPratap/ardupilot#9

arshPratap · 2023-07-05T09:11:40Z

@Ryanf55 @srmainwaring have rebased the branch to the latest master branch and have merged @srmainwaring 's PRs into this branch (as requested). @Ryanf55 I have also worked upon all the suggested changes and would love to know your thoughts on them

srmainwaring · 2023-07-05T09:46:43Z

Thanks @arshPratap - the commits to AP_DDS and Tools need to be separated which is why they were broken out in the additions I suggested. Within each of these libraries we could break the changes into something like:

AP_DDS: add ROS 2 arming service
Tools: add ROS 2 custom messages and services
Tools: add ROS 2 launch for eProsima integration service

and squash the changes within those groupings.

Alternatively we could break them out into 3 separate PRs - with the custom messages [2] going in first, then [1] which depends on [2], and finally [3] which is intended to simplify the process of setting up and running the service example. @Ryanf55 - your thoughts?

Did you manage to resolve the error reported in the integration service terminal that is raised when the service returns it's reply message? I see this in macOS and Ubuntu (so not a macOS thing).

Update:

This is the error message I see in the integration service terminal:

[Integration Service][INFO] [is::sh::ROS2] Receiving request from ROS 2 for service request topic 'arm_motors_Request'
[Integration Service][INFO] [is::sh::FastDDS::Server] Translating request from Integration Service to DDS for service request topic 'ArmMotorsService_Request': [[ Structure: <ap_custom_services/ArmMotors:request>
    [arm] <bool>  1
 ]]
2023-06-28 00:29:48.672 [RTPS_READER_HISTORY Error] Change payload size of '12' bytes is larger than the history payload size of '8' bytes and cannot be resized. -> Function can_change_be_added_nts
Request timeout.

Ryanf55 · 2023-07-05T14:35:47Z

Thanks @arshPratap - the commits to AP_DDS and Tools need to be separated which is why they were broken out in the additions I suggested. Within each of these libraries we could break the changes into something like:

AP_DDS: add ROS 2 arming service

Tools: add ROS 2 custom messages and services

Tools: add ROS 2 launch for eProsima integration service

and squash the changes within those groupings.

Alternatively we could break them out into 3 separate PRs - with the custom messages [2] going in first, then [1] which depends on [2], and finally [3] which is intended to simplify the process of setting up and running the service example. @Ryanf55 - your thoughts?

Did you manage to resolve the error reported in the integration service terminal that is raised when the service returns it's reply message? I see this in macOS and Ubuntu (so not a macOS thing).

Update:

This is the error message I see in the integration service terminal:
[Integration Service][INFO] [is::sh::ROS2] Receiving request from ROS 2 for service request topic 'arm_motors_Request'
[Integration Service][INFO] [is::sh::FastDDS::Server] Translating request from Integration Service to DDS for service request topic 'ArmMotorsService_Request': [[ Structure: <ap_custom_services/ArmMotors:request>
    [arm] <bool>  1
 ]]
2023-06-28 00:29:48.672 [RTPS_READER_HISTORY Error] Change payload size of '12' bytes is larger than the history payload size of '8' bytes and cannot be resized. -> Function can_change_be_added_nts
Request timeout.

Either approach of 3 commits or 3 PR's sounds good to me, whichever works best for Arsh. I agree- it cannot be done in one single commit.

srmainwaring · 2023-07-07T11:54:29Z

@arshPratap - here are the logs from running the Arming_DDS_Server example. I see the same error in the IS terminal when running in SITL.

Running these versions:

repositories:
  ardupilot_ros2:
    type: git
    url: https://github.com/arshPratap/ardupilot_ros2.git
    version: ArmingClient
  ROS2_DDS_Service_Demo:
    type: git
    url: https://github.com/arshPratap/ROS2_DDS_Service_Demo.git
    version: Ros2ArmingServer

  Micro-CDR:
    type: git
    url: https://github.com/ArduPilot/Micro-CDR.git
    version: master
  Micro-XRCE-DDS-Client:
    type: git
    url: https://github.com/ArduPilot/Micro-XRCE-DDS-Client.git
    version: master
  Micro-XRCE-DDS-Agent:
    type: git
    url: https://github.com/eProsima/Micro-XRCE-DDS-Agent.git
    version: v2.4.0

  Integration-Service:
    type: git
    url: https://github.com/eProsima/Integration-Service.git
    version: main
  FastDDS-SH:
    type: git
    url: https://github.com/srmainwaring/FastDDS-SH
    version: wips/wip-macos
  ROS2-SH:
    type: git
    url: https://github.com/eProsima/ROS2-SH
    version: main

MicroXRCEAgent:

$ MicroXRCEAgent udp4 -p 2019 -v6
[1688730229.983349] info     | UDPv4AgentLinux.cpp | init                     | running...             | port: 2019
[1688730229.984016] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 6
[1688730263.205322] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x00000000, len: 24, data: 
0000: 80 00 00 00 00 01 10 00 58 52 43 45 01 00 01 0F CC CC DD DD 81 00 FC 01
[1688730263.205465] info     | Root.cpp           | create_client            | create                 | client_key: 0xCCCCDDDD, session_id: 0x81
[1688730263.205503] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0xCCCCDDDD, address: 127.0.0.1:4073
[1688730263.205556] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xCCCCDDDD, len: 19, data: 
0000: 81 00 00 00 04 01 0B 00 00 00 58 52 43 45 01 00 01 0F 00
[1688730263.205634] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xCCCCDDDD, len: 280, data: 
0000: 81 80 00 00 01 05 60 00 00 0A 00 11 01 02 00 42 51 00 00 00 3C 64 64 73 3E 3C 70 61 72 74 69 63
0020: 69 70 61 6E 74 3E 3C 72 74 70 73 3E 3C 6E 61 6D 65 3E 41 72 6D 4D 6F 74 6F 72 73 5F 53 65 72 76
0040: 65 72 3C 2F 6E 61 6D 65 3E 3C 2F 72 74 70 73 3E 3C 2F 70 61 72 74 69 63 69 70 61 6E 74 3E 3C 2F
0060: 64 64 73 3E 00 8B 00 00 01 05 AC 00 00 0B 00 18 08 02 00 00 9E 00 00 00 3C 64 64 73 3E 3C 72 65
0080: 70 6C 69 65 72 20 70 72 6F 66 69 6C 65 5F 6E 61 6D 65 3D 22 41 72 6D 4D 6F 74 6F 72 73 5F 52 65
00A0: 70 6C 69 65 72 22 73 65 72 76 69 63 65 5F 6E 61 6D 65 3D 22 41 72 6D 4D 6F 74 6F 72 73 53 65 72
00C0: 76 69 63 65 22 72 65 71 75 65 73 74 5F 74 79 70 65 3D 22 41 72 6D 4D 6F 74 6F 72 73 5F 52 65 71
00E0: 75 65 73 74 22 72 65 70 6C 79 5F 74 79 70 65 3D 22 41 72 6D 4D 6F 74 6F 72 73 5F 52 65 73 70 6F
0100: 6E 73 65 22 3E 3C 2F 72 65 70 6C 69 65 72 3E 3C 2F 64 64 73 3E 00 00 11
[1688730263.218969] info     | ProxyClient.cpp    | create_participant       | participant created    | client_key: 0xCCCCDDDD, participant_id: 0x001(1)
[1688730263.219024] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xCCCCDDDD, len: 14, data: 
0000: 81 80 00 00 05 01 06 00 00 0A 00 11 00 00
[1688730263.219195] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xCCCCDDDD, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 01 00 00 00 80
[1688730263.219869] info     | ProxyClient.cpp    | create_replier           | replier created        | client_key: 0xCCCCDDDD, requester_id: 0x001(7), participant_id: 0x001(1)
[1688730263.219902] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xCCCCDDDD, len: 14, data: 
0000: 81 80 01 00 05 01 06 00 00 0B 00 18 00 00
[1688730263.219916] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xCCCCDDDD, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 01 00 00 00 80
[1688730263.219951] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xCCCCDDDD, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 02 00 00 00 80
[1688730263.219969] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xCCCCDDDD, len: 24, data: 
0000: 81 80 01 00 08 01 10 00 00 0C 00 18 80 00 00 01 FF FF 00 00 00 00 00 00
[1688730263.220060] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xCCCCDDDD, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 02 00 00 00 80
[1688730422.636606] debug    | Replier.cpp        | read_fn                  | [==>> DDS <<==]        | client_key: 0x00000001, len: 28, data: 
0000: 01 0F 57 FA 62 F2 8D 42 01 00 00 00 00 00 01 03 00 00 00 00 01 00 00 00 01 00 00 00
[1688730422.636692] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xCCCCDDDD, len: 40, data: 
0000: 81 80 02 00 09 01 20 00 00 0C 00 18 01 0F 57 FA 62 F2 8D 42 01 00 00 00 00 00 01 03 00 00 00 00
0020: 01 00 00 00 01 00 00 00
[1688730422.636784] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xCCCCDDDD, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 03 00 00 00 80
[1688730423.636880] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xCCCCDDDD, len: 44, data: 
0000: 81 80 02 00 07 01 24 00 00 0D 00 18 01 0F 57 FA 62 F2 8D 42 01 00 00 00 00 00 01 03 00 00 00 00
0020: 01 00 00 00 01 00 00 00 00 00 00 00
[1688730423.637018] debug    | Replier.cpp        | write                    | [** <<DDS>> **]        | client_key: 0x00000001, len: 32, data: 
0000: 01 0F 57 FA 62 F2 8D 42 01 00 00 00 00 00 01 03 00 00 00 00 01 00 00 00 01 00 00 00 00 00 00 00
[1688730423.637115] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xCCCCDDDD, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 03 00 00 00 80

Arming_DDS_Server:

$ ./a.out 127.0.0.1 2019 1
Entities Created Successfully 
Request recieved for Arming 
Reply : Armed

Integration Service

$ LD_LIBRARY_PATH=/Users/rhys/Code/ros2/xrce-dds/eprosima_ws/install/is-fastdds/lib integration-service ./src/ROS2_DDS_Service_Demo/AP_Services/Arming_DDS_Server/Arm_Motors_DDS_IS_config.yaml --is-prefix-path /Users/rhys/Code/ros2/xrce-dds/eprosima_ws/install/is-ros2/lib/is/ros2 --is-prefix-path /Users/rhys/Code/ros2/xrce-dds/eprosima_ws/install/is-ros2-mix-generator/lib/is
[Integration Service][WARN] [is::sh::FastDDS] Participant not provided in configuration file. A participant using the default transport locators and Domain ID 0 will be created.
[Integration Service][INFO] [is::sh::FastDDS::Participant] Created Fast DDS participant 'default_IS-FastDDS-SH_participant' with default QoS attributes and Domain ID: 0
[Integration Service][INFO] [is::sh::FastDDS] Configured!
[Integration Service][INFO] [is::sh::ROS2] Created node '/is_ros2_node_16807' with Domain ID: 0
[Integration Service][INFO] [is::sh::ROS2] Configured!
[Integration Service][INFO] [is::sh::FastDDS::Server] Publisher for topic 'ArmMotorsService_Request' matched
[Integration Service][INFO] [is::sh::FastDDS::Server] Subscriber for topic 'ArmMotorsService_Reply' matched
[Integration Service][INFO] [is::sh::FastDDS] Server created for service 'ArmMotorsService', with request_type 'ArmMotors_Request' and reply_type 'ArmMotors_Response'
[Integration Service][INFO] [is::core::Config]  [dds SystemHandle] Produced a service provider for the service 'arm_motors', with request type 'ArmMotors_Request' and reply type 'ArmMotors_Response'.
[Integration Service][INFO] [is::sh::ROS2] Creating entity using the default QoS.
[Integration Service][INFO] [is::sh::ROS2] Created service client for service 'arm_motors' with type 'ap_custom_services/ArmMotors:response' on node '/is_ros2_node_16807'
[Integration Service][INFO] [is::core::Config] [ros2 SystemHandle] Produced a service client for the service 'arm_motors', with request type 'ArmMotors_Request' and reply type 'ArmMotors_Response'.
[Integration Service][INFO] [is::sh::ROS2] Receiving request from ROS 2 for service request topic 'arm_motors_Request'
[Integration Service][INFO] [is::sh::FastDDS::Server] Translating request from Integration Service to DDS for service request topic 'ArmMotorsService_Request': [[ Structure: <ap_custom_services/ArmMotors:request>
    [arm] <bool>  1
 ]]
2023-07-07 12:47:03.637 [RTPS_READER_HISTORY Error] Change payload size of '12' bytes is larger than the history payload size of '8' bytes and cannot be resized. -> Function can_change_be_added_nts
Request timeout.

Arm Motors Client:

$ ros2 run ap_service_clients arm_motors_client 1
[INFO] [1688730369.125680900] [rclcpp]: Enter Arming Command
arm
[INFO] [1688730427.639468326] [rclcpp]: Motors : Disarmed Successfully

or alternatively the ROS 2 service call may be made directly:

$ ros2 service call /arm_motors ap_custom_services/srv/ArmMotors "{arm: 0}"        
requester: making request: ap_custom_services.srv.ArmMotors_Request(arm=False)


response:
ap_custom_services.srv.ArmMotors_Response(result=False)

srmainwaring · 2023-07-08T15:17:05Z

@arshPratap - additional tools: https://github.com/srmainwaring/ROS2_DDS_Service_Demo/tree/wips/wip-ros2-arming-server-fastdds

Build:

Build a FastDDS server and client based on the AddTwoInts example, and the Micro XRCE Client example service using cmake:

$ cd ~/ws/src/ROS2_DDS_Service_Demo/AP_Services/Arming_DDS_Server
$ mkdir build && cd build
$ cmake ..
$ make

Expected build outputs:

ArmMotors the FastDDS server / client
ArmMotorsMicro the Micro XRCE Client arming service (previously built directly and referred to as a.out above)

Example 1: ROS 2 <=> FastDDS:

Run an arming service using FastDDS (i.e. no Micro XRCE Agent invoked).
Run the Integration Service to translate from ROS 2 to FastDDS.
Run a service call using ROS 2 CLI.

Run

Integration Service:

cd ~ws
$ LD_LIBRARY_PATH=$(pwd)/install/is-fastdds/lib integration-service $(pwd)/src/ROS2_DDS_Service_Demo/AP_Services/Arming_DDS_Server/Arm_Motors_DDS_IS_config.yaml --is-prefix-path $(pwd)/install/is-ros2/lib/is/ros2 --is-prefix-path $(pwd)/install/is-ros2-mix-generator/lib/is

[Integration Service][WARN] [is::sh::FastDDS] Participant not provided in configuration file. A participant using the default transport locators and Domain ID 0 will be created.
[Integration Service][INFO] [is::sh::FastDDS::Participant] Created Fast DDS participant 'default_IS-FastDDS-SH_participant' with default QoS attributes and Domain ID: 0
[Integration Service][INFO] [is::sh::FastDDS] Configured!
[Integration Service][INFO] [is::sh::ROS2] Created node '/is_ros2_node_16807' with Domain ID: 0
[Integration Service][INFO] [is::sh::ROS2] Configured!
[Integration Service][INFO] [is::sh::FastDDS] Server created for service 'ArmMotorsService', with request_type 'ArmMotors_Request' and reply_type 'ArmMotors_Response'
[Integration Service][INFO] [is::core::Config]  [dds SystemHandle] Produced a service provider for the service 'arm_motors', with request type 'ArmMotors_Request' and reply type 'ArmMotors_Response'.
[Integration Service][INFO] [is::sh::ROS2] Creating entity using the default QoS.
[Integration Service][INFO] [is::sh::ROS2] Created service client for service 'arm_motors' with type 'ap_custom_services/ArmMotors:response' on node '/is_ros2_node_16807'
[Integration Service][INFO] [is::core::Config] [ros2 SystemHandle] Produced a service client for the service 'arm_motors', with request type 'ArmMotors_Request' and reply type 'ArmMotors_Response'.
[Integration Service][INFO] [is::sh::FastDDS::Server] Publisher for topic 'ArmMotorsService_Request' matched
[Integration Service][INFO] [is::sh::FastDDS::Server] Subscriber for topic 'ArmMotorsService_Reply' matched

FastDDS arming service:

$ cd ~/ws/src/ROS2_DDS_Service_Demo/AP_Services/Arming_DDS_Server/build
$ ./ArmMotors -m server

ArmMotorsService server running under DDS Domain ID: 0
ArmMotorsService server running. Please press enter to stop it at any time.

ROS 2 CLI service call:

ros2 service call /arm_motors ap_custom_services/srv/ArmMotors "{arm: 1}"
requester: making request: ap_custom_services.srv.ArmMotors_Request(arm=True)

response:
ap_custom_services.srv.ArmMotors_Response(result=True)

Outputs

Integration Service output:

[Integration Service][INFO] [is::sh::ROS2] Receiving request from ROS 2 for service request topic 'arm_motors_Request'
[Integration Service][INFO] [is::sh::FastDDS::Server] Translating request from Integration Service to DDS for service request topic 'ArmMotorsService_Request': [[ Structure: <ap_custom_services/ArmMotors:request>
    [arm] <bool>  1
 ]]
[Integration Service][INFO] [is::sh::FastDDS::Server] Receiving reply from DDS for service reply topic 'ArmMotorsService_Reply'
[Integration Service][INFO] [is::sh::ROS2] Translating reply from Integration Service to ROS 2 for service reply topic 'arm_motors_Reply': [[ Structure: <ArmMotors_Response>
    [result] <bool>  1
 ]]

FastDDS arming service:

ArmMotorsService server: processing incoming request, arm 1
	 - Sending back response: 1

Example 2: FastDDS <=> Micro XRCE Agent <=> Micro XRCE Client:

Run an arming service using the Micro XRCE Client API.
Run the Micro XRCE Agent using udp4 on port 2019.
Run the integration service to translate from ROS 2 or FastDDS to FastDDS.
Run a service call using FastDDS or the ROS 2 CLI.

Run

Integration Service:

(As per the first example).

Micro XRCE Agent:

MicroXRCEAgent udp4 -p 2019 -v6
[1688828954.031760] info     | UDPv4AgentLinux.cpp | init                     | running...             | port: 2019
[1688828954.032428] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 6

Micro XRCE Client arming service:

$ cd ~/ws/src/ROS2_DDS_Service_Demo/AP_Services/Arming_DDS_Server/build
./ArmMotorsMicro 127.0.0.1 2019

FastDDS service call:

$ cd ~/ws/src/ROS2_DDS_Service_Demo/AP_Services/Arming_DDS_Server/build
$ ./ArmMotors -m client -c 1
ArmMotorsService client running under DDS Domain ID: 0
ArmMotorsService client performing 1 requests.
ArmMotorsService client:
	 - Request 1
2023-07-08 16:15:11.918 [RTPS_READER_HISTORY Error] Change payload size of '12' bytes is larger than the history payload size of '11' bytes and cannot be resized. -> Function can_change_be_added_nts
	 - Error: client exceeded maximum time to wait for a response!

Outputs

Integration Service output:

$ LD_LIBRARY_PATH=$(pwd)/install/is-fastdds/lib integration-service $(pwd)/src/ROS2_DDS_Service_Demo/AP_Services/Arming_DDS_Server/Arm_Motors_DDS_IS_config.yaml --is-prefix-path $(pwd)/install/is-ros2/lib/is/ros2 --is-prefix-path $(pwd)/install/is-ros2-mix-generator/lib/is

[Integration Service][WARN] [is::sh::FastDDS] Participant not provided in configuration file. A participant using the default transport locators and Domain ID 0 will be created.
[Integration Service][INFO] [is::sh::FastDDS::Participant] Created Fast DDS participant 'default_IS-FastDDS-SH_participant' with default QoS attributes and Domain ID: 0
[Integration Service][INFO] [is::sh::FastDDS] Configured!
[Integration Service][INFO] [is::sh::ROS2] Created node '/is_ros2_node_16807' with Domain ID: 0
[Integration Service][INFO] [is::sh::ROS2] Configured!
[Integration Service][INFO] [is::sh::FastDDS::Server] Publisher for topic 'ArmMotorsService_Request' matched
[Integration Service][INFO] [is::sh::FastDDS::Server] Subscriber for topic 'ArmMotorsService_Reply' matched
[Integration Service][INFO] [is::sh::FastDDS] Server created for service 'ArmMotorsService', with request_type 'ArmMotors_Request' and reply_type 'ArmMotors_Response'
[Integration Service][INFO] [is::core::Config]  [dds SystemHandle] Produced a service provider for the service 'arm_motors', with request type 'ArmMotors_Request' and reply type 'ArmMotors_Response'.
[Integration Service][INFO] [is::sh::ROS2] Creating entity using the default QoS.
[Integration Service][INFO] [is::sh::ROS2] Created service client for service 'arm_motors' with type 'ap_custom_services/ArmMotors:response' on node '/is_ros2_node_16807'
[Integration Service][INFO] [is::core::Config] [ros2 SystemHandle] Produced a service client for the service 'arm_motors', with request type 'ArmMotors_Request' and reply type 'ArmMotors_Response'.
2023-07-08 16:15:11.918 [RTPS_READER_HISTORY Error] Change payload size of '12' bytes is larger than the history payload size of '8' bytes and cannot be resized. -> Function can_change_be_added_nts

Micro XRCE Client arming service:

$ ./ArmMotorsMicro 127.0.0.1 2019
Entities Created Successfully 
Request recieved for Arming 
Reply : Armed

Micro XRCE Agent:

$ MicroXRCEAgent udp4 -p 2019 -v6
[1688829296.438878] info     | UDPv4AgentLinux.cpp | init                     | running...             | port: 2019
[1688829296.439526] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 6
[1688829299.514055] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0x00000000, len: 24, data: 
0000: 80 00 00 00 00 01 10 00 58 52 43 45 01 00 01 0F CC CC DD DD 81 00 FC 01
[1688829299.514166] info     | Root.cpp           | create_client            | create                 | client_key: 0xCCCCDDDD, session_id: 0x81
[1688829299.514204] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0xCCCCDDDD, address: 127.0.0.1:43263
[1688829299.514260] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xCCCCDDDD, len: 19, data: 
0000: 81 00 00 00 04 01 0B 00 00 00 58 52 43 45 01 00 01 0F 00
[1688829299.514337] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xCCCCDDDD, len: 280, data: 
0000: 81 80 00 00 01 05 60 00 00 0A 00 11 01 02 00 42 51 00 00 00 3C 64 64 73 3E 3C 70 61 72 74 69 63
0020: 69 70 61 6E 74 3E 3C 72 74 70 73 3E 3C 6E 61 6D 65 3E 41 72 6D 4D 6F 74 6F 72 73 5F 53 65 72 76
0040: 65 72 3C 2F 6E 61 6D 65 3E 3C 2F 72 74 70 73 3E 3C 2F 70 61 72 74 69 63 69 70 61 6E 74 3E 3C 2F
0060: 64 64 73 3E 00 8B 00 00 01 05 AC 00 00 0B 00 18 08 02 00 00 9E 00 00 00 3C 64 64 73 3E 3C 72 65
0080: 70 6C 69 65 72 20 70 72 6F 66 69 6C 65 5F 6E 61 6D 65 3D 22 41 72 6D 4D 6F 74 6F 72 73 5F 52 65
00A0: 70 6C 69 65 72 22 73 65 72 76 69 63 65 5F 6E 61 6D 65 3D 22 41 72 6D 4D 6F 74 6F 72 73 53 65 72
00C0: 76 69 63 65 22 72 65 71 75 65 73 74 5F 74 79 70 65 3D 22 41 72 6D 4D 6F 74 6F 72 73 5F 52 65 71
00E0: 75 65 73 74 22 72 65 70 6C 79 5F 74 79 70 65 3D 22 41 72 6D 4D 6F 74 6F 72 73 5F 52 65 73 70 6F
0100: 6E 73 65 22 3E 3C 2F 72 65 70 6C 69 65 72 3E 3C 2F 64 64 73 3E 00 00 11
[1688829299.530461] info     | ProxyClient.cpp    | create_participant       | participant created    | client_key: 0xCCCCDDDD, participant_id: 0x001(1)
[1688829299.530528] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xCCCCDDDD, len: 14, data: 
0000: 81 80 00 00 05 01 06 00 00 0A 00 11 00 00
[1688829299.530736] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xCCCCDDDD, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 01 00 00 00 80
[1688829299.532110] info     | ProxyClient.cpp    | create_replier           | replier created        | client_key: 0xCCCCDDDD, requester_id: 0x001(7), participant_id: 0x001(1)
[1688829299.532153] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xCCCCDDDD, len: 14, data: 
0000: 81 80 01 00 05 01 06 00 00 0B 00 18 00 00
[1688829299.532171] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xCCCCDDDD, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 01 00 00 00 80
[1688829299.532221] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xCCCCDDDD, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 02 00 00 00 80
[1688829299.532269] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xCCCCDDDD, len: 24, data: 
0000: 81 80 01 00 08 01 10 00 00 0C 00 18 80 00 00 01 FF FF 00 00 00 00 00 00
[1688829299.532365] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xCCCCDDDD, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 02 00 00 00 80
[1688829310.917022] debug    | Replier.cpp        | read_fn                  | [==>> DDS <<==]        | client_key: 0x00000001, len: 28, data: 
0000: 01 0F 7A 56 DE 01 9C E7 01 00 00 00 00 00 01 03 00 00 00 00 01 00 00 00 01 00 00 00
[1688829310.917083] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xCCCCDDDD, len: 40, data: 
0000: 81 80 02 00 09 01 20 00 00 0C 00 18 01 0F 7A 56 DE 01 9C E7 01 00 00 00 00 00 01 03 00 00 00 00
0020: 01 00 00 00 01 00 00 00
[1688829310.917152] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xCCCCDDDD, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 03 00 00 00 80
[1688829311.918279] debug    | UDPv4AgentLinux.cpp | recv_message             | [==>> UDP <<==]        | client_key: 0xCCCCDDDD, len: 44, data: 
0000: 81 80 02 00 07 01 24 00 00 0D 00 18 01 0F 7A 56 DE 01 9C E7 01 00 00 00 00 00 01 03 00 00 00 00
0020: 01 00 00 00 01 00 00 00 00 00 00 00
[1688829311.918446] debug    | Replier.cpp        | write                    | [** <<DDS>> **]        | client_key: 0x00000001, len: 32, data: 
0000: 01 0F 7A 56 DE 01 9C E7 01 00 00 00 00 00 01 03 00 00 00 00 01 00 00 00 01 00 00 00 00 00 00 00
[1688829311.918508] debug    | UDPv4AgentLinux.cpp | send_message             | [** <<UDP>> **]        | client_key: 0xCCCCDDDD, len: 13, data: 
0000: 81 00 00 00 0A 01 05 00 03 00 00 00 80

Notes

Don't require the Integration Service - it's enough to run the Micro XRCE Agent / Client and FastDDS client.

MicroXRCEAgent udp4 -p 2019 -v6
./ArmMotorsMicro 127.0.0.1 2019
./ArmMotors -m client -c 1

So the timeout issue has nothing to do with the ROS2-SH plugin.

libraries/AP_DDS/AP_DDS_Client.cpp

srmainwaring · 2023-07-12T07:20:00Z

Hi @arshPratap, when including:

Tools: add custom ROS 2 messages and service interfaces arshPratap/ardupilot#8
Tools: add ROS 2 launch file for eProsima integration service arshPratap/ardupilot#9

the commits to Tools/ros2 should be split into separate commits from those to AP_DDS.

Might be easier if we break them out from this PR, get - arshPratap#8 merged first and follow up with arshPratap#9 after this.

Update Submitted PRs for the contributions to Tools/ros2.

libraries/AP_DDS/AP_DDS_Client.cpp

libraries/AP_DDS/README.md

srmainwaring · 2023-08-07T17:58:21Z

Hi @arshPratap - thanks for getting all the changes in. I have run a clean build and test of the most recent version and appears to be working. Tested the following:

Build - OK.
- Added request to update README to include extra cmake flags for the is-ros-mix-generator SH.
SITL - OK
- Run using the UDP launch.py.
Integration Service - OK
- Need additional flags to locate the .mix files and libs on macOS. Not required on Ubuntu.
DDS_DEBUG param - OK
Service call - OK
- Vehicle arms / disarms.
- A second call to arm should return false if the vehicle is already armed, however I think this is an upstream issue as the service call is directly returning AP::arming().arm(AP_Arming::Method::DDS).

Ryanf55 · 2023-08-08T15:39:59Z

With the testing of @srmainwaring , I'm happy to let this in and resolve the integration service compilation issues on my side separately. @arshPratap Please fix the merge conflicts, and we can merge it. Many thanks.

arshPratap · 2023-08-09T05:56:41Z

@srmainwaring @Ryanf55 thanks for the review and testing .. I have made the requested changes and have pushed in the code

Ryanf55 · 2023-08-09T06:01:54Z

Please run pre-commit, it is failing on libraries/AP_DDS/dds_xrce_profile.xml. Ready to merge otherwise.

cd /your/path/to/ardupilot
python3 -m pip install pre-commit
pre-commit install
pre-commit run --files libraries/AP_DDS/* -v

arshPratap · 2023-08-09T06:11:42Z

Please run pre-commit, it is failing on libraries/AP_DDS/dds_xrce_profile.xml. Ready to merge otherwise.
cd /your/path/to/ardupilot
python3 -m pip install pre-commit
pre-commit install
pre-commit run --files libraries/AP_DDS/* -v

thanks @Ryanf55 ... have applied the changes

Ryanf55

As mentioned in chat, I am strongly opposed to the modified logging method that is globally applied to all the logging. I would be happy to accept the PR without it. If it's not too work, split those changes to another PR, and this can go in. Changing the logging methodology to hide errors is not part of ROS 2 service support.

arshPratap · 2023-08-09T06:18:13Z

As mentioned in chat, I am strongly opposed to the modified logging method that is globally applied to all the logging. I would be happy to accept the PR without it. If it's not too work, split those changes to another PR, and this can go in. Changing the logging methodology to hide errors is not part of ROS 2 service support.

@tridge is it alright if I split the logging changes to another PR as @Ryanf55 suggests ? Or should it be in this PR

tridge · 2023-08-10T02:19:42Z

@Ryanf55 I'm fine with the debug being a separate PR. It should probably take an enum level. Main reason for changing is right now we do SEND_TEXT on every message, which would be complete impractical while flying (would flood the radio link). For example, on every joystick pkt

Ryanf55 · 2023-08-10T02:41:09Z

@Ryanf55 I'm fine with the debug being a separate PR. It should probably take an enum level. Main reason for changing is right now we do SEND_TEXT on every message, which would be complete impractical while flying (would flood the radio link). For example, on every joystick pkt

Thanks for the explanation. Totally on board; it wasn't the intent to leave those once the interfaces were developed. We can figure better plan in the next PR.

libraries/AP_DDS/AP_DDS_Client.cpp

arshPratap · 2023-08-10T02:59:02Z

thanks @Ryanf55 for the review..have made the changes as requested

arshPratap changed the title ~~AP_DDS: Added ROS-2 service support [skip_ci]~~ AP_DDS: Added ROS-2 service support Jun 27, 2023

arshPratap force-pushed the ddsServiceSupport branch from bf928b8 to aa8dd3e Compare June 27, 2023 08:22

srmainwaring added ROS Enhancement labels Jun 27, 2023

srmainwaring self-requested a review June 27, 2023 08:37

Ryanf55 reviewed Jun 27, 2023

View reviewed changes

arshPratap force-pushed the ddsServiceSupport branch from aa8dd3e to 0d230db Compare June 27, 2023 16:29

arshPratap force-pushed the ddsServiceSupport branch from 0d230db to 2e34ba2 Compare June 28, 2023 02:03

arshPratap changed the title ~~AP_DDS: Added ROS-2 service support~~ AP_DDS: Added ROS 2 service support Jun 28, 2023

arshPratap force-pushed the ddsServiceSupport branch from 32e4c6c to 5beb219 Compare July 5, 2023 09:06

arshPratap force-pushed the ddsServiceSupport branch from 5beb219 to 65bb56c Compare July 6, 2023 10:04

tridge requested changes Jul 12, 2023

View reviewed changes

libraries/AP_DDS/AP_DDS_Client.cpp Outdated Show resolved Hide resolved

libraries/AP_DDS/AP_DDS_Client.cpp Outdated Show resolved Hide resolved

arshPratap marked this pull request as ready for review July 12, 2023 02:25

srmainwaring reviewed Jul 12, 2023

View reviewed changes

libraries/AP_DDS/AP_DDS_Client.cpp Outdated Show resolved Hide resolved

This was referenced Jul 12, 2023

Tools: add custom ROS 2 messages and service interfaces #24277

Merged

Tools: Install ROS 2 eProsima integration service profile #24280

Closed

arshPratap force-pushed the ddsServiceSupport branch 6 times, most recently from 6542c3d to 9456585 Compare July 23, 2023 14:34

srmainwaring reviewed Aug 7, 2023

View reviewed changes

libraries/AP_DDS/AP_DDS_Client.cpp Outdated Show resolved Hide resolved

srmainwaring reviewed Aug 7, 2023

View reviewed changes

libraries/AP_DDS/README.md Show resolved Hide resolved

arshPratap force-pushed the ddsServiceSupport branch 2 times, most recently from 24dbca4 to f5456c9 Compare August 9, 2023 05:55

arshPratap force-pushed the ddsServiceSupport branch from f5456c9 to 073ec4e Compare August 9, 2023 06:10

Ryanf55 requested changes Aug 9, 2023

View reviewed changes

arshPratap force-pushed the ddsServiceSupport branch from 073ec4e to b9534a3 Compare August 10, 2023 02:42

Ryanf55 reviewed Aug 10, 2023

View reviewed changes

libraries/AP_DDS/AP_DDS_Client.cpp Outdated Show resolved Hide resolved

arshPratap force-pushed the ddsServiceSupport branch from b9534a3 to 680814f Compare August 10, 2023 02:57

Ryanf55 approved these changes Aug 10, 2023

View reviewed changes

Ryanf55 added MergeOnCIPass and removed NeedsTesting labels Aug 10, 2023

arshPratap force-pushed the ddsServiceSupport branch from 680814f to bbef54e Compare August 10, 2023 19:04

arshPratap added 2 commits August 11, 2023 02:08

AP_Arming: Added DDS Method for Arming/Disarming

6c883ab

AP_DDS: Added ROS 2 service support

e5b77eb

arshPratap force-pushed the ddsServiceSupport branch from bbef54e to e5b77eb Compare August 10, 2023 20:38

tridge merged commit 47efaf9 into ArduPilot:master Aug 11, 2023
81 checks passed

This was referenced Aug 19, 2023

Tools : Updated description for Arming Motor Service #24704

Merged

AP_DDS: Added verbose debugging output messages #24740

Open

srmainwaring mentioned this pull request Sep 5, 2023

AP_DDS: Add service support #23822

Closed

srmainwaring removed the MergeOnCIPass label Feb 27, 2024

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

AP_DDS: Added ROS 2 service support #24145

AP_DDS: Added ROS 2 service support #24145

arshPratap commented Jun 27, 2023

Ryanf55 left a comment

arshPratap commented Jun 27, 2023

srmainwaring commented Jun 28, 2023 •

edited

arshPratap commented Jul 5, 2023

srmainwaring commented Jul 5, 2023 •

edited

Ryanf55 commented Jul 5, 2023

srmainwaring commented Jul 7, 2023 •

edited

srmainwaring commented Jul 8, 2023 •

edited

srmainwaring commented Jul 12, 2023 •

edited

srmainwaring commented Aug 7, 2023

Ryanf55 commented Aug 8, 2023

arshPratap commented Aug 9, 2023

Ryanf55 commented Aug 9, 2023 •

edited

arshPratap commented Aug 9, 2023

Ryanf55 left a comment •

edited

arshPratap commented Aug 9, 2023

tridge commented Aug 10, 2023 •

edited

Ryanf55 commented Aug 10, 2023

arshPratap commented Aug 10, 2023

AP_DDS: Added ROS 2 service support #24145

AP_DDS: Added ROS 2 service support #24145

Conversation

arshPratap commented Jun 27, 2023

Ryanf55 left a comment

Choose a reason for hiding this comment

arshPratap commented Jun 27, 2023

srmainwaring commented Jun 28, 2023 • edited

1. Move the ArduPilot custom service definitions into ardupilot/Tools/ros2

2. Include Integration-Services in the ros2-ardupilot workspace

3. Install the integration service config file alongside dds_xrce_profile.xml

4. Add a launch script for the integration-service process

arshPratap commented Jul 5, 2023

srmainwaring commented Jul 5, 2023 • edited

Ryanf55 commented Jul 5, 2023

srmainwaring commented Jul 7, 2023 • edited

srmainwaring commented Jul 8, 2023 • edited

Build:

Example 1: ROS 2 <=> FastDDS:

Run

Outputs

Example 2: FastDDS <=> Micro XRCE Agent <=> Micro XRCE Client:

Run

Outputs

Notes

srmainwaring commented Jul 12, 2023 • edited

srmainwaring commented Aug 7, 2023

Ryanf55 commented Aug 8, 2023

arshPratap commented Aug 9, 2023

Ryanf55 commented Aug 9, 2023 • edited

arshPratap commented Aug 9, 2023

Ryanf55 left a comment • edited

Choose a reason for hiding this comment

arshPratap commented Aug 9, 2023

tridge commented Aug 10, 2023 • edited

Ryanf55 commented Aug 10, 2023

arshPratap commented Aug 10, 2023

srmainwaring commented Jun 28, 2023 •

edited

1. Move the ArduPilot custom service definitions into `ardupilot/Tools/ros2`

2. Include Integration-Services in the `ros2-ardupilot` workspace

3. Install the integration service config file alongside `dds_xrce_profile.xml`

4. Add a launch script for the `integration-service` process

srmainwaring commented Jul 5, 2023 •

edited

srmainwaring commented Jul 7, 2023 •

edited

srmainwaring commented Jul 8, 2023 •

edited

srmainwaring commented Jul 12, 2023 •

edited

Ryanf55 commented Aug 9, 2023 •

edited

Ryanf55 left a comment •

edited

tridge commented Aug 10, 2023 •

edited