Cartographer SITL SLAM not working, VisOdom: not Healthy

[basmango]

I have setup the simulation exactly as specified in the repository. (dependencies, param settings etc)
mavproxy shows the message VisOdom: not Healhy
to reproduce the issue I have prepared a Dockerfile (will need Nvidia container toolkit for GPU rendering):

FROM ardupilot/ardupilot-dev-ros:latest

RUN wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg 
RUN echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null
RUN apt-get update
RUN apt-get install -y socat terminator gz-garden libsdformat12-dev rapidjson-dev libgflags-dev ros-humble-ament-cmake-mypy ros-humble-image-transport ros-humble-image-transport-plugins ros-humble-geographic-msgs
ENV GZ_VERSION=garden
ENV ROS_DISTRO=humble
RUN pip3 install mavproxy
VOLUME ["/ros2_ws"]

RUN mkdir -p /ros2_ws/src 

WORKDIR /ros2_ws/src

# have tried with and without micro-ros
RUN git clone -b $ROS_DISTRO https://github.com/micro-ROS/micro_ros_setup.git micro_ros_setup

RUN wget https://raw.githubusercontent.com/ArduPilot/ardupilot_gz/main/ros2_gz.repos
RUN git clone https://github.com/ArduPilot/ardupilot_ros
RUN vcs import < ros2_gz.repos
WORKDIR /ros2_ws/src/ardupilot
RUN git submodule update --init --recursive
WORKDIR /ros2_ws
RUN /bin/bash -c  "source /opt/ros/$ROS_DISTRO/setup.sh && \
    apt-get update && \
    rosdep update && \
    rosdep install --rosdistro $ROS_DISTRO --from-paths src -i -r -y"

RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.sh && colcon build"

# For GPU support for gazebo

ENV NVIDIA_VISIBLE_DEVICES \
    ${NVIDIA_VISIBLE_DEVICES:-all}
ENV NVIDIA_DRIVER_CAPABILITIES \
    ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics

# append source to bashrc
RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash;source /ros2_ws/install/setup.bash;" >> ~/.bashrc

bash script to build image:

#!/bin/bash
sudo docker build --no-cache -t $USER/test_sim .

bash script to launch simulation using Nvidia graphics card:

xhost +local:*
sudo docker run -it --rm   -e DISPLAY=$DISPLAY \
    --env="QT_X11_NO_MITSHM=1" \
    --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \
    --env="XAUTHORITY=$XAUTH" \
    --volume="$XAUTH:$XAUTH" \
    --runtime=nvidia  \
    --gpus all \
    --privileged \
    --net=host \
     $USER/test_sim /bin/bash -c "terminator & terminator -e 'source /opt/ros/humble/setup.bash && source /ros2_ws/install/setup.bash && ros2 launch ardupilot_gz_bringup iris_maze.launch.py rviz:=false;/bin/bash ' & \
     terminator -e 'source /opt/ros/humble/setup.bash && source /ros2_ws/install/setup.bash && ros2 launch ardupilot_ros cartographer.launch.py;/bin/bash' "

otherwise:

xhost +local:*
sudo docker run -it --rm   -e DISPLAY=$DISPLAY \
    --env="QT_X11_NO_MITSHM=1" \
    --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \
    --env="XAUTHORITY=$XAUTH" \
    --volume="$XAUTH:$XAUTH" \
    --privileged \
    --net=host \
     $USER/test_sim  /bin/bash -c "terminator & terminator -e 'source /opt/ros/humble/setup.bash && source /ros2_ws/install/setup.bash && ros2 launch ardupilot_gz_bringup iris_maze.launch.py rviz:=false;/bin/bash ' & \
     terminator -e 'source /opt/ros/humble/setup.bash && source /ros2_ws/install/setup.bash && ros2 launch ardupilot_ros cartographer.launch.py;/bin/bash' "

start mavproxy

 mavproxy.py --console --map --aircraft test --master=:14550

Then parameters are set using mavproxy using the commands

param set AHRS_EKF_TYPE 3
param set EK2_ENABLE 0
param set EK3_ENABLE 1
param set EK3_SRC1_POSXY 6
param set EK3_SRC1_POSZ 1
param set EK3_SRC1_VELXY 6
param set EK3_SRC1_VELZ 6
param set EK3_SRC1_YAW 6
param set VISO_TYPE 1
param set ARMING_CHECK 388598

Then, SITL simulation is restarted.

[Ryanf55]

Thank you for reporting. I will give this a try soon and report back if I can reproduce or fix this problem.

[Ryanf55]

Any tips?

ryan@B650-970:~/Dev/ros2_ws/src/ardupilot_ros$ xhost local +    
xhost:  bad hostname "local"                                                                                                                                                                                                                        
access control disabled, clients can connect from any host

[basmango]

try xhost +local:*
fixed it in the issue description
either ways, terminals should pop up after a few seconds.

[lrabius]

The problem is that the tf topic changed several times in ardupilot. Since ArduPilot/ardupilot#25384 ardupilot_dds is listening to /ap/tf for the tf tree whereas it is published in ros on /tf. Therefore no visual odom messages are sent to ardupilot.
You can use a relay node from topic_tools to forward the tf tree to the right topic as mentioned in the pull request.

[BilalBabar-edgec]

Following this issue, any update on this ?
Getting the same visodom not healthy after setting up the parameters

[Ryanf55]

@pedro-fuoco Do you have any ability to chase this down. A bunch of users are unable to reproduce your cartographer work. Thanks!

[pedro-fuoco]

@Ryanf55 Sure thing, I will chase it down over the weekend.

As commented by @lrabius it is likely a TF remap/namespace issue, it is a bit of a mess in sitl as we discussed in Ryanf55/ardupilot_wiki#1 (comment). Tidying it up would be a good follow up to make it easier for the users.

[pedro-fuoco]

I got it up and running again. As @lrabius pointed out, the main problem is that cartographer was publishing on /tf instead of /ap/tf, so this node stopped working after ArduPilot/ardupilot#25384 merged.

I added the /ap prefix to ardupilot_gz and ardupilot_cartographer. However, it must be noted that users must set use_gz_tf:=false when running ros2 launch ardupilot_gz_bringup iris_maze.launch.py, so that the gazebo odometry isn't captured by ardupilot instead of cartographer's.
The good news about this warning is that I ended up having a few SITL test flights with gazebo as an external odometry source and it works well, so we "accidentally" have another out-of-the-box example of external odometry ready.

From @srmainwaring's comment on ArduPilot/ardupilot#25384 I understand that we shouldn't remap ardupilot_gz's robot_state_publisher directly to /ap/tf in order to not overwhelm AP_DDS. However, most SITL examples (and every external odometry source exemple) will require the TF tree from robot_state_publisher to be avaiable at /ap/tf. This gives us two options:

• Add a TF relay on ardupilot_cartographer to copy /tf into /ap/tf, since this is necessary for the cartographer environment to run but not mandatory for all users running SITL. Note that this might lead us to repetitive code since many SITL examples may require this.
• Remap robot_state_publisher directly to /ap/tf in ardupilot_gz, afterall this will only be used in a SITL environment, in which AP_DDS can handle significantly more load.

Let me know your thoughts, I can submit the PRs as soon as I know which route is preferable.
@Ryanf55 @srmainwaring

[lrabius]

Looking at ArduPilot/ardupilot#25384, it seems like the error occured using gazebo and SITL. I'm wondering if remapping or a relay node would help here at all.

Also ros tools like tf2 depend on tf messages being published to /tf. Would it be necessary for every ros2 node and tools using tf2 to remap /tf to /ap/tf when the /ap namespace is added to /tf?

It might be good to have the whole tf tree on /tf and only copy the relevant odom -> base_link transform to /ap/tf as suggested in the PR. Perhaps something like a filter or topic_tools transfrom can be used. But the transform node also sends empty tf messages to /ap/tf when every transform is filtered out and then AP_DDS is complaining about receiving an empty tf.

Node(
    package="topic_tools",
    executable="transform",
    arguments=[
        "/tf",
        "/ap/tf",
        "tf2_msgs/msg/TFMessage",
        "tf2_msgs.msg.TFMessage(transforms=[x for x in m.transforms if x.header.frame_id == 'odom'])",
        "--import",
        "tf2_msgs",
        "geometry_msgs",
    ],
)

[somikdhar729]

@pedro-fuoco I made all the changes as suggested but I am still getting VisOdom Not Healthy. I am using ROS2 Humble and Gazebo Harmonic.