From c04e6b726ad1a47e66fa66f095b1731bbc10d7d4 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Thu, 20 Jul 2023 09:26:12 +0100 Subject: [PATCH 01/16] AP_UROS: add micro-ROS library and set up build and params Signed-off-by: Rhys Mainwaring --- libraries/AP_UROS/AP_UROS_Client.cpp | 108 +++++++++++++++++++++++++++ libraries/AP_UROS/AP_UROS_Client.h | 43 +++++++++++ libraries/AP_UROS/wscript | 97 ++++++++++++++++++++++++ 3 files changed, 248 insertions(+) create mode 100644 libraries/AP_UROS/AP_UROS_Client.cpp create mode 100644 libraries/AP_UROS/AP_UROS_Client.h create mode 100644 libraries/AP_UROS/wscript diff --git a/libraries/AP_UROS/AP_UROS_Client.cpp b/libraries/AP_UROS/AP_UROS_Client.cpp new file mode 100644 index 0000000000000..f5727a36fb651 --- /dev/null +++ b/libraries/AP_UROS/AP_UROS_Client.cpp @@ -0,0 +1,108 @@ +#include + +#if AP_UROS_ENABLED + +#include +#include +#include +#include +#include +#include +#include + +#include "AP_UROS_Client.h" + +const AP_Param::GroupInfo AP_UROS_Client::var_info[] { + + // @Param: _ENABLE + // @DisplayName: UROS enable + // @Description: Enable UROS subsystem + // @Values: 0:Disabled,1:Enabled + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO_FLAGS("_ENABLE", 1, AP_UROS_Client, enabled, 0, AP_PARAM_FLAG_ENABLE), + + AP_GROUPEND +}; + +/* + start the DDS thread + */ +bool AP_UROS_Client::start(void) +{ + AP_Param::setup_object_defaults(this, var_info); + AP_Param::load_object_from_eeprom(this, var_info); + + if (enabled == 0) { + return true; + } + + if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_UROS_Client::main_loop, void), + "UROS", + 8192, AP_HAL::Scheduler::PRIORITY_IO, 1)) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR,"UROS: thread create failed"); + return false; + } + return true; +} + +/* + main loop for UROS thread + */ +void AP_UROS_Client::main_loop(void) +{ + if (!init() || !create()) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR,"UROS: creation failed"); + return; + } + GCS_SEND_TEXT(MAV_SEVERITY_INFO,"UROS: initialization passed"); + + while (true) { + hal.scheduler->delay(1); + update(); + } +} + +bool AP_UROS_Client::init() +{ + GCS_SEND_TEXT(MAV_SEVERITY_INFO,"UROS: init complete"); + + return true; +} + +bool AP_UROS_Client::create() +{ + WITH_SEMAPHORE(csem); + + GCS_SEND_TEXT(MAV_SEVERITY_INFO,"UROS: create complete"); + + return true; +} + +void AP_UROS_Client::update() +{ + WITH_SEMAPHORE(csem); +} + +#if CONFIG_HAL_BOARD != HAL_BOARD_SITL +extern "C" { + int clock_gettime(clockid_t clockid, struct timespec *ts); +} + +int clock_gettime(clockid_t clockid, struct timespec *ts) +{ + //! @todo the value of clockid is ignored here. + //! A fallback mechanism is employed against the caller's choice of clock. + uint64_t utc_usec; + if (!AP::rtc().get_utc_usec(utc_usec)) { + utc_usec = AP_HAL::micros64(); + } + ts->tv_sec = utc_usec / 1000000ULL; + ts->tv_nsec = (utc_usec % 1000000ULL) * 1000UL; + return 0; +} +#endif // CONFIG_HAL_BOARD + +#endif // AP_UROS_ENABLED + + diff --git a/libraries/AP_UROS/AP_UROS_Client.h b/libraries/AP_UROS/AP_UROS_Client.h new file mode 100644 index 0000000000000..71e949e85b0bb --- /dev/null +++ b/libraries/AP_UROS/AP_UROS_Client.h @@ -0,0 +1,43 @@ +#pragma once + +#if AP_UROS_ENABLED + +#include +#include +#include +#include + +#include "fcntl.h" + +#include + +extern const AP_HAL::HAL& hal; + +class AP_UROS_Client +{ +private: + + AP_Int8 enabled; + + HAL_Semaphore csem; + +public: + bool start(void); + void main_loop(void); + + //! @brief Initialize the client. + //! @return True on successful initialization, false on failure. + bool init() WARN_IF_UNUSED; + + //! @brief Set up the client. + //! @return True on successful creation, false on failure + bool create() WARN_IF_UNUSED; + + //! @brief Update the client. + void update(); + + //! @brief Parameter storage. + static const struct AP_Param::GroupInfo var_info[]; +}; + +#endif // AP_UROS_ENABLED diff --git a/libraries/AP_UROS/wscript b/libraries/AP_UROS/wscript new file mode 100644 index 0000000000000..53f330064c2ae --- /dev/null +++ b/libraries/AP_UROS/wscript @@ -0,0 +1,97 @@ +#!/usr/bin/env python3 + + +import pathlib + + +def configure(cfg): + extra_src = [ + # 'modules/Micro-XRCE-DDS-Client/src/c/core/session/stream/*.c', + # 'modules/Micro-XRCE-DDS-Client/src/c/core/session/*.c', + # 'modules/Micro-XRCE-DDS-Client/src/c/core/serialization/*.c', + # 'modules/Micro-XRCE-DDS-Client/src/c/util/*.c', + # 'modules/Micro-XRCE-DDS-Client/src/c/profile/transport/custom/custom_transport.c', + # 'modules/Micro-XRCE-DDS-Client/src/c/profile/transport/stream_framing/stream_framing_protocol.c', + # 'modules/Micro-CDR/src/c/types/*.c', + # 'modules/Micro-CDR/src/c/common.c', + ] + + cfg.env.AP_LIB_EXTRA_SOURCES['AP_UROS'] = [] + for pattern in extra_src: + s = cfg.srcnode.ant_glob(pattern, dir=False, src=True) + for x in s: + cfg.env.AP_LIB_EXTRA_SOURCES['AP_UROS'].append(str(x)) + + extra_src_inc = [ + # 'modules/Micro-XRCE-DDS-Client/include', + # 'modules/Micro-XRCE-DDS-Client/include/uxr/client', + # 'modules/Micro-CDR/src/c', + # 'modules/Micro-CDR/include', + # 'modules/Micro-CDR/include/ucdr', + ] + for inc in extra_src_inc: + cfg.env.INCLUDES += [str(cfg.srcnode.make_node(inc))] + + # auto update submodules + # cfg.env.append_value('GIT_SUBMODULES', 'Micro-XRCE-DDS-Client') + # cfg.env.append_value('GIT_SUBMODULES', 'Micro-CDR') + + +def build(bld): + config_h_in = [ + # 'modules/Micro-XRCE-DDS-Client/include/uxr/client/config.h.in', + # 'modules/Micro-CDR/include/ucdr/config.h.in', + ] + # config_h_in_nodes = [bld.srcnode.make_node(h) for h in config_h_in] + # config_h_nodes = [bld.bldnode.find_or_declare(h[:-3]) for h in config_h_in] + + # gen_path = "libraries/AP_UROS" + extra_bld_inc = [ + # 'modules/Micro-CDR/include', + # 'modules/Micro-XRCE-DDS-Client/include', + # str(pathlib.PurePath(gen_path, "generated")), + ] + for inc in extra_bld_inc: + bld.env.INCLUDES += [bld.bldnode.find_or_declare(inc).abspath()] + + # for i in range(len(config_h_nodes)): + # print(f"building {config_h_nodes[i].abspath()}") + # bld( + # # build config.h file + # source=config_h_in_nodes[i], + # target=config_h_nodes[i], + # rule="%s %s/%s %s %s" + # % ( + # bld.env.get_flat('PYTHON'), + # bld.env.SRCROOT, + # "libraries/AP_UROS/gen_config_h.py", + # config_h_in_nodes[i].abspath(), + # config_h_nodes[i].abspath(), + # ), + # group='dynamic_sources', + # ) + + # TODO instead of keeping standard IDL files in the source tree, copy them with "ros2 pkg prefix --share " tools + # idl_files = bld.srcnode.ant_glob('libraries/AP_UROS/Idl/**/*.idl') + # idl_include_path = bld.srcnode.make_node(str(pathlib.PurePath(gen_path, "Idl"))).abspath() + + # for idl in idl_files: + # idl_path = pathlib.PurePath(idl.abspath()) + # b = idl_path.name + + # gen_h = idl_path.with_suffix('.h').name + # gen_c = idl_path.with_suffix('.c').name + + # idl_folder = idl_path.parts[-3:-1] + # dst_dir = pathlib.PurePath(gen_path, "generated", *idl_folder) + # gen = [bld.bldnode.find_or_declare(str(dst_dir / name)) for name in [gen_h, gen_c]] + # gen_cmd = f"{bld.env.MICROXRCEDDSGEN[0]} -cs -replace -d {dst_dir} -I {idl_include_path} {idl}" + # bld( + # # build IDL file + # source=idl, + # target=gen, + # rule=gen_cmd, + # group='dynamic_sources', + # ) + + # bld.env.AP_LIB_EXTRA_SOURCES['AP_UROS'] += [str(gen[1])] From 16c57511b11fbf1520c8235f8cd59d2ee75fd053 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Thu, 20 Jul 2023 09:26:55 +0100 Subject: [PATCH 02/16] waf: add micro-ROS build option --enable-uros Signed-off-by: Rhys Mainwaring --- wscript | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/wscript b/wscript index db52f181a7ddd..7441451a1c133 100644 --- a/wscript +++ b/wscript @@ -267,8 +267,10 @@ submodules at specific revisions. help="Enables GPS logging") g.add_option('--enable-dds', action='store_true', - help="Enable the dds client to connect with ROS2/DDS" - ) + help="Enable the dds client to connect with ROS2/DDS") + + g.add_option('--enable-uros', action='store_true', + help="Enable the micro-ros client to connect with ROS2/DDS") g.add_option('--enable-dronecan-tests', action='store_true', default=False, @@ -720,6 +722,9 @@ def _build_dynamic_sources(bld): if bld.env.ENABLE_DDS: bld.recurse("libraries/AP_DDS") + if bld.env.ENABLE_UROS: + bld.recurse("libraries/AP_UROS") + def write_version_header(tsk): bld = tsk.generator.bld return bld.write_version_header(tsk.outputs[0].abspath()) From 2707cd2a67727e393dd5f0595be62d558e6b78f6 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Sat, 30 Sep 2023 12:35:51 +0100 Subject: [PATCH 03/16] modules: add ardupilot_uros Signed-off-by: Rhys Mainwaring modules: update ardupliot_uros - Added mode_switch service interface for esp32 - Added mode_switch service interface for stm32. Signed-off-by: Rhys Mainwaring --- .gitmodules | 3 +++ modules/ardupilot_uros | 1 + 2 files changed, 4 insertions(+) create mode 160000 modules/ardupilot_uros diff --git a/.gitmodules b/.gitmodules index fbfe76753a51d..11cc7809d58b0 100644 --- a/.gitmodules +++ b/.gitmodules @@ -39,3 +39,6 @@ path = modules/Micro-CDR url = https://github.com/ardupilot/Micro-CDR.git branch = master +[submodule "modules/ardupilot_uros"] + path = modules/ardupilot_uros + url = https://github.com/srmainwaring/ardupilot_uros.git diff --git a/modules/ardupilot_uros b/modules/ardupilot_uros new file mode 160000 index 0000000000000..fa20883f1a671 --- /dev/null +++ b/modules/ardupilot_uros @@ -0,0 +1 @@ +Subproject commit fa20883f1a671d415b5b62dcd2dd5cb5661c55f2 From c8eabfaa78c4bc0b0839bd8dac35a968eac4e520 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Thu, 20 Jul 2023 09:28:51 +0100 Subject: [PATCH 04/16] Tools: add micro-ROS build option --enable-uros - Add option --enable-uros. - Add define AP_UROS_ENABLED. Signed-off-by: Rhys Mainwaring --- Tools/ardupilotwaf/ardupilotwaf.py | 1 + Tools/ardupilotwaf/boards.py | 12 ++++++++++++ Tools/autotest/sim_vehicle.py | 5 +++++ 3 files changed, 18 insertions(+) diff --git a/Tools/ardupilotwaf/ardupilotwaf.py b/Tools/ardupilotwaf/ardupilotwaf.py index 75c9975b826bd..f706b177ca860 100644 --- a/Tools/ardupilotwaf/ardupilotwaf.py +++ b/Tools/ardupilotwaf/ardupilotwaf.py @@ -138,6 +138,7 @@ def get_legacy_defines(sketch_name, bld): 'doc', 'AP_Scripting', # this gets explicitly included when it is needed and should otherwise never be globbed in 'AP_DDS', + 'AP_UROS', ] diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index faf500cc24ee3..5f3c41f6b9d02 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -84,6 +84,18 @@ def srcpath(path): env.ENABLE_DDS = False env.DEFINES.update(AP_DDS_ENABLED = 0) + # configurations for UROS + if cfg.options.enable_uros: + cfg.recurse('libraries/AP_UROS') + env.ENABLE_UROS = True + env.AP_LIBRARIES += [ + 'AP_UROS' + ] + env.DEFINES.update(AP_UROS_ENABLED = 1) + else: + env.ENABLE_UROS = False + env.DEFINES.update(AP_UROS_ENABLED = 0) + # setup for supporting onvif cam control if cfg.options.enable_onvif: cfg.recurse('libraries/AP_ONVIF') diff --git a/Tools/autotest/sim_vehicle.py b/Tools/autotest/sim_vehicle.py index 3e0fb79eb4c56..96522c3022633 100755 --- a/Tools/autotest/sim_vehicle.py +++ b/Tools/autotest/sim_vehicle.py @@ -411,6 +411,9 @@ def do_build(opts, frame_options): if opts.enable_dds: cmd_configure.append("--enable-dds") + if opts.enable_uros: + cmd_configure.append("--enable-uros") + pieces = [shlex.split(x) for x in opts.waf_configure_args] for piece in pieces: cmd_configure.extend(piece) @@ -1326,6 +1329,8 @@ def generate_frame_help(): help="IP address of the simulator. Defaults to localhost") group_sim.add_option("--enable-dds", action='store_true', help="Enable the dds client to connect with ROS2/DDS") +group_sim.add_option("--enable-uros", action='store_true', + help="Enable the micro-ros client to connect with ROS2/DDS") parser.add_option_group(group_sim) From 59b07773d2f38761d7a4dd23334e5c61d285e4d7 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Thu, 20 Jul 2023 09:30:29 +0100 Subject: [PATCH 05/16] Tools: ros2: enable micro-ROS in colcon build Signed-off-by: Rhys Mainwaring Tools: ros2: disable DSS in colcon build Signed-off-by: Rhys Mainwaring Tools: ros2: update README Signed-off-by: Rhys Mainwaring --- Tools/ros2/README.md | 24 ++++++++++++++++++++++-- Tools/ros2/ardupilot_sitl/CMakeLists.txt | 4 ++-- 2 files changed, 24 insertions(+), 4 deletions(-) diff --git a/Tools/ros2/README.md b/Tools/ros2/README.md index bdd055e3f1329..691c9c5ac08c1 100644 --- a/Tools/ros2/README.md +++ b/Tools/ros2/README.md @@ -239,8 +239,28 @@ master:=tcp:127.0.0.1:5760 \ sitl:=127.0.0.1:5501 ``` -UDP version +### DDS UDP version -``` +```bash ros2 launch ardupilot_sitl sitl_dds_udp.launch.py transport:=udp4 refs:=$(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/dds_xrce_profile.xml synthetic_clock:=True wipe:=False model:=quad speedup:=1 slave:=0 instance:=0 defaults:=$(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/default_params/copter.parm,$(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/default_params/dds_udp.parm sim_address:=127.0.0.1 master:=tcp:127.0.0.1:5760 sitl:=127.0.0.1:5501 ``` + +### UROS UDP + +Launch agent: + +```bash +ros2 launch ardupilot_sitl micro_ros_agent.launch.py transport:=udp4 port:=2019 verbose:=6 +``` + +Launch SITL: + +```bash +ros2 launch ardupilot_sitl sitl.launch.py synthetic_clock:=True wipe:=False model:=quad speedup:=1 slave:=0 instance:=0 defaults:=$(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/default_params/copter.parm,$(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/default_params/dds_udp.parm sim_address:=127.0.0.1 +``` + +Launch non-interactive MAVProxy: + +```bash +ros2 launch ardupilot_sitl mavproxy.launch.py master:=tcp:127.0.0.1:5760 sitl:=127.0.0.1:5501 +``` diff --git a/Tools/ros2/ardupilot_sitl/CMakeLists.txt b/Tools/ros2/ardupilot_sitl/CMakeLists.txt index 9d40ea746d43b..8992b6c758045 100644 --- a/Tools/ros2/ardupilot_sitl/CMakeLists.txt +++ b/Tools/ros2/ardupilot_sitl/CMakeLists.txt @@ -24,12 +24,12 @@ message(STATUS "WAF_COMMAND: ${WAF_COMMAND}") set(WAF_CONFIG $<$,$>:"--debug">) add_custom_target(ardupilot_configure ALL - ${WAF_COMMAND} configure --board sitl --enable-dds ${WAF_CONFIG} + ${WAF_COMMAND} configure --board sitl --enable-uros ${WAF_CONFIG} WORKING_DIRECTORY ${ARDUPILOT_ROOT} ) add_custom_target(ardupilot_build ALL - ${WAF_COMMAND} build --enable-dds ${WAF_CONFIG} + ${WAF_COMMAND} build --enable-uros ${WAF_CONFIG} WORKING_DIRECTORY ${ARDUPILOT_ROOT} ) add_dependencies(ardupilot_build ardupilot_configure) From ab6d78d79de15c8b86e2dd2c494451e9a745b4a9 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Thu, 20 Jul 2023 09:29:47 +0100 Subject: [PATCH 06/16] AP_Vehicle: add micro-ROS client Signed-off-by: Rhys Mainwaring --- libraries/AP_Vehicle/AP_Vehicle.cpp | 23 +++++++++++++++++++++++ libraries/AP_Vehicle/AP_Vehicle.h | 7 +++++++ 2 files changed, 30 insertions(+) diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index ff333b0a424ba..ec2deaaeb998d 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -18,6 +18,7 @@ #include #endif #include +#include #if HAL_WITH_IO_MCU #include extern AP_IOMCU iomcu; @@ -205,6 +206,11 @@ const AP_Param::GroupInfo AP_Vehicle::var_info[] = { AP_GROUPINFO("FLTMODE_GCSBLOCK", 20, AP_Vehicle, flight_mode_GCS_block, 0), #endif // APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_Rover) +#if AP_UROS_ENABLED + // @Group: UROS + // @Path: ../AP_UROS/AP_UROS_Client.cpp + AP_SUBGROUPPTR(uros_client, "UROS", 21, AP_Vehicle, AP_UROS_Client), +#endif // AP_UROS_ENABLED #if AP_NETWORKING_ENABLED // @Group: NET_ @@ -407,6 +413,12 @@ void AP_Vehicle::setup() GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "DDS Client: Failed to Initialize"); } #endif + +#if AP_UROS_ENABLED + if (!init_uros_client()) { + gcs().send_text(MAV_SEVERITY_ERROR, "UROS: Failed to initialize"); + } +#endif } void AP_Vehicle::loop() @@ -945,6 +957,17 @@ bool AP_Vehicle::init_dds_client() } #endif // AP_DDS_ENABLED +#if AP_UROS_ENABLED +bool AP_Vehicle::init_uros_client() +{ + uros_client = new AP_UROS_Client(); + if (uros_client == nullptr) { + return false; + } + return uros_client->start(); +} +#endif // AP_UROS_ENABLED + // Check if this mode can be entered from the GCS #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_Rover) bool AP_Vehicle::block_GCS_mode_change(uint8_t mode_num, const uint8_t *mode_list, uint8_t mode_list_length) const diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 18b43fc0209ca..faafa119c2cb8 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -64,6 +64,7 @@ #include class AP_DDS_Client; +class AP_UROS_Client; class AP_Vehicle : public AP_HAL::HAL::Callbacks { @@ -427,6 +428,12 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { bool init_dds_client() WARN_IF_UNUSED; #endif +#if AP_UROS_ENABLED + // Declare the micro-ros client for communication with ROS 2 and DDS (common for all vehicles) + AP_UROS_Client *uros_client; + bool init_uros_client() WARN_IF_UNUSED; +#endif + // Check if this mode can be entered from the GCS bool block_GCS_mode_change(uint8_t mode_num, const uint8_t *mode_list, uint8_t mode_list_length) const; From b1e6861c9edc412ae82728e20b7611925ddb2a39 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Sat, 30 Sep 2023 13:47:55 +0100 Subject: [PATCH 07/16] AP_UROS: implement micro-ROS client AP_UROS: search for micro-ROS client library in ardupilot_uros module Signed-off-by: Rhys Mainwaring AP_UROS: add vector3 subscriber example - PoC - will only work for SITL / POSIX (must add use custom transport) Signed-off-by: Rhys Mainwaring AP_UROS: move init and create code into respective functions - Make rcl objects member variables. - Update checks macros to report to GCS. - Move rcl initialisation to init(). - Move subscriber creation to create(). Signed-off-by: Rhys Mainwaring AP_UROS: add custom transport - Adapt UDP custom transport from AP_DDS. Signed-off-by: Rhys Mainwaring AP_UROS: enable custom transport Signed-off-by: Rhys Mainwaring AP_UROS: add publisher example Signed-off-by: Rhys Mainwaring AP_UROS: add publisher example - Send message to GCS. Signed-off-by: Rhys Mainwaring AP_UROS: include additional ROS message types Signed-off-by: Rhys Mainwaring AP_UROS: add time publisher - Add time publisher. - Rename node to 'ardupilot_uros'. - Change timer period to 1 ms Signed-off-by: Rhys Mainwaring AP_UROS: add publishers for local pose and twist Signed-off-by: Rhys Mainwaring AP_UROS: add publishers for battery, clock, nav sat, static tf Signed-off-by: Rhys Mainwaring AP_UROS: clarify number of handles calculation Signed-off-by: Rhys Mainwaring AP_UROS: implement topic writers using code from AP_DDS - Strings and sequence values are not populated as memory must be allocated. Signed-off-by: Rhys Mainwaring AP_UROS: add code for static transform from AP_DDS - Strings and sequence values are not populated as memory must be allocated. Signed-off-by: Rhys Mainwaring AP_UROS: manage memory for topic strings and sequences Signed-off-by: Rhys Mainwaring AP_UROS: only update nav sat fix on new data to match AP_DDS Signed-off-by: Rhys Mainwaring AP_UROS: add subscriber to sensor_msgs/msg/Joy Signed-off-by: Rhys Mainwaring AP_UROS: remove demo subscriber to geometry_msgs/msg/Vector3 Signed-off-by: Rhys Mainwaring AP_UROS: update @todo formatting Signed-off-by: Rhys Mainwaring AP_UROS: add function declarations for Ubuntu build Signed-off-by: Rhys Mainwaring AP_UROS: disable tf2_msgs for esp32 dev Signed-off-by: Rhys Mainwaring AP_UROS: move global variables into AP_UROS_Client class - Make singleton. - Make update_topic member function. - Add trampoline for thread task. - Move thread to core 1 and adjust priority. - Customise initialisation. Signed-off-by: Rhys Mainwaring AP_UROS: fix initialisation of local twist publisher Signed-off-by: Rhys Mainwaring AP_UROS: publish geopose, static_tf, and subscribe to tf Signed-off-by: Rhys Mainwaring AP_UROS: use trampoline functions for timer and message callbacks Signed-off-by: Rhys Mainwaring AP_UROS: add arm motors service Signed-off-by: Rhys Mainwaring AP_UROS: add parameter server Signed-off-by: Rhys Mainwaring AP_UROS: add README Signed-off-by: Rhys Mainwaring AP_UROS: display parameter changes in GCS Signed-off-by: Rhys Mainwaring AP_UROS: implementation of custom serial transport - Note this is ESP32 specific. - Requires micro-ROS client library to be built for custom transport. - UART_NUM_2 not managed by AP_HAL - Return if publisher or subscriber initialisation fails Signed-off-by: Rhys Mainwaring AP_UROS: call uros_port->begin(0) to ensure thread owns port Signed-off-by: Rhys Mainwaring AP_UROS: support build for stm32 - disable param server Signed-off-by: Rhys Mainwaring AP_UROS: support both esp32 and stm32 Signed-off-by: Rhys Mainwaring AP_UROS: add compile option for param server Signed-off-by: Rhys Mainwaring AP_UROS: update macros for debug and error messages Signed-off-by: Rhys Mainwaring AP_UROS: fix bug introduced into entity handle count Signed-off-by: Rhys Mainwaring AP_UROS: update print format specifier for TF message Signed-off-by: Rhys Mainwaring AP_UROS: enable UDP transport for SITL Signed-off-by: Rhys Mainwaring AP_UROS: update print format specifier for TF message Signed-off-by: Rhys Mainwaring AP_UROS: add microros library distro options to wscript Signed-off-by: Rhys Mainwaring AP_UROS: fix library paths in wscript Signed-off-by: Rhys Mainwaring AP_UROS: add stubs for mode switch service Signed-off-by: Rhys Mainwaring AP_UROS: implement arming service Signed-off-by: Rhys Mainwaring AP_UROS: implement mode switch service Signed-off-by: Rhys Mainwaring AP_UROS: add stub for velocity control subscriber Signed-off-by: Rhys Mainwaring AP_UROS: add classes adapted from AP_DDS to manage external control - Functions are stubs only. Signed-off-by: Rhys Mainwaring AP_UROS: adapt external control functions to micro-ROS message types Signed-off-by: Rhys Mainwaring AP_UROS: enable external odom in uros client Signed-off-by: Rhys Mainwaring AP_UROS: update GCS messages for services - Add UROS: prefix. Signed-off-by: Rhys Mainwaring AP_UROS: update README Signed-off-by: Rhys Mainwaring --- libraries/AP_UROS/AP_UROS_Client.cpp | 1331 ++++++++++++++++- libraries/AP_UROS/AP_UROS_Client.h | 231 ++- libraries/AP_UROS/AP_UROS_ExternalControl.cpp | 43 + libraries/AP_UROS/AP_UROS_ExternalControl.h | 11 + libraries/AP_UROS/AP_UROS_External_Odom.cpp | 74 + libraries/AP_UROS/AP_UROS_External_Odom.h | 34 + libraries/AP_UROS/AP_UROS_Frames.h | 7 + libraries/AP_UROS/AP_UROS_Serial.cpp | 108 ++ .../AP_UROS/AP_UROS_Type_Conversions.cpp | 13 + libraries/AP_UROS/AP_UROS_Type_Conversions.h | 17 + libraries/AP_UROS/AP_UROS_UDP.cpp | 92 ++ libraries/AP_UROS/AP_UROS_config.h | 33 + libraries/AP_UROS/README.md | 273 ++++ libraries/AP_UROS/wscript | 143 +- 14 files changed, 2303 insertions(+), 107 deletions(-) create mode 100644 libraries/AP_UROS/AP_UROS_ExternalControl.cpp create mode 100644 libraries/AP_UROS/AP_UROS_ExternalControl.h create mode 100644 libraries/AP_UROS/AP_UROS_External_Odom.cpp create mode 100644 libraries/AP_UROS/AP_UROS_External_Odom.h create mode 100644 libraries/AP_UROS/AP_UROS_Frames.h create mode 100644 libraries/AP_UROS/AP_UROS_Serial.cpp create mode 100644 libraries/AP_UROS/AP_UROS_Type_Conversions.cpp create mode 100644 libraries/AP_UROS/AP_UROS_Type_Conversions.h create mode 100644 libraries/AP_UROS/AP_UROS_UDP.cpp create mode 100644 libraries/AP_UROS/AP_UROS_config.h create mode 100644 libraries/AP_UROS/README.md diff --git a/libraries/AP_UROS/AP_UROS_Client.cpp b/libraries/AP_UROS/AP_UROS_Client.cpp index f5727a36fb651..c2abc01841639 100644 --- a/libraries/AP_UROS/AP_UROS_Client.cpp +++ b/libraries/AP_UROS/AP_UROS_Client.cpp @@ -2,16 +2,715 @@ #if AP_UROS_ENABLED +#include "AP_UROS_Client.h" + +#include +#include + +// micro-ROS +#include +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_ESP32 +#include +#endif + +#include +#include +#include #include #include -#include #include +#include +#include #include -#include -#include -#include "AP_UROS_Client.h" +#include +#if AP_EXTERNAL_CONTROL_ENABLED +#include "AP_UROS_ExternalControl.h" +#endif +#include "AP_UROS_Frames.h" +#include "AP_UROS_External_Odom.h" + +#define UROS_DEBUG 1 +#define UROS_INFO 1 +#define UROS_ERROR 1 + +#if UROS_DEBUG +#define uros_debug(fmt, args...) do {\ + hal.console->printf(fmt "\n", ##args);\ + GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, fmt, ##args);\ +} while(0) +#else +#define uros_debug(fmt, args...) do {} while(0) +#endif + +#if UROS_INFO +#define uros_info(fmt, args...) do {\ + hal.console->printf(fmt "\n", ##args);\ + GCS_SEND_TEXT(MAV_SEVERITY_INFO, fmt, ##args);\ +} while(0) +#else +#define uros_debug(fmt, args...) do {} while(0) +#endif + +#if UROS_ERROR +#define uros_error(fmt, args...) do {\ + hal.console->printf(fmt "\n", ##args);\ + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, fmt, ##args);\ +} while(0) +#else +#define uros_error(fmt, args...) do {} while(0) +#endif + +#define UROS_CHECK(fn, msg) {\ + rcl_ret_t temp_rc = fn;\ + if ((temp_rc != RCL_RET_OK)) {\ + uros_error("UROS: " msg "... FAILED, line %d: code: %d",\ + __LINE__, (int)temp_rc);\ + /* will be empty when RCUTILS_AVOID_DYNAMIC_ALLOCATION is defined */\ + const rcutils_error_state_t *err = rcutils_get_error_state();\ + uros_error("UROS: error: %s, file: %s, line: %llu, ",\ + err->message, err->file, err->line_number);\ + return false;\ + } else {\ + uros_debug("UROS: " msg"... OK");\ + }\ +} + +#define UROS_SOFTCHECK(fn) {\ + rcl_ret_t temp_rc = fn;\ + if ((temp_rc != RCL_RET_OK)) {\ + uros_info("UROS: failed status, line %d: code: %d",\ + __LINE__, (int)temp_rc);\ + }\ +} + +// publishers +constexpr uint16_t DELAY_BATTERY_STATE_TOPIC_MS = 1000; +constexpr uint16_t DELAY_CLOCK_TOPIC_MS = 10; +constexpr uint16_t DELAY_GEO_POSE_TOPIC_MS = 33; +constexpr uint16_t DELAY_LOCAL_POSE_TOPIC_MS = 33; +constexpr uint16_t DELAY_LOCAL_TWIST_TOPIC_MS = 33; +constexpr uint16_t DELAY_STATIC_TRANSFORM_TOPIC_MS = 1000; +constexpr uint16_t DELAY_TIME_TOPIC_MS = 10; + +// constructor +AP_UROS_Client::AP_UROS_Client() { + if (_singleton) { + uros_error("PANIC: Too many AP_UROS_Client instances"); + } + _singleton = this; +} + +// singleton +AP_UROS_Client *AP_UROS_Client::_singleton = nullptr; + +AP_UROS_Client *AP_UROS_Client::get_singleton() { + return AP_UROS_Client::_singleton; +} + +// update published topics + +// implementation copied from: +// void AP_DDS_Client::update_topic(sensor_msgs_msg_BatteryState& msg, const uint8_t instance) +void AP_UROS_Client::update_topic(sensor_msgs__msg__BatteryState& msg) +{ + const uint8_t instance = 0; + + if (instance >= AP_BATT_MONITOR_MAX_INSTANCES) { + return; + } + + update_topic(msg.header.stamp); + auto &battery = AP::battery(); + + if (!battery.healthy(instance)) { + msg.power_supply_status = 3; //POWER_SUPPLY_HEALTH_DEAD + msg.present = false; + return; + } + msg.present = true; + + msg.voltage = battery.voltage(instance); + + float temperature; + msg.temperature = (battery.get_temperature(temperature, instance)) ? temperature : NAN; + + float current; + msg.current = (battery.current_amps(current, instance)) ? -1 * current : NAN; + + const float design_capacity = (float)battery.pack_capacity_mah(instance) * 0.001; + msg.design_capacity = design_capacity; + + uint8_t percentage; + if (battery.capacity_remaining_pct(percentage, instance)) { + msg.percentage = percentage * 0.01; + msg.charge = (percentage * design_capacity) * 0.01; + } else { + msg.percentage = NAN; + msg.charge = NAN; + } + + msg.capacity = NAN; + + if (battery.current_amps(current, instance)) { + if (percentage == 100) { + msg.power_supply_status = 4; //POWER_SUPPLY_STATUS_FULL + } else if (current < 0.0) { + msg.power_supply_status = 1; //POWER_SUPPLY_STATUS_CHARGING + } else if (current > 0.0) { + msg.power_supply_status = 2; //POWER_SUPPLY_STATUS_DISCHARGING + } else { + msg.power_supply_status = 3; //POWER_SUPPLY_STATUS_NOT_CHARGING + } + } else { + msg.power_supply_status = 0; //POWER_SUPPLY_STATUS_UNKNOWN + } + + msg.power_supply_health = (battery.overpower_detected(instance)) ? 4 : 1; //POWER_SUPPLY_HEALTH_OVERVOLTAGE or POWER_SUPPLY_HEALTH_GOOD + + 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.data); + } +} + +// implementation copied from: +// void AP_DDS_Client::update_topic(rosgraph_msgs_msg_Clock& msg) +void AP_UROS_Client::update_topic(rosgraph_msgs__msg__Clock& msg) +{ + update_topic(msg.clock); +} + +// implementation copied from: +// void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg) +void AP_UROS_Client::update_topic(geographic_msgs__msg__GeoPoseStamped& msg) +{ + update_topic(msg.header.stamp); + strcpy(msg.header.frame_id.data, BASE_LINK_FRAME_ID); + + auto &ahrs = AP::ahrs(); + WITH_SEMAPHORE(ahrs.get_semaphore()); + + Location loc; + if (ahrs.get_location(loc)) { + msg.pose.position.latitude = loc.lat * 1E-7; + msg.pose.position.longitude = loc.lng * 1E-7; + msg.pose.position.altitude = loc.alt * 0.01; // Transform from cm to m + } + + // In ROS REP 103, axis orientation uses the following convention: + // X - Forward + // Y - Left + // Z - Up + // https://www.ros.org/reps/rep-0103.html#axis-orientation + // As a consequence, to follow ROS REP 103, it is necessary to switch X and Y, + // as well as invert Z (NED to ENU convertion) as well as a 90 degree rotation in the Z axis + // for x to point forward + Quaternion orientation; + if (ahrs.get_quaternion(orientation)) { + Quaternion aux(orientation[0], orientation[2], orientation[1], -orientation[3]); //NED to ENU transformation + Quaternion transformation(sqrtF(2) * 0.5, 0, 0, sqrtF(2) * 0.5); // Z axis 90 degree rotation + orientation = aux * transformation; + msg.pose.orientation.w = orientation[0]; + msg.pose.orientation.x = orientation[1]; + msg.pose.orientation.y = orientation[2]; + msg.pose.orientation.z = orientation[3]; + } +} + +// implementation copied from: +// void AP_DDS_Client::update_topic(geometry_msgs_msg_PoseStamped& msg) +void AP_UROS_Client::update_topic(geometry_msgs__msg__PoseStamped& msg) +{ + update_topic(msg.header.stamp); + strcpy(msg.header.frame_id.data, BASE_LINK_FRAME_ID); + + auto &ahrs = AP::ahrs(); + WITH_SEMAPHORE(ahrs.get_semaphore()); + + // ROS REP 103 uses the ENU convention: + // X - East + // Y - North + // Z - Up + // https://www.ros.org/reps/rep-0103.html#axis-orientation + // AP_AHRS uses the NED convention + // X - North + // Y - East + // Z - Down + // As a consequence, to follow ROS REP 103, it is necessary to switch X and Y, + // as well as invert Z + + Vector3f position; + if (ahrs.get_relative_position_NED_home(position)) { + msg.pose.position.x = position[1]; + msg.pose.position.y = position[0]; + msg.pose.position.z = -position[2]; + } + + // In ROS REP 103, axis orientation uses the following convention: + // X - Forward + // Y - Left + // Z - Up + // https://www.ros.org/reps/rep-0103.html#axis-orientation + // As a consequence, to follow ROS REP 103, it is necessary to switch X and Y, + // as well as invert Z (NED to ENU convertion) as well as a 90 degree rotation in the Z axis + // for x to point forward + Quaternion orientation; + if (ahrs.get_quaternion(orientation)) { + Quaternion aux(orientation[0], orientation[2], orientation[1], -orientation[3]); //NED to ENU transformation + Quaternion transformation (sqrtF(2) * 0.5,0,0,sqrtF(2) * 0.5); // Z axis 90 degree rotation + orientation = aux * transformation; + msg.pose.orientation.w = orientation[0]; + msg.pose.orientation.x = orientation[1]; + msg.pose.orientation.y = orientation[2]; + msg.pose.orientation.z = orientation[3]; + } +} + +// implementation copied from: +// void AP_DDS_Client::update_topic(geometry_msgs_msg_TwistStamped& msg) +void AP_UROS_Client::update_topic(geometry_msgs__msg__TwistStamped& msg) +{ + update_topic(msg.header.stamp); + strcpy(msg.header.frame_id.data, BASE_LINK_FRAME_ID); + + auto &ahrs = AP::ahrs(); + WITH_SEMAPHORE(ahrs.get_semaphore()); + + // ROS REP 103 uses the ENU convention: + // X - East + // Y - North + // Z - Up + // https://www.ros.org/reps/rep-0103.html#axis-orientation + // AP_AHRS uses the NED convention + // X - North + // Y - East + // Z - Down + // As a consequence, to follow ROS REP 103, it is necessary to switch X and Y, + // as well as invert Z + Vector3f velocity; + if (ahrs.get_velocity_NED(velocity)) { + msg.twist.linear.x = velocity[1]; + msg.twist.linear.y = velocity[0]; + msg.twist.linear.z = -velocity[2]; + } + + // In ROS REP 103, axis orientation uses the following convention: + // X - Forward + // Y - Left + // Z - Up + // https://www.ros.org/reps/rep-0103.html#axis-orientation + // The gyro data is received from AP_AHRS in body-frame + // X - Forward + // Y - Right + // Z - Down + // As a consequence, to follow ROS REP 103, it is necessary to invert Y and Z + Vector3f angular_velocity = ahrs.get_gyro(); + msg.twist.angular.x = angular_velocity[0]; + msg.twist.angular.y = -angular_velocity[1]; + msg.twist.angular.z = -angular_velocity[2]; +} + +// implementation copied from: +// void AP_DDS_Client::populate_static_transforms(tf2_msgs_msg_TFMessage& msg) +void AP_UROS_Client::update_topic(tf2_msgs__msg__TFMessage& msg) +{ + msg.transforms.size = 0; + + auto &gps = AP::gps(); + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + const auto gps_type = gps.get_type(i); + if (gps_type == AP_GPS::GPS_Type::GPS_TYPE_NONE) { + continue; + } + update_topic(msg.transforms.data[i].header.stamp); + char gps_frame_id[16]; + //! @todo should GPS frame ID's be 0 or 1 indexed in ROS? + hal.util->snprintf(gps_frame_id, sizeof(gps_frame_id), "GPS_%u", i); + strcpy(msg.transforms.data[i].header.frame_id.data, BASE_LINK_FRAME_ID); + strcpy(msg.transforms.data[i].child_frame_id.data, gps_frame_id); + // The body-frame offsets + // X - Forward + // Y - Right + // Z - Down + // https://ardupilot.org/copter/docs/common-sensor-offset-compensation.html#sensor-position-offset-compensation + + const auto offset = gps.get_antenna_offset(i); + + // In ROS REP 103, it follows this convention + // X - Forward + // Y - Left + // Z - Up + // https://www.ros.org/reps/rep-0103.html#axis-orientation + + msg.transforms.data[i].transform.translation.x = offset[0]; + msg.transforms.data[i].transform.translation.y = -1 * offset[1]; + msg.transforms.data[i].transform.translation.z = -1 * offset[2]; + + msg.transforms.size++; + } +} + +// implementation copied from: +// bool AP_DDS_Client::update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t instance) +bool AP_UROS_Client::update_topic(sensor_msgs__msg__NavSatFix& msg) +{ + const uint8_t instance = 0; + + // Add a lambda that takes in navsatfix msg and populates the cov + // Make it constexpr if possible + // https://www.fluentcpp.com/2021/12/13/the-evolutions-of-lambdas-in-c14-c17-and-c20/ + // constexpr auto times2 = [] (sensor_msgs_msg_NavSatFix* msg) { return n * 2; }; + + // assert(instance >= GPS_MAX_RECEIVERS); + if (instance >= GPS_MAX_RECEIVERS) { + return false; + } + + auto &gps = AP::gps(); + WITH_SEMAPHORE(gps.get_semaphore()); + + if (!gps.is_healthy(instance)) { + msg.status.status = -1; // STATUS_NO_FIX + msg.status.service = 0; // No services supported + msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN + return false; + } + + // No update is needed + const auto last_fix_time_ms = gps.last_fix_time_ms(instance); + if (last_nav_sat_fix_time_ms == last_fix_time_ms) { + return false; + } else { + last_nav_sat_fix_time_ms = last_fix_time_ms; + } + + + update_topic(msg.header.stamp); + strcpy(msg.header.frame_id.data, WGS_84_FRAME_ID); + msg.status.service = 0; // SERVICE_GPS + msg.status.status = -1; // STATUS_NO_FIX + + + //! @todo What about glonass, compass, galileo? + //! This will be properly designed and implemented to spec in #23277 + msg.status.service = 1; // SERVICE_GPS + + const auto status = gps.status(instance); + switch (status) { + case AP_GPS::NO_GPS: + case AP_GPS::NO_FIX: + msg.status.status = -1; // STATUS_NO_FIX + msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN + return true; + case AP_GPS::GPS_OK_FIX_2D: + case AP_GPS::GPS_OK_FIX_3D: + msg.status.status = 0; // STATUS_FIX + break; + case AP_GPS::GPS_OK_FIX_3D_DGPS: + msg.status.status = 1; // STATUS_SBAS_FIX + break; + case AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT: + case AP_GPS::GPS_OK_FIX_3D_RTK_FIXED: + msg.status.status = 2; // STATUS_SBAS_FIX + break; + default: + //! @todo Can we not just use an enum class and not worry about this condition? + break; + } + const auto loc = gps.location(instance); + msg.latitude = loc.lat * 1E-7; + msg.longitude = loc.lng * 1E-7; + + int32_t alt_cm; + if (!loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_cm)) { + // With absolute frame, this condition is unlikely + msg.status.status = -1; // STATUS_NO_FIX + msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN + return true; + } + msg.altitude = alt_cm * 0.01; + + // ROS allows double precision, ArduPilot exposes float precision today + Matrix3f cov; + msg.position_covariance_type = (uint8_t)gps.position_covariance(instance, cov); + msg.position_covariance[0] = cov[0][0]; + msg.position_covariance[1] = cov[0][1]; + msg.position_covariance[2] = cov[0][2]; + msg.position_covariance[3] = cov[1][0]; + msg.position_covariance[4] = cov[1][1]; + msg.position_covariance[5] = cov[1][2]; + msg.position_covariance[6] = cov[2][0]; + msg.position_covariance[7] = cov[2][1]; + msg.position_covariance[8] = cov[2][2]; + + return true; +} + +// implementation copied from: +// void AP_DDS_Client::update_topic(builtin_interfaces_msg_Time& msg) +void AP_UROS_Client::update_topic(builtin_interfaces__msg__Time& msg) +{ + uint64_t utc_usec; + if (!AP::rtc().get_utc_usec(utc_usec)) { + utc_usec = AP_HAL::micros64(); + } + msg.sec = utc_usec / 1000000ULL; + msg.nanosec = (utc_usec % 1000000ULL) * 1000UL; +} + +// call publishers +void AP_UROS_Client::timer_callback_trampoline(rcl_timer_t * timer, int64_t last_call_time) +{ + AP_UROS_Client *uros = AP_UROS_Client::get_singleton(); + uros->timer_callback(timer, last_call_time); +} + +void AP_UROS_Client::timer_callback(rcl_timer_t * timer, int64_t last_call_time) +{ + (void) last_call_time; + + WITH_SEMAPHORE(csem); + + // get the timer clock + rcl_clock_t * clock; + UROS_SOFTCHECK(rcl_timer_clock(timer, &clock)); + + //! @todo(srmainwaring) - use the rcl_clock? + const auto cur_time_ms = AP_HAL::millis64(); + + if (timer != NULL) { + + if (battery_state_pub_init && + cur_time_ms - last_battery_state_time_ms > DELAY_BATTERY_STATE_TOPIC_MS) { + update_topic(battery_state_msg); + last_battery_state_time_ms = cur_time_ms; + UROS_SOFTCHECK(rcl_publish(&battery_state_publisher, &battery_state_msg, NULL)); + } + + if (clock_pub_init && + cur_time_ms - last_clock_time_ms > DELAY_CLOCK_TOPIC_MS) { + update_topic(clock_msg); + last_clock_time_ms = cur_time_ms; + UROS_SOFTCHECK(rcl_publish(&clock_publisher, &clock_msg, NULL)); + } + + if (local_pose_pub_init && + cur_time_ms - last_local_pose_time_ms > DELAY_LOCAL_POSE_TOPIC_MS) { + update_topic(local_pose_msg); + last_local_pose_time_ms = cur_time_ms; + UROS_SOFTCHECK(rcl_publish(&local_pose_publisher, &local_pose_msg, NULL)); + } + + if (local_twist_pub_init && + cur_time_ms - last_local_twist_time_ms > DELAY_LOCAL_TWIST_TOPIC_MS) { + update_topic(local_twist_msg); + last_local_twist_time_ms = cur_time_ms; + UROS_SOFTCHECK(rcl_publish(&local_twist_publisher, &local_twist_msg, NULL)); + } + + if (nav_sat_fix_pub_init && + update_topic(nav_sat_fix_msg)) { + UROS_SOFTCHECK(rcl_publish(&nav_sat_fix_publisher, &nav_sat_fix_msg, NULL)); + } + + if (tx_static_transforms_pub_init && + cur_time_ms - last_tx_static_transforms_time_ms > DELAY_STATIC_TRANSFORM_TOPIC_MS) { + update_topic(tx_static_transforms_msg); + last_tx_static_transforms_time_ms = cur_time_ms; + UROS_SOFTCHECK(rcl_publish(&tx_static_transforms_publisher, &tx_static_transforms_msg, NULL)); + } + + if (geo_pose_pub_init && + cur_time_ms - last_geo_pose_time_ms > DELAY_GEO_POSE_TOPIC_MS) { + update_topic(geo_pose_msg); + last_geo_pose_time_ms = cur_time_ms; + UROS_SOFTCHECK(rcl_publish(&geo_pose_publisher, &geo_pose_msg, NULL)); + } + + if (time_pub_init && + cur_time_ms - last_time_time_ms > DELAY_TIME_TOPIC_MS) { + update_topic(time_msg); + last_time_time_ms = cur_time_ms; + UROS_SOFTCHECK(rcl_publish(&time_publisher, &time_msg, NULL)); + // hal.console->printf("UROS: sent time: %d, %d\n", time_msg.sec, time_msg.nanosec); + } + } +} + +// subscriber callbacks +void AP_UROS_Client::on_joy_msg_trampoline(const void * msgin, void* context) +{ + AP_UROS_Client *uros = (AP_UROS_Client*)context; + const sensor_msgs__msg__Joy *msg = (const sensor_msgs__msg__Joy *)msgin; + uros->on_joy_msg(msg); +} + +void AP_UROS_Client::on_joy_msg(const sensor_msgs__msg__Joy *msg) +{ + //! @todo(srmainwaring) implement joystick RC control to AP + if (msg->axes.size >= 4) { + uros_info("UROS: sensor_msgs/Joy: %f, %f, %f, %f", + msg->axes.data[0], msg->axes.data[1], msg->axes.data[2], msg->axes.data[3]); + } else { + uros_error("UROS: sensor_msgs/Joy must have axes size >= 4"); + } +} + +void AP_UROS_Client::on_velocity_control_msg_trampoline(const void * msgin, void* context) +{ + AP_UROS_Client *uros = (AP_UROS_Client*)context; + const geometry_msgs__msg__TwistStamped *msg = (const geometry_msgs__msg__TwistStamped *)msgin; + uros->on_velocity_control_msg(msg); +} + +void AP_UROS_Client::on_velocity_control_msg(const geometry_msgs__msg__TwistStamped *msg) +{ +#if AP_EXTERNAL_CONTROL_ENABLED + if (!AP_UROS_External_Control::handle_velocity_control(*msg)) { + // TODO #23430 handle velocity control failure through rosout, throttled. + } +#endif // AP_EXTERNAL_CONTROL_ENABLED +} +void AP_UROS_Client::on_tf_msg_trampoline(const void * msgin, void* context) +{ + AP_UROS_Client *uros = (AP_UROS_Client*)context; + const tf2_msgs__msg__TFMessage *msg = (const tf2_msgs__msg__TFMessage *)msgin; + uros->on_tf_msg(msg); +} + +void AP_UROS_Client::on_tf_msg(const tf2_msgs__msg__TFMessage * msg) +{ + if (msg->transforms.size > 0) { +#if AP_UROS_VISUALODOM_ENABLED + AP_UROS_External_Odom::handle_external_odom(*msg); +#endif // AP_UROS_VISUALODOM_ENABLED + } else { + uros_error("UROS: tf2_msgs/TFMessage with no content"); + } +} + +// service callbacks +void AP_UROS_Client::arm_motors_callback_trampoline( + const void *req, void *res, void *context) +{ + AP_UROS_Client *uros = (AP_UROS_Client*)context; + ardupilot_msgs__srv__ArmMotors_Request *req_in = + (ardupilot_msgs__srv__ArmMotors_Request *) req; + ardupilot_msgs__srv__ArmMotors_Response *res_in = + (ardupilot_msgs__srv__ArmMotors_Response *) res; + uros->arm_motors_callback(req_in, res_in); +} + +void AP_UROS_Client::arm_motors_callback( + const ardupilot_msgs__srv__ArmMotors_Request *req, + ardupilot_msgs__srv__ArmMotors_Response *res) +{ + if (req->arm) { + uros_info("UROS: Request for arming received"); + res->result = AP::arming().arm(AP_Arming::Method::DDS); + } else { + uros_info("UROS: Request for disarming received"); + res->result = AP::arming().disarm(AP_Arming::Method::DDS); + } + + if (res->result) { + uros_info("UROS: Request for Arming/Disarming : SUCCESS"); + } else { + uros_info("UROS: Request for Arming/Disarming : FAIL"); + } +} + +void AP_UROS_Client::mode_switch_callback_trampoline( + const void *req, void *res, void *context) +{ + AP_UROS_Client *uros = (AP_UROS_Client*)context; + ardupilot_msgs__srv__ModeSwitch_Request *req_in = + (ardupilot_msgs__srv__ModeSwitch_Request *) req; + ardupilot_msgs__srv__ModeSwitch_Response *res_in = + (ardupilot_msgs__srv__ModeSwitch_Response *) res; + uros->mode_switch_callback(req_in, res_in); +} + +void AP_UROS_Client::mode_switch_callback( + const ardupilot_msgs__srv__ModeSwitch_Request *req, + ardupilot_msgs__srv__ModeSwitch_Response *res) +{ + res->status = AP::vehicle()->set_mode(req->mode, ModeReason::DDS_COMMAND); + res->curr_mode = AP::vehicle()->get_mode(); + + if (res->status) { + uros_info("UROS: Request for Mode Switch : SUCCESS"); + } else { + uros_info("UROS: Request for Mode Switch : FAIL"); + } +} + +#if AP_UROS_PARAM_SRV_ENABLED +// parameter server callback +bool AP_UROS_Client::on_parameter_changed_trampoline( + const Parameter * old_param, const Parameter * new_param, void * context) +{ + AP_UROS_Client *uros = (AP_UROS_Client*)context; + return uros->on_parameter_changed(old_param, new_param); +} + +bool AP_UROS_Client::on_parameter_changed( + const Parameter * old_param, const Parameter * new_param) +{ + //! @note copied from rcl_examples/src/example_parameter_server.c + if (old_param == NULL && new_param == NULL) { + uros_error("UROS: error updating parameters"); + return false; + } + + if (old_param == NULL) { + uros_info("UROS: creating new parameter %s", + new_param->name.data); + } else if (new_param == NULL) { + uros_info("UROS: deleting parameter %s", + old_param->name.data); + } else { + switch (old_param->value.type) { + case RCLC_PARAMETER_BOOL: + uros_info( + "UROS: parameter %s modified: " + "old value: %d, new value: %d (bool)", + old_param->name.data, + old_param->value.bool_value, + new_param->value.bool_value); + break; + case RCLC_PARAMETER_INT: + uros_info( + "UROS: parameter %s modified: " + "old value: %lld, new value: %lld (int)", + old_param->name.data, + old_param->value.integer_value, + new_param->value.integer_value); + break; + case RCLC_PARAMETER_DOUBLE: + uros_info( + "UROS: parameter %s modified: " + "old value: %f, new value: %f (double)", + old_param->name.data, + old_param->value.double_value, + new_param->value.double_value); + break; + default: + break; + } + } + + return true; +} +#endif + +// parameter group const AP_Param::GroupInfo AP_UROS_Client::var_info[] { // @Param: _ENABLE @@ -20,13 +719,24 @@ const AP_Param::GroupInfo AP_UROS_Client::var_info[] { // @Values: 0:Disabled,1:Enabled // @RebootRequired: True // @User: Advanced - AP_GROUPINFO_FLAGS("_ENABLE", 1, AP_UROS_Client, enabled, 0, AP_PARAM_FLAG_ENABLE), + AP_GROUPINFO_FLAGS("_ENABLE", 1, AP_UROS_Client, enabled, 0, + AP_PARAM_FLAG_ENABLE), + +#if AP_UROS_UDP_ENABLED + // @Param: _UDP_PORT + // @DisplayName: UROS UDP port + // @Description: UDP port number for UROS + // @Range: 1 65535 + // @RebootRequired: True + // @User: Standard + AP_GROUPINFO("_PORT", 2, AP_UROS_Client, udp.port, 2019), +#endif AP_GROUPEND }; /* - start the DDS thread + start the UROS thread */ bool AP_UROS_Client::start(void) { @@ -37,13 +747,50 @@ bool AP_UROS_Client::start(void) return true; } - if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_UROS_Client::main_loop, void), - "UROS", - 8192, AP_HAL::Scheduler::PRIORITY_IO, 1)) { - GCS_SEND_TEXT(MAV_SEVERITY_ERROR,"UROS: thread create failed"); + uros_debug("UROS: creating uros_thread..."); + +#if CONFIG_HAL_BOARD == HAL_BOARD_ESP32 + BaseType_t rc = xTaskCreatePinnedToCore( + &AP_UROS_Client::main_loop_trampoline, + "APM_UROS", + 16384, + this, + 10, //UROS_PRIO, + &uros_task_handle, 0); + + if (rc != pdPASS) { + uros_error("UROS: uros_thread failed to start"); + return false; + } else { + uros_info("UROS: uros_thread started"); + return true; + } +#else + if (!hal.scheduler->thread_create( + FUNCTOR_BIND_MEMBER(&AP_UROS_Client::main_loop, void), + "APM_UROS", + 16384, + AP_HAL::Scheduler::PRIORITY_IO, + 1)) { + uros_error("UROS: thread create failed"); return false; } return true; +#endif +} + +/* + trampoline for main loop for UROS thread + */ +void AP_UROS_Client::main_loop_trampoline(void *arg) { + AP_UROS_Client* uros = (AP_UROS_Client*)arg; + uros->main_loop(); + + // if main_loop returns something has gone wrong + uros_error("UROS: main_thread failed"); + while (true) { + hal.scheduler->delay(1000); + } } /* @@ -51,40 +798,555 @@ bool AP_UROS_Client::start(void) */ void AP_UROS_Client::main_loop(void) { + uros_debug("UROS: started main loop"); + if (!init() || !create()) { - GCS_SEND_TEXT(MAV_SEVERITY_ERROR,"UROS: creation failed"); + uros_error("UROS: init or create failed"); return; } - GCS_SEND_TEXT(MAV_SEVERITY_INFO,"UROS: initialization passed"); + uros_info("UROS: init and create succeed"); - while (true) { - hal.scheduler->delay(1); - update(); - } + // one-time actions + + // periodic actions + rclc_executor_spin(&executor); + +#if AP_UROS_PARAM_SRV_ENABLED + UROS_SOFTCHECK(rclc_parameter_server_fini(¶m_server, &node)); +#endif + + UROS_SOFTCHECK(rcl_service_fini(&arm_motors_service, &node)); + + UROS_SOFTCHECK(rcl_subscription_fini(&rx_joy_subscriber, &node)); + UROS_SOFTCHECK(rcl_subscription_fini(&rx_dynamic_transforms_subscriber, &node)); + + UROS_SOFTCHECK(rcl_publisher_fini(&battery_state_publisher, &node)); + UROS_SOFTCHECK(rcl_publisher_fini(&clock_publisher, &node)); + UROS_SOFTCHECK(rcl_publisher_fini(&geo_pose_publisher, &node)); + UROS_SOFTCHECK(rcl_publisher_fini(&local_pose_publisher, &node)); + UROS_SOFTCHECK(rcl_publisher_fini(&local_twist_publisher, &node)); + UROS_SOFTCHECK(rcl_publisher_fini(&nav_sat_fix_publisher, &node)); + UROS_SOFTCHECK(rcl_publisher_fini(&tx_static_transforms_publisher, &node)); + UROS_SOFTCHECK(rcl_publisher_fini(&time_publisher, &node)); + + UROS_SOFTCHECK(rcl_node_fini(&node)); } bool AP_UROS_Client::init() { - GCS_SEND_TEXT(MAV_SEVERITY_INFO,"UROS: init complete"); + WITH_SEMAPHORE(csem); + + // initialize transport + bool initTransportStatus = false; + +// #if defined(RMW_UXRCE_TRANSPORT_CUSTOM) + // serial init will fail if the SERIALn_PROTOCOL is not setup + if (!initTransportStatus) { + initTransportStatus = urosSerialInit(); + } +// #else +// #error micro-ROS transports misconfigured +// #endif + +#if AP_UROS_UDP_ENABLED + // fallback to UDP if available + if (!initTransportStatus) { + initTransportStatus = urosUdpInit(); + } +#endif + + if (initTransportStatus) { + uros_info("UROS: transport initializated"); + } + else { + uros_error("UROS: transport initialization failed"); + return false; + } + + // create allocator + allocator = rcl_get_default_allocator(); + + // either: custom initialisation + // uros_debug("UROS: create init options"); + // rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + // UROS_CHECK(rcl_init_options_init(&init_options, allocator), "create init options"); + + //! @todo add conditional check if using UDP + // uros_debug("UROS: set rmw init options"); + // rmw_init_options_t* rmw_options = rcl_init_options_get_rmw_init_options(&init_options); + // UROS_CHECK(rmw_uros_options_set_udp_address("192.168.1.31", "2019", rmw_options), + // "set rmw init options"); + + // create init_options + // uros_debug("UROS: initialise support"); + // UROS_CHECK(rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator), + // "initialise support"); + + // or: default initialisation + // create init_options + // uros_debug("UROS: initialise support"); + UROS_CHECK(rclc_support_init(&support, 0, NULL, &allocator), + "initialise support"); + + // create node + // uros_debug("UROS: initialise node"); + UROS_CHECK(rclc_node_init_default(&node, "ardupilot_uros", "", &support), + "initialise node"); + + uros_info("UROS: init complete"); return true; } bool AP_UROS_Client::create() { + uros_debug("UROS: create entities"); + WITH_SEMAPHORE(csem); - GCS_SEND_TEXT(MAV_SEVERITY_INFO,"UROS: create complete"); + // initialise strings and sequences + { + battery_state_conf.max_string_capacity = 32; + battery_state_conf.max_ros2_type_sequence_capacity = AP_BATT_MONITOR_CELLS_MAX; + battery_state_conf.max_basic_type_sequence_capacity = AP_BATT_MONITOR_CELLS_MAX; + + battery_state_mem_init = micro_ros_utilities_create_message_memory( + ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, BatteryState), + &battery_state_msg, + battery_state_conf + ); + if (!battery_state_mem_init) { + uros_error("UROS: configure battery state msg... FAILED"); + return false; + } + uros_debug("UROS: configure battery state msg... OK"); + } + { + geo_pose_conf.max_string_capacity = 32; + geo_pose_conf.max_ros2_type_sequence_capacity = 2; + geo_pose_conf.max_basic_type_sequence_capacity = 2; + + geo_pose_mem_init = micro_ros_utilities_create_message_memory( + ROSIDL_GET_MSG_TYPE_SUPPORT(geographic_msgs, msg, GeoPoseStamped), + &geo_pose_msg, + geo_pose_conf + ); + if (!geo_pose_mem_init) { + uros_error("UROS: configure geo pose msg... FAILED"); + return false; + } + uros_debug("UROS: configure geo pose msg... OK"); + } + { + local_pose_conf.max_string_capacity = 32; + local_pose_conf.max_ros2_type_sequence_capacity = 2; + local_pose_conf.max_basic_type_sequence_capacity = 2; + + local_pose_mem_init = micro_ros_utilities_create_message_memory( + ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, PoseStamped), + &local_pose_msg, + local_pose_conf + ); + if (!local_pose_mem_init) { + uros_error("UROS: configure local pose msg... FAILED"); + return false; + } + uros_debug("UROS: configure local pose msg... OK"); + } + { + local_twist_conf.max_string_capacity = 32; + local_twist_conf.max_ros2_type_sequence_capacity = 2; + local_twist_conf.max_basic_type_sequence_capacity = 2; + + local_twist_mem_init = micro_ros_utilities_create_message_memory( + ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, TwistStamped), + &local_twist_msg, + local_twist_conf + ); + if (!local_twist_mem_init) { + uros_error("UROS: configure local twist msg... FAILED"); + return false; + } + uros_debug("UROS: configure local twist msg... OK"); + } + { + nav_sat_fix_conf.max_string_capacity = 32; + nav_sat_fix_conf.max_ros2_type_sequence_capacity = 2; + nav_sat_fix_conf.max_basic_type_sequence_capacity = 2; + + nav_sat_fix_mem_init = micro_ros_utilities_create_message_memory( + ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, NavSatFix), + &nav_sat_fix_msg, + nav_sat_fix_conf + ); + if (!nav_sat_fix_mem_init) { + uros_error("UROS: configure nav sat fix msg... FAILED"); + return false; + } + uros_debug("UROS: configure nav sat fix msg... OK"); + } + { + tx_static_transforms_conf.max_string_capacity = 32; + tx_static_transforms_conf.max_ros2_type_sequence_capacity = GPS_MAX_RECEIVERS; + tx_static_transforms_conf.max_basic_type_sequence_capacity = GPS_MAX_RECEIVERS; + + tx_static_transforms_mem_init = micro_ros_utilities_create_message_memory( + ROSIDL_GET_MSG_TYPE_SUPPORT(tf2_msgs, msg, TFMessage), + &tx_static_transforms_msg, + tx_static_transforms_conf + ); + if (!nav_sat_fix_mem_init) { + uros_error("UROS: configure static transform msg... FAILED"); + return false; + } + uros_debug("UROS: configure static transform msg... OK"); + } + { + rx_joy_conf.max_string_capacity = 32; + rx_joy_conf.max_ros2_type_sequence_capacity = 32; + rx_joy_conf.max_basic_type_sequence_capacity = 32; + + rx_joy_mem_init = micro_ros_utilities_create_message_memory( + ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Joy), + &rx_joy_msg, + rx_joy_conf + ); + if (!rx_joy_mem_init) { + uros_error("UROS: configure joy msg... FAILED"); + return false; + } + uros_debug("UROS: configure joy msg... OK"); + } + { + rx_velocity_control_conf.max_string_capacity = 32; + rx_velocity_control_conf.max_ros2_type_sequence_capacity = 32; + rx_velocity_control_conf.max_basic_type_sequence_capacity = 2; + + rx_velocity_control_mem_init = micro_ros_utilities_create_message_memory( + ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, TwistStamped), + &rx_velocity_control_msg, + rx_velocity_control_conf + ); + if (!rx_velocity_control_mem_init) { + uros_error("UROS: configure velocity control msg... FAILED"); + return false; + } + uros_debug("UROS: configure velocity control msg... OK"); + } + { + rx_dynamic_transforms_conf.max_string_capacity = 32; + rx_dynamic_transforms_conf.max_ros2_type_sequence_capacity = 16; + rx_dynamic_transforms_conf.max_basic_type_sequence_capacity = 16; + + rx_dynamic_transforms_mem_init = micro_ros_utilities_create_message_memory( + ROSIDL_GET_MSG_TYPE_SUPPORT(tf2_msgs, msg, TFMessage), + &rx_dynamic_transforms_msg, + rx_dynamic_transforms_conf + ); + if (!rx_dynamic_transforms_mem_init) { + uros_error("UROS: configure tf msg... FAILED"); + return false; + } + uros_debug("UROS: configure tf msg... OK"); + } + + // create publishers + uint8_t number_of_publishers = 0; + + if (battery_state_mem_init) { + rcl_ret_t rc = rclc_publisher_init_default( + &battery_state_publisher, + &node, + ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, BatteryState), + "ap/battery/battery0"); + if (!(battery_state_pub_init = (rc == RCL_RET_OK))) { + uros_error("UROS: create battery state publisher... FAILED (%d)", (int16_t)rc); + return false; + } + number_of_publishers++; + uros_debug("UROS: create battery state publisher... OK"); + } + + { + rcl_ret_t rc = rclc_publisher_init_default( + &clock_publisher, + &node, + ROSIDL_GET_MSG_TYPE_SUPPORT(rosgraph_msgs, msg, Clock), + "ap/clock"); + if (!(clock_pub_init = (rc == RCL_RET_OK))) { + uros_error("UROS: create clock publisher... FAILED (%d)", (int16_t)rc); + return false; + } + number_of_publishers++; + uros_debug("UROS: create clock publisher... OK"); + } + + if (geo_pose_mem_init) { + rcl_ret_t rc = rclc_publisher_init_default( + &geo_pose_publisher, + &node, + ROSIDL_GET_MSG_TYPE_SUPPORT(geographic_msgs, msg, GeoPoseStamped), + "ap/geopose/filtered"); + if (!(geo_pose_pub_init = (rc == RCL_RET_OK))) { + uros_error("UROS: create geo pose publisher... FAILED (%d)",(int16_t)rc); + return false; + } + number_of_publishers++; + uros_debug("UROS: create geo pose publisher... OK"); + } + + if (local_pose_mem_init) { + rcl_ret_t rc = rclc_publisher_init_default( + &local_pose_publisher, + &node, + ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, PoseStamped), + "ap/pose/filtered"); + if (!(local_pose_pub_init = (rc == RCL_RET_OK))) { + uros_error("UROS: create local pose publisher... FAILED (%d)", (int16_t)rc); + return false; + } + number_of_publishers++; + uros_debug("UROS: create local pose publisher... OK"); + } + + if (local_twist_mem_init) { + rcl_ret_t rc = rclc_publisher_init_default( + &local_twist_publisher, + &node, + ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, TwistStamped), + "ap/twist/filtered"); + if (!(local_twist_pub_init = (rc == RCL_RET_OK))) { + uros_error("UROS: create local twist publisher... FAILED (%d)", (int16_t)rc); + return false; + } + number_of_publishers++; + uros_debug("UROS: create local twist publisher... OK"); + } + + if (nav_sat_fix_mem_init) { + rcl_ret_t rc = rclc_publisher_init_default( + &nav_sat_fix_publisher, + &node, + ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, NavSatFix), + "ap/navsat/navsat0"); + if (!(nav_sat_fix_pub_init = (rc == RCL_RET_OK))) { + uros_error("UROS: create nav sat fix publisher... FAILED (%d)", (int16_t)rc); + return false; + } + number_of_publishers++; + uros_debug("UROS: create nav sat fix publisher... OK"); + } + + if (tx_static_transforms_mem_init) { + rcl_ret_t rc = rclc_publisher_init_default( + &tx_static_transforms_publisher, + &node, + ROSIDL_GET_MSG_TYPE_SUPPORT(tf2_msgs, msg, TFMessage), + "ap/tf_static"); + if (!(tx_static_transforms_pub_init = (rc == RCL_RET_OK))) { + uros_error("UROS: create static transform publisher... FAILED (%d)", (int16_t)rc); + return false; + } + number_of_publishers++; + uros_debug("UROS: create static transform publisher... OK"); + } + + { + rcl_ret_t rc = rclc_publisher_init_default( + &time_publisher, + &node, + ROSIDL_GET_MSG_TYPE_SUPPORT(builtin_interfaces, msg, Time), + "ap/time"); + if (!(time_pub_init = (rc == RCL_RET_OK))) { + uros_error("UROS: create time publisher... FAILED (%d)", (int16_t)rc); + return false; + } + number_of_publishers++; + uros_debug("UROS: create time publisher... OK"); + } + + // create subscribers + uint8_t number_of_subscribers = 0; + + if (rx_joy_mem_init) { + rcl_ret_t rc = rclc_subscription_init_default( + &rx_joy_subscriber, + &node, + ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Joy), + "ap/joy"); + if (!(rx_joy_sub_init = (rc == RCL_RET_OK))) { + uros_error("UROS: create joy subscriber... FAILED (%d)", (int16_t)rc); + return false; + } + number_of_subscribers++; + uros_debug("UROS: create joy subscriber... OK"); + } + + if (rx_velocity_control_mem_init) { + rcl_ret_t rc = rclc_subscription_init_default( + &rx_velocity_control_subscriber, + &node, + ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, TwistStamped), + "ap/cmd_vel"); + if (!(rx_velocity_control_sub_init = (rc == RCL_RET_OK))) { + uros_error("UROS: create velocity control subscriber... FAILED (%d)", (int16_t)rc); + return false; + } + number_of_subscribers++; + uros_debug("UROS: create velocity control subscriber... OK"); + } + + if (rx_dynamic_transforms_mem_init) { + rcl_ret_t rc = rclc_subscription_init_default( + &rx_dynamic_transforms_subscriber, + &node, + ROSIDL_GET_MSG_TYPE_SUPPORT(tf2_msgs, msg, TFMessage), + "ap/tf"); + if (!(rx_dynamic_transforms_sub_init = (rc == RCL_RET_OK))) { + uros_error("UROS: create tf subscriber... FAILED (%d)", (int16_t)rc); + return false; + } + number_of_subscribers++; + uros_debug("UROS: create tf subscriber... OK"); + } + + // create services + uint8_t number_of_services = 0; + + { + rcl_ret_t rc = rclc_service_init_default( + &arm_motors_service, + &node, + ROSIDL_GET_SRV_TYPE_SUPPORT(ardupilot_msgs, srv, ArmMotors), + "/ap/arm_motors"); + if (!(arm_motors_srv_init = (rc == RCL_RET_OK))) { + uros_error("UROS: create arm_motors service... FAILED (%d)", (int16_t)rc); + return false; + } + number_of_services++; + uros_debug("UROS: create arm_motors service... OK"); + } + + { + rcl_ret_t rc = rclc_service_init_default( + &mode_switch_service, + &node, + ROSIDL_GET_SRV_TYPE_SUPPORT(ardupilot_msgs, srv, ModeSwitch), + "/ap/mode_switch"); + if (!(mode_switch_srv_init = (rc == RCL_RET_OK))) { + uros_error("UROS: create mode_switch service... FAILED (%d)", (int16_t)rc); + return false; + } + number_of_services++; + uros_debug("UROS: create mode_switch service... OK"); + } + +#if AP_UROS_PARAM_SRV_ENABLED + // create parameter server + { + hal.console->printf("UROS: create parameter server... "); + + // create parameter options + // defaults: + // notify_changed_over_dds: true + // max_params: 4 + // allow_undeclared_parameters: false + // low_mem_mode: false + const rclc_parameter_options_t options = { + .notify_changed_over_dds = true, + .max_params = 4, + .allow_undeclared_parameters = true, + .low_mem_mode = true + }; + + rcl_ret_t rc = rclc_parameter_server_init_with_option( + ¶m_server, &node, &options); + if ((param_srv_init = (rc == RCL_RET_OK))) { + //! @note the parameter server requires 5 services + 1 publisher + // https://micro.ros.org/docs/tutorials/programming_rcl_rclc/parameters/ + number_of_publishers++; + number_of_services += RCLC_EXECUTOR_PARAMETER_SERVER_HANDLES; + hal.console->printf("OK\n"); + } else { + hal.console->printf("FAIL: %d\n", (int16_t)rc); + } + } +#endif + + // create timer + // uros_debug("UROS: create timer"); + UROS_CHECK(rclc_timer_init_default( + &_timer, + &support, + RCL_MS_TO_NS(timer_timeout_ms), + timer_callback_trampoline), + "create timer"); + + // number of entities + uros_debug("UROS: number of publishers: %d", number_of_publishers); + uros_debug("UROS: number of subscribers: %d", number_of_subscribers); + uros_debug("UROS: number of services: %d", number_of_services); + uint8_t number_of_handles = + number_of_publishers + number_of_subscribers + number_of_services; + + // create executor + // uros_debug("UROS: initialise executor"); + executor = rclc_executor_get_zero_initialized_executor(); + UROS_CHECK(rclc_executor_init(&executor, &support.context, + number_of_handles, &allocator), + "initialise executor"); + + // uros_debug("UROS: add timer to executor"); + UROS_CHECK(rclc_executor_add_timer(&executor, &_timer), + "add timer to executor"); + + // uros_debug("UROS: add subscriptions to executor"); + if (rx_joy_sub_init) { + UROS_CHECK(rclc_executor_add_subscription_with_context(&executor, &rx_joy_subscriber, + &rx_joy_msg, &AP_UROS_Client::on_joy_msg_trampoline, this, ON_NEW_DATA), + "add subscription joy to executor"); + } + if (rx_velocity_control_sub_init) { + UROS_CHECK(rclc_executor_add_subscription_with_context(&executor, &rx_velocity_control_subscriber, + &rx_velocity_control_msg, &AP_UROS_Client::on_velocity_control_msg_trampoline, this, ON_NEW_DATA), + "add subscription velocity control to executor"); + } + if (rx_dynamic_transforms_sub_init) { + UROS_CHECK(rclc_executor_add_subscription_with_context(&executor, &rx_dynamic_transforms_subscriber, + &rx_dynamic_transforms_msg, &AP_UROS_Client::on_tf_msg_trampoline, this, ON_NEW_DATA), + "add subscription dynamic transforms to executor"); + } + + // uros_debug("UROS: add services to executor"); + if (arm_motors_srv_init) { + UROS_CHECK(rclc_executor_add_service_with_context(&executor, &arm_motors_service, + &arm_motors_req, &arm_motors_res, + &AP_UROS_Client::arm_motors_callback_trampoline, this), + "add service arm motors to executor"); + } + if (mode_switch_srv_init) { + UROS_CHECK(rclc_executor_add_service_with_context(&executor, &mode_switch_service, + &mode_switch_req, &mode_switch_res, + &AP_UROS_Client::mode_switch_callback_trampoline, this), + "add service mode switch to executor"); + } + +#if AP_UROS_PARAM_SRV_ENABLED + // uros_debug("UROS: add parameter server to executor"); + if (param_srv_init) { + UROS_CHECK(rclc_executor_add_parameter_server_with_context(&executor, ¶m_server, + &AP_UROS_Client::on_parameter_changed_trampoline, this), + "add parameter server to executor"); + } +#endif + + uros_debug("UROS: create complete"); return true; } -void AP_UROS_Client::update() -{ - WITH_SEMAPHORE(csem); -} +#if CONFIG_HAL_BOARD != HAL_BOARD_SITL && CONFIG_HAL_BOARD != HAL_BOARD_ESP32 -#if CONFIG_HAL_BOARD != HAL_BOARD_SITL extern "C" { int clock_gettime(clockid_t clockid, struct timespec *ts); } @@ -101,6 +1363,29 @@ int clock_gettime(clockid_t clockid, struct timespec *ts) ts->tv_nsec = (utc_usec % 1000000ULL) * 1000UL; return 0; } + + +// Placeholder for fprintf() usage in: +// +// rosidl_runtime_c/src/string_functions.c +// rosidl_runtime_c__String__fini(rosidl_runtime_c__String * str) +// +// rosidl_runtime_c/src/u16string_functions.c +// rosidl_runtime_c__U16String__fini(rosidl_runtime_c__U16String * str) +// +#include + +extern "C" { + size_t __wrap_fwrite(const void *ptr, size_t size, size_t nmemb, FILE *stream); +} + +size_t __wrap_fwrite(const void *ptr, size_t size, size_t nmemb, FILE *stream) +{ + // return dll_fwrite(ptr, size, nmemb, stream); + return 0; +} + + #endif // CONFIG_HAL_BOARD #endif // AP_UROS_ENABLED diff --git a/libraries/AP_UROS/AP_UROS_Client.h b/libraries/AP_UROS/AP_UROS_Client.h index 71e949e85b0bb..88a47c69d41b2 100644 --- a/libraries/AP_UROS/AP_UROS_Client.h +++ b/libraries/AP_UROS/AP_UROS_Client.h @@ -2,6 +2,32 @@ #if AP_UROS_ENABLED +// micro-xrce-dds +#include +#include + +// micro-ROS +#include +#include +#include +#include +#include +#include + +// ROS msgs +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + #include #include #include @@ -10,6 +36,11 @@ #include "fcntl.h" #include +#include "AP_UROS_config.h" + +#if AP_UROS_UDP_ENABLED +#include +#endif extern const AP_HAL::HAL& hal; @@ -21,7 +52,199 @@ class AP_UROS_Client HAL_Semaphore csem; + // micro-ROS + rcl_allocator_t allocator; + rclc_support_t support; + rcl_node_t node; + rclc_executor_t executor; + rcl_timer_t _timer; + const unsigned int timer_timeout_ms = 1; + + // publishers + rcl_publisher_t battery_state_publisher; + sensor_msgs__msg__BatteryState battery_state_msg; + uint64_t last_battery_state_time_ms; + micro_ros_utilities_memory_conf_t battery_state_conf; + bool battery_state_mem_init = false; + bool battery_state_pub_init = false; + + rcl_publisher_t clock_publisher; + rosgraph_msgs__msg__Clock clock_msg; + uint64_t last_clock_time_ms; + bool clock_pub_init = false; + + rcl_publisher_t geo_pose_publisher; + geographic_msgs__msg__GeoPoseStamped geo_pose_msg; + uint64_t last_geo_pose_time_ms; + micro_ros_utilities_memory_conf_t geo_pose_conf; + bool geo_pose_mem_init = false; + bool geo_pose_pub_init = false; + + rcl_publisher_t local_pose_publisher; + geometry_msgs__msg__PoseStamped local_pose_msg; + uint64_t last_local_pose_time_ms; + micro_ros_utilities_memory_conf_t local_pose_conf; + bool local_pose_mem_init = false; + bool local_pose_pub_init = false; + + rcl_publisher_t local_twist_publisher; + geometry_msgs__msg__TwistStamped local_twist_msg; + uint64_t last_local_twist_time_ms; + micro_ros_utilities_memory_conf_t local_twist_conf; + bool local_twist_mem_init = false; + bool local_twist_pub_init = false; + + rcl_publisher_t nav_sat_fix_publisher; + sensor_msgs__msg__NavSatFix nav_sat_fix_msg; + uint64_t last_nav_sat_fix_time_ms; + micro_ros_utilities_memory_conf_t nav_sat_fix_conf; + bool nav_sat_fix_mem_init = false; + bool nav_sat_fix_pub_init = false; + + rcl_publisher_t time_publisher; + builtin_interfaces__msg__Time time_msg; + uint64_t last_time_time_ms; + bool time_pub_init = false; + + // outgoing transforms + rcl_publisher_t tx_static_transforms_publisher; + tf2_msgs__msg__TFMessage tx_static_transforms_msg; + uint64_t last_tx_static_transforms_time_ms; + micro_ros_utilities_memory_conf_t tx_static_transforms_conf; + bool tx_static_transforms_mem_init = false; + bool tx_static_transforms_pub_init = false; + + // subscribers + rcl_subscription_t rx_joy_subscriber; + sensor_msgs__msg__Joy rx_joy_msg; + micro_ros_utilities_memory_conf_t rx_joy_conf; + bool rx_joy_mem_init = false; + bool rx_joy_sub_init = false; + + // velocity control (ROS REP 147) + rcl_subscription_t rx_velocity_control_subscriber; + geometry_msgs__msg__TwistStamped rx_velocity_control_msg; + micro_ros_utilities_memory_conf_t rx_velocity_control_conf; + bool rx_velocity_control_mem_init = false; + bool rx_velocity_control_sub_init = false; + + // incoming transforms + rcl_subscription_t rx_dynamic_transforms_subscriber; + tf2_msgs__msg__TFMessage rx_dynamic_transforms_msg; + micro_ros_utilities_memory_conf_t rx_dynamic_transforms_conf; + bool rx_dynamic_transforms_mem_init = false; + bool rx_dynamic_transforms_sub_init = false; + + // services + rcl_service_t arm_motors_service; + ardupilot_msgs__srv__ArmMotors_Request arm_motors_req; + ardupilot_msgs__srv__ArmMotors_Response arm_motors_res; + bool arm_motors_srv_init = false; + + rcl_service_t mode_switch_service; + ardupilot_msgs__srv__ModeSwitch_Request mode_switch_req; + ardupilot_msgs__srv__ModeSwitch_Response mode_switch_res; + bool mode_switch_srv_init = false; + +#if AP_UROS_PARAM_SRV_ENABLED + // parameter server + rclc_parameter_server_t param_server; + bool param_srv_init = false; +#endif + +#if CONFIG_HAL_BOARD == HAL_BOARD_ESP32 + // thread handle + TaskHandle_t uros_task_handle; +#endif + + // singleton + static AP_UROS_Client *_singleton; + + // publishers + void update_topic(sensor_msgs__msg__BatteryState& msg); + void update_topic(rosgraph_msgs__msg__Clock& msg); + void update_topic(geographic_msgs__msg__GeoPoseStamped& msg); + void update_topic(geometry_msgs__msg__PoseStamped& msg); + void update_topic(geometry_msgs__msg__TwistStamped& msg); + bool update_topic(sensor_msgs__msg__NavSatFix& msg); + void update_topic(builtin_interfaces__msg__Time& msg); + void update_topic(tf2_msgs__msg__TFMessage& msg); + + // subscribers + static void on_joy_msg_trampoline(const void * msgin, void *context); + void on_joy_msg(const sensor_msgs__msg__Joy * msg); + + static void on_velocity_control_msg_trampoline(const void * msgin, void *context); + void on_velocity_control_msg(const geometry_msgs__msg__TwistStamped * msg); + + static void on_tf_msg_trampoline(const void * msgin, void *context); + void on_tf_msg(const tf2_msgs__msg__TFMessage * msg); + + // service callbacks + static void arm_motors_callback_trampoline(const void *req, void *res, void *context); + void arm_motors_callback( + const ardupilot_msgs__srv__ArmMotors_Request *req, + ardupilot_msgs__srv__ArmMotors_Response *res); + + static void mode_switch_callback_trampoline(const void *req, void *res, void *context); + void mode_switch_callback( + const ardupilot_msgs__srv__ModeSwitch_Request *req, + ardupilot_msgs__srv__ModeSwitch_Response *res); + +#if AP_UROS_PARAM_SRV_ENABLED + // parameter server callback + static bool on_parameter_changed_trampoline( + const Parameter * old_param, const Parameter * new_param, void * context); + bool on_parameter_changed( + const Parameter * old_param, const Parameter * new_param); +#endif + + // timer callbacks + static void timer_callback_trampoline(rcl_timer_t * timer, int64_t last_call_time); + void timer_callback(rcl_timer_t * timer, int64_t last_call_time); + + static void main_loop_trampoline(void *arg); + + // functions for serial transport + bool urosSerialInit(); + static bool serial_transport_open(uxrCustomTransport* transport); + static bool serial_transport_close(uxrCustomTransport* transport); + static size_t serial_transport_write(uxrCustomTransport* transport, + const uint8_t* buf, size_t len, uint8_t* error); + static size_t serial_transport_read(uxrCustomTransport* transport, + uint8_t* buf, size_t len, int timeout, uint8_t* error); + + struct { + AP_HAL::UARTDriver *port; + uxrCustomTransport transport; + } serial; + +#if AP_UROS_UDP_ENABLED + // functions for udp transport + bool urosUdpInit(); + static bool udp_transport_open(uxrCustomTransport* transport); + static bool udp_transport_close(uxrCustomTransport* transport); + static size_t udp_transport_write(uxrCustomTransport* transport, + const uint8_t* buf, size_t len, uint8_t* error); + static size_t udp_transport_read(uxrCustomTransport* transport, + uint8_t* buf, size_t len, int timeout, uint8_t* error); + + struct { + AP_Int32 port; + // UDP endpoint + const char* ip = "127.0.0.1"; + // UDP Allocation + uxrCustomTransport transport; + SocketAPM *socket; + } udp; +#endif + + // client key we present + static constexpr uint32_t uniqueClientKey = 0xAAAABBBB; + public: + AP_UROS_Client(); + bool start(void); void main_loop(void); @@ -33,11 +256,13 @@ class AP_UROS_Client //! @return True on successful creation, false on failure bool create() WARN_IF_UNUSED; - //! @brief Update the client. - void update(); - //! @brief Parameter storage. static const struct AP_Param::GroupInfo var_info[]; + + //! @note A singleton is needed for the timer_callback_trampoline as + //! rcl and rlcl do not provide a function to initialise a timer with + //! a context as is case for the subscriber callbacks. + static AP_UROS_Client *get_singleton(); }; #endif // AP_UROS_ENABLED diff --git a/libraries/AP_UROS/AP_UROS_ExternalControl.cpp b/libraries/AP_UROS/AP_UROS_ExternalControl.cpp new file mode 100644 index 0000000000000..0e777a287ef05 --- /dev/null +++ b/libraries/AP_UROS/AP_UROS_ExternalControl.cpp @@ -0,0 +1,43 @@ +#if AP_UROS_ENABLED + +#include "AP_UROS_ExternalControl.h" +#include "AP_UROS_Frames.h" +#include + +#include + +bool AP_UROS_External_Control::handle_velocity_control(const geometry_msgs__msg__TwistStamped& cmd_vel) +{ + auto *external_control = AP::externalcontrol(); + if (external_control == nullptr) { + return false; + } + + if (strcmp(cmd_vel.header.frame_id.data, BASE_LINK_FRAME_ID) == 0) { + // Convert commands from body frame (x-forward, y-left, z-up) to NED. + Vector3f linear_velocity; + Vector3f linear_velocity_base_link { + float(cmd_vel.twist.linear.x), + float(cmd_vel.twist.linear.y), + float(-cmd_vel.twist.linear.z) }; + const float yaw_rate = -cmd_vel.twist.angular.z; + + auto &ahrs = AP::ahrs(); + linear_velocity = ahrs.body_to_earth(linear_velocity_base_link); + return external_control->set_linear_velocity_and_yaw_rate(linear_velocity, yaw_rate); + } + + else if (strcmp(cmd_vel.header.frame_id.data, MAP_FRAME) == 0) { + // Convert commands from ENU to NED frame + Vector3f linear_velocity { + float(cmd_vel.twist.linear.y), + float(cmd_vel.twist.linear.x), + float(-cmd_vel.twist.linear.z) }; + const float yaw_rate = -cmd_vel.twist.angular.z; + return external_control->set_linear_velocity_and_yaw_rate(linear_velocity, yaw_rate); + } + return false; +} + + +#endif // AP_UROS_ENABLED \ No newline at end of file diff --git a/libraries/AP_UROS/AP_UROS_ExternalControl.h b/libraries/AP_UROS/AP_UROS_ExternalControl.h new file mode 100644 index 0000000000000..23e958a3fb513 --- /dev/null +++ b/libraries/AP_UROS/AP_UROS_ExternalControl.h @@ -0,0 +1,11 @@ +#pragma once + +#if AP_UROS_ENABLED +#include + +class AP_UROS_External_Control +{ +public: + static bool handle_velocity_control(const geometry_msgs__msg__TwistStamped& cmd_vel); +}; +#endif // AP_UROS_ENABLED diff --git a/libraries/AP_UROS/AP_UROS_External_Odom.cpp b/libraries/AP_UROS/AP_UROS_External_Odom.cpp new file mode 100644 index 0000000000000..b31007a3e0768 --- /dev/null +++ b/libraries/AP_UROS/AP_UROS_External_Odom.cpp @@ -0,0 +1,74 @@ +#include "AP_UROS_External_Odom.h" +#include "AP_UROS_Type_Conversions.h" + +#if AP_UROS_VISUALODOM_ENABLED + +#include +#include + +void AP_UROS_External_Odom::handle_external_odom(const tf2_msgs__msg__TFMessage& msg) +{ + auto *visual_odom = AP::visualodom(); + if (visual_odom == nullptr) { + return; + } + + for (size_t i = 0; i < msg.transforms.size; i++) { + const auto& ros_transform_stamped = msg.transforms.data[i]; + if (!is_odometry_frame(ros_transform_stamped)) { + continue; + } + const uint64_t remote_time_us {AP_UROS_Type_Conversions::time_u64_micros(ros_transform_stamped.header.stamp)}; + + Vector3f ap_position; + Quaternion ap_rotation; + + convert_transform(ros_transform_stamped.transform, ap_position, ap_rotation); + // Although ROS convention states quaternions in ROS messages should be normalized, it's not guaranteed. + // Before propogating a potentially inaccurate quaternion to the rest of AP, normalize it here. + // TODO what if the quaternion is NaN? + ap_rotation.normalize(); + + // No error is available in TF, trust the data as-is + const float posErr {0.0}; + const float angErr {0.0}; + // The odom to base_link transform used is locally consistent per ROS REP-105. + // https://www.ros.org/reps/rep-0105.html#id16 + // Thus, there will not be any resets. + const uint8_t reset_counter {0}; + // TODO imlement jitter correction similar to GCS_MAVLINK::correct_offboard_timestamp_usec_to_ms(remote_time_us, sizeof(msg)); + const uint32_t time_ms {static_cast(remote_time_us * 1E-3)}; + visual_odom->handle_pose_estimate(remote_time_us, time_ms, ap_position.x, ap_position.y, ap_position.z, ap_rotation, posErr, angErr, reset_counter); + + } +} + +bool AP_UROS_External_Odom::is_odometry_frame(const geometry_msgs__msg__TransformStamped& msg) +{ + char odom_parent[] = "odom"; + char odom_child[] = "base_link"; + // Assume the frame ID's are null terminated. + return (strcmp(msg.header.frame_id.data, odom_parent) == 0) && + (strcmp(msg.child_frame_id.data, odom_child) == 0); +} + +void AP_UROS_External_Odom::convert_transform(const geometry_msgs__msg__Transform& ros_transform, Vector3f& translation, Quaternion& rotation) +{ + // convert from x-forward, y-left, z-up to NED + // https://github.com/mavlink/mavros/issues/49#issuecomment-51614130 + translation = { + static_cast(ros_transform.translation.x), + static_cast(-ros_transform.translation.y), + static_cast(-ros_transform.translation.z) + }; + + // In AP, q1 is the quaternion's scalar component. + // In ROS, w is the quaternion's scalar component. + // https://docs.ros.org/en/humble/Tutorials/Intermediate/Tf2/Quaternion-Fundamentals.html#components-of-a-quaternion + rotation.q1 = ros_transform.rotation.w; + rotation.q2 = ros_transform.rotation.x; + rotation.q3 = -ros_transform.rotation.y; + rotation.q4 = -ros_transform.rotation.z; +} + +#endif // AP_UROS_VISUALODOM_ENABLED diff --git a/libraries/AP_UROS/AP_UROS_External_Odom.h b/libraries/AP_UROS/AP_UROS_External_Odom.h new file mode 100644 index 0000000000000..fed3c2cff8b6e --- /dev/null +++ b/libraries/AP_UROS/AP_UROS_External_Odom.h @@ -0,0 +1,34 @@ +// Class for handling external localization data. +// For historical reasons, it's called odometry to match AP_VisualOdom. + +#pragma once + +#include "AP_UROS_config.h" +#if AP_UROS_VISUALODOM_ENABLED + +#include +#include + +#include +#include + +class AP_UROS_External_Odom +{ +public: + + // Handler for external position localization + static void handle_external_odom(const tf2_msgs__msg__TFMessage& msg); + + // Checks the child and parent frames match a set needed for external odom. + // Since multiple different transforms can be sent, this validates the specific transform is + // for odometry. + static bool is_odometry_frame(const geometry_msgs__msg__TransformStamped& msg); + + // Helper to convert from ROS transform to AP datatypes + // ros_transform is in ENU + // translation is in NED + static void convert_transform(const geometry_msgs__msg__Transform& ros_transform, Vector3f& translation, Quaternion& rotation); + +}; + +#endif // AP_UROS_VISUALODOM_ENABLED diff --git a/libraries/AP_UROS/AP_UROS_Frames.h b/libraries/AP_UROS/AP_UROS_Frames.h new file mode 100644 index 0000000000000..0bdaf354ec116 --- /dev/null +++ b/libraries/AP_UROS/AP_UROS_Frames.h @@ -0,0 +1,7 @@ +#pragma once + +static constexpr char WGS_84_FRAME_ID[] = "WGS-84"; +// https://www.ros.org/reps/rep-0105.html#base-link +static constexpr char BASE_LINK_FRAME_ID[] = "base_link"; +// https://www.ros.org/reps/rep-0105.html#map +static constexpr char MAP_FRAME[] = "map"; diff --git a/libraries/AP_UROS/AP_UROS_Serial.cpp b/libraries/AP_UROS/AP_UROS_Serial.cpp new file mode 100644 index 0000000000000..d95ae28bddf90 --- /dev/null +++ b/libraries/AP_UROS/AP_UROS_Serial.cpp @@ -0,0 +1,108 @@ +#include "AP_UROS_Client.h" + +#include + +#include + +#include + +/* + open connection on a serial port + */ +bool AP_UROS_Client::serial_transport_open(uxrCustomTransport *t) +{ + AP_UROS_Client *uros = (AP_UROS_Client *)t->args; + AP_SerialManager *serial_manager = AP_SerialManager::get_singleton(); + AP_HAL::UARTDriver *uros_port = serial_manager->find_serial(AP_SerialManager::SerialProtocol_DDS_XRCE, 0); + if (uros_port == nullptr) { + hal.console->printf("UROS: serial transport open... FAIL\n"); + return false; + } + + // ensure we own the UART + uros_port->begin(0); + uros->serial.port = uros_port; + + return true; +} + +/* + close serial transport + */ +bool AP_UROS_Client::serial_transport_close(uxrCustomTransport *t) +{ + // we don't actually close the UART + return true; +} + +/* + write on serial transport + */ +size_t AP_UROS_Client::serial_transport_write(uxrCustomTransport *t, const uint8_t* buf, size_t len, uint8_t* error) +{ + AP_UROS_Client *uros = (AP_UROS_Client *)t->args; + if (uros->serial.port == nullptr) { + *error = EINVAL; + return 0; + } + ssize_t bytes_written = uros->serial.port->write(buf, len); + if (bytes_written <= 0) { + *error = 1; + return 0; + } + //! @todo populate the error code correctly + *error = 0; + return bytes_written; +} + +/* + read from a serial transport + */ +size_t AP_UROS_Client::serial_transport_read(uxrCustomTransport *t, uint8_t* buf, size_t len, int timeout_ms, uint8_t* error) +{ + AP_UROS_Client *uros = (AP_UROS_Client *)t->args; + if (uros->serial.port == nullptr) { + *error = EINVAL; + return 0; + } + const uint32_t tstart = AP_HAL::millis(); + while (AP_HAL::millis() - tstart < uint32_t(timeout_ms) && + uros->serial.port->available() < len) { + hal.scheduler->delay_microseconds(100); // TODO select or poll this is limiting speed (100us) + } + ssize_t bytes_read = uros->serial.port->read(buf, len); + if (bytes_read <= 0) { + *error = 1; + return 0; + } + //! @todo Add error reporting + *error = 0; + return bytes_read; +} + +/* + initialise serial connection + */ +bool AP_UROS_Client::urosSerialInit() +{ + // fail transport initialisation if port not configured + AP_SerialManager *serial_manager = AP_SerialManager::get_singleton(); + auto *uros_port = serial_manager->find_serial(AP_SerialManager::SerialProtocol_DDS_XRCE, 0); + if (uros_port == nullptr) { + hal.console->printf("UROS: serial port not configured\n"); + return false; + } + + // setup a framed transport for serial + hal.console->printf("UROS: initialising custom transport\n"); + rmw_ret_t rcl_ret = rmw_uros_set_custom_transport( + true, + (void *) &serial.transport, + serial_transport_open, + serial_transport_close, + serial_transport_write, + serial_transport_read + ); + + return (rcl_ret == RCL_RET_OK); +} diff --git a/libraries/AP_UROS/AP_UROS_Type_Conversions.cpp b/libraries/AP_UROS/AP_UROS_Type_Conversions.cpp new file mode 100644 index 0000000000000..09ca55aa6c023 --- /dev/null +++ b/libraries/AP_UROS/AP_UROS_Type_Conversions.cpp @@ -0,0 +1,13 @@ +#include "AP_UROS_Type_Conversions.h" +#if AP_UROS_ENABLED + +#include + + +uint64_t AP_UROS_Type_Conversions::time_u64_micros(const builtin_interfaces__msg__Time& ros_time) +{ + return (uint64_t(ros_time.sec) * 1000000ULL) + (ros_time.nanosec / 1000ULL); +} + + +#endif // AP_UROS_ENABLED diff --git a/libraries/AP_UROS/AP_UROS_Type_Conversions.h b/libraries/AP_UROS/AP_UROS_Type_Conversions.h new file mode 100644 index 0000000000000..7cec182497181 --- /dev/null +++ b/libraries/AP_UROS/AP_UROS_Type_Conversions.h @@ -0,0 +1,17 @@ +// Class for handling type conversions for UROS. + +#pragma once + +#if AP_UROS_ENABLED + +#include + +class AP_UROS_Type_Conversions +{ +public: + + // Convert ROS time to a uint64_t [μS] + static uint64_t time_u64_micros(const builtin_interfaces__msg__Time& ros_time); +}; + +#endif // AP_UROS_ENABLED diff --git a/libraries/AP_UROS/AP_UROS_UDP.cpp b/libraries/AP_UROS/AP_UROS_UDP.cpp new file mode 100644 index 0000000000000..4ac902a10ec8e --- /dev/null +++ b/libraries/AP_UROS/AP_UROS_UDP.cpp @@ -0,0 +1,92 @@ +#include "AP_UROS_Client.h" + +#if AP_UROS_UDP_ENABLED + +#include + +#include + +/* + open connection on UDP + */ +bool AP_UROS_Client::udp_transport_open(uxrCustomTransport *t) +{ + AP_UROS_Client *uros = (AP_UROS_Client *)t->args; + auto *sock = new SocketAPM(true); + if (sock == nullptr) { + return false; + } + if (!sock->connect(uros->udp.ip, uros->udp.port.get())) { + return false; + } + uros->udp.socket = sock; + return true; +} + +/* + close UDP connection + */ +bool AP_UROS_Client::udp_transport_close(uxrCustomTransport *t) +{ + AP_UROS_Client *uros = (AP_UROS_Client *)t->args; + delete uros->udp.socket; + uros->udp.socket = nullptr; + return true; +} + +/* + write on UDP + */ +size_t AP_UROS_Client::udp_transport_write(uxrCustomTransport *t, + const uint8_t* buf, size_t len, uint8_t* error) +{ + AP_UROS_Client *uros = (AP_UROS_Client *)t->args; + if (uros->udp.socket == nullptr) { + *error = EINVAL; + return 0; + } + const ssize_t ret = uros->udp.socket->send(buf, len); + if (ret <= 0) { + *error = errno; + return 0; + } + return ret; +} + +/* + read from UDP + */ +size_t AP_UROS_Client::udp_transport_read(uxrCustomTransport *t, + uint8_t* buf, size_t len, int timeout_ms, uint8_t* error) +{ + AP_UROS_Client *uros = (AP_UROS_Client *)t->args; + if (uros->udp.socket == nullptr) { + *error = EINVAL; + return 0; + } + const ssize_t ret = uros->udp.socket->recv(buf, len, timeout_ms); + if (ret <= 0) { + *error = errno; + return 0; + } + return ret; +} + +/* + initialise UDP connection + */ +bool AP_UROS_Client::urosUdpInit() +{ + // setup a non-framed transport for UDP + rmw_ret_t rcl_ret = rmw_uros_set_custom_transport( + false, + (void*)this, + udp_transport_open, + udp_transport_close, + udp_transport_write, + udp_transport_read); + + return (rcl_ret == RCL_RET_OK); +} + +#endif // AP_UROS_UDP_ENABLED diff --git a/libraries/AP_UROS/AP_UROS_config.h b/libraries/AP_UROS/AP_UROS_config.h new file mode 100644 index 0000000000000..7b7da3a4afed9 --- /dev/null +++ b/libraries/AP_UROS/AP_UROS_config.h @@ -0,0 +1,33 @@ +#pragma once + +#include + +#ifndef AP_UROS_ENABLED +#define AP_UROS_ENABLED 1 +#endif // AP_UROS_ENABLED + +// UDP only on SITL for now +#ifndef AP_UROS_UDP_ENABLED +#define AP_UROS_UDP_ENABLED AP_UROS_ENABLED && (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif // AP_UROS_UDP_ENABLED + +#include +#ifndef AP_UROS_VISUALODOM_ENABLED +#define AP_UROS_VISUALODOM_ENABLED HAL_VISUALODOM_ENABLED && AP_UROS_ENABLED +#endif // AP_UROS_VISUALODOM_ENABLED + +// Only enable parameter server on SITL or EPS32 +#ifndef AP_UROS_PARAM_SRV_ENABLED +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_ESP32 +#define AP_UROS_PARAM_SRV_ENABLED 1 +#else +#define AP_UROS_PARAM_SRV_ENABLED 0 +#endif +#endif // AP_UROS_PARAM_SRV_ENABLED + +// esp32 - free-rtos +#if CONFIG_HAL_BOARD == HAL_BOARD_ESP32 +#include +#include +#include +#endif diff --git a/libraries/AP_UROS/README.md b/libraries/AP_UROS/README.md new file mode 100644 index 0000000000000..c44409d3c9bc8 --- /dev/null +++ b/libraries/AP_UROS/README.md @@ -0,0 +1,273 @@ +# AP_UROS: micro-ROS client library + +The `AP_UROS` library is an alternative to `AP_DDS` for direct ROS +integration in ArduPilot. It uses the micro-ROS client library rather +than the lower level XRCE-DDS-Client library. + +The library implements the same features as `AP_DDS` and may be used as +drop in replacement. + +## Build + +Build the ArduPilot binaries for SITL: + +```bash +./waf configure --board sitl --enable-uros +./waf build +``` + +Build as part of a `colcon` workspace: + +```bash +colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_CXX_STANDARD=17 --packages-select ardupilot_sitl +``` + +## Usage + +The most convenient method to bringup a simulation session using the `AP_UROS` +library is to launch the runway world available in [`ardupilot_gz`](https://github.com/ArduPilot/ardupilot_gz): + +```bash +ros2 launch ardupilot_gz_bringup iris_runway.launch.py rviz:=true use_gz_tf:=true +``` + +Attach a MAVProxy session and enable `UROS`: + +```bash +mavproxy.py --master udp:127.0.0.1:14550 --console --map +STABILIZE> param set UROS_ENABLE 1 +STABILIZE> param show UROS* +STABILIZE> UROS_ENABLE 1.0 +UROS_PORT 2019.0 +``` + +The console should display various status messages prefixed with `AP: UROS: ` +as the library is initialised. + +### ROS 2 features + +#### Nodes + +```bash +$ ros2 node list +/ardupilot_uros +``` + +```bash +$ ros2 node info /ardupilot_uros +/ardupilot_uros + Subscribers: + /ap/cmd_vel: geometry_msgs/msg/TwistStamped + /ap/joy: sensor_msgs/msg/Joy + /ap/tf: tf2_msgs/msg/TFMessage + Publishers: + /ap/battery/battery0: sensor_msgs/msg/BatteryState + /ap/clock: rosgraph_msgs/msg/Clock + /ap/geopose/filtered: geographic_msgs/msg/GeoPoseStamped + /ap/navsat/navsat0: sensor_msgs/msg/NavSatFix + /ap/pose/filtered: geometry_msgs/msg/PoseStamped + /ap/tf_static: tf2_msgs/msg/TFMessage + /ap/time: builtin_interfaces/msg/Time + /ap/twist/filtered: geometry_msgs/msg/TwistStamped + /parameter_events: rcl_interfaces/msg/ParameterEvent + Service Servers: + /ap/arm_motors: ardupilot_msgs/srv/ArmMotors + /ap/mode_switch: ardupilot_msgs/srv/ModeSwitch + /ardupilot_uros/describe_parameters: rcl_interfaces/srv/DescribeParameters + /ardupilot_uros/get_parameter_types: rcl_interfaces/srv/GetParameterTypes + /ardupilot_uros/get_parameters: rcl_interfaces/srv/GetParameters + /ardupilot_uros/list_parameters: rcl_interfaces/srv/ListParameters + /ardupilot_uros/set_parameters: rcl_interfaces/srv/SetParameters + Service Clients: + + Action Servers: + + Action Clients: + ``` + +#### Topics + +List topics published by `ardupilot_uros`: + +```bash +% ros2 topic list | grep /ap +/ap/battery/battery0 +/ap/clock +/ap/cmd_vel +/ap/geopose/filtered +/ap/joy +/ap/navsat/navsat0 +/ap/pose/filtered +/ap/tf +/ap/tf_static +/ap/time +/ap/twist/filtered +``` + +Subscribe to a filtered pose: + +```bash +% ros2 topic echo /ap/pose/filtered --once +header: + stamp: + sec: 1696260581 + nanosec: 150658000 + frame_id: base_link +pose: + position: + x: -0.009078041650354862 + y: 0.011131884530186653 + z: 0.10999999940395355 + orientation: + x: -0.0008264112402684987 + y: 6.37705234112218e-05 + z: 0.7135183215141296 + w: 0.700636088848114 +--- +``` + +Publish a `Joy` message: + +```bash +$ ros2 topic pub /ap/joy sensor_msgs/msg/Joy "{axes: [1, 1, 1, 1]}" --once +publisher: beginning loop +publishing #1: sensor_msgs.msg.Joy(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id=''), axes=[1.0, 1.0, 1.0, 1.0], buttons=[]) +``` + +The MAVProxy console should show: + +```console +AP: UROS: sensor_msgs/Joy: 1.000000, 1.000000, 1.000000, 1.000000 +``` + +#### Services + +```bash +$ ros2 service list | grep /ap +/ap/arm_motors +/ap/mode_switch +``` + +Switch to `GUIDED` mode (`mode=4`): + +```bash +$ ros2 service call /ap/mode_switch ardupilot_msgs/srv/ModeSwitch "{mode: 4}" +requester: making request: ardupilot_msgs.srv.ModeSwitch_Request(mode=4) + +response: +ardupilot_msgs.srv.ModeSwitch_Response(status=True, curr_mode=4) +``` + +```console +Mode GUIDED +AP: UROS: Request for Mode Switch : SUCCESS +``` + +Arm motors: + +```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) +``` + +```console +AP: UROS: Request for arming received +AP: Arming motors +AP: UROS: Request for Arming/Disarming : SUCCESS +ARMED +``` + +#### Parameters + +The parameter server is proof of concept only and is not integrated +with ArduPilot's parameter system. + +Create: + +```bash +ros2 param set /ardupilot_uros GPS_TYPE 11 +``` + +Dump: + +```bash +ros2 param dump /ardupilot_uros +/ardupilot_uros: + ros__parameters: + GPS_TYPE: 11 +``` + +Get: + +```bash +$ ros2 param get /ardupilot_uros GPS_TYPE +Integer value is: 11 +``` + +Set: + +```bash +$ ros2 param get /ardupilot_uros GPS_TYPE +Integer value is: 11 +``` + +```bash +$ ros2 param set /ardupilot_uros GPS_TYPE 1 +Set parameter successful + +```bash +$ ros2 param get /ardupilot_uros GPS_TYPE +Integer value is: 1 +``` + + +## Appendix A: ESP32 + +Notes for building the micro-ROS client library using the package +`micro_ros_espidf_component`. + +### Config + +- Update `app-colcon.meta` to allow more publishers and subscribers. + - 1 node + - 16 pub/sub/service/client +- Update `libmicroros.mk` to add message and service interface packages. + - Added `ardupilot` for `ardupilot_msgs` + - Added `geographic_info` for `geographic_msgs` + - Added `geometry2` for `tf2_msgs` + - Added `ardupilot` for `ardupilot_msgs` + +### Build (standalone example) + +This refers to a standalone example used to test the micro-ROS client build. + +```bash +cd ./firmware/toolchain +. ./esp-idf/export.sh +``` + +```bash +cd ./firmware/esp32_examples/ardupilot_uros +idf.py set-target esp32 +idf.py build +idf.py -p /dev/cu.usbserial-0001 flash +``` + +### Run + +```bash +cd ./firmware/esp32_examples/ardupilot_uros +idf.py monitor +``` + +```bash +cd ./firmware/esp32_examples/ardupilot_uros +idf.py monitor +``` + +```bash +ros2 run micro_ros_agent micro_ros_agent udp4 --port 2019 -v6 +``` \ No newline at end of file diff --git a/libraries/AP_UROS/wscript b/libraries/AP_UROS/wscript index 53f330064c2ae..6237224a26a52 100644 --- a/libraries/AP_UROS/wscript +++ b/libraries/AP_UROS/wscript @@ -1,97 +1,78 @@ #!/usr/bin/env python3 - import pathlib +import platform def configure(cfg): - extra_src = [ - # 'modules/Micro-XRCE-DDS-Client/src/c/core/session/stream/*.c', - # 'modules/Micro-XRCE-DDS-Client/src/c/core/session/*.c', - # 'modules/Micro-XRCE-DDS-Client/src/c/core/serialization/*.c', - # 'modules/Micro-XRCE-DDS-Client/src/c/util/*.c', - # 'modules/Micro-XRCE-DDS-Client/src/c/profile/transport/custom/custom_transport.c', - # 'modules/Micro-XRCE-DDS-Client/src/c/profile/transport/stream_framing/stream_framing_protocol.c', - # 'modules/Micro-CDR/src/c/types/*.c', - # 'modules/Micro-CDR/src/c/common.c', - ] - - cfg.env.AP_LIB_EXTRA_SOURCES['AP_UROS'] = [] - for pattern in extra_src: - s = cfg.srcnode.ant_glob(pattern, dir=False, src=True) - for x in s: - cfg.env.AP_LIB_EXTRA_SOURCES['AP_UROS'].append(str(x)) + host_system = platform.system() + host_processor = platform.processor() + host_machine = platform.machine() + host_release = platform.release() # kernel version + toolchain = cfg.env.TOOLCHAIN + + print('host_system: {}'.format(host_system)) + print('host_processor: {}'.format(host_processor)) + print('host_machine: {}'.format(host_machine)) + print('host_release: {}'.format(host_release)) + print('toolchain: {}'.format(toolchain)) + + distro = None + if toolchain == 'native': + if host_system == 'Darwin': + if host_machine == 'amd64': + distro = 'uros-macos-13.4-amd64' + elif host_machine == 'arm64': + distro = 'uros-macos-13.4-arm64' + elif host_system == 'Linux': + if host_machine == 'amd64': + distro = 'uros-ubuntu-22-amd64' + elif host_machine == 'arm64': + distro = 'uros-ubuntu-22-arm64' + else: + print('Cannot configure AP_UROS. Unsupported host: {}'.format(host_system)) + return + elif toolchain == 'arm-none-eabi': + distro = 'uros-stm32-m7-custom' + elif toolchain == 'xtensa-esp32-elf': + distro = 'uros-esp-idf-4.4-custom' + else: + print('Cannot configure AP_UROS. Unsupported toolchain: {}'.format(toolchain)) + return + + uros_include_dir = str(pathlib.PurePath('modules', 'ardupilot_uros', distro, 'uros', 'include')) + uros_library_dir = str(pathlib.PurePath('modules', 'ardupilot_uros', distro, 'uros', 'lib')) + cfg.env.UROS_INCLUDE_DIR = uros_include_dir + cfg.env.UROS_LIBRARY_DIR = uros_library_dir extra_src_inc = [ - # 'modules/Micro-XRCE-DDS-Client/include', - # 'modules/Micro-XRCE-DDS-Client/include/uxr/client', - # 'modules/Micro-CDR/src/c', - # 'modules/Micro-CDR/include', - # 'modules/Micro-CDR/include/ucdr', + f'{uros_include_dir}', + f'{uros_include_dir}/rcl', + f'{uros_include_dir}/rcl_action', + f'{uros_include_dir}/rcl_interfaces', + f'{uros_include_dir}/rcutils', + f'{uros_include_dir}/rmw', + f'{uros_include_dir}/rosidl_runtime_c', + f'{uros_include_dir}/rosidl_typesupport_interface', + f'{uros_include_dir}/rosidl_typesupport_introspection_c', + f'{uros_include_dir}/action_msgs', + f'{uros_include_dir}/ardupilot_msgs', + f'{uros_include_dir}/builtin_interfaces', + f'{uros_include_dir}/geographic_msgs', + f'{uros_include_dir}/geometry_msgs', + f'{uros_include_dir}/rosgraph_msgs', + f'{uros_include_dir}/sensor_msgs', + f'{uros_include_dir}/std_msgs', + f'{uros_include_dir}/tf2_msgs', + f'{uros_include_dir}/unique_identifier_msgs', ] for inc in extra_src_inc: cfg.env.INCLUDES += [str(cfg.srcnode.make_node(inc))] # auto update submodules - # cfg.env.append_value('GIT_SUBMODULES', 'Micro-XRCE-DDS-Client') - # cfg.env.append_value('GIT_SUBMODULES', 'Micro-CDR') + cfg.env.append_value('GIT_SUBMODULES', 'ardupilot_uros') def build(bld): - config_h_in = [ - # 'modules/Micro-XRCE-DDS-Client/include/uxr/client/config.h.in', - # 'modules/Micro-CDR/include/ucdr/config.h.in', - ] - # config_h_in_nodes = [bld.srcnode.make_node(h) for h in config_h_in] - # config_h_nodes = [bld.bldnode.find_or_declare(h[:-3]) for h in config_h_in] - - # gen_path = "libraries/AP_UROS" - extra_bld_inc = [ - # 'modules/Micro-CDR/include', - # 'modules/Micro-XRCE-DDS-Client/include', - # str(pathlib.PurePath(gen_path, "generated")), - ] - for inc in extra_bld_inc: - bld.env.INCLUDES += [bld.bldnode.find_or_declare(inc).abspath()] - - # for i in range(len(config_h_nodes)): - # print(f"building {config_h_nodes[i].abspath()}") - # bld( - # # build config.h file - # source=config_h_in_nodes[i], - # target=config_h_nodes[i], - # rule="%s %s/%s %s %s" - # % ( - # bld.env.get_flat('PYTHON'), - # bld.env.SRCROOT, - # "libraries/AP_UROS/gen_config_h.py", - # config_h_in_nodes[i].abspath(), - # config_h_nodes[i].abspath(), - # ), - # group='dynamic_sources', - # ) - - # TODO instead of keeping standard IDL files in the source tree, copy them with "ros2 pkg prefix --share " tools - # idl_files = bld.srcnode.ant_glob('libraries/AP_UROS/Idl/**/*.idl') - # idl_include_path = bld.srcnode.make_node(str(pathlib.PurePath(gen_path, "Idl"))).abspath() - - # for idl in idl_files: - # idl_path = pathlib.PurePath(idl.abspath()) - # b = idl_path.name - - # gen_h = idl_path.with_suffix('.h').name - # gen_c = idl_path.with_suffix('.c').name - - # idl_folder = idl_path.parts[-3:-1] - # dst_dir = pathlib.PurePath(gen_path, "generated", *idl_folder) - # gen = [bld.bldnode.find_or_declare(str(dst_dir / name)) for name in [gen_h, gen_c]] - # gen_cmd = f"{bld.env.MICROXRCEDDSGEN[0]} -cs -replace -d {dst_dir} -I {idl_include_path} {idl}" - # bld( - # # build IDL file - # source=idl, - # target=gen, - # rule=gen_cmd, - # group='dynamic_sources', - # ) - - # bld.env.AP_LIB_EXTRA_SOURCES['AP_UROS'] += [str(gen[1])] + bld.env.LIB += ['microros'] + bld.env.LIBPATH += [str(pathlib.PurePath(bld.env.SRCROOT, bld.env.UROS_LIBRARY_DIR))] From 7014529b3826c4039cdff3a84b33aa9dab69498a Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Wed, 4 Oct 2023 15:43:32 +0100 Subject: [PATCH 08/16] modules: update ardupilot_uros for macOS x86_64 Signed-off-by: Rhys Mainwaring --- modules/ardupilot_uros | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/ardupilot_uros b/modules/ardupilot_uros index fa20883f1a671..7cc26e86f1169 160000 --- a/modules/ardupilot_uros +++ b/modules/ardupilot_uros @@ -1 +1 @@ -Subproject commit fa20883f1a671d415b5b62dcd2dd5cb5661c55f2 +Subproject commit 7cc26e86f11691e5419e373fc1038946ba456b5e From db83a5ef84f68263d1844de1ff8fb0f2caca07c7 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Wed, 4 Oct 2023 15:44:16 +0100 Subject: [PATCH 09/16] AP_UROS: update wscript for macOS x86_64 Signed-off-by: Rhys Mainwaring --- libraries/AP_UROS/wscript | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/libraries/AP_UROS/wscript b/libraries/AP_UROS/wscript index 6237224a26a52..fe85c6083d489 100644 --- a/libraries/AP_UROS/wscript +++ b/libraries/AP_UROS/wscript @@ -17,11 +17,15 @@ def configure(cfg): print('host_release: {}'.format(host_release)) print('toolchain: {}'.format(toolchain)) + # suggestion for alternative package names + # - uros-darwin-22.6.0-x86_64 + # - uros-darwin-22.5.0-arm64 + distro = None if toolchain == 'native': if host_system == 'Darwin': - if host_machine == 'amd64': - distro = 'uros-macos-13.4-amd64' + if host_machine == 'x86_64': + distro = 'uros-macos-13.4-x86_64' elif host_machine == 'arm64': distro = 'uros-macos-13.4-arm64' elif host_system == 'Linux': From 1700b24cced202d00c5ccdf9923a1291a2c4138e Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Wed, 4 Oct 2023 18:21:56 +0100 Subject: [PATCH 10/16] modules: update ardupilot_uros for Ubuntu x86_64 Signed-off-by: Rhys Mainwaring --- modules/ardupilot_uros | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/ardupilot_uros b/modules/ardupilot_uros index 7cc26e86f1169..8518b56453ebf 160000 --- a/modules/ardupilot_uros +++ b/modules/ardupilot_uros @@ -1 +1 @@ -Subproject commit 7cc26e86f11691e5419e373fc1038946ba456b5e +Subproject commit 8518b56453ebfd237dc782718eb985bf1ad2f980 From 19feee6b28677867f4914fa6b2b142b9827fbf8b Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Wed, 4 Oct 2023 23:44:58 +0100 Subject: [PATCH 11/16] modules: update ardupilot_uros for Ubuntu aarch64 Signed-off-by: Rhys Mainwaring --- modules/ardupilot_uros | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/ardupilot_uros b/modules/ardupilot_uros index 8518b56453ebf..a898c6a836147 160000 --- a/modules/ardupilot_uros +++ b/modules/ardupilot_uros @@ -1 +1 @@ -Subproject commit 8518b56453ebfd237dc782718eb985bf1ad2f980 +Subproject commit a898c6a836147afbc1400d4029119f70f561d9df From 1eaf64b1c85c145b1eece1d220e833fab9975f84 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Wed, 4 Oct 2023 23:46:04 +0100 Subject: [PATCH 12/16] AP_UROS: update wscript for ubuntu x86_64 and aarch64 Signed-off-by: Rhys Mainwaring --- libraries/AP_UROS/wscript | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_UROS/wscript b/libraries/AP_UROS/wscript index fe85c6083d489..fb1362bffe701 100644 --- a/libraries/AP_UROS/wscript +++ b/libraries/AP_UROS/wscript @@ -29,10 +29,10 @@ def configure(cfg): elif host_machine == 'arm64': distro = 'uros-macos-13.4-arm64' elif host_system == 'Linux': - if host_machine == 'amd64': - distro = 'uros-ubuntu-22-amd64' - elif host_machine == 'arm64': - distro = 'uros-ubuntu-22-arm64' + if host_machine == 'x86_64': + distro = 'uros-ubuntu-22-x86_64' + elif host_machine == 'aarch64': + distro = 'uros-ubuntu-22-aarch64' else: print('Cannot configure AP_UROS. Unsupported host: {}'.format(host_system)) return From eb1652d514671bcce12e0ed2437def6fab7e4123 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Thu, 5 Oct 2023 10:04:39 +0100 Subject: [PATCH 13/16] AP_UROS: update string formatting Signed-off-by: Rhys Mainwaring --- libraries/AP_UROS/AP_UROS_Client.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/AP_UROS/AP_UROS_Client.cpp b/libraries/AP_UROS/AP_UROS_Client.cpp index c2abc01841639..637293eb3cfa1 100644 --- a/libraries/AP_UROS/AP_UROS_Client.cpp +++ b/libraries/AP_UROS/AP_UROS_Client.cpp @@ -70,8 +70,8 @@ __LINE__, (int)temp_rc);\ /* will be empty when RCUTILS_AVOID_DYNAMIC_ALLOCATION is defined */\ const rcutils_error_state_t *err = rcutils_get_error_state();\ - uros_error("UROS: error: %s, file: %s, line: %llu, ",\ - err->message, err->file, err->line_number);\ + uros_error("UROS: error: %s, file: %s, line: %u, ",\ + err->message, err->file, (uint32_t)err->line_number);\ return false;\ } else {\ uros_debug("UROS: " msg"... OK");\ @@ -688,10 +688,10 @@ bool AP_UROS_Client::on_parameter_changed( case RCLC_PARAMETER_INT: uros_info( "UROS: parameter %s modified: " - "old value: %lld, new value: %lld (int)", + "old value: %d, new value: %d (int)", old_param->name.data, - old_param->value.integer_value, - new_param->value.integer_value); + (int32_t)old_param->value.integer_value, + (int32_t)new_param->value.integer_value); break; case RCLC_PARAMETER_DOUBLE: uros_info( From e501872b8b4d41f056aed15ea6f48574b85ab339 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Tue, 3 Oct 2023 13:10:00 +0100 Subject: [PATCH 14/16] AP_ROS: factor out common code from AP_DDS and AP_UROS Signed-off-by: Rhys Mainwaring AP_ROS: add space after template keyword Signed-off-by: Rhys Mainwaring AP_ROS: add accessor templates for strings and a sequence of transforms Signed-off-by: Rhys Mainwaring AP_ROS: use accessor templates for strings in external control Signed-off-by: Rhys Mainwaring AP_ROS: add mutable versions of accessor templates Signed-off-by: Rhys Mainwaring AP_ROS: consolidate code for populating published messages Signed-off-by: Rhys Mainwaring AP_ROS: consolidate code for publishing nav sat fix Signed-off-by: Rhys Mainwaring AP_ROS: rename mutable transforms Signed-off-by: Rhys Mainwaring AP_ROS: use mutable transforms template for static transforms Signed-off-by: Rhys Mainwaring AP_ROS: fix formatting Signed-off-by: Rhys Mainwaring AP_ROS: add accessor templates for battery state Signed-off-by: Rhys Mainwaring AP_ROS: consolidate code for publishing battery state Signed-off-by: Rhys Mainwaring --- libraries/AP_ROS/AP_ROS_Client.h | 393 ++++++++++++++++++++++ libraries/AP_ROS/AP_ROS_ExternalControl.h | 49 +++ libraries/AP_ROS/AP_ROS_ExternalOdom.h | 97 ++++++ libraries/AP_ROS/AP_ROS_Frames.h | 7 + libraries/AP_ROS/AP_ROS_TypeConversions.h | 68 ++++ 5 files changed, 614 insertions(+) create mode 100644 libraries/AP_ROS/AP_ROS_Client.h create mode 100644 libraries/AP_ROS/AP_ROS_ExternalControl.h create mode 100644 libraries/AP_ROS/AP_ROS_ExternalOdom.h create mode 100644 libraries/AP_ROS/AP_ROS_Frames.h create mode 100644 libraries/AP_ROS/AP_ROS_TypeConversions.h diff --git a/libraries/AP_ROS/AP_ROS_Client.h b/libraries/AP_ROS/AP_ROS_Client.h new file mode 100644 index 0000000000000..192eea9d2b3df --- /dev/null +++ b/libraries/AP_ROS/AP_ROS_Client.h @@ -0,0 +1,393 @@ +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include "AP_ROS_Frames.h" +#include "AP_ROS_TypeConversions.h" + + +class AP_ROS_Client +{ +public: + + template + static void update_battery_state(BatteryState& msg, + const uint8_t instance); + + template + static void update_clock(Clock& msg); + + template + static void update_geopose_stamped(GeoPoseStamped& msg); + + template + static void update_pose_stamped(PoseStamped& msg); + + template + static void update_twist_stamped(TwistStamped& msg); + + template + static void update_static_transforms(TFMessage& msg); + + template + static bool update_nav_sat_fix(NavSatFix& msg, + const uint8_t instance, uint64_t& last_nav_sat_fix_time_ms); + + template + static void update_time(Time& msg); + +}; + +template +void AP_ROS_Client::update_battery_state(BatteryState& msg, + const uint8_t instance) +{ + if (instance >= AP_BATT_MONITOR_MAX_INSTANCES) { + return; + } + + update_time(msg.header.stamp); + auto &battery = AP::battery(); + + if (!battery.healthy(instance)) { + msg.power_supply_status = 3; //POWER_SUPPLY_HEALTH_DEAD + msg.present = false; + return; + } + msg.present = true; + + msg.voltage = battery.voltage(instance); + + float temperature; + msg.temperature = (battery.get_temperature(temperature, instance)) ? temperature : NAN; + + float current; + msg.current = (battery.current_amps(current, instance)) ? -1 * current : NAN; + + const float design_capacity = (float)battery.pack_capacity_mah(instance) * 0.001; + msg.design_capacity = design_capacity; + + uint8_t percentage; + if (battery.capacity_remaining_pct(percentage, instance)) { + msg.percentage = percentage * 0.01; + msg.charge = (percentage * design_capacity) * 0.01; + } else { + msg.percentage = NAN; + msg.charge = NAN; + } + + msg.capacity = NAN; + + if (battery.current_amps(current, instance)) { + if (percentage == 100) { + msg.power_supply_status = 4; //POWER_SUPPLY_STATUS_FULL + } else if (current < 0.0) { + msg.power_supply_status = 1; //POWER_SUPPLY_STATUS_CHARGING + } else if (current > 0.0) { + msg.power_supply_status = 2; //POWER_SUPPLY_STATUS_DISCHARGING + } else { + msg.power_supply_status = 3; //POWER_SUPPLY_STATUS_NOT_CHARGING + } + } else { + msg.power_supply_status = 0; //POWER_SUPPLY_STATUS_UNKNOWN + } + + msg.power_supply_health = (battery.overpower_detected(instance)) ? 4 : 1; //POWER_SUPPLY_HEALTH_OVERVOLTAGE or POWER_SUPPLY_HEALTH_GOOD + + 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, + mutable_cell_voltage_data(msg)); + } +} + +template +void AP_ROS_Client::update_clock(Clock& msg) +{ + update_time(msg.clock); +} + +template +void AP_ROS_Client::update_geopose_stamped(GeoPoseStamped& msg) +{ + update_time(msg.header.stamp); + strcpy(mutable_string_data(msg.header.frame_id), BASE_LINK_FRAME_ID); + + auto &ahrs = AP::ahrs(); + WITH_SEMAPHORE(ahrs.get_semaphore()); + + Location loc; + if (ahrs.get_location(loc)) { + msg.pose.position.latitude = loc.lat * 1E-7; + msg.pose.position.longitude = loc.lng * 1E-7; + msg.pose.position.altitude = loc.alt * 0.01; // Transform from cm to m + } + + // In ROS REP 103, axis orientation uses the following convention: + // X - Forward + // Y - Left + // Z - Up + // https://www.ros.org/reps/rep-0103.html#axis-orientation + // As a consequence, to follow ROS REP 103, it is necessary to switch X and Y, + // as well as invert Z (NED to ENU convertion) as well as a 90 degree rotation in the Z axis + // for x to point forward + Quaternion orientation; + if (ahrs.get_quaternion(orientation)) { + Quaternion aux(orientation[0], orientation[2], orientation[1], -orientation[3]); //NED to ENU transformation + Quaternion transformation(sqrtF(2) * 0.5, 0, 0, sqrtF(2) * 0.5); // Z axis 90 degree rotation + orientation = aux * transformation; + msg.pose.orientation.w = orientation[0]; + msg.pose.orientation.x = orientation[1]; + msg.pose.orientation.y = orientation[2]; + msg.pose.orientation.z = orientation[3]; + } +} + +template +void AP_ROS_Client::update_pose_stamped(PoseStamped& msg) +{ + update_time(msg.header.stamp); + strcpy(mutable_string_data(msg.header.frame_id), BASE_LINK_FRAME_ID); + + auto &ahrs = AP::ahrs(); + WITH_SEMAPHORE(ahrs.get_semaphore()); + + // ROS REP 103 uses the ENU convention: + // X - East + // Y - North + // Z - Up + // https://www.ros.org/reps/rep-0103.html#axis-orientation + // AP_AHRS uses the NED convention + // X - North + // Y - East + // Z - Down + // As a consequence, to follow ROS REP 103, it is necessary to switch X and Y, + // as well as invert Z + + Vector3f position; + if (ahrs.get_relative_position_NED_home(position)) { + msg.pose.position.x = position[1]; + msg.pose.position.y = position[0]; + msg.pose.position.z = -position[2]; + } + + // In ROS REP 103, axis orientation uses the following convention: + // X - Forward + // Y - Left + // Z - Up + // https://www.ros.org/reps/rep-0103.html#axis-orientation + // As a consequence, to follow ROS REP 103, it is necessary to switch X and Y, + // as well as invert Z (NED to ENU convertion) as well as a 90 degree rotation in the Z axis + // for x to point forward + Quaternion orientation; + if (ahrs.get_quaternion(orientation)) { + Quaternion aux(orientation[0], orientation[2], orientation[1], -orientation[3]); //NED to ENU transformation + Quaternion transformation (sqrtF(2) * 0.5,0,0,sqrtF(2) * 0.5); // Z axis 90 degree rotation + orientation = aux * transformation; + msg.pose.orientation.w = orientation[0]; + msg.pose.orientation.x = orientation[1]; + msg.pose.orientation.y = orientation[2]; + msg.pose.orientation.z = orientation[3]; + } +} + +template +void AP_ROS_Client::update_twist_stamped(TwistStamped& msg) +{ + update_time(msg.header.stamp); + strcpy(mutable_string_data(msg.header.frame_id), BASE_LINK_FRAME_ID); + + auto &ahrs = AP::ahrs(); + WITH_SEMAPHORE(ahrs.get_semaphore()); + + // ROS REP 103 uses the ENU convention: + // X - East + // Y - North + // Z - Up + // https://www.ros.org/reps/rep-0103.html#axis-orientation + // AP_AHRS uses the NED convention + // X - North + // Y - East + // Z - Down + // As a consequence, to follow ROS REP 103, it is necessary to switch X and Y, + // as well as invert Z + Vector3f velocity; + if (ahrs.get_velocity_NED(velocity)) { + msg.twist.linear.x = velocity[1]; + msg.twist.linear.y = velocity[0]; + msg.twist.linear.z = -velocity[2]; + } + + // In ROS REP 103, axis orientation uses the following convention: + // X - Forward + // Y - Left + // Z - Up + // https://www.ros.org/reps/rep-0103.html#axis-orientation + // The gyro data is received from AP_AHRS in body-frame + // X - Forward + // Y - Right + // Z - Down + // As a consequence, to follow ROS REP 103, it is necessary to invert Y and Z + Vector3f angular_velocity = ahrs.get_gyro(); + msg.twist.angular.x = angular_velocity[0]; + msg.twist.angular.y = -angular_velocity[1]; + msg.twist.angular.z = -angular_velocity[2]; +} + +template +void AP_ROS_Client::update_static_transforms(TFMessage& msg) +{ + mutable_transforms_size(msg) = 0; + + auto &gps = AP::gps(); + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + const auto gps_type = gps.get_type(i); + if (gps_type == AP_GPS::GPS_Type::GPS_TYPE_NONE) { + continue; + } + auto* transforms = mutable_transforms_data(msg); + update_time(transforms[i].header.stamp); + char gps_frame_id[16]; + //! @todo should GPS frame ID's be 0 or 1 indexed in ROS? + hal.util->snprintf(gps_frame_id, sizeof(gps_frame_id), "GPS_%u", i); + strcpy(mutable_string_data(transforms[i].header.frame_id), BASE_LINK_FRAME_ID); + strcpy(mutable_string_data(transforms[i].child_frame_id), gps_frame_id); + // The body-frame offsets + // X - Forward + // Y - Right + // Z - Down + // https://ardupilot.org/copter/docs/common-sensor-offset-compensation.html#sensor-position-offset-compensation + + const auto offset = gps.get_antenna_offset(i); + + // In ROS REP 103, it follows this convention + // X - Forward + // Y - Left + // Z - Up + // https://www.ros.org/reps/rep-0103.html#axis-orientation + + transforms[i].transform.translation.x = offset[0]; + transforms[i].transform.translation.y = -1 * offset[1]; + transforms[i].transform.translation.z = -1 * offset[2]; + + mutable_transforms_size(msg)++; + } +} + +template +bool AP_ROS_Client::update_nav_sat_fix(NavSatFix& msg, + const uint8_t instance, uint64_t& last_nav_sat_fix_time_ms) +{ + // Add a lambda that takes in navsatfix msg and populates the cov + // Make it constexpr if possible + // https://www.fluentcpp.com/2021/12/13/the-evolutions-of-lambdas-in-c14-c17-and-c20/ + // constexpr auto times2 = [] (sensor_msgs_msg_NavSatFix* msg) { return n * 2; }; + + // assert(instance >= GPS_MAX_RECEIVERS); + if (instance >= GPS_MAX_RECEIVERS) { + return false; + } + + auto &gps = AP::gps(); + WITH_SEMAPHORE(gps.get_semaphore()); + + if (!gps.is_healthy(instance)) { + msg.status.status = -1; // STATUS_NO_FIX + msg.status.service = 0; // No services supported + msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN + return false; + } + + // No update is needed + const auto last_fix_time_ms = gps.last_fix_time_ms(instance); + if (last_nav_sat_fix_time_ms == last_fix_time_ms) { + return false; + } else { + last_nav_sat_fix_time_ms = last_fix_time_ms; + } + + + update_time(msg.header.stamp); + strcpy(mutable_string_data(msg.header.frame_id), WGS_84_FRAME_ID); + msg.status.service = 0; // SERVICE_GPS + msg.status.status = -1; // STATUS_NO_FIX + + + //! @todo What about glonass, compass, galileo? + //! This will be properly designed and implemented to spec in #23277 + msg.status.service = 1; // SERVICE_GPS + + const auto status = gps.status(instance); + switch (status) { + case AP_GPS::NO_GPS: + case AP_GPS::NO_FIX: + msg.status.status = -1; // STATUS_NO_FIX + msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN + return true; + case AP_GPS::GPS_OK_FIX_2D: + case AP_GPS::GPS_OK_FIX_3D: + msg.status.status = 0; // STATUS_FIX + break; + case AP_GPS::GPS_OK_FIX_3D_DGPS: + msg.status.status = 1; // STATUS_SBAS_FIX + break; + case AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT: + case AP_GPS::GPS_OK_FIX_3D_RTK_FIXED: + msg.status.status = 2; // STATUS_SBAS_FIX + break; + default: + //! @todo Can we not just use an enum class and not worry about this condition? + break; + } + const auto loc = gps.location(instance); + msg.latitude = loc.lat * 1E-7; + msg.longitude = loc.lng * 1E-7; + + int32_t alt_cm; + if (!loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_cm)) { + // With absolute frame, this condition is unlikely + msg.status.status = -1; // STATUS_NO_FIX + msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN + return true; + } + msg.altitude = alt_cm * 0.01; + + // ROS allows double precision, ArduPilot exposes float precision today + Matrix3f cov; + msg.position_covariance_type = (uint8_t)gps.position_covariance(instance, cov); + msg.position_covariance[0] = cov[0][0]; + msg.position_covariance[1] = cov[0][1]; + msg.position_covariance[2] = cov[0][2]; + msg.position_covariance[3] = cov[1][0]; + msg.position_covariance[4] = cov[1][1]; + msg.position_covariance[5] = cov[1][2]; + msg.position_covariance[6] = cov[2][0]; + msg.position_covariance[7] = cov[2][1]; + msg.position_covariance[8] = cov[2][2]; + + return true; +} + +template +void AP_ROS_Client::update_time(Time& msg) +{ + uint64_t utc_usec; + if (!AP::rtc().get_utc_usec(utc_usec)) { + utc_usec = AP_HAL::micros64(); + } + msg.sec = utc_usec / 1000000ULL; + msg.nanosec = (utc_usec % 1000000ULL) * 1000UL; +} + diff --git a/libraries/AP_ROS/AP_ROS_ExternalControl.h b/libraries/AP_ROS/AP_ROS_ExternalControl.h new file mode 100644 index 0000000000000..d92deba5cd0f1 --- /dev/null +++ b/libraries/AP_ROS/AP_ROS_ExternalControl.h @@ -0,0 +1,49 @@ +#pragma once + +#include +#include + +#include "AP_ROS_Frames.h" +#include "AP_ROS_TypeConversions.h" + +class AP_ROS_External_Control +{ +public: + template + static bool handle_velocity_control(const TTwistStamped& cmd_vel); + +}; + +template +bool AP_ROS_External_Control::handle_velocity_control(const TTwistStamped& cmd_vel) +{ + auto *external_control = AP::externalcontrol(); + if (external_control == nullptr) { + return false; + } + + if (strcmp(string_data(cmd_vel.header.frame_id), BASE_LINK_FRAME_ID) == 0) { + // Convert commands from body frame (x-forward, y-left, z-up) to NED. + Vector3f linear_velocity; + Vector3f linear_velocity_base_link { + float(cmd_vel.twist.linear.x), + float(cmd_vel.twist.linear.y), + float(-cmd_vel.twist.linear.z) }; + const float yaw_rate = -cmd_vel.twist.angular.z; + + auto &ahrs = AP::ahrs(); + linear_velocity = ahrs.body_to_earth(linear_velocity_base_link); + return external_control->set_linear_velocity_and_yaw_rate(linear_velocity, yaw_rate); + } + + else if (strcmp(string_data(cmd_vel.header.frame_id), MAP_FRAME) == 0) { + // Convert commands from ENU to NED frame + Vector3f linear_velocity { + float(cmd_vel.twist.linear.y), + float(cmd_vel.twist.linear.x), + float(-cmd_vel.twist.linear.z) }; + const float yaw_rate = -cmd_vel.twist.angular.z; + return external_control->set_linear_velocity_and_yaw_rate(linear_velocity, yaw_rate); + } + return false; +} diff --git a/libraries/AP_ROS/AP_ROS_ExternalOdom.h b/libraries/AP_ROS/AP_ROS_ExternalOdom.h new file mode 100644 index 0000000000000..76d3b36bfe4f7 --- /dev/null +++ b/libraries/AP_ROS/AP_ROS_ExternalOdom.h @@ -0,0 +1,97 @@ +#pragma once + +#include +#include +#include +#include + +#include "AP_ROS_TypeConversions.h" + +class AP_ROS_External_Odom +{ +public: + // Handler for external position localization + template + static void handle_external_odom(const TFMessage& msg); + + // Checks the child and parent frames match a set needed for external odom. + // Since multiple different transforms can be sent, this validates the specific transform is + // for odometry. + template + static bool is_odometry_frame(const TransformStamped& msg); + + // Helper to convert from ROS transform to AP datatypes + // ros_transform is in ENU + // translation is in NED + template + static void convert_transform(const Transform& ros_transform, Vector3f& translation, Quaternion& rotation); + +}; + +template +void AP_ROS_External_Odom::handle_external_odom(const TFMessage& msg) +{ + auto *visual_odom = AP::visualodom(); + if (visual_odom == nullptr) { + return; + } + + for (size_t i = 0; i < transforms_size(msg); i++) { + const auto& ros_transform_stamped = transforms_data(msg)[i]; + if (!is_odometry_frame(ros_transform_stamped)) { + continue; + } + const uint64_t remote_time_us {AP_ROS_TypeConversions::time_u64_micros(ros_transform_stamped.header.stamp)}; + + Vector3f ap_position; + Quaternion ap_rotation; + + convert_transform(ros_transform_stamped.transform, ap_position, ap_rotation); + // Although ROS convention states quaternions in ROS messages should be normalized, it's not guaranteed. + // Before propogating a potentially inaccurate quaternion to the rest of AP, normalize it here. + // TODO what if the quaternion is NaN? + ap_rotation.normalize(); + + // No error is available in TF, trust the data as-is + const float posErr {0.0}; + const float angErr {0.0}; + // The odom to base_link transform used is locally consistent per ROS REP-105. + // https://www.ros.org/reps/rep-0105.html#id16 + // Thus, there will not be any resets. + const uint8_t reset_counter {0}; + // TODO imlement jitter correction similar to GCS_MAVLINK::correct_offboard_timestamp_usec_to_ms(remote_time_us, sizeof(msg)); + const uint32_t time_ms {static_cast(remote_time_us * 1E-3)}; + visual_odom->handle_pose_estimate(remote_time_us, time_ms, ap_position.x, ap_position.y, ap_position.z, ap_rotation, posErr, angErr, reset_counter); + + } +} + +template +bool AP_ROS_External_Odom::is_odometry_frame(const TransformStamped& msg) +{ + char odom_parent[] = "odom"; + char odom_child[] = "base_link"; + // Assume the frame ID's are null terminated. + return (strcmp(string_data(msg.header.frame_id), odom_parent) == 0) && + (strcmp(string_data(msg.child_frame_id), odom_child) == 0); +} + +template +void AP_ROS_External_Odom::convert_transform(const Transform& ros_transform, Vector3f& translation, Quaternion& rotation) +{ + // convert from x-forward, y-left, z-up to NED + // https://github.com/mavlink/mavros/issues/49#issuecomment-51614130 + translation = { + static_cast(ros_transform.translation.x), + static_cast(-ros_transform.translation.y), + static_cast(-ros_transform.translation.z) + }; + + // In AP, q1 is the quaternion's scalar component. + // In ROS, w is the quaternion's scalar component. + // https://docs.ros.org/en/humble/Tutorials/Intermediate/Tf2/Quaternion-Fundamentals.html#components-of-a-quaternion + rotation.q1 = ros_transform.rotation.w; + rotation.q2 = ros_transform.rotation.x; + rotation.q3 = -ros_transform.rotation.y; + rotation.q4 = -ros_transform.rotation.z; +} diff --git a/libraries/AP_ROS/AP_ROS_Frames.h b/libraries/AP_ROS/AP_ROS_Frames.h new file mode 100644 index 0000000000000..0bdaf354ec116 --- /dev/null +++ b/libraries/AP_ROS/AP_ROS_Frames.h @@ -0,0 +1,7 @@ +#pragma once + +static constexpr char WGS_84_FRAME_ID[] = "WGS-84"; +// https://www.ros.org/reps/rep-0105.html#base-link +static constexpr char BASE_LINK_FRAME_ID[] = "base_link"; +// https://www.ros.org/reps/rep-0105.html#map +static constexpr char MAP_FRAME[] = "map"; diff --git a/libraries/AP_ROS/AP_ROS_TypeConversions.h b/libraries/AP_ROS/AP_ROS_TypeConversions.h new file mode 100644 index 0000000000000..bab8c9a8d3d37 --- /dev/null +++ b/libraries/AP_ROS/AP_ROS_TypeConversions.h @@ -0,0 +1,68 @@ +// Class for handling type conversions for ROS. + +#pragma once + +#include + +class AP_ROS_TypeConversions +{ +public: + + // Convert ROS time to a uint64_t [μS] + template + static uint64_t time_u64_micros(const TTime& ros_time); +}; + +template +uint64_t AP_ROS_TypeConversions::time_u64_micros(const TTime& ros_time) +{ + return (uint64_t(ros_time.sec) * 1000000ULL) + (ros_time.nanosec / 1000ULL); +} + +// string accessor templates +template +const char* string_data(const S* str); + +template +char* mutable_string_data(S* str); + +template +const char* string_data(const S& str); + +template +char* mutable_string_data(S& str); + +// sequence accessor templates +// see: https://stackoverflow.com/questions/15911890/overriding-return-type-in-function-template-specialization + +template +struct transforms_size_type { typedef uint32_t type; }; + +template +struct mutable_transforms_size_type { typedef uint32_t& type; }; + +template +typename transforms_size_type::type transforms_size(const T& msg); + +template +typename mutable_transforms_size_type::type mutable_transforms_size(T& msg); + +template +struct transforms_type { typedef void* type; }; + +template +struct mutable_transforms_type { typedef void* type; }; + +template +typename transforms_type::type transforms_data(const T& msg); + +template +typename mutable_transforms_type::type mutable_transforms_data(T& msg); + +// battery state : cell_voltage + +template +struct mutable_cell_voltage_type { typedef void* type; }; + +template +typename mutable_cell_voltage_type::type mutable_cell_voltage_data(T& msg); From ff5e6d0463758b749674926f1c6a944d19482f03 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Tue, 3 Oct 2023 13:10:42 +0100 Subject: [PATCH 15/16] AP_UROS: use common code from AP_ROS Signed-off-by: Rhys Mainwaring AP_UROS: add specialisations for string and sequence accessors Signed-off-by: Rhys Mainwaring AP_UROS: adjust indentation Signed-off-by: Rhys Mainwaring AP_UROS: add mutable versions of accessor templates Signed-off-by: Rhys Mainwaring AP_UROS: use common code from AP_ROS for published topics Signed-off-by: Rhys Mainwaring AP_UROS: consolidate code for publishing nav sat fix Signed-off-by: Rhys Mainwaring AP_UROS: delete code moved to AP_ROS Signed-off-by: Rhys Mainwaring AP_UROS: rename mutable transforms Signed-off-by: Rhys Mainwaring AP_UROS: consolidate code for publishing static transforms Signed-off-by: Rhys Mainwaring AP_UROS: move template specialisation definitions to cpp to prevent duplicate symbols Signed-off-by: Rhys Mainwaring AP_UROS: add accessor templates for battery state Signed-off-by: Rhys Mainwaring --- libraries/AP_UROS/AP_UROS_Client.cpp | 330 +----------------- libraries/AP_UROS/AP_UROS_ExternalControl.cpp | 36 +- libraries/AP_UROS/AP_UROS_External_Odom.cpp | 59 +--- .../AP_UROS/AP_UROS_Type_Conversions.cpp | 99 +++++- libraries/AP_UROS/AP_UROS_Type_Conversions.h | 59 ++++ 5 files changed, 173 insertions(+), 410 deletions(-) diff --git a/libraries/AP_UROS/AP_UROS_Client.cpp b/libraries/AP_UROS/AP_UROS_Client.cpp index 637293eb3cfa1..82b5f875560bf 100644 --- a/libraries/AP_UROS/AP_UROS_Client.cpp +++ b/libraries/AP_UROS/AP_UROS_Client.cpp @@ -3,6 +3,7 @@ #if AP_UROS_ENABLED #include "AP_UROS_Client.h" +#include "AP_UROS_Type_Conversions.h" #include #include @@ -25,11 +26,13 @@ #include #include +#include + #include #if AP_EXTERNAL_CONTROL_ENABLED #include "AP_UROS_ExternalControl.h" #endif -#include "AP_UROS_Frames.h" +// #include "AP_UROS_Frames.h" #include "AP_UROS_External_Odom.h" #define UROS_DEBUG 1 @@ -112,355 +115,46 @@ AP_UROS_Client *AP_UROS_Client::get_singleton() { // update published topics -// implementation copied from: -// void AP_DDS_Client::update_topic(sensor_msgs_msg_BatteryState& msg, const uint8_t instance) void AP_UROS_Client::update_topic(sensor_msgs__msg__BatteryState& msg) { const uint8_t instance = 0; - - if (instance >= AP_BATT_MONITOR_MAX_INSTANCES) { - return; - } - - update_topic(msg.header.stamp); - auto &battery = AP::battery(); - - if (!battery.healthy(instance)) { - msg.power_supply_status = 3; //POWER_SUPPLY_HEALTH_DEAD - msg.present = false; - return; - } - msg.present = true; - - msg.voltage = battery.voltage(instance); - - float temperature; - msg.temperature = (battery.get_temperature(temperature, instance)) ? temperature : NAN; - - float current; - msg.current = (battery.current_amps(current, instance)) ? -1 * current : NAN; - - const float design_capacity = (float)battery.pack_capacity_mah(instance) * 0.001; - msg.design_capacity = design_capacity; - - uint8_t percentage; - if (battery.capacity_remaining_pct(percentage, instance)) { - msg.percentage = percentage * 0.01; - msg.charge = (percentage * design_capacity) * 0.01; - } else { - msg.percentage = NAN; - msg.charge = NAN; - } - - msg.capacity = NAN; - - if (battery.current_amps(current, instance)) { - if (percentage == 100) { - msg.power_supply_status = 4; //POWER_SUPPLY_STATUS_FULL - } else if (current < 0.0) { - msg.power_supply_status = 1; //POWER_SUPPLY_STATUS_CHARGING - } else if (current > 0.0) { - msg.power_supply_status = 2; //POWER_SUPPLY_STATUS_DISCHARGING - } else { - msg.power_supply_status = 3; //POWER_SUPPLY_STATUS_NOT_CHARGING - } - } else { - msg.power_supply_status = 0; //POWER_SUPPLY_STATUS_UNKNOWN - } - - msg.power_supply_health = (battery.overpower_detected(instance)) ? 4 : 1; //POWER_SUPPLY_HEALTH_OVERVOLTAGE or POWER_SUPPLY_HEALTH_GOOD - - 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.data); - } + AP_ROS_Client::update_battery_state(msg, instance); } -// implementation copied from: -// void AP_DDS_Client::update_topic(rosgraph_msgs_msg_Clock& msg) void AP_UROS_Client::update_topic(rosgraph_msgs__msg__Clock& msg) { - update_topic(msg.clock); + AP_ROS_Client::update_clock(msg); } -// implementation copied from: -// void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg) void AP_UROS_Client::update_topic(geographic_msgs__msg__GeoPoseStamped& msg) { - update_topic(msg.header.stamp); - strcpy(msg.header.frame_id.data, BASE_LINK_FRAME_ID); - - auto &ahrs = AP::ahrs(); - WITH_SEMAPHORE(ahrs.get_semaphore()); - - Location loc; - if (ahrs.get_location(loc)) { - msg.pose.position.latitude = loc.lat * 1E-7; - msg.pose.position.longitude = loc.lng * 1E-7; - msg.pose.position.altitude = loc.alt * 0.01; // Transform from cm to m - } - - // In ROS REP 103, axis orientation uses the following convention: - // X - Forward - // Y - Left - // Z - Up - // https://www.ros.org/reps/rep-0103.html#axis-orientation - // As a consequence, to follow ROS REP 103, it is necessary to switch X and Y, - // as well as invert Z (NED to ENU convertion) as well as a 90 degree rotation in the Z axis - // for x to point forward - Quaternion orientation; - if (ahrs.get_quaternion(orientation)) { - Quaternion aux(orientation[0], orientation[2], orientation[1], -orientation[3]); //NED to ENU transformation - Quaternion transformation(sqrtF(2) * 0.5, 0, 0, sqrtF(2) * 0.5); // Z axis 90 degree rotation - orientation = aux * transformation; - msg.pose.orientation.w = orientation[0]; - msg.pose.orientation.x = orientation[1]; - msg.pose.orientation.y = orientation[2]; - msg.pose.orientation.z = orientation[3]; - } + AP_ROS_Client::update_geopose_stamped(msg); } -// implementation copied from: -// void AP_DDS_Client::update_topic(geometry_msgs_msg_PoseStamped& msg) void AP_UROS_Client::update_topic(geometry_msgs__msg__PoseStamped& msg) { - update_topic(msg.header.stamp); - strcpy(msg.header.frame_id.data, BASE_LINK_FRAME_ID); - - auto &ahrs = AP::ahrs(); - WITH_SEMAPHORE(ahrs.get_semaphore()); - - // ROS REP 103 uses the ENU convention: - // X - East - // Y - North - // Z - Up - // https://www.ros.org/reps/rep-0103.html#axis-orientation - // AP_AHRS uses the NED convention - // X - North - // Y - East - // Z - Down - // As a consequence, to follow ROS REP 103, it is necessary to switch X and Y, - // as well as invert Z - - Vector3f position; - if (ahrs.get_relative_position_NED_home(position)) { - msg.pose.position.x = position[1]; - msg.pose.position.y = position[0]; - msg.pose.position.z = -position[2]; - } - - // In ROS REP 103, axis orientation uses the following convention: - // X - Forward - // Y - Left - // Z - Up - // https://www.ros.org/reps/rep-0103.html#axis-orientation - // As a consequence, to follow ROS REP 103, it is necessary to switch X and Y, - // as well as invert Z (NED to ENU convertion) as well as a 90 degree rotation in the Z axis - // for x to point forward - Quaternion orientation; - if (ahrs.get_quaternion(orientation)) { - Quaternion aux(orientation[0], orientation[2], orientation[1], -orientation[3]); //NED to ENU transformation - Quaternion transformation (sqrtF(2) * 0.5,0,0,sqrtF(2) * 0.5); // Z axis 90 degree rotation - orientation = aux * transformation; - msg.pose.orientation.w = orientation[0]; - msg.pose.orientation.x = orientation[1]; - msg.pose.orientation.y = orientation[2]; - msg.pose.orientation.z = orientation[3]; - } + AP_ROS_Client::update_pose_stamped(msg); } -// implementation copied from: -// void AP_DDS_Client::update_topic(geometry_msgs_msg_TwistStamped& msg) void AP_UROS_Client::update_topic(geometry_msgs__msg__TwistStamped& msg) { - update_topic(msg.header.stamp); - strcpy(msg.header.frame_id.data, BASE_LINK_FRAME_ID); - - auto &ahrs = AP::ahrs(); - WITH_SEMAPHORE(ahrs.get_semaphore()); - - // ROS REP 103 uses the ENU convention: - // X - East - // Y - North - // Z - Up - // https://www.ros.org/reps/rep-0103.html#axis-orientation - // AP_AHRS uses the NED convention - // X - North - // Y - East - // Z - Down - // As a consequence, to follow ROS REP 103, it is necessary to switch X and Y, - // as well as invert Z - Vector3f velocity; - if (ahrs.get_velocity_NED(velocity)) { - msg.twist.linear.x = velocity[1]; - msg.twist.linear.y = velocity[0]; - msg.twist.linear.z = -velocity[2]; - } - - // In ROS REP 103, axis orientation uses the following convention: - // X - Forward - // Y - Left - // Z - Up - // https://www.ros.org/reps/rep-0103.html#axis-orientation - // The gyro data is received from AP_AHRS in body-frame - // X - Forward - // Y - Right - // Z - Down - // As a consequence, to follow ROS REP 103, it is necessary to invert Y and Z - Vector3f angular_velocity = ahrs.get_gyro(); - msg.twist.angular.x = angular_velocity[0]; - msg.twist.angular.y = -angular_velocity[1]; - msg.twist.angular.z = -angular_velocity[2]; + AP_ROS_Client::update_twist_stamped(msg); } -// implementation copied from: -// void AP_DDS_Client::populate_static_transforms(tf2_msgs_msg_TFMessage& msg) void AP_UROS_Client::update_topic(tf2_msgs__msg__TFMessage& msg) { - msg.transforms.size = 0; - - auto &gps = AP::gps(); - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { - const auto gps_type = gps.get_type(i); - if (gps_type == AP_GPS::GPS_Type::GPS_TYPE_NONE) { - continue; - } - update_topic(msg.transforms.data[i].header.stamp); - char gps_frame_id[16]; - //! @todo should GPS frame ID's be 0 or 1 indexed in ROS? - hal.util->snprintf(gps_frame_id, sizeof(gps_frame_id), "GPS_%u", i); - strcpy(msg.transforms.data[i].header.frame_id.data, BASE_LINK_FRAME_ID); - strcpy(msg.transforms.data[i].child_frame_id.data, gps_frame_id); - // The body-frame offsets - // X - Forward - // Y - Right - // Z - Down - // https://ardupilot.org/copter/docs/common-sensor-offset-compensation.html#sensor-position-offset-compensation - - const auto offset = gps.get_antenna_offset(i); - - // In ROS REP 103, it follows this convention - // X - Forward - // Y - Left - // Z - Up - // https://www.ros.org/reps/rep-0103.html#axis-orientation - - msg.transforms.data[i].transform.translation.x = offset[0]; - msg.transforms.data[i].transform.translation.y = -1 * offset[1]; - msg.transforms.data[i].transform.translation.z = -1 * offset[2]; - - msg.transforms.size++; - } + AP_ROS_Client::update_static_transforms(msg); } -// implementation copied from: -// bool AP_DDS_Client::update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t instance) bool AP_UROS_Client::update_topic(sensor_msgs__msg__NavSatFix& msg) { const uint8_t instance = 0; - - // Add a lambda that takes in navsatfix msg and populates the cov - // Make it constexpr if possible - // https://www.fluentcpp.com/2021/12/13/the-evolutions-of-lambdas-in-c14-c17-and-c20/ - // constexpr auto times2 = [] (sensor_msgs_msg_NavSatFix* msg) { return n * 2; }; - - // assert(instance >= GPS_MAX_RECEIVERS); - if (instance >= GPS_MAX_RECEIVERS) { - return false; - } - - auto &gps = AP::gps(); - WITH_SEMAPHORE(gps.get_semaphore()); - - if (!gps.is_healthy(instance)) { - msg.status.status = -1; // STATUS_NO_FIX - msg.status.service = 0; // No services supported - msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN - return false; - } - - // No update is needed - const auto last_fix_time_ms = gps.last_fix_time_ms(instance); - if (last_nav_sat_fix_time_ms == last_fix_time_ms) { - return false; - } else { - last_nav_sat_fix_time_ms = last_fix_time_ms; - } - - - update_topic(msg.header.stamp); - strcpy(msg.header.frame_id.data, WGS_84_FRAME_ID); - msg.status.service = 0; // SERVICE_GPS - msg.status.status = -1; // STATUS_NO_FIX - - - //! @todo What about glonass, compass, galileo? - //! This will be properly designed and implemented to spec in #23277 - msg.status.service = 1; // SERVICE_GPS - - const auto status = gps.status(instance); - switch (status) { - case AP_GPS::NO_GPS: - case AP_GPS::NO_FIX: - msg.status.status = -1; // STATUS_NO_FIX - msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN - return true; - case AP_GPS::GPS_OK_FIX_2D: - case AP_GPS::GPS_OK_FIX_3D: - msg.status.status = 0; // STATUS_FIX - break; - case AP_GPS::GPS_OK_FIX_3D_DGPS: - msg.status.status = 1; // STATUS_SBAS_FIX - break; - case AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT: - case AP_GPS::GPS_OK_FIX_3D_RTK_FIXED: - msg.status.status = 2; // STATUS_SBAS_FIX - break; - default: - //! @todo Can we not just use an enum class and not worry about this condition? - break; - } - const auto loc = gps.location(instance); - msg.latitude = loc.lat * 1E-7; - msg.longitude = loc.lng * 1E-7; - - int32_t alt_cm; - if (!loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_cm)) { - // With absolute frame, this condition is unlikely - msg.status.status = -1; // STATUS_NO_FIX - msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN - return true; - } - msg.altitude = alt_cm * 0.01; - - // ROS allows double precision, ArduPilot exposes float precision today - Matrix3f cov; - msg.position_covariance_type = (uint8_t)gps.position_covariance(instance, cov); - msg.position_covariance[0] = cov[0][0]; - msg.position_covariance[1] = cov[0][1]; - msg.position_covariance[2] = cov[0][2]; - msg.position_covariance[3] = cov[1][0]; - msg.position_covariance[4] = cov[1][1]; - msg.position_covariance[5] = cov[1][2]; - msg.position_covariance[6] = cov[2][0]; - msg.position_covariance[7] = cov[2][1]; - msg.position_covariance[8] = cov[2][2]; - - return true; + return AP_ROS_Client::update_nav_sat_fix(msg, instance, last_nav_sat_fix_time_ms); } -// implementation copied from: -// void AP_DDS_Client::update_topic(builtin_interfaces_msg_Time& msg) void AP_UROS_Client::update_topic(builtin_interfaces__msg__Time& msg) { - uint64_t utc_usec; - if (!AP::rtc().get_utc_usec(utc_usec)) { - utc_usec = AP_HAL::micros64(); - } - msg.sec = utc_usec / 1000000ULL; - msg.nanosec = (utc_usec % 1000000ULL) * 1000UL; + AP_ROS_Client::update_time(msg); } // call publishers diff --git a/libraries/AP_UROS/AP_UROS_ExternalControl.cpp b/libraries/AP_UROS/AP_UROS_ExternalControl.cpp index 0e777a287ef05..e9eb97c3288f1 100644 --- a/libraries/AP_UROS/AP_UROS_ExternalControl.cpp +++ b/libraries/AP_UROS/AP_UROS_ExternalControl.cpp @@ -1,43 +1,13 @@ #if AP_UROS_ENABLED #include "AP_UROS_ExternalControl.h" -#include "AP_UROS_Frames.h" #include - #include +#include bool AP_UROS_External_Control::handle_velocity_control(const geometry_msgs__msg__TwistStamped& cmd_vel) { - auto *external_control = AP::externalcontrol(); - if (external_control == nullptr) { - return false; - } - - if (strcmp(cmd_vel.header.frame_id.data, BASE_LINK_FRAME_ID) == 0) { - // Convert commands from body frame (x-forward, y-left, z-up) to NED. - Vector3f linear_velocity; - Vector3f linear_velocity_base_link { - float(cmd_vel.twist.linear.x), - float(cmd_vel.twist.linear.y), - float(-cmd_vel.twist.linear.z) }; - const float yaw_rate = -cmd_vel.twist.angular.z; - - auto &ahrs = AP::ahrs(); - linear_velocity = ahrs.body_to_earth(linear_velocity_base_link); - return external_control->set_linear_velocity_and_yaw_rate(linear_velocity, yaw_rate); - } - - else if (strcmp(cmd_vel.header.frame_id.data, MAP_FRAME) == 0) { - // Convert commands from ENU to NED frame - Vector3f linear_velocity { - float(cmd_vel.twist.linear.y), - float(cmd_vel.twist.linear.x), - float(-cmd_vel.twist.linear.z) }; - const float yaw_rate = -cmd_vel.twist.angular.z; - return external_control->set_linear_velocity_and_yaw_rate(linear_velocity, yaw_rate); - } - return false; + return AP_ROS_External_Control::handle_velocity_control(cmd_vel); } - -#endif // AP_UROS_ENABLED \ No newline at end of file +#endif // AP_UROS_ENABLED diff --git a/libraries/AP_UROS/AP_UROS_External_Odom.cpp b/libraries/AP_UROS/AP_UROS_External_Odom.cpp index b31007a3e0768..26aebfbf60d0f 100644 --- a/libraries/AP_UROS/AP_UROS_External_Odom.cpp +++ b/libraries/AP_UROS/AP_UROS_External_Odom.cpp @@ -3,72 +3,21 @@ #if AP_UROS_VISUALODOM_ENABLED -#include -#include +#include void AP_UROS_External_Odom::handle_external_odom(const tf2_msgs__msg__TFMessage& msg) { - auto *visual_odom = AP::visualodom(); - if (visual_odom == nullptr) { - return; - } - - for (size_t i = 0; i < msg.transforms.size; i++) { - const auto& ros_transform_stamped = msg.transforms.data[i]; - if (!is_odometry_frame(ros_transform_stamped)) { - continue; - } - const uint64_t remote_time_us {AP_UROS_Type_Conversions::time_u64_micros(ros_transform_stamped.header.stamp)}; - - Vector3f ap_position; - Quaternion ap_rotation; - - convert_transform(ros_transform_stamped.transform, ap_position, ap_rotation); - // Although ROS convention states quaternions in ROS messages should be normalized, it's not guaranteed. - // Before propogating a potentially inaccurate quaternion to the rest of AP, normalize it here. - // TODO what if the quaternion is NaN? - ap_rotation.normalize(); - - // No error is available in TF, trust the data as-is - const float posErr {0.0}; - const float angErr {0.0}; - // The odom to base_link transform used is locally consistent per ROS REP-105. - // https://www.ros.org/reps/rep-0105.html#id16 - // Thus, there will not be any resets. - const uint8_t reset_counter {0}; - // TODO imlement jitter correction similar to GCS_MAVLINK::correct_offboard_timestamp_usec_to_ms(remote_time_us, sizeof(msg)); - const uint32_t time_ms {static_cast(remote_time_us * 1E-3)}; - visual_odom->handle_pose_estimate(remote_time_us, time_ms, ap_position.x, ap_position.y, ap_position.z, ap_rotation, posErr, angErr, reset_counter); - - } + AP_ROS_External_Odom::handle_external_odom(msg); } bool AP_UROS_External_Odom::is_odometry_frame(const geometry_msgs__msg__TransformStamped& msg) { - char odom_parent[] = "odom"; - char odom_child[] = "base_link"; - // Assume the frame ID's are null terminated. - return (strcmp(msg.header.frame_id.data, odom_parent) == 0) && - (strcmp(msg.child_frame_id.data, odom_child) == 0); + return AP_ROS_External_Odom::is_odometry_frame(msg); } void AP_UROS_External_Odom::convert_transform(const geometry_msgs__msg__Transform& ros_transform, Vector3f& translation, Quaternion& rotation) { - // convert from x-forward, y-left, z-up to NED - // https://github.com/mavlink/mavros/issues/49#issuecomment-51614130 - translation = { - static_cast(ros_transform.translation.x), - static_cast(-ros_transform.translation.y), - static_cast(-ros_transform.translation.z) - }; - - // In AP, q1 is the quaternion's scalar component. - // In ROS, w is the quaternion's scalar component. - // https://docs.ros.org/en/humble/Tutorials/Intermediate/Tf2/Quaternion-Fundamentals.html#components-of-a-quaternion - rotation.q1 = ros_transform.rotation.w; - rotation.q2 = ros_transform.rotation.x; - rotation.q3 = -ros_transform.rotation.y; - rotation.q4 = -ros_transform.rotation.z; + AP_ROS_External_Odom::convert_transform(ros_transform, translation, rotation); } #endif // AP_UROS_VISUALODOM_ENABLED diff --git a/libraries/AP_UROS/AP_UROS_Type_Conversions.cpp b/libraries/AP_UROS/AP_UROS_Type_Conversions.cpp index 09ca55aa6c023..cefbb7839b613 100644 --- a/libraries/AP_UROS/AP_UROS_Type_Conversions.cpp +++ b/libraries/AP_UROS/AP_UROS_Type_Conversions.cpp @@ -1,13 +1,104 @@ #include "AP_UROS_Type_Conversions.h" #if AP_UROS_ENABLED -#include - - uint64_t AP_UROS_Type_Conversions::time_u64_micros(const builtin_interfaces__msg__Time& ros_time) { - return (uint64_t(ros_time.sec) * 1000000ULL) + (ros_time.nanosec / 1000ULL); + return AP_ROS_TypeConversions::time_u64_micros(ros_time); +} + +// string specialisations +template <> +const char* string_data(const rosidl_runtime_c__String& str) { + return str.data; +} + +template <> +char* mutable_string_data(rosidl_runtime_c__String& str) { + return str.data; +} + +// transform specialisations +template <> +typename transforms_size_type::type +transforms_size(const tf2_msgs__msg__TFMessage& msg) { + return msg.transforms.size; +} + +template <> +typename mutable_transforms_size_type::type +mutable_transforms_size(tf2_msgs__msg__TFMessage& msg) { + return msg.transforms.size; +} + +template <> +typename transforms_type::type +transforms_data(const tf2_msgs__msg__TFMessage& msg) { + return msg.transforms.data; +} + +template <> +typename mutable_transforms_type::type +mutable_transforms_data(tf2_msgs__msg__TFMessage& msg) { + return msg.transforms.data; +} + +// cell_voltage specialisations +template <> +typename mutable_cell_voltage_type::type +mutable_cell_voltage_data(sensor_msgs__msg__BatteryState& msg) { + return msg.cell_voltage.data; } +#if 0 +//! @todo(srmainwaring) move to test / examples sub-folder. +// tests for the accessor templates +#include + +extern const AP_HAL::HAL& hal; + +template +void test_read(const TFMessage& msg) { + // read msg.transforms.size + hal.console->printf("%u", (uint32_t)transforms_size(msg)); + + // read msg.transforms[i] + double sum_x = 0.0; + for (size_t i = 0; i < transforms_size(msg); ++i) { + const auto& transform_stamped = transforms_data(msg)[i]; + sum_x += transform_stamped.transform.translation.x; + } + + hal.console->printf("%f", sum_x); +} + +template +void test_write(TFMessage& msg) { + // write msg.transforms.size + mutable_transforms_size(msg) = 0; + for (size_t i = 0; i < 4; ++i) { + mutable_transforms_size(msg)++; + } + hal.console->printf("%u", (uint32_t)transforms_size(msg)); + + // write msg.transforms[i] + auto* transform_stamped = mutable_transforms_data(msg); + transform_stamped[0].transform.translation.x = 10.0; + + hal.console->printf("%f", + transforms_data(msg)[0].transform.translation.x); +} + +void test_read(const tf2_msgs__msg__TFMessage& msg) { + test_read(msg); +} + +void test_write(tf2_msgs__msg__TFMessage& msg) { + test_write(msg); +} + +void test_write2(tf2_msgs__msg__TFMessage& msg) { + test_write(msg); +} +#endif #endif // AP_UROS_ENABLED diff --git a/libraries/AP_UROS/AP_UROS_Type_Conversions.h b/libraries/AP_UROS/AP_UROS_Type_Conversions.h index 7cec182497181..34b873dbcbf52 100644 --- a/libraries/AP_UROS/AP_UROS_Type_Conversions.h +++ b/libraries/AP_UROS/AP_UROS_Type_Conversions.h @@ -4,7 +4,12 @@ #if AP_UROS_ENABLED +#include + #include +#include +#include +#include class AP_UROS_Type_Conversions { @@ -14,4 +19,58 @@ class AP_UROS_Type_Conversions static uint64_t time_u64_micros(const builtin_interfaces__msg__Time& ros_time); }; +// string specialisations +template <> +const char* string_data(const rosidl_runtime_c__String& str); + +template <> +char* mutable_string_data(rosidl_runtime_c__String& str); + +// transform specialisations +template <> +struct transforms_size_type { + typedef size_t type; +}; + +template <> +struct mutable_transforms_size_type { + typedef size_t& type; +}; + +template <> +typename transforms_size_type::type +transforms_size(const tf2_msgs__msg__TFMessage& msg); + +template <> +typename mutable_transforms_size_type::type +mutable_transforms_size(tf2_msgs__msg__TFMessage& msg); + +template <> +struct transforms_type { + typedef const geometry_msgs__msg__TransformStamped* type; +}; + +template <> +struct mutable_transforms_type { + typedef geometry_msgs__msg__TransformStamped* type; +}; + +template <> +typename transforms_type::type +transforms_data(const tf2_msgs__msg__TFMessage& msg); + +template <> +typename mutable_transforms_type::type +mutable_transforms_data(tf2_msgs__msg__TFMessage& msg); + +// cell_voltage specialisations +template <> +struct mutable_cell_voltage_type { + typedef float* type; +}; + +template <> +typename mutable_cell_voltage_type::type +mutable_cell_voltage_data(sensor_msgs__msg__BatteryState& msg); + #endif // AP_UROS_ENABLED From 6560d162a5a09d94dc0899115f462ca9387a9cbd Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Tue, 3 Oct 2023 17:09:44 +0100 Subject: [PATCH 16/16] AP_DDS: use common code from AP_ROS Signed-off-by: Rhys Mainwaring AP_DDS: use common code from AP_ROS for external control Signed-off-by: Rhys Mainwaring AP_DDS: use common code from AP_ROS for time type conversion Signed-off-by: Rhys Mainwaring AP_DDS: add mutable versions of accessor templates Signed-off-by: Rhys Mainwaring AP_DDS: use common code from AP_ROS for published topics Signed-off-by: Rhys Mainwaring AP_DDS: consolidate code for publishing nav sat fix Signed-off-by: Rhys Mainwaring AP_DDS: delete code moved to AP_ROS Signed-off-by: Rhys Mainwaring AP_DDS: rename mutable transforms Signed-off-by: Rhys Mainwaring AP_DDS: consolidate code for publishing static transforms Signed-off-by: Rhys Mainwaring AP_DDS: move template specialisation definitions to cpp to prevent duplicate symbols Signed-off-by: Rhys Mainwaring AP_DDS: fix formatting Signed-off-by: Rhys Mainwaring AP_DDS: add accessor templates for battery state Signed-off-by: Rhys Mainwaring AP_DDS: consolidate code for publishing battery state Signed-off-by: Rhys Mainwaring --- libraries/AP_DDS/AP_DDS_Client.cpp | 316 +------------------ libraries/AP_DDS/AP_DDS_ExternalControl.cpp | 35 +- libraries/AP_DDS/AP_DDS_External_Odom.cpp | 61 +--- libraries/AP_DDS/AP_DDS_Type_Conversions.cpp | 47 ++- libraries/AP_DDS/AP_DDS_Type_Conversions.h | 59 ++++ 5 files changed, 122 insertions(+), 396 deletions(-) diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index d5315e829705e..86cef43b11536 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -12,15 +12,19 @@ #include #include #include + +#include + #if AP_EXTERNAL_CONTROL_ENABLED #include "AP_DDS_ExternalControl.h" #endif -#include "AP_DDS_Frames.h" +// #include "AP_DDS_Frames.h" #include "AP_DDS_Client.h" #include "AP_DDS_Topic_Table.h" #include "AP_DDS_Service_Table.h" #include "AP_DDS_External_Odom.h" +#include "AP_DDS_Type_Conversions.h" // Enable DDS at runtime by default static constexpr uint8_t ENABLED_BY_DEFAULT = 1; @@ -65,335 +69,42 @@ const AP_Param::GroupInfo AP_DDS_Client::var_info[] { void AP_DDS_Client::update_topic(builtin_interfaces_msg_Time& msg) { - uint64_t utc_usec; - if (!AP::rtc().get_utc_usec(utc_usec)) { - utc_usec = AP_HAL::micros64(); - } - msg.sec = utc_usec / 1000000ULL; - msg.nanosec = (utc_usec % 1000000ULL) * 1000UL; - + AP_ROS_Client::update_time(msg); } bool AP_DDS_Client::update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t instance) { - // Add a lambda that takes in navsatfix msg and populates the cov - // Make it constexpr if possible - // https://www.fluentcpp.com/2021/12/13/the-evolutions-of-lambdas-in-c14-c17-and-c20/ - // constexpr auto times2 = [] (sensor_msgs_msg_NavSatFix* msg) { return n * 2; }; - - // assert(instance >= GPS_MAX_RECEIVERS); - if (instance >= GPS_MAX_RECEIVERS) { - return false; - } - - auto &gps = AP::gps(); - WITH_SEMAPHORE(gps.get_semaphore()); - - if (!gps.is_healthy(instance)) { - msg.status.status = -1; // STATUS_NO_FIX - msg.status.service = 0; // No services supported - msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN - return false; - } - - // No update is needed - const auto last_fix_time_ms = gps.last_fix_time_ms(instance); - if (last_nav_sat_fix_time_ms == last_fix_time_ms) { - return false; - } else { - last_nav_sat_fix_time_ms = last_fix_time_ms; - } - - - update_topic(msg.header.stamp); - strcpy(msg.header.frame_id, WGS_84_FRAME_ID); - msg.status.service = 0; // SERVICE_GPS - msg.status.status = -1; // STATUS_NO_FIX - - - //! @todo What about glonass, compass, galileo? - //! This will be properly designed and implemented to spec in #23277 - msg.status.service = 1; // SERVICE_GPS - - const auto status = gps.status(instance); - switch (status) { - case AP_GPS::NO_GPS: - case AP_GPS::NO_FIX: - msg.status.status = -1; // STATUS_NO_FIX - msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN - return true; - case AP_GPS::GPS_OK_FIX_2D: - case AP_GPS::GPS_OK_FIX_3D: - msg.status.status = 0; // STATUS_FIX - break; - case AP_GPS::GPS_OK_FIX_3D_DGPS: - msg.status.status = 1; // STATUS_SBAS_FIX - break; - case AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT: - case AP_GPS::GPS_OK_FIX_3D_RTK_FIXED: - msg.status.status = 2; // STATUS_SBAS_FIX - break; - default: - //! @todo Can we not just use an enum class and not worry about this condition? - break; - } - const auto loc = gps.location(instance); - msg.latitude = loc.lat * 1E-7; - msg.longitude = loc.lng * 1E-7; - - int32_t alt_cm; - if (!loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_cm)) { - // With absolute frame, this condition is unlikely - msg.status.status = -1; // STATUS_NO_FIX - msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN - return true; - } - msg.altitude = alt_cm * 0.01; - - // ROS allows double precision, ArduPilot exposes float precision today - Matrix3f cov; - msg.position_covariance_type = (uint8_t)gps.position_covariance(instance, cov); - msg.position_covariance[0] = cov[0][0]; - msg.position_covariance[1] = cov[0][1]; - msg.position_covariance[2] = cov[0][2]; - msg.position_covariance[3] = cov[1][0]; - msg.position_covariance[4] = cov[1][1]; - msg.position_covariance[5] = cov[1][2]; - msg.position_covariance[6] = cov[2][0]; - msg.position_covariance[7] = cov[2][1]; - msg.position_covariance[8] = cov[2][2]; - - return true; + return AP_ROS_Client::update_nav_sat_fix(msg, instance, last_nav_sat_fix_time_ms); } void AP_DDS_Client::populate_static_transforms(tf2_msgs_msg_TFMessage& msg) { - msg.transforms_size = 0; - - auto &gps = AP::gps(); - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { - const auto gps_type = gps.get_type(i); - if (gps_type == AP_GPS::GPS_Type::GPS_TYPE_NONE) { - continue; - } - update_topic(msg.transforms[i].header.stamp); - char gps_frame_id[16]; - //! @todo should GPS frame ID's be 0 or 1 indexed in ROS? - hal.util->snprintf(gps_frame_id, sizeof(gps_frame_id), "GPS_%u", i); - strcpy(msg.transforms[i].header.frame_id, BASE_LINK_FRAME_ID); - strcpy(msg.transforms[i].child_frame_id, gps_frame_id); - // The body-frame offsets - // X - Forward - // Y - Right - // Z - Down - // https://ardupilot.org/copter/docs/common-sensor-offset-compensation.html#sensor-position-offset-compensation - - const auto offset = gps.get_antenna_offset(i); - - // In ROS REP 103, it follows this convention - // X - Forward - // Y - Left - // Z - Up - // https://www.ros.org/reps/rep-0103.html#axis-orientation - - msg.transforms[i].transform.translation.x = offset[0]; - msg.transforms[i].transform.translation.y = -1 * offset[1]; - msg.transforms[i].transform.translation.z = -1 * offset[2]; - - msg.transforms_size++; - } - + AP_ROS_Client::update_static_transforms(msg); } void AP_DDS_Client::update_topic(sensor_msgs_msg_BatteryState& msg, const uint8_t instance) { - if (instance >= AP_BATT_MONITOR_MAX_INSTANCES) { - return; - } - - update_topic(msg.header.stamp); - auto &battery = AP::battery(); - - if (!battery.healthy(instance)) { - msg.power_supply_status = 3; //POWER_SUPPLY_HEALTH_DEAD - msg.present = false; - return; - } - msg.present = true; - - msg.voltage = battery.voltage(instance); - - float temperature; - msg.temperature = (battery.get_temperature(temperature, instance)) ? temperature : NAN; - - float current; - msg.current = (battery.current_amps(current, instance)) ? -1 * current : NAN; - - const float design_capacity = (float)battery.pack_capacity_mah(instance) * 0.001; - msg.design_capacity = design_capacity; - - uint8_t percentage; - if (battery.capacity_remaining_pct(percentage, instance)) { - msg.percentage = percentage * 0.01; - msg.charge = (percentage * design_capacity) * 0.01; - } else { - msg.percentage = NAN; - msg.charge = NAN; - } - - msg.capacity = NAN; - - if (battery.current_amps(current, instance)) { - if (percentage == 100) { - msg.power_supply_status = 4; //POWER_SUPPLY_STATUS_FULL - } else if (current < 0.0) { - msg.power_supply_status = 1; //POWER_SUPPLY_STATUS_CHARGING - } else if (current > 0.0) { - msg.power_supply_status = 2; //POWER_SUPPLY_STATUS_DISCHARGING - } else { - msg.power_supply_status = 3; //POWER_SUPPLY_STATUS_NOT_CHARGING - } - } else { - msg.power_supply_status = 0; //POWER_SUPPLY_STATUS_UNKNOWN - } - - msg.power_supply_health = (battery.overpower_detected(instance)) ? 4 : 1; //POWER_SUPPLY_HEALTH_OVERVOLTAGE or POWER_SUPPLY_HEALTH_GOOD - - 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); - } + AP_ROS_Client::update_battery_state(msg, instance); } void AP_DDS_Client::update_topic(geometry_msgs_msg_PoseStamped& msg) { - update_topic(msg.header.stamp); - strcpy(msg.header.frame_id, BASE_LINK_FRAME_ID); - - auto &ahrs = AP::ahrs(); - WITH_SEMAPHORE(ahrs.get_semaphore()); - - // ROS REP 103 uses the ENU convention: - // X - East - // Y - North - // Z - Up - // https://www.ros.org/reps/rep-0103.html#axis-orientation - // AP_AHRS uses the NED convention - // X - North - // Y - East - // Z - Down - // As a consequence, to follow ROS REP 103, it is necessary to switch X and Y, - // as well as invert Z - - Vector3f position; - if (ahrs.get_relative_position_NED_home(position)) { - msg.pose.position.x = position[1]; - msg.pose.position.y = position[0]; - msg.pose.position.z = -position[2]; - } - - // In ROS REP 103, axis orientation uses the following convention: - // X - Forward - // Y - Left - // Z - Up - // https://www.ros.org/reps/rep-0103.html#axis-orientation - // As a consequence, to follow ROS REP 103, it is necessary to switch X and Y, - // as well as invert Z (NED to ENU convertion) as well as a 90 degree rotation in the Z axis - // for x to point forward - Quaternion orientation; - if (ahrs.get_quaternion(orientation)) { - Quaternion aux(orientation[0], orientation[2], orientation[1], -orientation[3]); //NED to ENU transformation - Quaternion transformation (sqrtF(2) * 0.5,0,0,sqrtF(2) * 0.5); // Z axis 90 degree rotation - orientation = aux * transformation; - msg.pose.orientation.w = orientation[0]; - msg.pose.orientation.x = orientation[1]; - msg.pose.orientation.y = orientation[2]; - msg.pose.orientation.z = orientation[3]; - } + AP_ROS_Client::update_pose_stamped(msg); } void AP_DDS_Client::update_topic(geometry_msgs_msg_TwistStamped& msg) { - update_topic(msg.header.stamp); - strcpy(msg.header.frame_id, BASE_LINK_FRAME_ID); - - auto &ahrs = AP::ahrs(); - WITH_SEMAPHORE(ahrs.get_semaphore()); - - // ROS REP 103 uses the ENU convention: - // X - East - // Y - North - // Z - Up - // https://www.ros.org/reps/rep-0103.html#axis-orientation - // AP_AHRS uses the NED convention - // X - North - // Y - East - // Z - Down - // As a consequence, to follow ROS REP 103, it is necessary to switch X and Y, - // as well as invert Z - Vector3f velocity; - if (ahrs.get_velocity_NED(velocity)) { - msg.twist.linear.x = velocity[1]; - msg.twist.linear.y = velocity[0]; - msg.twist.linear.z = -velocity[2]; - } - - // In ROS REP 103, axis orientation uses the following convention: - // X - Forward - // Y - Left - // Z - Up - // https://www.ros.org/reps/rep-0103.html#axis-orientation - // The gyro data is received from AP_AHRS in body-frame - // X - Forward - // Y - Right - // Z - Down - // As a consequence, to follow ROS REP 103, it is necessary to invert Y and Z - Vector3f angular_velocity = ahrs.get_gyro(); - msg.twist.angular.x = angular_velocity[0]; - msg.twist.angular.y = -angular_velocity[1]; - msg.twist.angular.z = -angular_velocity[2]; + AP_ROS_Client::update_twist_stamped(msg); } void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg) { - update_topic(msg.header.stamp); - strcpy(msg.header.frame_id, BASE_LINK_FRAME_ID); - - auto &ahrs = AP::ahrs(); - WITH_SEMAPHORE(ahrs.get_semaphore()); - - Location loc; - if (ahrs.get_location(loc)) { - msg.pose.position.latitude = loc.lat * 1E-7; - msg.pose.position.longitude = loc.lng * 1E-7; - msg.pose.position.altitude = loc.alt * 0.01; // Transform from cm to m - } - - // In ROS REP 103, axis orientation uses the following convention: - // X - Forward - // Y - Left - // Z - Up - // https://www.ros.org/reps/rep-0103.html#axis-orientation - // As a consequence, to follow ROS REP 103, it is necessary to switch X and Y, - // as well as invert Z (NED to ENU convertion) as well as a 90 degree rotation in the Z axis - // for x to point forward - Quaternion orientation; - if (ahrs.get_quaternion(orientation)) { - Quaternion aux(orientation[0], orientation[2], orientation[1], -orientation[3]); //NED to ENU transformation - Quaternion transformation(sqrtF(2) * 0.5, 0, 0, sqrtF(2) * 0.5); // Z axis 90 degree rotation - orientation = aux * transformation; - msg.pose.orientation.w = orientation[0]; - msg.pose.orientation.x = orientation[1]; - msg.pose.orientation.y = orientation[2]; - msg.pose.orientation.z = orientation[3]; - } + AP_ROS_Client::update_geopose_stamped(msg); } void AP_DDS_Client::update_topic(rosgraph_msgs_msg_Clock& msg) { - update_topic(msg.clock); + AP_ROS_Client::update_clock(msg); } /* @@ -839,6 +550,7 @@ void AP_DDS_Client::write_battery_state_topic() } } } + void AP_DDS_Client::write_local_pose_topic() { WITH_SEMAPHORE(csem); diff --git a/libraries/AP_DDS/AP_DDS_ExternalControl.cpp b/libraries/AP_DDS/AP_DDS_ExternalControl.cpp index a72536d6ffde8..f8fbf7a3e6fc2 100644 --- a/libraries/AP_DDS/AP_DDS_ExternalControl.cpp +++ b/libraries/AP_DDS/AP_DDS_ExternalControl.cpp @@ -1,44 +1,13 @@ #if AP_DDS_ENABLED #include "AP_DDS_ExternalControl.h" -#include "AP_DDS_Frames.h" #include - #include +#include bool AP_DDS_External_Control::handle_velocity_control(geometry_msgs_msg_TwistStamped& cmd_vel) { - auto *external_control = AP::externalcontrol(); - if (external_control == nullptr) { - return false; - } - - if (strcmp(cmd_vel.header.frame_id, BASE_LINK_FRAME_ID) == 0) { - // Convert commands from body frame (x-forward, y-left, z-up) to NED. - Vector3f linear_velocity; - Vector3f linear_velocity_base_link { - float(cmd_vel.twist.linear.x), - float(cmd_vel.twist.linear.y), - float(-cmd_vel.twist.linear.z) }; - const float yaw_rate = -cmd_vel.twist.angular.z; - - auto &ahrs = AP::ahrs(); - linear_velocity = ahrs.body_to_earth(linear_velocity_base_link); - return external_control->set_linear_velocity_and_yaw_rate(linear_velocity, yaw_rate); - } - - else if (strcmp(cmd_vel.header.frame_id, MAP_FRAME) == 0) { - // Convert commands from ENU to NED frame - Vector3f linear_velocity { - float(cmd_vel.twist.linear.y), - float(cmd_vel.twist.linear.x), - float(-cmd_vel.twist.linear.z) }; - const float yaw_rate = -cmd_vel.twist.angular.z; - return external_control->set_linear_velocity_and_yaw_rate(linear_velocity, yaw_rate); - } - - return false; + return AP_ROS_External_Control::handle_velocity_control(cmd_vel); } - #endif // AP_DDS_ENABLED \ No newline at end of file diff --git a/libraries/AP_DDS/AP_DDS_External_Odom.cpp b/libraries/AP_DDS/AP_DDS_External_Odom.cpp index cf121f408521a..9694d2add4f1f 100644 --- a/libraries/AP_DDS/AP_DDS_External_Odom.cpp +++ b/libraries/AP_DDS/AP_DDS_External_Odom.cpp @@ -1,76 +1,23 @@ - - #include "AP_DDS_External_Odom.h" #include "AP_DDS_Type_Conversions.h" #if AP_DDS_VISUALODOM_ENABLED -#include -#include +#include void AP_DDS_External_Odom::handle_external_odom(const tf2_msgs_msg_TFMessage& msg) { - auto *visual_odom = AP::visualodom(); - if (visual_odom == nullptr) { - return; - } - - for (size_t i = 0; i < msg.transforms_size; i++) { - const auto& ros_transform_stamped = msg.transforms[i]; - if (!is_odometry_frame(ros_transform_stamped)) { - continue; - } - const uint64_t remote_time_us {AP_DDS_Type_Conversions::time_u64_micros(ros_transform_stamped.header.stamp)}; - - Vector3f ap_position; - Quaternion ap_rotation; - - convert_transform(ros_transform_stamped.transform, ap_position, ap_rotation); - // Although ROS convention states quaternions in ROS messages should be normalized, it's not guaranteed. - // Before propogating a potentially inaccurate quaternion to the rest of AP, normalize it here. - // TODO what if the quaternion is NaN? - ap_rotation.normalize(); - - // No error is available in TF, trust the data as-is - const float posErr {0.0}; - const float angErr {0.0}; - // The odom to base_link transform used is locally consistent per ROS REP-105. - // https://www.ros.org/reps/rep-0105.html#id16 - // Thus, there will not be any resets. - const uint8_t reset_counter {0}; - // TODO imlement jitter correction similar to GCS_MAVLINK::correct_offboard_timestamp_usec_to_ms(remote_time_us, sizeof(msg)); - const uint32_t time_ms {static_cast(remote_time_us * 1E-3)}; - visual_odom->handle_pose_estimate(remote_time_us, time_ms, ap_position.x, ap_position.y, ap_position.z, ap_rotation, posErr, angErr, reset_counter); - - } + AP_ROS_External_Odom::handle_external_odom(msg); } bool AP_DDS_External_Odom::is_odometry_frame(const geometry_msgs_msg_TransformStamped& msg) { - char odom_parent[] = "odom"; - char odom_child[] = "base_link"; - // Assume the frame ID's are null terminated. - return (strcmp(msg.header.frame_id, odom_parent) == 0) && - (strcmp(msg.child_frame_id, odom_child) == 0); + return AP_ROS_External_Odom::is_odometry_frame(msg); } void AP_DDS_External_Odom::convert_transform(const geometry_msgs_msg_Transform& ros_transform, Vector3f& translation, Quaternion& rotation) { - // convert from x-forward, y-left, z-up to NED - // https://github.com/mavlink/mavros/issues/49#issuecomment-51614130 - translation = { - static_cast(ros_transform.translation.x), - static_cast(-ros_transform.translation.y), - static_cast(-ros_transform.translation.z) - }; - - // In AP, q1 is the quaternion's scalar component. - // In ROS, w is the quaternion's scalar component. - // https://docs.ros.org/en/humble/Tutorials/Intermediate/Tf2/Quaternion-Fundamentals.html#components-of-a-quaternion - rotation.q1 = ros_transform.rotation.w; - rotation.q2 = ros_transform.rotation.x; - rotation.q3 = -ros_transform.rotation.y; - rotation.q4 = -ros_transform.rotation.z; + AP_ROS_External_Odom::convert_transform(ros_transform, translation, rotation); } #endif // AP_DDS_VISUALODOM_ENABLED diff --git a/libraries/AP_DDS/AP_DDS_Type_Conversions.cpp b/libraries/AP_DDS/AP_DDS_Type_Conversions.cpp index 2aa4caf543f89..7508acec1389a 100644 --- a/libraries/AP_DDS/AP_DDS_Type_Conversions.cpp +++ b/libraries/AP_DDS/AP_DDS_Type_Conversions.cpp @@ -1,13 +1,52 @@ #include "AP_DDS_Type_Conversions.h" #if AP_DDS_ENABLED -#include "builtin_interfaces/msg/Time.h" - - uint64_t AP_DDS_Type_Conversions::time_u64_micros(const builtin_interfaces_msg_Time& ros_time) { - return (uint64_t(ros_time.sec) * 1000000ULL) + (ros_time.nanosec / 1000ULL); + return AP_ROS_TypeConversions::time_u64_micros(ros_time); +} + +// string specialisations +template <> +const char* string_data(const char* str) { + return str; +} + +template <> +char* mutable_string_data(char* str) { + return str; +} + +// transform specialisations +template <> +typename transforms_size_type::type +transforms_size(const tf2_msgs_msg_TFMessage& msg) { + return msg.transforms_size; } +template <> +typename mutable_transforms_size_type::type +mutable_transforms_size(tf2_msgs_msg_TFMessage& msg) { + return msg.transforms_size; +} + +template <> +typename transforms_type::type +transforms_data(const tf2_msgs_msg_TFMessage& msg) { + return msg.transforms; +} + +template <> +typename mutable_transforms_type::type +mutable_transforms_data(tf2_msgs_msg_TFMessage& msg) { + return msg.transforms; +} + +// cell_voltage specialisations +template <> +typename mutable_cell_voltage_type::type +mutable_cell_voltage_data(sensor_msgs_msg_BatteryState& msg) { + return msg.cell_voltage; +} #endif // AP_DDS_ENABLED diff --git a/libraries/AP_DDS/AP_DDS_Type_Conversions.h b/libraries/AP_DDS/AP_DDS_Type_Conversions.h index 71b2e3182de29..096821b0c51cf 100644 --- a/libraries/AP_DDS/AP_DDS_Type_Conversions.h +++ b/libraries/AP_DDS/AP_DDS_Type_Conversions.h @@ -4,7 +4,12 @@ #if AP_DDS_ENABLED +#include + #include "builtin_interfaces/msg/Time.h" +#include "geometry_msgs/msg/TransformStamped.h" +#include "sensor_msgs/msg/BatteryState.h" +#include "tf2_msgs/msg/TFMessage.h" class AP_DDS_Type_Conversions { @@ -14,4 +19,58 @@ class AP_DDS_Type_Conversions static uint64_t time_u64_micros(const builtin_interfaces_msg_Time& ros_time); }; +// string specialisations +template <> +const char* string_data(const char* str); + +template <> +char* mutable_string_data(char* str); + +// transform specialisations +template <> +struct transforms_size_type { + typedef uint32_t type; +}; + +template <> +struct mutable_transforms_size_type { + typedef uint32_t& type; +}; + +template <> +typename transforms_size_type::type +transforms_size(const tf2_msgs_msg_TFMessage& msg); + +template <> +typename mutable_transforms_size_type::type +mutable_transforms_size(tf2_msgs_msg_TFMessage& msg); + +template <> +struct transforms_type { + typedef const geometry_msgs_msg_TransformStamped* type; +}; + +template <> +struct mutable_transforms_type { + typedef geometry_msgs_msg_TransformStamped* type; +}; + +template <> +typename transforms_type::type +transforms_data(const tf2_msgs_msg_TFMessage& msg); + +template <> +typename mutable_transforms_type::type +mutable_transforms_data(tf2_msgs_msg_TFMessage& msg); + +// cell_voltage specialisations +template <> +struct mutable_cell_voltage_type { + typedef float* type; +}; + +template <> +typename mutable_cell_voltage_type::type +mutable_cell_voltage_data(sensor_msgs_msg_BatteryState& msg); + #endif // AP_DDS_ENABLED