AP_DDS: Add dynamic TF subscriber support for odometry #24155

Ryanf55 · 2023-06-28T03:45:12Z

This is the first step for GSOC Cartographer external odometry input

pedro-fuoco · 2023-06-29T13:31:30Z

Thanks for the help! I have a few suggestions, tell me if they are in the scope of this PR

Ryanf55 · 2023-06-29T22:32:25Z

Thanks for the help! I have a few suggestions, tell me if they are in the scope of this PR

Not sure if you meant to add anything, please share when you get time.

pedro-fuoco · 2023-07-08T03:33:51Z

If we use the /tf topic instead of /ap/tf we could use tf2 listeners instead of subscribing. At first glance this seems like a better way to do it as we allow users to use all of the tf2 tools.

libraries/AP_DDS/AP_DDS_Client.cpp

pedro-fuoco · 2023-06-29T14:22:35Z

libraries/AP_DDS/AP_DDS_Topic_Table.h

+        .topic_profile_label = "dynamictransforms__t",
+        .dw_profile_label = "",
+        .dr_profile_label = "dynamictransforms__dr",


Changes to make it consistent with dds_xrce_profile.xml:

Suggested change

.topic_profile_label = "dynamictransforms__t",

.dw_profile_label = "",

.dr_profile_label = "dynamictransforms__dr",

.topic_profile_label = "dynamictf__t",

.dw_profile_label = "",

.dr_profile_label = "dynamictf___dr",

An error was appearing before and the subscriber wasn't created. With this changes it is now working:

This should solve the test dds / build (stm32h7) (pull_request) CI fail

Ryanf55 · 2023-07-12T00:02:31Z

Some updates from today's meeting regarding external odometry input into ArduPilot libs:

Add Ap_VisualOdom.h, add support in handle_vision_position_estimate with a Vector3F pos (which is either single or double precision depending on the platform). To start, do it with single precision, and upgrade it later? This would be an extreme ardupilot sort of feature at distances >100km. For GSoc, let's ignore it.

For home vs origin, see writeExtNavData. Users can set home mid mission, while the ROS convention specifies that origin and map are static. Within the EKF, there is a public orign (exposed to AHRS), absolutely static till reboot. Shadow origin is updated internally updated at 1Hz to current vehicle location. This deals doesn't deal with flat earth well.

For setting the origin, there is a mavros way to do it set_origin.py.

Ardupilot only requires a global origin, not GPS. Pedro sets origin manually in a script. This would be a good service if we want a pure ROS solution. OR, assume we initially have GPS then lose GPS and use vision. Either solution is OK for Tridge. Pedro can time to look into it. Tridge recommends just doing the GPS method because it's easier to initialize the EKF. Without GPS, the EKF gets stuck waiting for lag to determine its buffer size. get_lag(gps, delay) Visual odom library has a get_delay_ms which is a param VISO_DELAY_MS, defaulted 10mS.

For the time between sensor measurement and position estimate, it needs to be under 250mS due to Ardupilot EKF. Fusion is max 100Hz, and that is when the forward propogation happens. Further than 250mS, it degrades a t^2 and linear CPU increase.

For high lag data, there is a different API (recent addition).See AP_NavEKf3. See AP_NavEkf3_PosVelFusion with the EK3_FEATURE_POSITION_RESET. Later, we could support downsampled velocity history.

Action for pedro: Benchmark the algorithm on hardware, then we can decide.

In SITL testing, GPS_DELAY_MS could be bumped up to match the vision estimate issue. Paul put this to save RAM, but it's problematic for dual GPS. Current behavior fixes it by clamping the timestamp and fuses at the wrong time. TBD what the behavior for vision is.

In EKF source selection, set primary to GPS, secondary to Vision, and then use RC or Lua to switch if we required GPS lock.

Once we get logs working, ping Tridge to review how to look at EKF innovations and VISO message VISP. Check the VISP messages match EKF position estimates.

pedro-fuoco

Changes for the code to build

libraries/AP_DDS/AP_DDS_Topic_Table.h

pedro-fuoco · 2023-07-14T03:26:44Z

libraries/AP_DDS/AP_DDS_Client.cpp

+        uint32_t* count_ptr = (uint32_t*) args;
+        (*count_ptr)++;
+        if (rx_dynamic_transforms_topic.transforms_size > 0) {
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Received tf2_msgs/TFMessage of length: %lu",


Suggested change

GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Received tf2_msgs/TFMessage of length: %lu",

GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Received tf2_msgs/TFMessage of length: %u",

pedro-fuoco

After building ardupilot and ardupilot_sitl I ran the iris_maze launch from ardupilot_gz. In order to test the tf subscriber, I remapped the robot_state_publisher node from /tf to /ap/tf.

AP_DDS_Client received a message but this apparently caused a seg fault, any idea why that might be? Here's the log:

[dds_udp.parm --synthetic-clock -4] ERROR: segmentation fault - aborting
[dds_udp.parm --synthetic-clock -4] Running: sh dumpstack.sh 97015 >dumpstack.sh_arducopter.97015.out 2>&1
[mavproxy.py --out 127.0.0.1:14550 --out 127.0.0.1:14551 --master tcp:127.0.0.1:5760 --sitl 127.0.0.1:5501 --non-interactive -5] AP: Received tf2_msgs/TFMessage of length: 6
[dds_udp.parm --synthetic-clock -4] Failed
[dds_udp.parm --synthetic-clock -4] Running: sh dumpcore.sh 97015 >dumpcore.sh_arducopter.97015.out 2>&1
[dds_udp.parm --synthetic-clock -4] Failed
[dds_udp.parm --synthetic-clock -4] Aborted (core dumped)
[ERROR] [dds_udp.parm --synthetic-clock -4]: process has died [pid 97012, exit code 134, cmd 'arducopter --model json --speedup 1 --slave 0 --sim-address=127.0.0.1 --instance 0 --defaults /home/fuoco/ros2_ws/install/ardupilot_sitl/share/ardupilot_sitl/config/default_params/gazebo-iris.parm,/home/fuoco/ros2_ws/install/ardupilot_sitl/share/ardupilot_sitl/config/default_params/dds_udp.parm --synthetic-clock '].
[mavproxy.py --out 127.0.0.1:14550 --out 127.0.0.1:14551 --master tcp:127.0.0.1:5760 --sitl 127.0.0.1:5501 --non-interactive -5] EOF on TCP socket
[mavproxy.py --out 127.0.0.1:14550 --out 127.0.0.1:14551 --master tcp:127.0.0.1:5760 --sitl 127.0.0.1:5501 --non-interactive -5] Attempting reconnect
[mavproxy.py --out 127.0.0.1:14550 --out 127.0.0.1:14551 --master tcp:127.0.0.1:5760 --sitl 127.0.0.1:5501 --non-interactive -5] [Errno 111] Connection refused sleeping
[mavproxy.py --out 127.0.0.1:14550 --out 127.0.0.1:14551 --master tcp:127.0.0.1:5760 --sitl 127.0.0.1:5501 --non-interactive -5] [Errno 111] Connection refused sleeping
[mavproxy.py --out 127.0.0.1:14550 --out 127.0.0.1:14551 --master tcp:127.0.0.1:5760 --sitl 127.0.0.1:5501 --non-interactive -5] Attempting reconnect

The crash doesn't happen if I don't remap the topic (presumably because nothing reaches the subscriber handler)

srmainwaring · 2023-07-14T12:29:51Z

@pedro-fuoco have you tried publishing TF from the command line?

ros2 topic pub /ap/tf tf2_msgs/msg/TFMessage 'transforms: [{header: {stamp: {sec: 1, nanosec: 0}, frame_id: "base_link"}, child_frame_id: "GPS_0", transform: {translation: {x: 0.0, y: 0.0, z: 0.0}, rotation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}, {header: {stamp: {sec: 2, nanosec: 0}, frame_id: "base_link"}, child_frame_id: "GPS_0", transform: {translation: {x: 0.0, y: 0.0, z: 0.0}, rotation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}, {header: {stamp: {sec: 3, nanosec: 0}, frame_id: "base_link"}, child_frame_id: "GPS_0", transform: {translation: {x: 0.0, y: 0.0, z: 0.0}, rotation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}, {header: {stamp: {sec: 4, nanosec: 0}, frame_id: "base_link"}, child_frame_id: "GPS_0", transform: {translation: {x: 0.0, y: 0.0, z: 0.0}, rotation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}, {header: {stamp: {sec: 5, nanosec: 0}, frame_id: "base_link"}, child_frame_id: "GPS_0", transform: {translation: {x: 0.0, y: 0.0, z: 0.0}, rotation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}]' --once

I'm running a rebased version of the PR with the fixes you noted included: https://github.com/srmainwaring/ardupilot/tree/wips/wip-tf-subscriber-rebased-fixes

Update

Can replicate the segfault when forwarding TF from the example:

ros2 launch ardupilot_gz_bringup iris_runway.launch.py`

I suspect it's a combination of the message size and rate.

/tf is published from Gazebo at approx. 1000 Hz (i.e. 0.001 s time step in the physics engine).
The iris tf tree contains 8 items. This seems to be about the limit of what will be processed when publishing using the ROS 2 CLI.

Looks like message size. This will cause a segfault:

Run SITL session (no Gazebo):

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

Send single TF (6 items of the 8 published from the iris example):

ros2 topic pub /ap/tf tf2_msgs/msg/TFMessage 'transforms: [
{header: {stamp: {sec: 227, nanosec: 91000000}, frame_id: iris}, child_frame_id: iris/tilt_link, transform: {translation: {x: 0.019984060916003184, y: -0.020015926647110993, z: -0.09998399286175172}, rotation: {x: -0.4999997609802308, y: 0.49999975742835057, z: 0.4996019204931798, w: 0.5003982440325244}}},
{header: {stamp: {sec: 227, nanosec: 91000000}, frame_id: iris}, child_frame_id: iris/roll_link,
transform: {translation: {x: -2.2827193214895528e-11, y: -2.3581028733108545e-08, z: -0.09999990013228542}, rotation: {x: 0.0002823774170257476, y: 0.7068251247013926,
z: -0.0002807098464664981, w: 0.70738821346969}}},
{header: {stamp: {sec: 227, nanosec: 91000000}, frame_id: iris}, child_frame_id: iris/gimbal_link, transform: {translation: {x: -7.861318384707755e-19, y: -0.01, z: -0.11}, rotation: {x: 0.5000000095078997, y: 0.4996016684687246, z: 0.500000009775746, w: 0.500397995179458}}},
{header: {stamp: {sec: 227, nanosec: 91000000}, frame_id: iris}, child_frame_id: iris/rotor_0, transform: {translation: {x: 0.13000000000182527, y: -0.2199999999962856, z: 0.023000096313720075}, rotation: {x: -6.217004150247587e-12, y: 1.5109553369859743e-12, z: -0.00010602049440067152, w: 0.9999999943798272}}},
{header: {stamp: {sec: 227, nanosec: 91000000}, frame_id: iris}, child_frame_id: iris/rotor_1, transform: {translation: {x: -0.12999999999845072, y: 0.20000000000354365, z: 0.023000096309283957}, rotation: {x: -6.217004150247587e-12, y: 1.5109553369859743e-12, z: -0.00010602049440067152, w: 0.9999999943798272}}},
{header: {stamp: {sec: 227, nanosec: 91000000}, frame_id: iris}, child_frame_id: iris/rotor_3, transform: {translation: {x: -0.12999999999818787, y: -0.1999999999964565, z: 0.02300009631425745}, rotation: {x: -6.217004150247587e-12, y: 1.5109553369859743e-12, z: -0.00010602049440067152, w: 0.9999999943798272}}}]' --once

Additional notes:

Using topic_tools can throttle the TF from the iris_example to a reasonable rate - still get the segfault as before, confirming message size is the issue:

# throttle from 1000 Hz to 1 Hz and relay to /ap/tf 
ros2 run topic_tools throttle messages /tf 1 /ap/tf

pedro-fuoco · 2023-07-28T02:34:13Z

libraries/AP_DDS/AP_DDS_Client.cpp

+        uint32_t* count_ptr = (uint32_t*) args;
+        (*count_ptr)++;
+        if (rx_dynamic_transforms_topic.transforms_size > 0) {
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Received tf2_msgs/TFMessage of length: %zu",


Apparently the Ardupilot C compiler doesn't support %z. It failed CI and I also wasn't able to compile it locally.

Which board is it failing for you on?

With the last commit, it passes CI with the use of a static cast.
Please approve if you are happy.

pedro-fuoco · 2023-07-28T02:49:44Z

Tested it with the ardupilot_gz example iris_maze.launch like before. The crash problem seems to be solved, I received the message correctly. LGTM on CI pass and squash.

libraries/AP_DDS/AP_DDS_Client.cpp

* This is the first step for GSOC Cartographer external odometry input * Moved subscriber data members to class member variables Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

Ryanf55 added ROS For-4.5 Planned for 4.5 release labels Jun 28, 2023

Ryanf55 self-assigned this Jun 28, 2023

Ryanf55 force-pushed the tf-subscriber branch from 70eb3d4 to c61adc1 Compare June 28, 2023 03:51

Ryanf55 changed the title ~~Add dynamic TF subscriber support~~ AP_DDS: Add dynamic TF subscriber support Jun 28, 2023

Ryanf55 changed the title ~~AP_DDS: Add dynamic TF subscriber support~~ AP_DDS: Add dynamic TF subscriber support for odometry Jul 11, 2023

pedro-fuoco reviewed Jul 11, 2023

View reviewed changes

pedro-fuoco suggested changes Jul 14, 2023

View reviewed changes

pedro-fuoco reviewed Jul 14, 2023

View reviewed changes

srmainwaring mentioned this pull request Jul 14, 2023

AP_DDS: make subscriber topic data static #24304

Closed

This was referenced Jul 14, 2023

AP_DDS: Protect subscribers from crashing the main thread #24306

Closed

AP_DDS: fix reliable stream buffer size #24348

Merged

Ryanf55 force-pushed the tf-subscriber branch 3 times, most recently from 01bf723 to f1e871e Compare July 26, 2023 00:42

Ryanf55 requested a review from pedro-fuoco July 26, 2023 22:19

pedro-fuoco reviewed Jul 28, 2023

View reviewed changes

Ryanf55 commented Jul 28, 2023

View reviewed changes

libraries/AP_DDS/AP_DDS_Client.cpp Outdated Show resolved Hide resolved

pedro-fuoco approved these changes Jul 30, 2023

View reviewed changes

AP_DDS: Add dynamic TF subscriber support

ea42a54

* This is the first step for GSOC Cartographer external odometry input * Moved subscriber data members to class member variables Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

Ryanf55 force-pushed the tf-subscriber branch from 9abab9b to ea42a54 Compare July 30, 2023 05:24

Ryanf55 added the MergeOnCIPass label Jul 30, 2023

tridge merged commit 01c5f44 into ArduPilot:master Jul 31, 2023
81 checks passed

srmainwaring removed the MergeOnCIPass label Feb 27, 2024

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

AP_DDS: Add dynamic TF subscriber support for odometry #24155

AP_DDS: Add dynamic TF subscriber support for odometry #24155

Ryanf55 commented Jun 28, 2023

pedro-fuoco commented Jun 29, 2023

Ryanf55 commented Jun 29, 2023

pedro-fuoco commented Jul 8, 2023

pedro-fuoco Jun 29, 2023

Ryanf55 commented Jul 12, 2023

pedro-fuoco left a comment •

edited

pedro-fuoco Jul 14, 2023

pedro-fuoco Jul 14, 2023 •

edited

pedro-fuoco left a comment •

edited

srmainwaring commented Jul 14, 2023 •

edited

pedro-fuoco Jul 28, 2023

Ryanf55 Jul 28, 2023

pedro-fuoco Jul 29, 2023

Ryanf55 Jul 29, 2023

pedro-fuoco commented Jul 28, 2023 •

edited

	GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Received tf2_msgs/TFMessage of length: %lu",
	GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Received tf2_msgs/TFMessage of length: %u",

AP_DDS: Add dynamic TF subscriber support for odometry #24155

AP_DDS: Add dynamic TF subscriber support for odometry #24155

Conversation

Ryanf55 commented Jun 28, 2023

pedro-fuoco commented Jun 29, 2023

Ryanf55 commented Jun 29, 2023

pedro-fuoco commented Jul 8, 2023

pedro-fuoco Jun 29, 2023

Choose a reason for hiding this comment

Ryanf55 commented Jul 12, 2023

pedro-fuoco left a comment • edited

Choose a reason for hiding this comment

pedro-fuoco Jul 14, 2023

Choose a reason for hiding this comment

pedro-fuoco Jul 14, 2023 • edited

Choose a reason for hiding this comment

pedro-fuoco left a comment • edited

Choose a reason for hiding this comment

srmainwaring commented Jul 14, 2023 • edited

Update

Additional notes:

pedro-fuoco Jul 28, 2023

Choose a reason for hiding this comment

Ryanf55 Jul 28, 2023

Choose a reason for hiding this comment

pedro-fuoco Jul 29, 2023

Choose a reason for hiding this comment

Ryanf55 Jul 29, 2023

Choose a reason for hiding this comment

pedro-fuoco commented Jul 28, 2023 • edited

pedro-fuoco left a comment •

edited

pedro-fuoco Jul 14, 2023 •

edited

pedro-fuoco left a comment •

edited

srmainwaring commented Jul 14, 2023 •

edited

pedro-fuoco commented Jul 28, 2023 •

edited