diff --git a/.github/workflows/build_ros2.yml b/.github/workflows/build_ros2.yml index 3921ce308..13fc1ea47 100644 --- a/.github/workflows/build_ros2.yml +++ b/.github/workflows/build_ros2.yml @@ -70,3 +70,24 @@ jobs: # - name: Run OpenVINS Simulation! # run: | # docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws openvins /bin/bash -c "cd /catkin_ws && source install/setup.bash && ros2 run ov_msckf run_simulation src/open_vins/config/rpng_sim/estimator_config.yaml" + build_2404: + name: "ROS2 Ubuntu 24.04" + runs-on: ubuntu-latest + steps: + - name: Code Checkout + uses: actions/checkout@v2 + - name: Create Workspace and Docker Image + run: | + export REPO=$(basename $GITHUB_REPOSITORY) && + cd $GITHUB_WORKSPACE/.. && mkdir src/ && + mv $REPO/ src/ && mkdir $REPO/ && mv src/ $REPO/ && cd $REPO/ && + docker build -t openvins -f $GITHUB_WORKSPACE/src/$REPO/Dockerfile_ros2_24_04 . + - name: Echo Enviroment + run: | + docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws openvins /bin/bash -c "echo $ROS_DISTRO && echo $ROS_VERSION" + - name: Run Build in Docker + run: | + docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws openvins /bin/bash -c "cd /catkin_ws && colcon build" + - name: Run OpenVINS Simulation! + run: | + docker run -t --mount type=bind,source=$GITHUB_WORKSPACE,target=/catkin_ws openvins /bin/bash -c "cd /catkin_ws && source install/setup.bash && ros2 run ov_msckf run_simulation src/open_vins/config/rpng_sim/estimator_config.yaml" \ No newline at end of file diff --git a/Dockerfile_ros2_24_04 b/Dockerfile_ros2_24_04 new file mode 100644 index 000000000..90e4cb5a0 --- /dev/null +++ b/Dockerfile_ros2_24_04 @@ -0,0 +1,46 @@ +FROM osrf/ros:jazzy-desktop + +# ========================================================= +# ========================================================= + +# Are you are looking for how to use this docker file? +# - https://docs.openvins.com/dev-docker.html +# - https://docs.docker.com/get-started/ +# - http://wiki.ros.org/docker/Tutorials/Docker + +# ========================================================= +# ========================================================= + +# Dependencies we use, catkin tools is very good build system +# Also some helper utilities for fast in terminal edits (nano etc) +RUN apt-get update && apt-get install -y libeigen3-dev nano git sudo + +# Ceres solver install and setup +RUN apt-get update && apt-get install -y cmake libgoogle-glog-dev libgflags-dev libatlas-base-dev libeigen3-dev libsuitesparse-dev libceres-dev +# ENV CERES_VERSION="2.2.0" +# RUN git clone https://ceres-solver.googlesource.com/ceres-solver && \ +# cd ceres-solver && \ +# git checkout tags/${CERES_VERSION} && \ +# mkdir build && cd build && \ +# cmake .. && \ +# make -j$(nproc) install && \ +# rm -rf ../../ceres-solver + +# Seems this has Python 3.12 installed on it... +RUN apt-get update && apt-get install -y python3-dev python3-matplotlib python3-numpy python3-psutil python3-tk + +# Install deps needed for clion remote debugging +# https://blog.jetbrains.com/clion/2020/01/using-docker-with-clion/ +# RUN sed -i '6i\source "/catkin_ws/install/setup.bash"\' /ros_entrypoint.sh +RUN apt-get update && apt-get install -y ssh build-essential gcc g++ \ + gdb clang cmake rsync tar python3 && apt-get clean +RUN ( \ + echo 'LogLevel DEBUG2'; \ + echo 'PermitRootLogin yes'; \ + echo 'PasswordAuthentication yes'; \ + echo 'Subsystem sftp /usr/lib/openssh/sftp-server'; \ + ) > /etc/ssh/sshd_config_test_clion \ + && mkdir /run/sshd +RUN useradd -m user && yes password | passwd user +RUN usermod -s /bin/bash user +CMD ["/usr/sbin/sshd", "-D", "-e", "-f", "/etc/ssh/sshd_config_test_clion"] \ No newline at end of file diff --git a/ov_core/CMakeLists.txt b/ov_core/CMakeLists.txt index ecc1cc82b..9d2a11b9a 100644 --- a/ov_core/CMakeLists.txt +++ b/ov_core/CMakeLists.txt @@ -9,7 +9,7 @@ if (NOT OpenCV_FOUND) find_package(OpenCV 4 REQUIRED) endif () find_package(Boost REQUIRED COMPONENTS system filesystem thread date_time) -message(STATUS "OPENCV: " ${OpenCV_VERSION} " | BOOST: " ${Boost_VERSION}) +message(STATUS "EIGEN: " ${Eigen3_VERSION} " | OPENCV: " ${OpenCV_VERSION} " | BOOST: " ${Boost_VERSION}) # By default we build with ROS, but you can disable this and just build as a library option(ENABLE_ROS "Enable or disable building with ROS (if it is found)" ON) diff --git a/ov_core/cmake/ROS1.cmake b/ov_core/cmake/ROS1.cmake index 93d016424..90422050c 100644 --- a/ov_core/cmake/ROS1.cmake +++ b/ov_core/cmake/ROS1.cmake @@ -23,6 +23,7 @@ endif () # Include our header files include_directories( src + ${EIGEN3_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} diff --git a/ov_core/cmake/ROS2.cmake b/ov_core/cmake/ROS2.cmake index b4ed9b7e3..0deebb990 100644 --- a/ov_core/cmake/ROS2.cmake +++ b/ov_core/cmake/ROS2.cmake @@ -15,6 +15,7 @@ add_definitions(-DROS_AVAILABLE=2) # Include our header files include_directories( src + ${EIGEN3_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIRS} ) diff --git a/ov_eval/CMakeLists.txt b/ov_eval/CMakeLists.txt index f60c1959e..a57104f1d 100644 --- a/ov_eval/CMakeLists.txt +++ b/ov_eval/CMakeLists.txt @@ -4,6 +4,7 @@ project(ov_eval) # Include libraries find_package(Eigen3 REQUIRED) find_package(Boost REQUIRED COMPONENTS system filesystem thread date_time) +message(STATUS "EIGEN: " ${Eigen3_VERSION} " | BOOST: " ${Boost_VERSION}) # By default we build with ROS, but you can disable this and just build as a library option(ENABLE_ROS "Enable or disable building with ROS (if it is found)" ON) diff --git a/ov_eval/cmake/ROS1.cmake b/ov_eval/cmake/ROS1.cmake index 9d10e6f57..ad5fcda89 100644 --- a/ov_eval/cmake/ROS1.cmake +++ b/ov_eval/cmake/ROS1.cmake @@ -23,6 +23,7 @@ endif () # Include our header files include_directories( src + ${EIGEN3_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIRS} ${PYTHON_INCLUDE_DIRS} diff --git a/ov_eval/cmake/ROS2.cmake b/ov_eval/cmake/ROS2.cmake index 59590eed3..e6005011f 100644 --- a/ov_eval/cmake/ROS2.cmake +++ b/ov_eval/cmake/ROS2.cmake @@ -15,6 +15,7 @@ add_definitions(-DROS_AVAILABLE=2) # Include our header files include_directories( src + ${EIGEN3_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIRS} ${PYTHON_INCLUDE_DIRS} diff --git a/ov_init/CMakeLists.txt b/ov_init/CMakeLists.txt index 0e7c58ade..91c176f9a 100644 --- a/ov_init/CMakeLists.txt +++ b/ov_init/CMakeLists.txt @@ -11,7 +11,7 @@ if (NOT OpenCV_FOUND) endif () find_package(Boost REQUIRED COMPONENTS system filesystem thread date_time) find_package(Ceres REQUIRED) -message(STATUS "OPENCV: " ${OpenCV_VERSION} " | BOOST: " ${Boost_VERSION} " | CERES: " ${Ceres_VERSION}) +message(STATUS "EIGEN: " ${Eigen3_VERSION} " | OPENCV: " ${OpenCV_VERSION} " | BOOST: " ${Boost_VERSION} " | CERES: " ${Ceres_VERSION}) # By default we build with ROS, but you can disable this and just build as a library option(ENABLE_ROS "Enable or disable building with ROS (if it is found)" ON) diff --git a/ov_init/cmake/ROS1.cmake b/ov_init/cmake/ROS1.cmake index c83be64c3..3f830b6ff 100644 --- a/ov_init/cmake/ROS1.cmake +++ b/ov_init/cmake/ROS1.cmake @@ -23,6 +23,7 @@ endif () # Include our header files include_directories( src + ${EIGEN3_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIRS} ${CERES_INCLUDE_DIRS} diff --git a/ov_init/cmake/ROS2.cmake b/ov_init/cmake/ROS2.cmake index 0d1634f73..8fc45db09 100644 --- a/ov_init/cmake/ROS2.cmake +++ b/ov_init/cmake/ROS2.cmake @@ -16,6 +16,7 @@ add_definitions(-DROS_AVAILABLE=2) # Include our header files include_directories( src + ${EIGEN3_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIRS} ${CERES_INCLUDE_DIRS} diff --git a/ov_msckf/CMakeLists.txt b/ov_msckf/CMakeLists.txt index 5f8a2eb63..a1a6c0707 100644 --- a/ov_msckf/CMakeLists.txt +++ b/ov_msckf/CMakeLists.txt @@ -10,7 +10,7 @@ if (NOT OpenCV_FOUND) endif () find_package(Boost REQUIRED COMPONENTS system filesystem thread date_time) find_package(Ceres REQUIRED) -message(STATUS "OPENCV: " ${OpenCV_VERSION} " | BOOST: " ${Boost_VERSION} " | CERES: " ${Ceres_VERSION}) +message(STATUS "EIGEN: " ${Eigen3_VERSION} " | OPENCV: " ${OpenCV_VERSION} " | BOOST: " ${Boost_VERSION} " | CERES: " ${Ceres_VERSION}) # By default we build with ROS, but you can disable this and just build as a library option(ENABLE_ROS "Enable or disable building with ROS (if it is found)" ON) diff --git a/ov_msckf/cmake/ROS1.cmake b/ov_msckf/cmake/ROS1.cmake index 2ea4292cf..2b50f9929 100644 --- a/ov_msckf/cmake/ROS1.cmake +++ b/ov_msckf/cmake/ROS1.cmake @@ -24,6 +24,7 @@ endif () # Include our header files include_directories( src + ${EIGEN3_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIRS} ${CERES_INCLUDE_DIRS} diff --git a/ov_msckf/cmake/ROS2.cmake b/ov_msckf/cmake/ROS2.cmake index eacffbb20..12b3d705d 100644 --- a/ov_msckf/cmake/ROS2.cmake +++ b/ov_msckf/cmake/ROS2.cmake @@ -24,6 +24,7 @@ add_definitions(-DROS_AVAILABLE=2) # Include our header files include_directories( src + ${EIGEN3_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIRS} ${CERES_INCLUDE_DIRS} diff --git a/ov_msckf/src/ros/ROS2Visualizer.cpp b/ov_msckf/src/ros/ROS2Visualizer.cpp index e91cd7430..48dd83a9e 100644 --- a/ov_msckf/src/ros/ROS2Visualizer.cpp +++ b/ov_msckf/src/ros/ROS2Visualizer.cpp @@ -36,7 +36,7 @@ using namespace ov_type; using namespace ov_msckf; ROS2Visualizer::ROS2Visualizer(std::shared_ptr node, std::shared_ptr app, std::shared_ptr sim) - : _node(node), _app(app), _sim(sim), thread_update_running(false) { + : _node(node), _app(app), _sim(sim), thread_update_running(false), run_visualize_thread(true) { // Setup our transform broadcaster mTfBr = std::make_shared(node); @@ -149,17 +149,50 @@ ROS2Visualizer::ROS2Visualizer(std::shared_ptr node, std::shared_p // Start thread for the image publishing if (_app->get_params().use_multi_threading_pubs) { - std::thread thread([&] { + thread_visualize = std::make_shared([this] { rclcpp::Rate loop_rate(20); - while (rclcpp::ok()) { + while (rclcpp::ok() && run_visualize_thread) { publish_images(); loop_rate.sleep(); } }); - thread.detach(); } } +ROS2Visualizer::~ROS2Visualizer() { + // Signal and join the visualize thread + run_visualize_thread = false; + if (thread_visualize && thread_visualize->joinable()) { + thread_visualize->join(); + } + + // Manually reset all publishers and subscriptions + it_pub_tracks.shutdown(); + it_pub_loop_img_depth.shutdown(); + it_pub_loop_img_depth_color.shutdown(); + pub_poseimu.reset(); + pub_odomimu.reset(); + pub_pathimu.reset(); + pub_points_msckf.reset(); + pub_points_slam.reset(); + pub_points_aruco.reset(); + pub_points_sim.reset(); + pub_loop_pose.reset(); + pub_loop_extrinsic.reset(); + pub_loop_point.reset(); + pub_loop_intrinsics.reset(); + mTfBr.reset(); + sub_imu.reset(); + for (auto &sub : subs_cam) + sub.reset(); + for (auto &sync : sync_cam) + sync.reset(); + for (auto &sub : sync_subs_cam) + sub.reset(); + pub_pathgt.reset(); + pub_posegt.reset(); +} + void ROS2Visualizer::setup_subscribers(std::shared_ptr parser) { // We need a valid parser diff --git a/ov_msckf/src/ros/ROS2Visualizer.h b/ov_msckf/src/ros/ROS2Visualizer.h index a737f80aa..3086da25b 100644 --- a/ov_msckf/src/ros/ROS2Visualizer.h +++ b/ov_msckf/src/ros/ROS2Visualizer.h @@ -25,7 +25,13 @@ #include #include #include + +#if __has_include() +#include +#else #include +#endif + #include #include #include @@ -42,7 +48,13 @@ #include #include #include + +#if __has_include() +#include +#else #include +#endif + #include #include @@ -53,7 +65,13 @@ #include #include #include + +#if __has_include() +#include +#else #include +#endif + namespace ov_core { class YamlParser; @@ -86,6 +104,11 @@ class ROS2Visualizer { */ ROS2Visualizer(std::shared_ptr node, std::shared_ptr app, std::shared_ptr sim = nullptr); + /** + * @brief Destructor to ensure proper cleanup of ROS 2 resources + */ + ~ROS2Visualizer(); + /** * @brief Will setup ROS subscribers and callbacks * @param parser Configuration file parser @@ -180,6 +203,8 @@ class ROS2Visualizer { // Thread atomics std::atomic thread_update_running; + std::atomic run_visualize_thread; + std::shared_ptr thread_visualize; /// Queue up camera measurements sorted by time and trigger once we have /// exactly one IMU measurement with timestamp newer than the camera measurement diff --git a/ov_msckf/src/ros/ROSVisualizerHelper.h b/ov_msckf/src/ros/ROSVisualizerHelper.h index 14c1514c6..59169ec72 100644 --- a/ov_msckf/src/ros/ROSVisualizerHelper.h +++ b/ov_msckf/src/ros/ROSVisualizerHelper.h @@ -34,9 +34,14 @@ #include #include #include + +#if __has_include() +#include +#else #include #endif +#endif namespace ov_type { class PoseJPL; } diff --git a/ov_msckf/src/run_simulation.cpp b/ov_msckf/src/run_simulation.cpp index 7b5ee4efc..4f9c5cc12 100644 --- a/ov_msckf/src/run_simulation.cpp +++ b/ov_msckf/src/run_simulation.cpp @@ -180,9 +180,15 @@ int main(int argc, char **argv) { // Final visualization #if ROS_AVAILABLE == 1 viz->visualize_final(); + viz.reset(); + sys.reset(); + sim.reset(); ros::shutdown(); #elif ROS_AVAILABLE == 2 viz->visualize_final(); + viz.reset(); + sys.reset(); + sim.reset(); rclcpp::shutdown(); #endif diff --git a/ov_msckf/src/run_subscribe_msckf.cpp b/ov_msckf/src/run_subscribe_msckf.cpp index 6a5b3413c..3d5f4b934 100644 --- a/ov_msckf/src/run_subscribe_msckf.cpp +++ b/ov_msckf/src/run_subscribe_msckf.cpp @@ -115,8 +115,12 @@ int main(int argc, char **argv) { // Final visualization viz->visualize_final(); #if ROS_AVAILABLE == 1 + viz.reset(); + sys.reset(); ros::shutdown(); #elif ROS_AVAILABLE == 2 + viz.reset(); + sys.reset(); rclcpp::shutdown(); #endif diff --git a/ov_msckf/src/update/UpdaterHelper.cpp b/ov_msckf/src/update/UpdaterHelper.cpp index b36c004f7..bc8d1f379 100644 --- a/ov_msckf/src/update/UpdaterHelper.cpp +++ b/ov_msckf/src/update/UpdaterHelper.cpp @@ -367,14 +367,14 @@ void UpdaterHelper::get_feature_jacobian_full(std::shared_ptr state, Upda state->_cam_intrinsics_cameras.at(pair.first)->compute_distort_jacobian(uv_norm, dz_dzn, dz_dzeta); // Normalized coordinates in respect to projection function - Eigen::MatrixXd dzn_dpfc = Eigen::MatrixXd::Zero(2, 3); + Eigen::Matrix dzn_dpfc = Eigen::Matrix::Zero(); dzn_dpfc << 1 / p_FinCi(2), 0, -p_FinCi(0) / (p_FinCi(2) * p_FinCi(2)), 0, 1 / p_FinCi(2), -p_FinCi(1) / (p_FinCi(2) * p_FinCi(2)); // Derivative of p_FinCi in respect to p_FinIi - Eigen::MatrixXd dpfc_dpfg = R_ItoC * R_GtoIi; + Eigen::Matrix3d dpfc_dpfg = R_ItoC * R_GtoIi; // Derivative of p_FinCi in respect to camera clone state - Eigen::MatrixXd dpfc_dclone = Eigen::MatrixXd::Zero(3, 6); + Eigen::Matrix dpfc_dclone = Eigen::Matrix::Zero(); dpfc_dclone.block(0, 0, 3, 3).noalias() = R_ItoC * skew_x(p_FinIi); dpfc_dclone.block(0, 3, 3, 3) = -dpfc_dpfg; @@ -382,8 +382,8 @@ void UpdaterHelper::get_feature_jacobian_full(std::shared_ptr state, Upda //========================================================================= // Precompute some matrices - Eigen::MatrixXd dz_dpfc = dz_dzn * dzn_dpfc; - Eigen::MatrixXd dz_dpfg = dz_dpfc * dpfc_dpfg; + Eigen::Matrix dz_dpfc = dz_dzn * dzn_dpfc; + Eigen::Matrix dz_dpfg = dz_dpfc * dpfc_dpfg; // CHAINRULE: get the total feature Jacobian H_f.block(2 * c, 0, 2, H_f.cols()).noalias() = dz_dpfg * dpfg_dlambda; @@ -404,7 +404,7 @@ void UpdaterHelper::get_feature_jacobian_full(std::shared_ptr state, Upda if (state->_options.do_calib_camera_pose) { // Calculate the Jacobian - Eigen::MatrixXd dpfc_dcalib = Eigen::MatrixXd::Zero(3, 6); + Eigen::Matrix dpfc_dcalib = Eigen::Matrix::Zero(); dpfc_dcalib.block(0, 0, 3, 3) = skew_x(p_FinCi - p_IinC); dpfc_dcalib.block(0, 3, 3, 3) = Eigen::Matrix::Identity();