diff --git a/Tools/ros2/README.md b/Tools/ros2/README.md
index 9832e412f999d5..d847eeea3a8422 100644
--- a/Tools/ros2/README.md
+++ b/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
diff --git a/Tools/ros2/ardupilot_dds_tests/package.xml b/Tools/ros2/ardupilot_dds_tests/package.xml
index b4cc9fa2a0e358..528f4794a1052e 100644
--- a/Tools/ros2/ardupilot_dds_tests/package.xml
+++ b/Tools/ros2/ardupilot_dds_tests/package.xml
@@ -27,12 +27,15 @@
ament_lint_auto
ardupilot_msgs
ardupilot_sitl
+ builtin_interfaces
launch
launch_pytest
launch_ros
micro_ros_msgs
python3-pytest
rclpy
+ sensor_msgs
+
ament_python
diff --git a/Tools/ros2/ardupilot_sitl/CMakeLists.txt b/Tools/ros2/ardupilot_sitl/CMakeLists.txt
index 4b50d602da341b..628623f608b42a 100644
--- a/Tools/ros2/ardupilot_sitl/CMakeLists.txt
+++ b/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}
diff --git a/Tools/ros2/ardupilot_sitl/package.xml b/Tools/ros2/ardupilot_sitl/package.xml
index 5cc0abfd72a73c..fd1cc359ed184b 100644
--- a/Tools/ros2/ardupilot_sitl/package.xml
+++ b/Tools/ros2/ardupilot_sitl/package.xml
@@ -11,7 +11,15 @@
ament_cmake
ament_cmake_python
+ ardupilot_msgs
+ builtin_interfaces
+ geographic_msgs
+ geometry_msgs
micro_ros_agent
+ rosgraph_msgs
+ sensor_msgs
+ std_msgs
+ tf2_msgs
ament_lint_auto
ament_cmake_black
diff --git a/Tools/ros2/ardupilot_sitl/src/ardupilot_sitl/launch.py b/Tools/ros2/ardupilot_sitl/src/ardupilot_sitl/launch.py
index 6578e537d8fbc7..12b7009df90f30 100644
--- a/Tools/ros2/ardupilot_sitl/src/ardupilot_sitl/launch.py
+++ b/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",
diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp
index 3daa69f6b86e0d..6d7902f3e43c00 100644
--- a/libraries/AP_DDS/AP_DDS_Client.cpp
+++ b/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(domain_id), participant_ref, UXR_REPLACE);
//Participant requests
constexpr uint8_t nRequestsParticipant = 1;
diff --git a/libraries/AP_DDS/AP_DDS_Client.h b/libraries/AP_DDS/AP_DDS_Client.h
index 18dcadfc9f55a9..27d103ca73e44b 100644
--- a/libraries/AP_DDS/AP_DDS_Client.h
+++ b/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;
diff --git a/libraries/AP_DDS/tests/test_ap_dds_external_odom.cpp b/libraries/AP_DDS/tests/test_ap_dds_external_odom.cpp
index 804d891e161806..35d37bd97d603c 100644
--- a/libraries/AP_DDS/tests/test_ap_dds_external_odom.cpp
+++ b/libraries/AP_DDS/tests/test_ap_dds_external_odom.cpp
@@ -1,9 +1,13 @@
#include
+#include
+
#include
#include "geometry_msgs/msg/TransformStamped.h"
#include
+#if AP_DDS_VISUALODOM_ENABLED
+
const AP_HAL::HAL &hal = AP_HAL::get_HAL();
TEST(AP_DDS_EXTERNAL_ODOM, test_is_odometry_success)
@@ -35,4 +39,6 @@ TEST(AP_DDS_EXTERNAL_ODOM, test_is_odometry_success)
ASSERT_FALSE(AP_DDS_External_Odom::is_odometry_frame(msg));
}
+#endif // AP_DDS_VISUALODOM_ENABLED
+
AP_GTEST_MAIN()