Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
27 changes: 27 additions & 0 deletions .github/workflows/build_rosfree.yml
Original file line number Diff line number Diff line change
@@ -0,0 +1,27 @@
name: ROS-free Workflow

on:
push:
branches: [ "master" ]
pull_request:
branches: [ "master" ]

jobs:

build_rosfree_linux:
name: "No ROS - Ubuntu 22.04"
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v4
with:
submodules: recursive
- name: Build core + headless CLI with no ROS
run: docker build . --file Dockerfile_rosfree_ubuntu_22_04 --tag mins:rosfree
- name: Run headless CLI (no ROS) end to end
run: |
docker run --rm -e MINS_ROOT=/work/src/MINS mins:rosfree bash -c '
set -eo pipefail
mkdir -p /outputs/tmp/0
timeout 600 /work/build/mins/mins_cli /work/src/MINS/mins/config/simulation/config.yaml 2>&1 | tee /tmp/cli.log
grep -q "RMSE average" /tmp/cli.log
'
30 changes: 30 additions & 0 deletions Dockerfile_rosfree_ubuntu_22_04
Original file line number Diff line number Diff line change
@@ -0,0 +1,30 @@
# Builds the MINS core library + headless CLI with NO ROS at all, to prove the
# core stays ROS-free. Plain cmake, only apt + bundled thirdparty deps.
FROM ubuntu:22.04

ENV DEBIAN_FRONTEND=noninteractive
RUN apt-get update && apt-get install --no-install-recommends -y \
build-essential cmake git ca-certificates \
libeigen3-dev libboost-all-dev libopencv-dev libpcl-dev libyaml-cpp-dev \
&& rm -rf /var/lib/apt/lists/*

WORKDIR /work

# Build the standalone thirdparty libs first (they change rarely) so editing MINS
# source does not rebuild them - COPY them alone before the rest of the tree.
COPY thirdparty/libnabo /work/src/MINS/thirdparty/libnabo
RUN cmake -S src/MINS/thirdparty/libnabo -B build/libnabo \
-DCMAKE_BUILD_TYPE=Release -DLIBNABO_BUILD_TESTS=OFF -DLIBNABO_BUILD_EXAMPLES=OFF -DLIBNABO_BUILD_PYTHON=OFF \
&& cmake --build build/libnabo -j"$(nproc)" && cmake --install build/libnabo

COPY thirdparty/libpointmatcher /work/src/MINS/thirdparty/libpointmatcher
RUN cmake -S src/MINS/thirdparty/libpointmatcher -B build/libpointmatcher \
-DCMAKE_BUILD_TYPE=Release -DBUILD_TESTS=OFF -DBUILD_EXAMPLES=OFF \
&& cmake --build build/libpointmatcher -j"$(nproc)" && cmake --install build/libpointmatcher

# Now the rest of the tree. MINS core + headless CLI, no ROS.
# ov_core is pulled in via add_subdirectory by ROS1.cmake.
COPY . /work/src/MINS
RUN cmake -S src/MINS/mins -B build/mins \
-DCMAKE_BUILD_TYPE=Release -DENABLE_ROS=OFF -DENABLE_ARUCO_TAGS=OFF \
&& cmake --build build/mins -j"$(nproc)"
1 change: 1 addition & 0 deletions ReadMe.md
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
# MINS
[![ROS 1 Workflow](https://github.com/rpng/MINS/actions/workflows/build_ros1.yml/badge.svg)](https://github.com/rpng/MINS/actions/workflows/build_ros1.yml)
[![ROS 2 Workflow](https://github.com/rpng/MINS/actions/workflows/build_ros2.yml/badge.svg)](https://github.com/rpng/MINS/actions/workflows/build_ros2.yml)
[![ROS-free Workflow](https://github.com/rpng/MINS/actions/workflows/build_rosfree.yml/badge.svg)](https://github.com/rpng/MINS/actions/workflows/build_rosfree.yml)

An efficient, robust, and tightly-coupled **Multisensor-aided Inertial Navigation System (MINS)** which is capable of
flexibly fusing all five sensing modalities (**IMU**, **wheel** **encoders**, **camera**, **GNSS**, and **LiDAR**) in a filtering
Expand Down
131 changes: 88 additions & 43 deletions mins/cmake/ROS1.cmake
Original file line number Diff line number Diff line change
@@ -1,38 +1,33 @@
cmake_minimum_required(VERSION 3.5.1)

find_package(catkin REQUIRED COMPONENTS roscpp rosbag tf std_msgs geometry_msgs sensor_msgs nav_msgs image_geometry visualization_msgs image_transport cv_bridge ov_core pcl_ros)
# Find ROS1 (catkin). QUIET (not REQUIRED) so this same file can also configure a
# ROS-free build of the core library when catkin is not present.
find_package(catkin QUIET COMPONENTS roscpp rosbag tf std_msgs geometry_msgs sensor_msgs nav_msgs image_geometry visualization_msgs image_transport cv_bridge ov_core pcl_ros)
option(ENABLE_ROS "Build the ROS integration and nodes when ROS is found" ON)

add_definitions(-DROS_AVAILABLE=1)

# Add catkin packages
catkin_package(
CATKIN_DEPENDS roscpp rosbag tf std_msgs geometry_msgs sensor_msgs nav_msgs image_geometry visualization_msgs image_transport cv_bridge ov_core pcl_ros
INCLUDE_DIRS src/
LIBRARIES mins_lib
)
# When ROS is absent, ov_core (normally a catkin package) is pulled in as a subproject.
# Do it here, before we populate LIBRARY_SOURCES: add_subdirectory shares this scope's
# variables with the child, and ov_core also appends to LIBRARY_SOURCES.
if (NOT (catkin_FOUND AND ENABLE_ROS))
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/open_vins/ov_core ${CMAKE_BINARY_DIR}/ov_core)
endif ()

# Include our header files
include_directories(
src
${EIGEN3_INCLUDE_DIR}
${PCL_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS}
${libpointmatcher_INCLUDE_DIRS}
)

# Set link libraries used by all binaries
list(APPEND thirdparty_libraries
${Boost_LIBRARIES}
${OpenCV_LIBRARIES}
${catkin_LIBRARIES}
${libpointmatcher_LIBRARIES}
)

##################################################
# Make the shared library
##################################################

# Core (ROS-free) library sources
list(APPEND LIBRARY_SOURCES
src/options/Options.cpp
src/options/OptionsCamera.cpp
Expand All @@ -45,15 +40,11 @@ list(APPEND LIBRARY_SOURCES
src/options/OptionsSystem.cpp
src/options/OptionsVicon.cpp
src/options/OptionsWheel.cpp
src/core/ROSPublisher.cpp
src/core/ROSSubscriber.cpp
src/core/ROSHelper.cpp
src/sim/Simulator.cpp
src/sim/ConstBsplineSE3.cpp
src/sim/SimVisualizer.cpp
src/utils/PackagePath.cpp
src/utils/Print_Logger.cpp
src/utils/Jabdongsani.cpp
src/sim/Simulator.cpp
src/sim/ConstBsplineSE3.cpp
src/state/State.cpp
src/state/StateHelper.cpp
src/state/Propagator.cpp
Expand All @@ -74,6 +65,46 @@ list(APPEND LIBRARY_SOURCES
src/init/imu_wheel/IW_Initializer.cpp
)

if (catkin_FOUND AND ENABLE_ROS)
message(STATUS "MINS: building WITH ROS1")
add_definitions(-DROS_AVAILABLE=1)

# Add catkin packages
catkin_package(
CATKIN_DEPENDS roscpp rosbag tf std_msgs geometry_msgs sensor_msgs nav_msgs image_geometry visualization_msgs image_transport cv_bridge ov_core pcl_ros
INCLUDE_DIRS src/
LIBRARIES mins_lib
)
include_directories(${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS})
list(APPEND thirdparty_libraries ${catkin_LIBRARIES})

# ROS integration lives at the boundary - compiled in only when we have ROS
list(APPEND LIBRARY_SOURCES
src/core/ROSPublisher.cpp
src/core/ROSSubscriber.cpp
src/core/ROSHelper.cpp
src/sim/SimVisualizer.cpp
)
else ()
message(WARNING "MINS: building WITHOUT ROS - core library + headless CLI only")
add_definitions(-DROS_AVAILABLE=0)
include(GNUInstallDirs)
set(CATKIN_PACKAGE_LIB_DESTINATION "${CMAKE_INSTALL_LIBDIR}")
set(CATKIN_PACKAGE_BIN_DESTINATION "${CMAKE_INSTALL_BINDIR}")
set(CATKIN_GLOBAL_INCLUDE_DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/mins/")

# ov_core was pulled in above (before LIBRARY_SOURCES); just link + find PCL here.
# (In a ROS build PCL comes via catkin/pcl_ros; standalone we link it ourselves.)
find_package(PCL REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
list(APPEND thirdparty_libraries ov_core_lib ${PCL_LIBRARIES})
endif ()

##################################################
# Make the shared library
##################################################

file(GLOB_RECURSE LIBRARY_HEADERS "src/*.h")
add_library(mins_lib SHARED ${LIBRARY_SOURCES} ${LIBRARY_HEADERS})
Expand All @@ -93,26 +124,40 @@ install(DIRECTORY src/
# Make binary files!
##################################################

add_executable(simulation src/run_simulation.cpp)
target_link_libraries(simulation mins_lib)
install(TARGETS simulation
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
if (catkin_FOUND AND ENABLE_ROS)

add_executable(bag src/run_bag.cpp)
target_link_libraries(bag mins_lib)
install(TARGETS bag
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
add_executable(simulation src/run_simulation.cpp)
target_link_libraries(simulation mins_lib)
install(TARGETS simulation
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

add_executable(subscribe src/run_subscribe.cpp)
target_link_libraries(subscribe mins_lib)
install(TARGETS subscribe
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
add_executable(bag src/run_bag.cpp)
target_link_libraries(bag mins_lib)
install(TARGETS bag
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

add_executable(subscribe src/run_subscribe.cpp)
target_link_libraries(subscribe mins_lib)
install(TARGETS subscribe
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

else ()

# Headless CLI: runs the simulator through the estimator with no ROS, writes
# results to file. This is the ROS-free runnable target.
add_executable(mins_cli src/run_simulation_cli.cpp)
target_link_libraries(mins_cli mins_lib)
install(TARGETS mins_cli
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

endif ()
132 changes: 132 additions & 0 deletions mins/src/run_simulation_cli.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,132 @@
/*
* MINS: Efficient and Robust Multisensor-aided Inertial Navigation System
* Copyright (C) 2023 Woosik Lee
* Copyright (C) 2023 Guoquan Huang
* Copyright (C) 2023 MINS Contributors
*
* This code is implemented based on:
* OpenVINS: An Open Platform for Visual-Inertial Research
* Copyright (C) 2018-2023 Patrick Geneva
* Copyright (C) 2018-2023 Guoquan Huang
* Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/

// Headless simulation runner - no ROS. Runs the simulator through the estimator
// and prints/saves accuracy, so the core can be built and exercised without ROS.

#include <memory>

#include "core/SystemManager.h"
#include "options/Options.h"
#include "options/OptionsEstimator.h"
#include "options/OptionsGPS.h"
#include "options/OptionsSystem.h"
#include "sim/Simulator.h"
#include "state/State.h"
#include "state/StateHelper.h"
#include "update/cam/CamTypes.h"
#include "update/gps/GPSTypes.h"
#include "update/gps/UpdaterGPS.h"
#include "update/vicon/ViconTypes.h"
#include "update/wheel/WheelTypes.h"
#include "utils/Print_Logger.h"
#include "utils/State_Logger.h"
#include "utils/TimeChecker.h"
#include "utils/colors.h"
#include "utils/opencv_yaml_parse.h"
#include "utils/sensor_data.h"
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

using namespace mins;
using namespace std;
using namespace Eigen;

int main(int argc, char **argv) {
// Config path comes from argv only (no ROS param server in this build)
string config_path = "unset_path_to_config.yaml";
if (argc > 1)
config_path = argv[1];

auto parser = make_shared<ov_core::YamlParser>(config_path);
auto op = make_shared<Options>();
op->load_print(parser);
op->sys->save_prints ? Print_Logger::open_file(op->sys->path_state, true) : void();

auto sim = make_shared<Simulator>(op);
auto sys = make_shared<SystemManager>(op->est, sim);
auto save = make_shared<State_Logger>(op, sim);

if (!parser->successful()) {
PRINT4(RED "unable to parse all parameters, please fix\n" RESET);
exit(EXIT_FAILURE);
}

// Running accuracy (same math the ROS visualizer does, minus the publishing)
double sum_rmse_ori = 0, sum_rmse_pos = 0, sum_nees_ori = 0, sum_nees_pos = 0;
int sum_cnt = 0;

while (sim->ok()) {
// IMU: propagate, and on a filter update print/accumulate accuracy
ov_core::ImuData imu;
if (sim->get_next_imu(imu)) {
if (sys->feed_measurement_imu(imu)) {
sim->trans_gt_to_ENU = op->est->gps->enabled && sys->up_gps->initialized;
auto imu_pose = sys->state->imu->pose();
Vector4d rn = sim->imu_rmse_nees(sys->state->time, imu_pose->value(), StateHelper::get_marginal_covariance(sys->state, {imu_pose}));
if (!isnan(rn(2)) && !isnan(rn(3))) {
sum_rmse_ori += rn(0);
sum_rmse_pos += rn(1);
sum_nees_ori += rn(2);
sum_nees_pos += rn(3);
sum_cnt++;
}
PRINT2("\033[A%.2f | RMSE: %.3f, %.3f (deg,m) | RMSE avg: %.3f, %.3f | NEES avg: %.1f, %.1f\n\n", sys->state->time, rn(0), rn(1),
sum_rmse_ori / sum_cnt, sum_rmse_pos / sum_cnt, sum_nees_ori / sum_cnt, sum_nees_pos / sum_cnt);
op->sys->save_state ? save->save_state_to_file(sys, sim) : void();
op->sys->save_trajectory ? save->save_trajectory_to_file(sys) : void();
}
}

CamSimData cam;
if (sim->get_next_cam(cam))
sys->feed_measurement_camsim(cam);

GPSData gps;
if (sim->get_next_gps(gps))
sys->feed_measurement_gps(gps, false);

WheelData wheel;
if (sim->get_next_wheel(wheel))
sys->feed_measurement_wheel(wheel);

std::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> lidar(new pcl::PointCloud<pcl::PointXYZ>);
if (sim->get_next_lidar(lidar))
sys->feed_measurement_lidar(lidar);

ViconData vicon;
if (sim->get_next_vicon(vicon))
sys->feed_measurement_vicon(vicon);
}

sys->visualize_final();
PRINT2(BOLDYELLOW "RMSE average: %.3f, %.3f (deg,m)\n" RESET, sum_rmse_ori / sum_cnt, sum_rmse_pos / sum_cnt);
PRINT2(BOLDYELLOW "NEES average: %.3f, %.3f (deg,m)\n" RESET, sum_nees_ori / sum_cnt, sum_nees_pos / sum_cnt);
op->sys->save_timing ? save->save_timing_to_file(sys->tc_sensors->get_total_sum()) : void();
save->check_files();
return EXIT_SUCCESS;
}
6 changes: 3 additions & 3 deletions mins/src/sim/Simulator.h
Original file line number Diff line number Diff line change
Expand Up @@ -128,6 +128,9 @@ class Simulator {
/// boolean for transforming groundtruth after GPS initialization
bool trans_gt_to_ENU = false;

/// Returns RMSE and NEES of IMU pose (ori rmse, pos rmse, ori nees, pos nees)
Vector4d imu_rmse_nees(double time, Matrix<double, 7, 1> imu, Matrix<double, 6, 6> cov);

protected:
friend class Initializer;
friend class SimVisualizer;
Expand All @@ -143,9 +146,6 @@ class Simulator {
/// Returns the true 3d map of camera features
std::unordered_map<size_t, Vector3d> get_cam_map();

/// Returns RMSE and NEES of IMU pose (ori rmse, pos rmse, ori nees, pos nees)
Vector4d imu_rmse_nees(double time, Matrix<double, 7, 1> imu, Matrix<double, 6, 6> cov);

/// a wrapper function of spline that returns IMU pose & velocities & accelerations
bool get_imu_acceleration(double timestamp, Matrix3d &R_GtoI, Vector3d &p_IinG, Vector3d &w_IinI, Vector3d &v_IinG, Vector3d &alpha_IinI, Vector3d &a_IinG);

Expand Down
Loading
Loading