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 https://github.com/ArduPilot/ardupilot/pull/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.