C++20/23 Support, Add New Teleoperation Folder (#538)

* Initial ansible files

* Add more files, try networking

* Copy over catkin profiles

* Keep working on ansbile

* Add macos files and other things

* Add temp test script

* Add bootstrap file

* Add ros apt

* Add ros key

* Update

* Install ros deps

* Bash scripting

* Minor

* Perms

* Oh my zsh :o

* Missing var

* Branch

* Change name

* Become

* Perms

* Perms

* Add fzf

* Networking

* Keep going

* Keep going

* Add jetson stats

* Add color

* Add ansible script

* Install jetson stats as root

* Cleanup

* C++23 support, use pch for perception, teleoperation vue and django project

* remove old

* Fix mypy warnings

* use clang++12 as cuda host compiler

* lto, add clang-12 to jetson ansible

* minor change

* Move clang-12 to percep dev

* ignore .cache for clangd, symlink clangd so vscode recognizes it, silence cmake dev warnings, add bun to zshrc

---------

Co-authored-by: umroverPerception <[email protected]>

.gitignore

@@ -34,3 +34,6 @@ moteus-cal*
 
 # Virtual Environment
 /venv/
+
+# clangd Output
+.cache/

CMakeLists.txt

@@ -1,9 +1,11 @@
 cmake_minimum_required(VERSION 3.16)
 project(mrover VERSION 2024.0.0 LANGUAGES CXX)
 
-## Compile as C++17, supported in ROS Kinetic and newer, enable some static analysis
-set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD 23)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
+if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
+    add_link_options(-fuse-ld=lld-16)
+endif ()
 set(MROVER_CPP_COMPILE_OPTIONS -Wall -Wextra -Werror -pedantic)
 
 # Generate compile_commands.json for clangd
@@ -122,6 +124,7 @@ macro(add_gazebo_plugin_macro name sources includes)
     target_link_directories(${name} PRIVATE ${GAZEBO_LIBRARY_DIRS} /opt/ros/noetic/lib)
     target_link_libraries(${name} PRIVATE ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES})
     target_include_directories(${name} SYSTEM PRIVATE ${GAZEBO_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
+    set_target_properties(${name} PROPERTIES CXX_STANDARD 17)
 endmacro()
 
 # launch install macro
@@ -217,9 +220,9 @@ else ()
         find_package(Eigen3 REQUIRED)
         find_package(ZED 2 QUIET)
         if (ZED_FOUND)
-            set(CMAKE_CUDA_SEPARABLE_COMPILATION ON)
             set(CMAKE_CUDA_STANDARD 17)
             set(CMAKE_CUDA_STANDARD_REQUIRED ON)
+            set(CMAKE_CUDA_SEPARABLE_COMPILATION ON)
             # Jetson Xavier NX is Volta 7.2 architecture
             # Perception Laptop (A4000, Quadro version of RTX 3080) is Ampere 8.6 architecture
             set(CMAKE_CUDA_ARCHITECTURES 72 86)
@@ -231,9 +234,10 @@ else ()
     # Put items here to build
     macro(add_and_link_macro)
         add_cpp_nodelet_macro(tag_detector_nodelet "src/perception/tag_detector/*.cpp" "src/perception/tag_detector")
+        target_precompile_headers(tag_detector_nodelet PRIVATE src/perception/tag_detector/pch.hpp)
         target_link_libraries(tag_detector_nodelet PRIVATE ${OpenCV_LIBRARIES} lie)
 
-        add_cpp_node_macro(brushed_motors "src/esw/brushed_motors/*")
+        add_cpp_node_macro(brushed_motors "src/esw/brushed_motors/*.cpp")
 
         add_cpp_library_macro(lie "src/util/lie/*.cpp" "src/util/lie")
 
@@ -250,6 +254,7 @@ else ()
             #            target_link_libraries(zed_node PRIVATE ${ZED_LIBRARIES} ${OpenCV_LIBRARIES} ${SPECIAL_OS_LIBS} lie tag_detector_nodelet)
 
             add_cpp_nodelet_macro(zed_nodelet "src/perception/zed_wrapper/*.c*" , "src/perception/zed_wrapper")
+            target_precompile_headers(zed_nodelet PRIVATE src/perception/zed_wrapper/pch.hpp)
             target_include_directories(zed_nodelet SYSTEM PRIVATE ${ZED_INCLUDE_DIRS} ${CUDA_INCLUDE_DIRS})
             target_link_libraries(zed_nodelet PRIVATE ${ZED_LIBRARIES} ${OpenCV_LIBRARIES} ${SPECIAL_OS_LIBS} lie)
             target_compile_definitions(zed_nodelet PRIVATE MROVER_IS_NODELET)

ansible/jetson.yml

@@ -5,8 +5,6 @@
     ubuntu_release: focal
   roles:
     - jetson_dev
-    - build
     - dev
     - jetson_networks
     - jetson_services
-

ansible/roles/build/files/profiles/ci/config.yaml

@@ -8,6 +8,7 @@ cmake_args:
   - -DCMAKE_C_COMPILER=clang-16
   - -DCMAKE_CXX_COMPILER=clang++-16
   - -DCMAKE_CXX_CLANG_TIDY=clang-tidy-16
+  - -Wno-dev
 devel_layout: linked
 devel_space: devel
 extend_path: null

ansible/roles/build/files/profiles/debug/config.yaml

@@ -9,6 +9,8 @@ cmake_args:
   - -DCMAKE_CXX_COMPILER=clang++-16
   - -DCMAKE_CXX_COMPILER_LAUNCHER=ccache
   - -DCMAKE_CUDA_COMPILER_LAUNCHER=ccache
+  - -DCMAKE_CUDA_HOST_COMPILER=clang++-12
+  - -Wno-dev
 devel_layout: linked
 devel_space: devel
 extend_path: null

ansible/roles/build/files/profiles/release/config.yaml

@@ -9,6 +9,9 @@ cmake_args:
   - -DCMAKE_CXX_COMPILER=clang++-16
   - -DCMAKE_CXX_COMPILER_LAUNCHER=ccache
   - -DCMAKE_CUDA_COMPILER_LAUNCHER=ccache
+  - -DCMAKE_CUDA_HOST_COMPILER=clang++-12
+  - -DCMAKE_INTERPROCEDURAL_OPTIMIZATION=ON
+  - -Wno-dev
 devel_layout: linked
 devel_space: devel
 extend_path: null

ansible/roles/build/tasks/main.yml

@@ -64,10 +64,8 @@
       - neovim
       - sudo
       - cmake
-      # Caches intermediate build files, speeds up compilation over time
-      - ccache
-      # Faster than make
-      - ninja-build
+      - ccache # Caches intermediate build files, speeds up compilation over time
+      - ninja-build # Faster than make
       - tmux
       - zstd
       - htop
@@ -153,6 +151,14 @@
     link: /usr/bin/clang
     priority: 160
 
+- name: Set clangd 16 as Default
+  become: True
+  alternatives:
+    name: clangd
+    path: /usr/bin/clangd-16
+    link: /usr/bin/clangd
+    priority: 160
+
 - name: Setup Python Virtual Environment
   pip:
     name:

ansible/roles/dev/files/home/.zshrc

@@ -108,3 +108,10 @@ readonly CATKIN_SETUP_PATH=${CATKIN_WORKSPACE_PATH}/devel/setup.zsh
 if [ -f ${CATKIN_SETUP_PATH} ]; then
     source ${CATKIN_SETUP_PATH}
 fi
+
+# bun completions
+[ -s "/home/mrover/.bun/_bun" ] && source "/home/mrover/.bun/_bun"
+
+# bun
+export BUN_INSTALL="$HOME/.bun"
+export PATH="$BUN_INSTALL/bin:$PATH"

ansible/roles/dev/tasks/main.yml

@@ -17,6 +17,18 @@
   become: true
   command: chsh --shell /usr/bin/zsh {{ ansible_user_id }}
 
+- name: GitHub CLI APT Key
+  become: True
+  apt_key:
+    url: https://cli.github.com/packages/githubcli-archive-keyring.gpg
+    keyring: /usr/share/keyrings/githubcli-archive-keyring.gpg
+
+- name: GitHub APT List
+  become: True
+  apt_repository:
+    repo: deb [arch=amd64 signed-by=/usr/share/keyrings/githubcli-archive-keyring.gpg] https://cli.github.com/packages stable main
+    filename: github-cli
+
 - name: Add VSCode APT Key
   become: true
   apt_key:
@@ -29,10 +41,11 @@
     filename: vscode
     repo: "deb [arch=amd64,arm64,armhf signed-by=/usr/share/keyrings/packages.microsoft.gpg] https://packages.microsoft.com/repos/code stable main"
 
-- name: Install VSCode APT Package
+- name: Install APT Packages
   become: true
   apt:
     cache_valid_time: 604800
     state: latest
     name:
+      - gh
       - code

ansible/roles/jetson_dev/tasks/main.yml

@@ -3,7 +3,7 @@
   pip:
     name: jetson-stats
 
-- name: Jetpack
+- name: Install JetPack APT
   become: True
   apt:
     cache_valid_time: 604800

ansible/roles/percep_dev/tasks/main.yml

@@ -1,3 +1,11 @@
+- name: Install APT Packages
+  become: True
+  apt:
+    cache_valid_time: 604800
+    state: latest
+    name:
+      - clang-12 # Host compiler for NVCC as 16 is too new
+
 - name: ZED SDK Download
   get_url:
     url: https://download.stereolabs.com/zedsdk/4.0/cu121/ubuntu20

launch/rover_core.launch

@@ -6,5 +6,5 @@
     <param name="robot_description" command="xacro $(find mrover)/rover_description/rover.xacro" />
 
     <!-- publish the state of each joint as a transform in the TF tree -->
-    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen" />
 </launch>

setup.py

@@ -4,6 +4,17 @@
 
 from catkin_pkg.python_setup import generate_distutils_setup
 
-d = generate_distutils_setup(packages=["localization", "util", "navigation", "esw", "teleop"], package_dir={"": "src"})
+d = generate_distutils_setup(
+    packages=[
+        "localization",
+        "util",
+        "navigation",
+        "navigation.failure_identification",
+        "esw",
+        "teleop",
+        "teleoperation.teleoperation",
+    ],
+    package_dir={"": "src"},
+)
 
 setup(**d)

src/esw/brushed_motors/Controller.h

@@ -2,13 +2,13 @@
 
 #include "I2C.h"                    // for I2C and IOFailure
 #include "UART.h"                   // for UART
-#include <assert.h>                 // for assert
+#include <cassert>                  // for assert
 #include <cmath>                    // for M_PI
+#include <cstring>                  // for string and memcpy
 #include <limits>                   // for numeric limits
 #include <mrover/LimitSwitchData.h> // for LimitSwitchData
 #include <mutex>                    // for mutex
 #include <ros/console.h>            // for ROS_ERROR
-#include <string.h>                 // for string and memcpy
 
 #define OFF_OP 0x00
 #define OFF_WB 0

src/localization/gps_linearization.py

@@ -1,14 +1,15 @@
 #!/usr/bin/env python3
+from typing import Optional, Tuple
+
+import numpy as np
 import rospy
-from util.SE3 import SE3
-from util.np_utils import numpify
+from geometry_msgs.msg import PoseWithCovarianceStamped, PoseWithCovariance, Pose, Point, Quaternion
 from mrover.msg import ImuAndMag
+from pymap3d.enu import geodetic2enu
 from sensor_msgs.msg import NavSatFix
-from geometry_msgs.msg import PoseWithCovarianceStamped, PoseWithCovariance, Pose, Point, Quaternion
 from std_msgs.msg import Header
-import numpy as np
-from pymap3d.enu import geodetic2enu
-from typing import List, Optional, Tuple
+from util.SE3 import SE3
+from util.np_utils import numpify
 
 
 class GPSLinearization:

src/navigation/failure_identification/failure_identification.py

@@ -83,8 +83,7 @@ def write_to_csv(self):
             # append to csv if csv exists else write to csv
             rospy.loginfo("writing to file")
             if self.path_name is None:
-                home = Path.home()
-                path = home / "catkin_ws/src/mrover/failure_data"
+                path = Path.cwd() / "failure_data"
                 path.mkdir(exist_ok=True)
 
                 file_name = f"failure_data_{rospy.Time.now()}.csv"

src/perception/point.hpp

@@ -1,9 +1,5 @@
 #pragma once
 
-#include <cstdint>
-
-#include <sensor_msgs/point_cloud2_iterator.h>
-
 namespace mrover {
 
     /**

src/perception/tag_detector/pch.hpp

@@ -0,0 +1,36 @@
+#pragma once
+
+#include <algorithm>
+#include <cassert>
+#include <cmath>
+#include <cstdint>
+#include <execution>
+#include <limits>
+#include <numeric>
+#include <optional>
+#include <string>
+#include <type_traits>
+#include <unordered_map>
+
+#include <boost_cpp23_workaround.hpp>
+
+#include <opencv2/aruco.hpp>
+#include <opencv2/core/mat.hpp>
+#include <opencv2/imgproc.hpp>
+
+#include <dynamic_reconfigure/server.h>
+#include <image_transport/image_transport.h>
+#include <nodelet/loader.h>
+#include <nodelet/nodelet.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/image_encodings.h>
+#include <sensor_msgs/point_cloud2_iterator.h>
+#include <std_srvs/SetBool.h>
+#include <tf/exceptions.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <mrover/DetectorParamsConfig.h>
+
+#include <loop_profiler.hpp>
+#include <se3.hpp>

src/perception/tag_detector/tag_detector.cpp

@@ -1,11 +1,5 @@
 #include "tag_detector.hpp"
 
-#include <limits>
-#include <string>
-#include <type_traits>
-
-#include <nodelet/loader.h>
-
 namespace mrover {
 
     void TagDetectorNodelet::onInit() {

src/perception/tag_detector/tag_detector.hpp

@@ -1,23 +1,4 @@
-#include <optional>
-#include <string>
-#include <unordered_map>
-
-#include <opencv2/aruco.hpp>
-#include <opencv2/core/mat.hpp>
-
-#include <dynamic_reconfigure/server.h>
-#include <image_transport/image_transport.h>
-#include <nodelet/nodelet.h>
-#include <sensor_msgs/PointCloud2.h>
-#include <std_srvs/SetBool.h>
-#include <tf2_ros/transform_broadcaster.h>
-#include <tf2_ros/transform_listener.h>
-
-#include <mrover/DetectorParamsConfig.h>
-
-#include <se3.hpp>
-
-#include "loop_profiler.hpp"
+#include "pch.hpp"
 
 namespace mrover {
 

src/perception/tag_detector/tag_detector.processing.cpp

@@ -2,17 +2,6 @@
 
 #include "../point.hpp"
 
-#include <sensor_msgs/image_encodings.h>
-#include <tf/exceptions.h>
-
-#include <opencv2/imgproc.hpp>
-
-#include <algorithm>
-#include <cassert>
-#include <cmath>
-#include <execution>
-#include <numeric>
-
 namespace mrover {
 
     /**
@@ -88,7 +77,7 @@ namespace mrover {
             Tag& tag = mTags[id];
             tag.hitCount = std::clamp(tag.hitCount + mTagIncrementWeight, 0, mMaxHitCount);
             tag.id = id;
-            tag.imageCenter = std::accumulate(mImmediateCorners[i].begin(), mImmediateCorners[i].end(), cv::Point2f{}) / static_cast<float>(mImmediateCorners[i].size());
+            tag.imageCenter = std::reduce(mImmediateCorners[i].begin(), mImmediateCorners[i].end()) / static_cast<float>(mImmediateCorners[i].size());
             tag.tagInCam = getTagInCamFromPixel(msg, std::lround(tag.imageCenter.x), std::lround(tag.imageCenter.y));
 
             if (tag.tagInCam) {
@@ -103,7 +92,7 @@ namespace mrover {
         auto it = mTags.begin();
         while (it != mTags.end()) {
             auto& [id, tag] = *it;
-            if (std::find(mImmediateIds.begin(), mImmediateIds.end(), id) == mImmediateIds.end()) {
+            if (std::ranges::find(mImmediateIds, id) == mImmediateIds.end()) {
                 tag.hitCount -= mTagDecrementWeight;
                 tag.tagInCam = std::nullopt;
                 if (tag.hitCount <= 0) {
@@ -160,7 +149,7 @@ namespace mrover {
             mImgMsg.is_bigendian = __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__;
             size_t size = mImgMsg.step * mImgMsg.height;
             mImgMsg.data.resize(size);
-            std::copy(std::execution::par_unseq, mImg.data, mImg.data + size, mImgMsg.data.begin());
+            std::uninitialized_copy(std::execution::par_unseq, mImg.data, mImg.data + size, mImgMsg.data.begin());
             mImgPub.publish(mImgMsg);
         }