diff --git a/libraries/AP_DDS/AP_DDS_Service_Table.h b/libraries/AP_DDS/AP_DDS_Service_Table.h index 962fa9197d..c5ffb5c737 100644 --- a/libraries/AP_DDS/AP_DDS_Service_Table.h +++ b/libraries/AP_DDS/AP_DDS_Service_Table.h @@ -14,8 +14,8 @@ constexpr struct AP_DDS_Client::Service_table AP_DDS_Client::services[] = { { .req_id = to_underlying(ServiceIndex::ARMING_MOTORS), .rep_id = to_underlying(ServiceIndex::ARMING_MOTORS), - .srv_profile_label = "ArmMotorsService", + .srv_profile_label = "", .req_profile_label = "", - .rep_profile_label = "ArmMotors_Replier", + .rep_profile_label = "arm_motors__replier", } }; diff --git a/libraries/AP_DDS/Is-Config/Arm_Motors_DDS_IS_config.yaml b/libraries/AP_DDS/Is-Config/Arm_Motors_DDS_IS_config.yaml deleted file mode 100644 index ef46bf6aa9..0000000000 --- a/libraries/AP_DDS/Is-Config/Arm_Motors_DDS_IS_config.yaml +++ /dev/null @@ -1,36 +0,0 @@ -types: - idls: - - > - struct ArmMotors_Request - { - boolean arm; - }; - - struct ArmMotors_Response - { - boolean result; - }; -systems: - dds: { type: fastdds } - ros2: { type: ros2 } - -routes: - dds_server: - server: dds - clients: ros2 - -services: - arm_motors: { - request_type: ArmMotors_Request, - reply_type: ArmMotors_Response, - route: dds_server, - remap: { - dds: { - topic: ArmMotorsService, - }, - ros2: { - request_type: "ardupilot_msgs/ArmMotors:request", - reply_type: "ardupilot_msgs/ArmMotors:response" - } - } - } diff --git a/libraries/AP_DDS/README.md b/libraries/AP_DDS/README.md index 819396257b..0a82ac1ab4 100644 --- a/libraries/AP_DDS/README.md +++ b/libraries/AP_DDS/README.md @@ -2,7 +2,7 @@ ## Architecture -Ardupilot contains the DDS Client library, which can run as SITL. Then, the DDS application runs a ROS2 node, an EProsima Integration Service, and the MicroXRCE Agent. The two systems communicate can communicate over serial or UDP. +Ardupilot contains the DDS Client library, which can run as SITL. Then, the DDS application runs a ROS 2 node, an eProsima Integration Service, and the MicroXRCE Agent. The two systems communicate over serial or UDP. ```mermaid --- @@ -18,7 +18,7 @@ graph LR end subgraph DDS Application - ros[ROS2 Node] <--> agent[Micro ROS Agent] + ros[ROS 2 Node] <--> agent[Micro ROS Agent] agent <-->port1[udp:2019] end @@ -41,7 +41,7 @@ graph LR end subgraph DDS Application - ros[ROS2 Node] <--> agent[Micro ROS Agent] + ros[ROS 2 Node] <--> agent[Micro ROS Agent] agent <--> port2[devUSB2] end @@ -109,7 +109,7 @@ Run the simulator with the following command. If using UDP, the only parameter y | DDS_ENABLE | Set to 1 to enable DDS, or 0 to disable | 1 | | SERIAL1_BAUD | The serial baud rate for DDS | 57 | | SERIAL1_PROTOCOL | Set this to 45 to use DDS on the serial port | 0 | -```bash +```console # Wipe params till you see "AP: ArduPilot Ready" # Select your favorite vehicle type sim_vehicle.py -w -v ArduPlane --console -DG --enable-dds @@ -135,7 +135,7 @@ Follow the steps to use the microROS Agent - https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html - Install geographic_msgs - ```bash + ```console sudo apt install ros-humble-geographic-msgs ``` @@ -151,11 +151,11 @@ Follow the steps to use the microROS Agent Until this [PR](https://github.com/micro-ROS/micro-ROS.github.io/pull/401) is merged, ignore the notes about `foxy`. It works on `humble`. -## Using the ROS2 CLI to Read Ardupilot Data +## Using the ROS 2 CLI to Read Ardupilot Data After your setups are complete, do the following: -- Source the ros2 installation - ```bash +- Source the ROS 2 installation + ```console source /opt/ros/humble/setup.bash ``` @@ -164,109 +164,118 @@ Next, follow the associated section for your chosen transport, and finally you c ### UDP (recommended for SITL) - Run the microROS agent - ```bash + ```console cd ardupilot/libraries/AP_DDS ros2 run micro_ros_agent micro_ros_agent udp4 -p 2019 -r dds_xrce_profile.xml ``` - Run SITL (remember to kill any terminals running ardupilot SITL beforehand) - ```bash + ```console sim_vehicle.py -v ArduPlane -DG --console --enable-dds ``` ### Serial - Start a virtual serial port with socat. Take note of the two `/dev/pts/*` ports. If yours are different, substitute as needed. - ```bash + ```console socat -d -d pty,raw,echo=0 pty,raw,echo=0 >>> 2023/02/21 05:26:06 socat[334] N PTY is /dev/pts/1 >>> 2023/02/21 05:26:06 socat[334] N PTY is /dev/pts/2 >>> 2023/02/21 05:26:06 socat[334] N starting data transfer loop with FDs [5,5] and [7,7] ``` - Run the microROS agent - ```bash + ```console cd ardupilot/libraries/AP_DDS # assuming we are using tty/pts/2 for DDS Application ros2 run micro_ros_agent micro_ros_agent serial -b 115200 -r dds_xrce_profile.xml -D /dev/pts/2 ``` - Run SITL (remember to kill any terminals running ardupilot SITL beforehand) - ```bash + ```console # assuming we are using /dev/pts/1 for Ardupilot SITL sim_vehicle.py -v ArduPlane -DG --console --enable-dds -A "--uartC=uart:/dev/pts/1" ``` ## Use ROS 2 CLI -- You should be able to see the agent here and view the data output. - ```bash - $ ros2 node list - /Ardupilot_DDS_XRCE_Client +You should be able to see the agent here and view the data output. - $ ros2 topic list -v - Published topics: - * /ap/battery/battery0 [sensor_msgs/msg/BatteryState] 1 publisher - * /ap/clock [rosgraph_msgs/msg/Clock] 1 publisher - * /ap/geopose/filtered [geographic_msgs/msg/GeoPoseStamped] 1 publisher - * /ap/navsat/navsat0 [sensor_msgs/msg/NavSatFix] 1 publisher - * /ap/pose/filtered [geometry_msgs/msg/PoseStamped] 1 publisher - * /ap/tf_static [tf2_msgs/msg/TFMessage] 1 publisher - * /ap/time [builtin_interfaces/msg/Time] 1 publisher - * /ap/twist/filtered [geometry_msgs/msg/TwistStamped] 1 publisher - * /parameter_events [rcl_interfaces/msg/ParameterEvent] 1 publisher - * /rosout [rcl_interfaces/msg/Log] 1 publisher +```bash +$ ros2 node list +/Ardupilot_DDS_XRCE_Client +``` - Subscribed topics: - * /ap/joy [sensor_msgs/msg/Joy] 1 subscriber - * /ap/cmd_vel [geometry_msgs/msg/TwistStamped] 1 subscriber - * /tf [tf2_msgs/msg/TFMessage] 1 subscriber +```bash +$ ros2 topic list -v +Published topics: + * /ap/battery/battery0 [sensor_msgs/msg/BatteryState] 1 publisher + * /ap/clock [rosgraph_msgs/msg/Clock] 1 publisher + * /ap/geopose/filtered [geographic_msgs/msg/GeoPoseStamped] 1 publisher + * /ap/navsat/navsat0 [sensor_msgs/msg/NavSatFix] 1 publisher + * /ap/pose/filtered [geometry_msgs/msg/PoseStamped] 1 publisher + * /ap/tf_static [tf2_msgs/msg/TFMessage] 1 publisher + * /ap/time [builtin_interfaces/msg/Time] 1 publisher + * /ap/twist/filtered [geometry_msgs/msg/TwistStamped] 1 publisher + * /parameter_events [rcl_interfaces/msg/ParameterEvent] 1 publisher + * /rosout [rcl_interfaces/msg/Log] 1 publisher - $ ros2 topic hz /ap/time - average rate: 50.115 - min: 0.012s max: 0.024s std dev: 0.00328s window: 52 +Subscribed topics: + * /ap/cmd_vel [geometry_msgs/msg/TwistStamped] 1 subscriber + * /ap/joy [sensor_msgs/msg/Joy] 1 subscriber + * /tf [tf2_msgs/msg/TFMessage] 1 subscriber +``` - $ ros2 topic echo /ap/time - sec: 1678668735 - nanosec: 729410000 +```bash +$ ros2 topic hz /ap/time +average rate: 50.115 + min: 0.012s max: 0.024s std dev: 0.00328s window: 52 +``` - $ ros2 service list - /arm_motors - --- - ``` +```bash +$ ros2 topic echo /ap/time +sec: 1678668735 +nanosec: 729410000 +``` - The static transforms for enabled sensors are also published, and can be recieved like so: - ```console - ros2 topic echo /ap/tf_static --qos-depth 1 --qos-history keep_last --qos-reliability reliable --qos-durability transient_local --once - ``` - In order to consume the transforms, it's highly recommended to [create and run a transform broadcaster in ROS 2](https://docs.ros.org/en/humble/Concepts/About-Tf2.html#tutorials). +```bash +$ ros2 service list +/ap/arm_motors +--- +``` -## Using ROS 2 services (with Integration Services) +The static transforms for enabled sensors are also published, and can be received like so: -### Prerequisites -- Install and setup [Micro-XRCE Agent](https://micro-xrce-dds.docs.eprosima.com/en/latest/installation.html#installing-the-agent-standalone) -- Install and setup [Integration Services](https://integration-service.docs.eprosima.com/en/latest/installation_manual/installation.html) (it would be good to have a separate workspace for this) - - Get System Handles for [ROS 2](https://github.com/eProsima/ROS2-SH) - - Get System Handles for [Fast-DDS](https://github.com/eProsima/FastDDS-SH) -- Once the above-mentioned System Handles have been cloned, build the Integration Services with the following command : -`colcon build --cmake-args -DMIX_ROS_PACKAGES="example_interfaces ardupilot_msgs"` +```bash +ros2 topic echo /ap/tf_static --qos-depth 1 --qos-history keep_last --qos-reliability reliable --qos-durability transient_local --once +``` -### Setup -- The necessary ROS 2 messages and service defintions (especially for the Arming/Disarming Services) are already defined in the `ardupilot_msgs` folder in the `Tools` directory. +In order to consume the transforms, it's highly recommended to [create and run a transform broadcaster in ROS 2](https://docs.ros.org/en/humble/Concepts/About-Tf2.html#tutorials). -### Terminal 1 (XRCE Agent) -- Move to the **AP_DDS** folder and run the XRCE Agent as follows `MicroXRCEAgent udp4 -p 2019 -r dds_xrce_profile.xml` +## Using ROS 2 services -### Terminal 2 (Integration Service) -- Source ROS 2 installation -- Source Integration Service installation -- Move to the **AP_DDS** folder and run the following command `integration-service Is-Config/Arm_Motors_DDS_IS_config.yaml` +The `AP_DDS` libraary exposes services which are automatically mapped to ROS 2 +services using appropriate naming conventions for topics and message and service +types. An earlier version of `AP_DDS` required the use of the eProsima +[Integration Service](https://github.com/eProsima/Integration-Service) to map +the request / reply topics from DDS to ROS 2, but this is no longer required. -### Terminal 3 (Ardupilot) -- Make sure you have successfully setup Ardupilot and the `DDS_ENABLE` param is set to 1 -- Run SITL with the following command `sim_vehicle.py -v ArduPlane -DG --console --enable-dds` +List the available services: -### Terminal 4 (ROS 2 Client) -- Run the following command : `ros2 service call /arm_motors ardupilot_msgs/srv/ArmMotors "{arm: True}"` +```bash +$ ros2 service list -t +/ap/arm_motors [ardupilot_msgs/srv/ArmMotors] +``` + +Call the arm motors service: + +```bash +$ ros2 service call /ap/arm_motors ardupilot_msgs/srv/ArmMotors "{arm: True}" +requester: making request: ardupilot_msgs.srv.ArmMotors_Request(arm=True) + +response: +ardupilot_msgs.srv.ArmMotors_Response(result=True) +``` -## Contributing to AP_DDS library +## Contributing to `AP_DDS` library + ### Adding DDS messages to Ardupilot Unlike the use of ROS 2 `.msg` files, since Ardupilot supports native DDS, the message files follow [OMG IDL DDS v4.2](https://www.omg.org/spec/IDL/4.2/PDF). @@ -274,25 +283,77 @@ This package is intended to work with any `.idl` file complying with those exten Over time, these restrictions will ideally go away. -To get a new IDL file from ROS2, follow this process: -```console +To get a new IDL file from ROS 2, follow this process: + +```bash cd ardupilot source /opt/ros/humble/setup.bash + # Find the IDL file find /opt/ros/$ROS_DISTRO -type f -wholename \*builtin_interfaces/msg/Time.idl + # Create the directory in the source tree if it doesn't exist similar to the one found in the ros directory mkdir -p libraries/AP_DDS/Idl/builtin_interfaces/msg/ + # Copy the IDL cp /opt/ros/humble/share/builtin_interfaces/msg/Time.idl libraries/AP_DDS/Idl/builtin_interfaces/msg/ + # Build the code again with the `--enable-dds` flag as described above ``` +### Rules for adding topics and services to `dds_xrce_profile.xml` + +Topics and services available from `AP_DDS` are automatically mapped into ROS 2 +provided a few rules are followed when defining the entries in +`dds_xrce_profile.xml`. + +#### ROS 2 message and service interface types + +ROS 2 message and interface defintions are mangled by the `rosidl_adapter` when +mapping from ROS 2 to DDS to avoid naming conflicts in the C/C++ libraries. +The ROS 2 object `namespace::Struct` is mangled to `namespace::dds_::Struct_` +for DDS. The table below provides some example mappings: + +| ROS 2 | DDS | +| --- | --- | +| `rosgraph_msgs::msg::Clock` | `rosgraph_msgs::msg::dds_::Clock_` | +| `sensor_msgs::msg::NavSatFix` | `sensor_msgs::msg::dds_::NavSatFix_` | +| `ardupilot_msgs::srv::ArmMotors_Request` | `ardupilot_msgs::srv::dds_::ArmMotors_Request_` | +| `ardupilot_msgs::srv::ArmMotors_Response` | `ardupilot_msgs::srv::dds_::ArmMotors_Response_` | + +Note that a service interface always requires a Request / Response pair. + +#### ROS 2 topic and service names + +The ROS 2 design article: [Topic and Service name mapping to DDS](https://design.ros2.org/articles/topic_and_service_names.html) describes the mapping of ROS 2 topic and service +names to DDS. Each ROS 2 subsytem is provided a prefix when mapped to DDS. +The request / response pair for services require an additional suffix. + +| ROS 2 subsystem | DDS Prefix | DDS Suffix | +| --- | --- | --- | +| topics | rt/ | | +| service request | rq/ | Request | +| service response | rr/ | Reply | +| service | rs/ | | +| parameter | rp/ | | +| action | ra/ | | + +The table below provides example mappings for topics and services + +| ROS 2 | DDS | +| --- | --- | +| ap/clock | rt/ap/clock | +| ap/navsat/navsat0 | rt/ap/navsat/navsat0 | +| ap/arm_motors | rq/ap/arm_motorsRequest, rr/ap/arm_motorsReply | + +Refer to existing mappings in [`dds_xrce_profile.xml`](https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_DDS/dds_xrce_profile.xml) for additional details. + ### Development Requirements Astyle is used to format the C++ code in AP_DDS. This is required for CI to pass the build. See [Tools/CodeStyle/ardupilot-astyle.sh](../../Tools/CodeStyle/ardupilot-astyle.sh). -```console +```bash ./Tools/CodeStyle/ardupilot-astyle.sh libraries/AP_DDS/*.h libraries/AP_DDS/*.cpp ``` @@ -301,7 +362,7 @@ This will run the tools automatically when you commit. If there are changes, jus 1. Install [pre-commit](https://pre-commit.com/#installation) python package. 1. Install ArduPilot's hooks in the root of the repo, then commit like normal - ```console + ```bash cd ardupilot pre-commit install git commit diff --git a/libraries/AP_DDS/dds_xrce_profile.xml b/libraries/AP_DDS/dds_xrce_profile.xml index 2ff5207f68..03baed3bbb 100644 --- a/libraries/AP_DDS/dds_xrce_profile.xml +++ b/libraries/AP_DDS/dds_xrce_profile.xml @@ -268,6 +268,13 @@ geometry_msgs::msg::dds_::TwistStamped_ - + + rq/ap/arm_motorsRequest + rr/ap/arm_motorsReply +