DDS Backports to 4.5

Tools/ros2/README.md

@@ -1,18 +1,34 @@
 # ArduPilot ROS 2 packages
 
- This directory contains ROS 2 packages and configuration files for running
- ROS 2 processes and nodes that communicate with the ArduPilot DDS client
- library using the microROS agent. It contains the following packages:
+This directory contains ROS 2 packages and configuration files for running
+ROS 2 processes and nodes that communicate with the ArduPilot DDS client
+library using the microROS agent. It contains the following packages:
 
 #### `ardupilot_sitl`
 
-A `colcon` package for building and running ArduPilot SITL using the ROS 2 CLI.
+This is a `colcon` package for building and running ArduPilot SITL using the ROS 2 CLI.
 For example `ardurover` SITL may be launched with:
 
 ```bash
 ros2 launch ardupilot_sitl sitl.launch.py command:=ardurover model:=rover
 ```
 
+Other launch files are included with many arguments.
+Some common arguments are exposed and forwarded to the underlying process.
+
+For example, MAVProxy can be launched, and you can enable the `console` and `map`.
+```bash
+ros2 launch ardupilot_sitl sitl_mavproxy.launch.py map:=True console:=True 
+```
+
+ArduPilot SITL does not yet expose all arguments from the underlying binary.
+See [#27714](https://github.com/ArduPilot/ardupilot/issues/27714) for context.
+
+To see all current options, use the `-s` argument:
+```bash
+ros2 launch ardupilot_sitl sitl.launch.py -s
+```
+
 #### `ardupilot_dds_test`
 
 A `colcon` package for testing communication between `micro_ros_agent` and the

Tools/ros2/ardupilot_dds_tests/package.xml

@@ -27,12 +27,15 @@
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ardupilot_msgs</test_depend>
   <test_depend>ardupilot_sitl</test_depend>
+  <test_depend>builtin_interfaces</test_depend>
   <test_depend>launch</test_depend>
   <test_depend>launch_pytest</test_depend>
   <test_depend>launch_ros</test_depend>
   <exec_depend>micro_ros_msgs</exec_depend>
   <test_depend>python3-pytest</test_depend>
   <test_depend>rclpy</test_depend>
+  <test_depend>sensor_msgs</test_depend>
+
 
   <export>
     <build_type>ament_python</build_type>

Tools/ros2/ardupilot_sitl/CMakeLists.txt

@@ -79,6 +79,12 @@ install(DIRECTORY
   DESTINATION share/${PROJECT_NAME}/config/
 )
 
+# Install additional autotest model params.
+install(DIRECTORY
+  ${ARDUPILOT_ROOT}/Tools/autotest/models
+  DESTINATION share/${PROJECT_NAME}/config/
+)
+
 # Install Python package.
 ament_python_install_package(${PROJECT_NAME}
   PACKAGE_DIR src/${PROJECT_NAME}

Tools/ros2/ardupilot_sitl/package.xml

@@ -11,7 +11,15 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>ament_cmake_python</buildtool_depend>
 
+  <exec_depend>ardupilot_msgs</exec_depend>
+  <exec_depend>builtin_interfaces</exec_depend>
+  <exec_depend>geographic_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>micro_ros_agent</exec_depend>
+  <exec_depend>rosgraph_msgs</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>tf2_msgs</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_cmake_black</test_depend>

Tools/ros2/ardupilot_sitl/src/ardupilot_sitl/launch.py

@@ -31,6 +31,9 @@
 
 from .actions import ExecuteFunction
 
+TRUE_STRING = "True"
+FALSE_STRING = "False"
+BOOL_STRING_CHOICES = set([TRUE_STRING, FALSE_STRING])
 
 class VirtualPortsLaunch:
     """Launch functions for creating virtual ports using `socat`."""
@@ -284,28 +287,38 @@ def generate_action(context: LaunchContext, *args, **kwargs) -> ExecuteProcess:
 
         # Retrieve launch arguments.
         master = LaunchConfiguration("master").perform(context)
-        # out = LaunchConfiguration("out").perform(context)
+        out = LaunchConfiguration("out").perform(context)
         sitl = LaunchConfiguration("sitl").perform(context)
+        console = LaunchConfiguration("console").perform(context)
+        map = LaunchConfiguration("map").perform(context)
 
         # Display launch arguments.
         print(f"command:          {command}")
         print(f"master:           {master}")
         print(f"sitl:             {sitl}")
+        print(f"out:              {out}")
+        print(f"console:          {console}")
+        print(f"map:              {map}")
+
+        cmd = [
+            f"{command} ",
+            f"--out {out} ",
+            "--out ",
+            "127.0.0.1:14551 ",
+            f"--master {master} ",
+            f"--sitl {sitl} ",
+            "--non-interactive ",
+        ]
+
+        if console == TRUE_STRING:
+            cmd.append("--console ")
+
+        if map == TRUE_STRING:
+            cmd.append("--map ")
 
         # Create action.
         mavproxy_process = ExecuteProcess(
-            cmd=[
-                [
-                    f"{command} ",
-                    "--out ",
-                    "127.0.0.1:14550 ",
-                    "--out ",
-                    "127.0.0.1:14551 ",
-                    f"--master {master} ",
-                    f"--sitl {sitl} ",
-                    "--non-interactive ",
-                ]
-            ],
+            cmd=cmd,
             shell=True,
             output="both",
             respawn=False,
@@ -355,6 +368,18 @@ def generate_launch_arguments() -> List[DeclareLaunchArgument]:
                 default_value="127.0.0.1:5501",
                 description="SITL output port.",
             ),
+            DeclareLaunchArgument(
+                "map",
+                default_value="False",
+                description="Enable MAVProxy Map.",
+                choices=BOOL_STRING_CHOICES
+            ),
+            DeclareLaunchArgument(
+                "console",
+                default_value="False",
+                description="Enable MAVProxy Console.",
+                choices=BOOL_STRING_CHOICES
+            ),
         ]
 
 
@@ -399,12 +424,12 @@ def generate_action(context: LaunchContext, *args, **kwargs) -> ExecuteProcess:
 
         # Optional arguments.
         wipe = LaunchConfiguration("wipe").perform(context)
-        if wipe == "True":
+        if wipe == TRUE_STRING:
             cmd_args.append("--wipe ")
             print(f"wipe:             {wipe}")
 
         synthetic_clock = LaunchConfiguration("synthetic_clock").perform(context)
-        if synthetic_clock == "True":
+        if synthetic_clock == TRUE_STRING:
             cmd_args.append("--synthetic-clock ")
             print(f"synthetic_clock:  {synthetic_clock}")
 
@@ -566,13 +591,13 @@ def generate_launch_arguments() -> List[DeclareLaunchArgument]:
                 "wipe",
                 default_value="False",
                 description="Wipe eeprom.",
-                choices=["True", "False"],
+                choices=BOOL_STRING_CHOICES,
             ),
             DeclareLaunchArgument(
                 "synthetic_clock",
                 default_value="False",
                 description="Set synthetic clock mode.",
-                choices=["True", "False"],
+                choices=BOOL_STRING_CHOICES,
             ),
             DeclareLaunchArgument(
                 "home",

libraries/AP_DDS/AP_DDS_Client.cpp

@@ -72,9 +72,25 @@ const AP_Param::GroupInfo AP_DDS_Client::var_info[] {
 
 #endif
 
+    // @Param: _DOMAIN_ID
+    // @DisplayName: DDS DOMAIN ID
+    // @Description: Set the ROS_DOMAIN_ID
+    // @Range: 0 232
+    // @RebootRequired: True
+    // @User: Standard
+    AP_GROUPINFO("_DOMAIN_ID", 4, AP_DDS_Client, domain_id, 0),
+
     AP_GROUPEND
 };
 
+static void initialize(geometry_msgs_msg_Quaternion& q)
+{
+    q.x = 0.0;
+    q.y = 0.0;
+    q.z = 0.0;
+    q.w = 1.0;
+}
+
 AP_DDS_Client::~AP_DDS_Client()
 {
     // close transport
@@ -224,6 +240,9 @@ void AP_DDS_Client::populate_static_transforms(tf2_msgs_msg_TFMessage& msg)
         msg.transforms[i].transform.translation.y = -1 * offset[1];
         msg.transforms[i].transform.translation.z = -1 * offset[2];
 
+        // Ensure rotation is initialized.
+        initialize(msg.transforms[i].transform.rotation);
+
         msg.transforms_size++;
     }
 
@@ -286,8 +305,11 @@ void AP_DDS_Client::update_topic(sensor_msgs_msg_BatteryState& msg, const uint8_
     msg.power_supply_technology = 0; //POWER_SUPPLY_TECHNOLOGY_UNKNOWN
 
     if (battery.has_cell_voltages(instance)) {
-        const uint16_t* cellVoltages = battery.get_cell_voltages(instance).cells;
-        std::copy(cellVoltages, cellVoltages + AP_BATT_MONITOR_CELLS_MAX, msg.cell_voltage);
+        const auto &cells = battery.get_cell_voltages(instance);
+        const uint8_t ncells_max = MIN(ARRAY_SIZE(msg.cell_voltage), ARRAY_SIZE(cells.cells));
+        for (uint8_t i=0; i< ncells_max; i++) {
+            msg.cell_voltage[i] = cells.cells[i] * 0.001;
+        }
     }
 }
 
@@ -335,6 +357,8 @@ void AP_DDS_Client::update_topic(geometry_msgs_msg_PoseStamped& msg)
         msg.pose.orientation.x = orientation[1];
         msg.pose.orientation.y = orientation[2];
         msg.pose.orientation.z = orientation[3];
+    } else {
+        initialize(msg.pose.orientation);
     }
 }
 
@@ -414,6 +438,8 @@ void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg)
         msg.pose.orientation.x = orientation[1];
         msg.pose.orientation.y = orientation[2];
         msg.pose.orientation.z = orientation[3];
+    } else {
+        initialize(msg.pose.orientation);
     }
 }
 
@@ -743,7 +769,8 @@ bool AP_DDS_Client::create()
         .type = UXR_PARTICIPANT_ID
     };
     const char* participant_ref = "participant_profile";
-    const auto participant_req_id = uxr_buffer_create_participant_ref(&session, reliable_out, participant_id,0,participant_ref,UXR_REPLACE);
+    const auto participant_req_id = uxr_buffer_create_participant_ref(&session, reliable_out, participant_id,
+                                    static_cast<uint16_t>(domain_id), participant_ref, UXR_REPLACE);
 
     //Participant requests
     constexpr uint8_t nRequestsParticipant = 1;

libraries/AP_DDS/AP_DDS_Client.h

@@ -198,6 +198,9 @@ class AP_DDS_Client
     //! @brief Parameter storage
     static const struct AP_Param::GroupInfo var_info[];
 
+    //! @brief ROS_DOMAIN_ID
+    AP_Int32 domain_id;
+
     //! @brief Convenience grouping for a single "channel" of data
     struct Topic_table {
         const uint8_t topic_id;