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()