GSoC’ 23 : Improvements for the Native DDS Support in Ardupilot

arshPratap · July 10, 2023, 2:19pm

Introduction

Hello Ardupilot Community ! My name is Arsh Pratap and I have been selected again this year to work with this amazing community under the GSoC’ 23 programme . The title of my GSoC project for this term is Improvements for the Native DDS Support in Ardupilot and just as the name suggests will be a natural extension for my last project (blog details below)

GSoC 2021 : Native ROS2 Support

which focused on primarily providing DDS Support for Ardupilot .

Before we start , I would like to thank my GSoC mentors @tridge and @rhys for helping me with setting up the project architecture and planning out several of the involved project tasks. I would also like to thank my friend @rfriedman for helping me with a lot of the nitty-gritty involved with ROS-2 projects , for always reviewing my PRs and providing valuable suggestions on improving the said pull requests.I would also like to thank @jmachuca , my GSoC mentor for the previous term without whom much of this project wouldn’t have materialised and would also like to thank @james_pattison for helping me with initial research involved with this project.

Recap

In my previous GSoC project , I had worked on implementing an XRCE-DDS client for Ardupilot vehicles so as to provide them with native DDS publisher functionalities.

Link to original PR :

github.com/ArduPilot/ardupilot

Dds prototype

ArduPilot:master ← arshPratap:ddsPrototype

opened 03:53PM - 15 Jun 21 UTC

arshPratap

+1375 -1

Hello Ardupilot community! This PR is work in progress and consists of the prot…otype code that aims to provide DDS functionality to Ardupilot for effective communication with ROS2 and Native DDS implementations Added 2 IDL files - A simple Num idl to test the xrce dds functionality - AP_INS idl file for sending INS info Added the eProsima XRCE Client lib as submodule Provides Ardupilot an XRCE client to communicate with DDS/ROS2 through : Serial(Main focus) UDP(Only for testing,to be removed in final version) Current Issues - [x] Have tested the code through serial communication and sometimes it is found that data is not properly flushed out in the serial ports resulting in communication to XRCE agent being cut.Working on implementing a function to take care of this issue. - [x] Have doubts related to setting up the build system to install the XRCE Client from the necessary submodules Thanks !

To further aid with the task of connecting with the ROS-2 domain we had utilised two different approaches :

Micro-ROS Agent (official approach ) : With directly connecting with the Micro-ROS agent we are directly able to publish topics in the ROS-2 domain and have them received by ROS-2 subscribers. Youtube Video Link

https://www.youtube.com/watch?v=fnDoRSWQ08Y

Micro-XRCE Agent (alternative approach) : We connect to the native XRCE Agent but require assistance from eProsima’s Integration Service to further carry out the task of connecting XRCE clients to the ROS-2 nodes. Youtube Video Link

https://www.youtube.com/watch?v=MUth9duNFys

It was followed by the creation of the ardupilot_ros2 repo that consisted of :

ROS-2 interface consisting of definitions for customised messages.
ROS-2 - DDS .yaml files that serve as Configuration files for setting up Integration Services
ROS-2 subscribers for listening into the ROS-2 standard messages and customised messages defined above.

An in-depth explanation for the project can be found here at my GSoC’21 blog.
Link to my GSoC’21 blog :

GSoC 2021 : Native ROS2 Support

Work Done

For this term, our vision was to heavily expand our work that was carried out previously and provide support for the following features :

ROS-2 Subscriber Support
ROS-2 Service Support
ROS-2 Action Support
Creation of a ROS-2 controller interface (described here)

Target Dependencies

For this project , we have decided to issue Ubuntu 22.04 as the target Operating System for testing out our work.

Also on the ROS-2 side , we are currently targeting the following versions :

ROS-2 Humble
Gazebo Garden

For all the interested readers out there who are ready to test out and replicate our work , make sure the above dependencies have been met.

UDP Support

In the previous term , Serial Transport was chosen as the standard mode of transport from the Ardupilot side for connecting the XRCE Client with the XRCE Agent. In this term, @tridge had provided us with the feedback that we should expand our mode of transport for DDS to support UDP based communication so as to sync in with the future STM32 boards (which are going to have a strong focus on the ethernet ports) .To carry out the task , we submitted the following patch based on the work done on this branch.

Link to original PR :

A huge thanks to @tridge in helping me with working on the UDP Transport functions in sync with the XRCE Client libraries.

Subscriber Support

Similar to how the publisher functionality was implemented we were able to submit a PR that provided subscriber support for the ROS-2 Joystick message.

Link to original PR :

github.com/ArduPilot/ardupilot

DDS: Added subscriber support

ArduPilot:master ← arshPratap:ddsSubscriberSupport

opened 11:24PM - 27 Apr 23 UTC

arshPratap

+187 -25

Aims to provide a subscriber support for the current DDS functionality in Ardupi…lot. Note : based on the changes made in PR #23586 Current Scenario : ![Screenshot from 2023-04-28 04-48-40](https://user-images.githubusercontent.com/62841337/235011233-cc4eff83-c7ea-4ff0-8335-4d57ce1d6659.png) Special thanks to @Ryanf55 for initiating the said work on his [branch](https://github.com/Ryanf55/ardupilot/tree/joy-input)

Services Support

For developing support for ROS-2 services , we have decided to utilise the following architecture :

To summarise the above architecture :

In place where Ardupilot is supposed to act as the server , Ardupilot creates a Replier entity (described here in the DDS Client Docs) that connects with a corresponding ROS-2 client (requesting access on the same services) via the Integration Service configuration file that handles the topic/service name mapping involved.
In place where Ardupilot is supposed to act as a client , Ardupilot creates a Requester entity (described here in the DDS Client Docs) that connects with a corresponding ROS-2 server (providing access on the same services) via the Integration Service configuration file that handles the topic/service name mapping involved.
We submitted a work in progress draft PR that aims to provide arming and disarming service and the Ardupilot vehicles can effectively be armed/disarmed from a ROS-2 node.

Link to original PR

Demo

ROS-2 Arming Service

Ardupilot Console (replier created successfully)

Ardupilot Console (Arming/Disarming Request recieved)

XRCE Agent Console (replier created successfully)

Full Console Log available Link :

gist.github.com

https://gist.github.com/arshPratap/811da7a1f5328dded0e1c4f8688f4cd6

Micro-XRCE-ArmingService-Log

arsh@arsh-Lenovo-ideapad-320-15IKB:~$ MicroXRCEAgent udp4 -p 2019 -r Ardupilot/ardupilot/libraries/AP_DDS/dds_xrce_profile.xml 
[1688997700.671069] info     | UDPv4AgentLinux.cpp | init                     | running...             | port: 2019
[1688997700.673175] info     | Root.cpp           | set_verbose_level        | logger setup           | verbose_level: 4
[1688997713.479047] info     | Root.cpp           | create_client            | create                 | client_key: 0xAAAABBBB, session_id: 0x81
[1688997713.479186] info     | SessionManager.hpp | establish_session        | session established    | client_key: 0xAAAABBBB, address: 127.0.0.1:40093
[1688997713.522352] info     | ProxyClient.cpp    | create_participant       | participant created    | client_key: 0xAAAABBBB, participant_id: 0x001(1)
[1688997713.522924] info     | ProxyClient.cpp    | create_topic             | topic created          | client_key: 0xAAAABBBB, topic_id: 0x000(2), participant_id: 0x001(1)
[1688997713.547810] info     | ProxyClient.cpp    | create_publisher         | publisher created      | client_key: 0xAAAABBBB, publisher_id: 0x000(3), participant_id: 0x001(1)
[1688997713.554582] info     | ProxyClient.cpp    | create_datawriter        | datawriter created     | client_key: 0xAAAABBBB, datawriter_id: 0x000(5), publisher_id: 0x000(3)
[1688997713.554987] info     | ProxyClient.cpp    | create_topic             | topic created          | client_key: 0xAAAABBBB, topic_id: 0x001(2), participant_id: 0x001(1)

This file has been truncated. show original

Integration Service Console
Full Console Log available Link :

gist.github.com

https://gist.github.com/arshPratap/d42737f203ac4887a6ac770dddfbea9c

IS-ArmService

integration-service Arm_Motors_DDS_IS_config.yaml 
[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_1804289383' 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_1804289383'

This file has been truncated. show original

Examples Repo

Given the lack of docs and examples pertaining to the varied use cases involved with implementing/using the XRCE Clients , we have created our own Example repository : ROS2 DDS Service Demo to test out several different test/use cases involved with the project. Currently you can find the following examples :

A DDS Client acting as Server for AddTwoInts service
A ROS-2 Client acting as Server for AddTwoInts service with an XRCE Client acting as the service client
A DDS Client acting as a Server for the custom ArmMotors.srv service file

Custom ROS-2 messages, services and clients

As described above the ardupilot_ros2 package was refactored into now having custom interfaces for ROS-2 services and ROS-2 clients. The repo currently has service definition and ROS-2 client for Arming/Disarming motors and a similar ROS-2 client is expected for Switching Drive Modes for vehicles.

A big thanks to @rhys for helping me with overhauling the build process by integrating the work done in this repo with the Ardupilot codebase for simplifying the task of setting up the said project.

Current Issues

Lack of micro-ROS agent approach for ROS-2 services : Due to the current architecture adopted for implementing support for ROS-2 services, the project in its current state has become a bit more complex and there is a major need of simplifying the said architecture. For this we are actively looking for a solution to directly integrating the XRCE service replier with the Micro-ROS Agent , as I believe this would help in simplifying the project and make it easier and streamlined for everyone around to set up the said project.

Link to Current Issue :

github.com/eProsima/Micro-XRCE-DDS-Client

Is there a direct way to communicate XRCE requesters and repliers and ROS-2 node via Micro-ROS agent

opened 10:26AM - 06 Jun 23 UTC

arshPratap

Currently I am trying to recreate the ReplyAdder example to communicate with ROS…-2 nodes and was fairly successful in doing so with the ROS-2 nodes via integration services. My current question is that is there a way to directly communicate with ROS-2 nodes using the micro-ros agent (similar to how it was tackled in #250 ). Based on the docs , my current replier configuration is as follows : ``` const char* replier_xml = "<dds>" "<replier profile_name=\"rr/my_requester\"" "service_name=\"rs/AddTwoInts\"" "request_type=\"example_interfaces::srv::dds_::AddTwoInts_Request\"" "reply_type=\"example_interfaces::srv::dds_::AddTwoInts_Response\">" "</replier>" "</dds>"; ``` But on the ROS-2 side there is no corresponding service listed when I run : `ros2 service list ` Any ideas how to proceed with this ? Thanks

ROS-2 Server : While we mentioned having a ROS-2 server example above, it seems there is a bug in the project where even though the demo example has been setup and initialised correctly (with proper/error-free messages being logged onto the Integration Service terminal) , there is no output currently being detected on the DDS Client Side.

Future Updates

I am looking forward to the following exciting updates for the project :

Once the ROS-2 services are rightfully implemented , I would love to initiate my work on implementing support for ROS-2 Actions. Currently, there is a very early WIP branch here, which is aimed to further be updated with new details.
I am also planning to look into working on the ROS-2 controller interface , once the ROS-2 service support is fully merged. I had a very interesting talk with @rmackay9 and @rfriedman , and they have provided me with some really interesting updates and pathways to tackle this task and I am very excited to work further on this task.
We are also planning to implement a custom XRCE profile creation feature that would help the users generate the said based on their requirements
Adding some Gazebo Examples for Using ROS-2 along with Ardupilot
One of most exciting updates that I am really looking forward is the work of my friend and fellow GSoC mentee @pedro-fuoco . I truly believe his GSoC project will provide for an excellent and valuable add on to the current DDS/ROS 2 functionalities present in Ardupilot.

Thanks