From 6d825b6b8cce0dbbee757d8eb936c12e375067f6 Mon Sep 17 00:00:00 2001 From: Sandro Covo Date: Wed, 7 Aug 2024 09:16:10 +0200 Subject: [PATCH] Port to ros2 --- Dockerfile_humble_22_04 | 27 ++ build_ros2.sh | 10 + mins/CMakeLists.txt | 128 +------ mins/cmake/ROS1.cmake | 117 ++++++ mins/cmake/ROS2.cmake | 151 ++++++++ mins/package.xml | 15 +- mins/src/core/ROS2Helper.cpp | 330 ++++++++++++++++ mins/src/core/ROS2Helper.h | 114 ++++++ mins/src/core/ROS2Publisher.cpp | 504 +++++++++++++++++++++++++ mins/src/core/ROS2Publisher.h | 131 +++++++ mins/src/core/ROS2Subscriber.cpp | 228 +++++++++++ mins/src/core/ROS2Subscriber.h | 113 ++++++ mins/src/core/SystemManager.cpp | 15 +- mins/src/core/SystemManager.h | 15 + mins/src/options/OptionsCamera.cpp | 5 + mins/src/options/OptionsCamera.h | 2 + mins/src/options/OptionsSimulation.cpp | 8 +- mins/src/options/OptionsSystem.cpp | 9 +- mins/src/run_simulation.cpp | 43 ++- mins/src/run_subscribe.cpp | 41 +- mins/src/sim/Sim2Visualizer.cpp | 262 +++++++++++++ mins/src/sim/Sim2Visualizer.h | 88 +++++ mins/src/sim/Simulator.h | 1 + mins_data/CMakeLists.txt | 11 +- mins_data/package.xml | 6 +- mins_eval/CMakeLists.txt | 80 +--- mins_eval/cmake/ROS1.cmake | 66 ++++ mins_eval/cmake/ROS2.cmake | 83 ++++ mins_eval/package.xml | 12 +- mins_eval/src/plot_consistency.cpp | 18 +- mins_eval/src/run_comparison.cpp | 105 +++--- 31 files changed, 2462 insertions(+), 276 deletions(-) create mode 100644 Dockerfile_humble_22_04 create mode 100644 build_ros2.sh create mode 100644 mins/cmake/ROS1.cmake create mode 100644 mins/cmake/ROS2.cmake create mode 100644 mins/src/core/ROS2Helper.cpp create mode 100644 mins/src/core/ROS2Helper.h create mode 100644 mins/src/core/ROS2Publisher.cpp create mode 100644 mins/src/core/ROS2Publisher.h create mode 100644 mins/src/core/ROS2Subscriber.cpp create mode 100644 mins/src/core/ROS2Subscriber.h create mode 100644 mins/src/sim/Sim2Visualizer.cpp create mode 100644 mins/src/sim/Sim2Visualizer.h create mode 100644 mins_eval/cmake/ROS1.cmake create mode 100644 mins_eval/cmake/ROS2.cmake diff --git a/Dockerfile_humble_22_04 b/Dockerfile_humble_22_04 new file mode 100644 index 0000000..3b5ddc4 --- /dev/null +++ b/Dockerfile_humble_22_04 @@ -0,0 +1,27 @@ +FROM ros:humble-perception-jammy + +RUN rm -f /etc/apt/apt.conf.d/docker-clean +RUN --mount=type=cache,target=/var/cache/apt apt-get update --fix-missing && apt-get upgrade -y + +RUN --mount=type=cache,target=/var/cache/apt\ + DEBIAN_FRONTEND=noninteractive \ + apt-get install --no-install-recommends --yes \ + python3-colcon-common-extensions \ + python3-rosdep + +WORKDIR /work + +RUN useradd -ms /bin/bash ros &&\ + chown ros:ros /work &&\ + usermod -aG sudo ros &&\ + echo "ros ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers &&\ + echo "ros:ros" | chpasswd + +USER ros + +COPY . /work/src/MINS + +RUN bash /work/src/MINS/build_ros2.sh + +RUN echo "source '/work/install/setup.bash'" >> ~/.bashrc + diff --git a/build_ros2.sh b/build_ros2.sh new file mode 100644 index 0000000..07fce44 --- /dev/null +++ b/build_ros2.sh @@ -0,0 +1,10 @@ +#!/bin/bash + +source /opt/ros/humble/setup.bash +colcon build --paths thirdparty/* +source install/setup.bash +colcon build --paths thirdparty/open_vins/* +source install/setup.bash +colcon build --paths mins mins_data +source install/setup.bash +colcon build --paths mins_eval diff --git a/mins/CMakeLists.txt b/mins/CMakeLists.txt index 4b73601..778eba5 100644 --- a/mins/CMakeLists.txt +++ b/mins/CMakeLists.txt @@ -33,119 +33,15 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-sign # Enable debug flags (use if you want to debug in gdb) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g3 -Wall -Wuninitialized -Wmaybe-uninitialized -fno-omit-frame-pointer") -# Find ROS build system -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) - -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 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 -################################################## - -list(APPEND LIBRARY_SOURCES - src/options/Options.cpp - src/options/OptionsCamera.cpp - src/options/OptionsEstimator.cpp - src/options/OptionsGPS.cpp - src/options/OptionsIMU.cpp - src/options/OptionsInit.cpp - src/options/OptionsLidar.cpp - src/options/OptionsSimulation.cpp - 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/Print_Logger.cpp - src/utils/Jabdongsani.cpp - src/state/State.cpp - src/state/StateHelper.cpp - src/state/Propagator.cpp - src/core/SystemManager.cpp - src/update/cam/CamTypes.cpp - src/update/cam/CamHelper.cpp - src/update/cam/UpdaterCamera.cpp - src/update/vicon/UpdaterVicon.cpp - src/update/gps/UpdaterGPS.cpp - src/update/wheel/UpdaterWheel.cpp - src/update/lidar/ikd_Tree.cpp - src/update/lidar/UpdaterLidar.cpp - src/update/lidar/LidarHelper.cpp - src/update/lidar/LidarTypes.cpp - src/update/UpdaterStatistics.cpp - src/init/Initializer.cpp - src/init/imu/I_Initializer.cpp - src/init/imu_wheel/IW_Initializer.cpp - ) - - -file(GLOB_RECURSE LIBRARY_HEADERS "src/*.h") -add_library(mins_lib SHARED ${LIBRARY_SOURCES} ${LIBRARY_HEADERS}) -target_link_libraries(mins_lib ${thirdparty_libraries}) -target_include_directories(mins_lib PUBLIC src/) -install(TARGETS mins_lib - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) -install(DIRECTORY src/ - DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" - ) - -################################################## -# 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} - ) - -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} - ) \ No newline at end of file +find_package(catkin QUIET COMPONENTS roscpp) +find_package(ament_cmake QUIET) +if (catkin_FOUND) + message(STATUS "ROS *1* version found, building ROS1.cmake") + include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/ROS1.cmake) +elseif (ament_cmake_FOUND) + message(STATUS "ROS *2* version found, building ROS2.cmake") + include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/ROS2.cmake) +else () + message(STATUS "No ROS versions found, building ROS1.cmake") + include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/ROS1.cmake) +endif () \ No newline at end of file diff --git a/mins/cmake/ROS1.cmake b/mins/cmake/ROS1.cmake new file mode 100644 index 0000000..8960daf --- /dev/null +++ b/mins/cmake/ROS1.cmake @@ -0,0 +1,117 @@ +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) + +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 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 +################################################## + +list(APPEND LIBRARY_SOURCES + src/options/Options.cpp + src/options/OptionsCamera.cpp + src/options/OptionsEstimator.cpp + src/options/OptionsGPS.cpp + src/options/OptionsIMU.cpp + src/options/OptionsInit.cpp + src/options/OptionsLidar.cpp + src/options/OptionsSimulation.cpp + 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/Print_Logger.cpp + src/utils/Jabdongsani.cpp + src/state/State.cpp + src/state/StateHelper.cpp + src/state/Propagator.cpp + src/core/SystemManager.cpp + src/update/cam/CamTypes.cpp + src/update/cam/CamHelper.cpp + src/update/cam/UpdaterCamera.cpp + src/update/vicon/UpdaterVicon.cpp + src/update/gps/UpdaterGPS.cpp + src/update/wheel/UpdaterWheel.cpp + src/update/lidar/ikd_Tree.cpp + src/update/lidar/UpdaterLidar.cpp + src/update/lidar/LidarHelper.cpp + src/update/lidar/LidarTypes.cpp + src/update/UpdaterStatistics.cpp + src/init/Initializer.cpp + src/init/imu/I_Initializer.cpp + src/init/imu_wheel/IW_Initializer.cpp + ) + + +file(GLOB_RECURSE LIBRARY_HEADERS "src/*.h") +add_library(mins_lib SHARED ${LIBRARY_SOURCES} ${LIBRARY_HEADERS}) +target_link_libraries(mins_lib ${thirdparty_libraries}) +target_include_directories(mins_lib PUBLIC src/) +install(TARGETS mins_lib + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) +install(DIRECTORY src/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" + ) + +################################################## +# 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} + ) + +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} + ) \ No newline at end of file diff --git a/mins/cmake/ROS2.cmake b/mins/cmake/ROS2.cmake new file mode 100644 index 0000000..f14574b --- /dev/null +++ b/mins/cmake/ROS2.cmake @@ -0,0 +1,151 @@ +cmake_minimum_required(VERSION 3.3) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rosbag2 REQUIRED COMPONENTS rmw_adapters message_filters rosbag2_storage_builtin) +find_package(tf2_ros REQUIRED COMPONENTS tf2_cpp) # Replaces tf + +# Find message packages (geometry_msgs, sensor_msgs, etc.) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(image_geometry REQUIRED) # Might require additional setup +find_package(visualization_msgs REQUIRED) +find_package(image_transport REQUIRED) # Might require additional setup +find_package(cv_bridge REQUIRED) # Might require additional setup +find_package(pcl_conversions REQUIRED) + +# Other dependencies (if available through ROS 2 packages) +# find_package(pcl_ros2 REQUIRED) # Might require building ROS 2 wrapper for PCL +find_package(ov_core REQUIRED) # Might not be available as a ROS 2 package + +add_definitions(-DROS_AVAILABLE=2) +ament_export_dependencies(rclcpp rosbag2 tf2_ros std_msgs geometry_msgs sensor_msgs nav_msgs std_srvs image_geometry visualization_msgs image_transport cv_bridge ov_core pcl_conversions) +# ament_export_include_directories(src/) +ament_export_libraries(mins_lib) + +list(APPEND ament_libraries + rclcpp + rosbag2 + tf2_ros + std_msgs + std_srvs + geometry_msgs + visualization_msgs + sensor_msgs + nav_msgs + cv_bridge + image_transport + ov_core + pcl_conversions + image_geometry +) + + +# Include our header files +include_directories( + src + ${EIGEN3_INCLUDE_DIR} + ${Boost_INCLUDE_DIRS} + ${libpointmatcher_INCLUDE_DIRS} +) + +# Set link libraries used by all binaries +list(APPEND thirdparty_libraries + ${Boost_LIBRARIES} + ${OpenCV_LIBRARIES} + ${libnabo_LIBRARIES} + ${PCL_LIBRARIES} + ${libpointmatcher_LIBRARIES} + ) + +################################################## +# Make the shared library +################################################## + +list(APPEND LIBRARY_SOURCES + src/options/Options.cpp + src/options/OptionsCamera.cpp + src/options/OptionsEstimator.cpp + src/options/OptionsGPS.cpp + src/options/OptionsIMU.cpp + src/options/OptionsInit.cpp + src/options/OptionsLidar.cpp + src/options/OptionsSimulation.cpp + src/options/OptionsSystem.cpp + src/options/OptionsVicon.cpp + src/options/OptionsWheel.cpp + src/core/ROS2Publisher.cpp + src/core/ROS2Subscriber.cpp + src/core/ROS2Helper.cpp + src/sim/Simulator.cpp + src/sim/ConstBsplineSE3.cpp + src/sim/Sim2Visualizer.cpp + src/utils/Print_Logger.cpp + src/utils/Jabdongsani.cpp + src/state/State.cpp + src/state/StateHelper.cpp + src/state/Propagator.cpp + src/core/SystemManager.cpp + src/update/cam/CamTypes.cpp + src/update/cam/CamHelper.cpp + src/update/cam/UpdaterCamera.cpp + src/update/vicon/UpdaterVicon.cpp + src/update/gps/UpdaterGPS.cpp + src/update/wheel/UpdaterWheel.cpp + src/update/lidar/ikd_Tree.cpp + src/update/lidar/UpdaterLidar.cpp + src/update/lidar/LidarHelper.cpp + src/update/lidar/LidarTypes.cpp + src/update/UpdaterStatistics.cpp + src/init/Initializer.cpp + src/init/imu/I_Initializer.cpp + src/init/imu_wheel/IW_Initializer.cpp + ) + + +file(GLOB_RECURSE LIBRARY_HEADERS "src/*.h") +add_library(mins_lib SHARED ${LIBRARY_SOURCES} ${LIBRARY_HEADERS}) +ament_target_dependencies(mins_lib ${ament_libraries}) +target_link_libraries(mins_lib ${thirdparty_libraries} ${rclcpp_LIBRARIES}) + +# target_include_directories(mins_lib PUBLIC src/) + +target_include_directories(mins_lib PUBLIC + $ + $ +) + +install(TARGETS mins_lib + EXPORT mins_targets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +install(DIRECTORY + DESTINATION include + FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" +) +ament_export_include_directories(include) +ament_export_targets(mins_targets) + +# Add packages +################################################## +# Make binary files! +################################################## +add_executable(simulation src/run_simulation.cpp) +target_link_libraries(simulation mins_lib) + +add_executable(subscribe src/run_subscribe.cpp) +target_link_libraries(subscribe mins_lib) + +# Install executables +install(TARGETS subscribe simulation DESTINATION lib/${PROJECT_NAME}) + +install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch/) +install(DIRECTORY config/ DESTINATION share/${PROJECT_NAME}/config/) + +ament_package() \ No newline at end of file diff --git a/mins/package.xml b/mins/package.xml index f328204..8e17346 100644 --- a/mins/package.xml +++ b/mins/package.xml @@ -16,10 +16,14 @@ GNU General Public License v3.0 - catkin + catkin + ament_cmake cmake_modules - roscpp - rosbag + roscpp + rosbag + + rclcpp + tf std_msgs sensor_msgs @@ -30,12 +34,11 @@ image_transport cv_bridge ov_core - pcl_ros - libpointmatcher - catkin + catkin + ament_cmake diff --git a/mins/src/core/ROS2Helper.cpp b/mins/src/core/ROS2Helper.cpp new file mode 100644 index 0000000..c299310 --- /dev/null +++ b/mins/src/core/ROS2Helper.cpp @@ -0,0 +1,330 @@ +/* + * 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 . + */ + +#include "ROS2Helper.h" +#include "options/OptionsCamera.h" +#include "pcl_conversions/pcl_conversions.h" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "sensor_msgs/point_cloud2_iterator.hpp" +#include "state/State.h" +#include "types/PoseJPL.h" +#include "types/Vec.h" +#include "update/gps/GPSTypes.h" +#include "update/vicon/ViconTypes.h" +#include "update/wheel/WheelTypes.h" +#include "utils/Print_Logger.h" +#include +#include + +using namespace sensor_msgs::msg; +using namespace mins; +PointCloud2 ROS2Helper::ToPointcloud(const std::vector &feats, std::string frame_id) { + + // Declare message and sizes + PointCloud2 cloud; + cloud.header.frame_id = frame_id; + cloud.header.stamp = rclcpp::Clock().now(); + cloud.width = feats.size(); + cloud.height = 1; + cloud.is_bigendian = false; + cloud.is_dense = false; // there may be invalid points + // Setup pointcloud fields + PointCloud2Modifier modifier(cloud); + modifier.setPointCloud2FieldsByString(1, "xyz"); + modifier.resize(feats.size()); + + // Iterators + PointCloud2Iterator out_x(cloud, "x"); + PointCloud2Iterator out_y(cloud, "y"); + PointCloud2Iterator out_z(cloud, "z"); + + // Fill our iterators + for (const auto &pt : feats) { + *out_x = (float)pt(0); + ++out_x; + *out_y = (float)pt(1); + ++out_y; + *out_z = (float)pt(2); + ++out_z; + } + + return cloud; +} + +geometry_msgs::msg::TransformStamped ROS2Helper::Pose2TF(const shared_ptr &pose, bool flip_trans) { + + // Need to flip the transform to the imu frame + Eigen::Vector4d q_ItoC = pose->quat(); + Eigen::Vector3d p_CinI = pose->pos(); + if (flip_trans) { + p_CinI = -pose->Rot().transpose() * pose->pos(); + } + + // publish our transform on TF + // NOTE: since we use JPL we have an implicit conversion to Hamilton when we publish + // NOTE: a rotation from ItoC in JPL has the same xyzw as a CtoI Hamilton rotation + geometry_msgs::msg::TransformStamped trans; + trans.header.stamp = rclcpp::Clock().now(); + geometry_msgs::msg::Quaternion quat; + quat.x = q_ItoC(0); + quat.y = q_ItoC(1); + quat.z = q_ItoC(2); + quat.w = q_ItoC(3); + trans.transform.rotation = quat; + geometry_msgs::msg::Vector3 orig; + orig.x = p_CinI(0); + orig.y = p_CinI(1); + orig.z = p_CinI(2); + trans.transform.translation = orig; + return trans; +} + +geometry_msgs::msg::TransformStamped ROS2Helper::Pos2TF(const shared_ptr &pos, bool flip_trans) { + assert(pos->size() == 3); + // Need to flip the transform to the imu frame + Eigen::Vector3d p_XinI = pos->value(); + + // publish our transform on TF + geometry_msgs::msg::TransformStamped trans; + trans.header.stamp = rclcpp::Clock().now(); + geometry_msgs::msg::Quaternion quat; + quat.w = 1; + trans.transform.rotation = quat; + + geometry_msgs::msg::Vector3 orig; + orig.x = p_XinI(0); + orig.y = p_XinI(1); + orig.z = p_XinI(2); + + trans.transform.translation = orig; + return trans; +} + +ov_core::ImuData ROS2Helper::Imu2Data(const sensor_msgs::msg::Imu::SharedPtr msg) { + ov_core::ImuData message; + message.timestamp = rclcpp::Time(msg->header.stamp).seconds(); + message.wm << msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z; + message.am << msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z; + return message; +} + +std::shared_ptr> ROS2Helper::rosPC2pclPC(const sensor_msgs::msg::PointCloud2::SharedPtr msg, int id) { + std::shared_ptr> pcl_pc2(new pcl::PointCloud); + pcl::fromROSMsg(*msg, *pcl_pc2); + pcl_pc2->header.frame_id = to_string(id); // overwrite the id match with system number + pcl_pc2->header.stamp = rclcpp::Time(msg->header.stamp).seconds() * 1000; // deliver this in msec + return pcl_pc2; +} + +GPSData ROS2Helper::NavSatFix2Data(const sensor_msgs::msg::NavSatFix::SharedPtr msg, int id) { + GPSData data; + data.time = rclcpp::Time(msg->header.stamp).seconds(); + data.id = id; + data.meas(0) = msg->latitude; + data.meas(1) = msg->longitude; + data.meas(2) = msg->altitude; + data.noise(0) = msg->position_covariance.at(0); + data.noise(1) = msg->position_covariance.at(4); + data.noise(2) = msg->position_covariance.at(8); + return data; +} + +GPSData ROS2Helper::NavSatFix2Data(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg, int id) { + GPSData data; + data.time = rclcpp::Time(msg->header.stamp).seconds(); + data.id = id; + data.meas(0) = msg->latitude; + data.meas(1) = msg->longitude; + data.meas(2) = msg->altitude; + data.noise(0) = msg->position_covariance.at(0); + data.noise(1) = msg->position_covariance.at(4); + data.noise(2) = msg->position_covariance.at(8); + return data; +} + +bool ROS2Helper::Image2Data(const sensor_msgs::msg::Image::ConstSharedPtr msg, int cam_id, ov_core::CameraData &cam, shared_ptr op) { + + cv_bridge::CvImageConstPtr cv_ptr; + try { + cv_ptr = cv_bridge::toCvShare(msg, image_encodings::MONO8); + } catch (cv_bridge::Exception &e) { + PRINT4("cv_bridge exception: %s", e.what()); + return false; + } + + // Create the measurement + cam.timestamp = rclcpp::Time(cv_ptr->header.stamp).seconds(); + cam.sensor_ids.push_back(cam_id); + cam.images.push_back(cv_ptr->image.clone()); + + // Load the mask if we are using it, else it is empty + if (op->use_mask.at(cam_id)) { + cam.masks.push_back(op->masks.at(cam_id)); + } else { + cam.masks.push_back(cv::Mat::zeros(cv_ptr->image.rows, cv_ptr->image.cols, CV_8UC1)); + } + return true; +} + +WheelData ROS2Helper::JointState2Data(const sensor_msgs::msg::JointState::SharedPtr msg) { + WheelData data; + data.time = rclcpp::Time(msg->header.stamp).seconds(); + data.m1 = msg->velocity.at(0); // front_left_wheel_joint velocity + data.m2 = msg->velocity.at(1); // front_right_wheel_joint velocity + return data; +} + +WheelData ROS2Helper::Odometry2Data(const nav_msgs::msg::Odometry::SharedPtr msg) { + WheelData data; + data.time = rclcpp::Time(msg->header.stamp).seconds(); + data.m1 = msg->twist.twist.angular.z; + data.m2 = msg->twist.twist.linear.x; + return data; +} + + + +ViconData ROS2Helper::PoseStamped2Data(const geometry_msgs::msg::PoseStamped::SharedPtr msg, int id) { + Eigen::Vector4d q(msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, msg->pose.orientation.w); + Eigen::Vector3d p(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z); + ViconData vicon; + vicon.time = rclcpp::Time(msg->header.stamp).seconds(); + vicon.id = id; + vicon.pose.block(0, 0, 3, 1) = ov_core::log_so3(ov_core::quat_2_Rot(q)); + vicon.pose.block(3, 0, 3, 1) = p; + + return vicon; +} + +ViconData ROS2Helper::PoseStamped2Data(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg, int id) { + + Eigen::Vector4d q(msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w); + Eigen::Vector3d p(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z); + ViconData vicon; + vicon.time = rclcpp::Time(msg->header.stamp).seconds(); + vicon.id = id; + vicon.pose.block(0, 0, 3, 1) = ov_core::log_so3(ov_core::quat_2_Rot(q)); + vicon.pose.block(3, 0, 3, 1) = p; + + return vicon; +} + +GPSData ROS2Helper::PoseStamped2Data(const geometry_msgs::msg::PoseStamped::SharedPtr msg, int id, double noise) { + Eigen::Vector3d p(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z); + GPSData gps; + gps.time = rclcpp::Time(msg->header.stamp).seconds(); + gps.id = id; + gps.meas = p; + gps.noise = noise * Eigen::Vector3d::Ones(); + return gps; +} + +bool ROS2Helper::Image2Data(const sensor_msgs::msg::CompressedImage::ConstSharedPtr msg, int cam_id, ov_core::CameraData &cam, shared_ptr op) { + + // Create the measurement + cam.timestamp = rclcpp::Time(msg->header.stamp).seconds(); + cam.sensor_ids.push_back(cam_id); + cam.images.push_back(cv::imdecode(cv::Mat(msg->data), 0)); + + // Load the mask if we are using it, else it is empty + // TODO: in the future we should get this from external pixel segmentation + if (op->use_mask.at(cam_id)) { + cam.masks.push_back(op->masks.at(cam_id)); + } else { + cam.masks.push_back(cv::Mat::zeros(cam.images[0].rows, cam.images[0].cols, CV_8UC1)); + } + return true; +} + +nav_msgs::msg::Odometry ROS2Helper::ToOdometry(Eigen::Matrix state) { + nav_msgs::msg::Odometry odom; + // The POSE component (orientation and position) + odom.pose.pose.orientation.x = state(0); + odom.pose.pose.orientation.y = state(1); + odom.pose.pose.orientation.z = state(2); + odom.pose.pose.orientation.w = state(3); + odom.pose.pose.position.x = state(4); + odom.pose.pose.position.y = state(5); + odom.pose.pose.position.z = state(6); + // The TWIST component (angular and linear velocities) + odom.twist.twist.linear.x = state(7); + odom.twist.twist.linear.y = state(8); + odom.twist.twist.linear.z = state(9); + odom.twist.twist.angular.x = state(10); + odom.twist.twist.angular.y = state(11); + odom.twist.twist.angular.z = state(12); + return odom; +} + +geometry_msgs::msg::PoseWithCovarianceStamped ROS2Helper::ToPoseCov(Eigen::Matrix state) { + geometry_msgs::msg::PoseWithCovarianceStamped odom; + // The POSE component (orientation and position) + odom.pose.pose.orientation.x = state(0); + odom.pose.pose.orientation.y = state(1); + odom.pose.pose.orientation.z = state(2); + odom.pose.pose.orientation.w = state(3); + odom.pose.pose.position.x = state(4); + odom.pose.pose.position.y = state(5); + odom.pose.pose.position.z = state(6); + return odom; +} + +geometry_msgs::msg::PoseStamped ROS2Helper::ToENU(geometry_msgs::msg::PoseStamped pose, Eigen::Matrix trans_WtoE) { + // Take the readings out + Vector4d q_WtoI; + q_WtoI(0) = pose.pose.orientation.x; + q_WtoI(1) = pose.pose.orientation.y; + q_WtoI(2) = pose.pose.orientation.z; + q_WtoI(3) = pose.pose.orientation.w; + Matrix3d R_WtoI = ov_core::quat_2_Rot(q_WtoI); + Vector3d p_IinW; + p_IinW(0) = pose.pose.position.x; + p_IinW(1) = pose.pose.position.y; + p_IinW(2) = pose.pose.position.z; + + // Get the transformation + Matrix3d R_WtoE = ov_core::quat_2_Rot(trans_WtoE.head(4)); + Vector3d p_WinE = trans_WtoE.tail(3); + + // Compute pose in ENU + Matrix3d R_EtoI = R_WtoI * R_WtoE.transpose(); + Vector4d q_EtoI = ov_core::rot_2_quat(R_EtoI); + Vector3d p_IinE = p_WinE + R_WtoE * p_IinW; + + // construct pose message and return + geometry_msgs::msg::PoseStamped poseENU; + poseENU.header = pose.header; + poseENU.pose.orientation.x = q_EtoI(0); + poseENU.pose.orientation.y = q_EtoI(1); + poseENU.pose.orientation.z = q_EtoI(2); + poseENU.pose.orientation.w = q_EtoI(3); + poseENU.pose.position.x = p_IinE(0); + poseENU.pose.position.y = p_IinE(1); + poseENU.pose.position.z = p_IinE(2); + return poseENU; +} \ No newline at end of file diff --git a/mins/src/core/ROS2Helper.h b/mins/src/core/ROS2Helper.h new file mode 100644 index 0000000..01cb435 --- /dev/null +++ b/mins/src/core/ROS2Helper.h @@ -0,0 +1,114 @@ +/* + * 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 . + */ + +#ifndef MINS_ROSVISUALIZER_HELPER_H +#define MINS_ROSVISUALIZER_HELPER_H + +#include "rclcpp/rclcpp.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace sensor_msgs; + +namespace ov_core { +struct ImuData; +struct CameraData; +} // namespace ov_core +namespace ov_type { +struct PoseJPL; +struct Vec; +} // namespace ov_type +namespace pcl { +class PointXYZ; +template class PointCloud; +} // namespace pcl + +namespace mins { +class State; +struct OptionsCamera; +struct WheelData; +struct RoverWheelData; +struct ViconData; +struct GPSData; +struct TLIOData; + +/** + * @brief Helper class that handles some common versions into and out of ROS formats + */ +class ROS2Helper { + +public: + /// Collection of format converters + static sensor_msgs::msg::PointCloud2 ToPointcloud(const vector &feats, string frame_id); + + static geometry_msgs::msg::TransformStamped Pose2TF(const shared_ptr &pose, bool flip_trans); + + static geometry_msgs::msg::TransformStamped Pos2TF(const shared_ptr &pos, bool flip_trans); + + static ov_core::ImuData Imu2Data(const sensor_msgs::msg::Imu::SharedPtr msg); + + static bool Image2Data(const sensor_msgs::msg::Image::ConstSharedPtr msg, int cam_id, ov_core::CameraData &cam, shared_ptr op); + + static bool Image2Data(const sensor_msgs::msg::CompressedImage::ConstSharedPtr msg, int cam_id, ov_core::CameraData &cam, shared_ptr op); + + static WheelData JointState2Data(const sensor_msgs::msg::JointState::SharedPtr msg); + + static WheelData Odometry2Data(const nav_msgs::msg::Odometry::SharedPtr msg); + + static ViconData PoseStamped2Data(const geometry_msgs::msg::PoseStamped::SharedPtr msg, int id); + + static ViconData PoseStamped2Data(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg, int id); + + static GPSData PoseStamped2Data(const geometry_msgs::msg::PoseStamped::SharedPtr msg, int id, double noise); + + static std::shared_ptr> rosPC2pclPC(const sensor_msgs::msg::PointCloud2::SharedPtr msg, int id); + + static GPSData NavSatFix2Data(const sensor_msgs::msg::NavSatFix::SharedPtr msg, int id); + + static GPSData NavSatFix2Data(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg, int id); + + static nav_msgs::msg::Odometry ToOdometry(Eigen::Matrix state); + + static geometry_msgs::msg::PoseWithCovarianceStamped ToPoseCov(Eigen::Matrix state); + + static geometry_msgs::msg::PoseStamped ToENU(geometry_msgs::msg::PoseStamped pose, Eigen::Matrix trans_WtoE); +}; + +} // namespace mins + +#endif // MINS_ROSVISUALIZER_HELPER_H diff --git a/mins/src/core/ROS2Publisher.cpp b/mins/src/core/ROS2Publisher.cpp new file mode 100644 index 0000000..9dc6d8c --- /dev/null +++ b/mins/src/core/ROS2Publisher.cpp @@ -0,0 +1,504 @@ +/* + * 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 . + */ + +#include "ROS2Publisher.h" +#include "ROS2Helper.h" +#include "SystemManager.h" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "nav_msgs/msg/odometry.h" +#include "nav_msgs/msg/path.hpp" +#include "options/Options.h" +#include "options/OptionsCamera.h" +#include "options/OptionsEstimator.h" +#include "options/OptionsGPS.h" +#include "options/OptionsIMU.h" +#include "options/OptionsLidar.h" +#include "options/OptionsVicon.h" +#include "options/OptionsWheel.h" +#include "pcl_conversions/pcl_conversions.h" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "state/Propagator.h" +#include "state/State.h" +#include "state/StateHelper.h" +#include "types/IMU.h" +#include "update/cam/UpdaterCamera.h" +#include "update/gps/GPSTypes.h" +#include "update/gps/MathGPS.h" +#include "update/gps/PoseJPL_4DOF.h" +#include "update/gps/UpdaterGPS.h" +#include "update/lidar/LidarTypes.h" +#include "update/lidar/UpdaterLidar.h" +#include "update/lidar/ikd_Tree.h" +#include "update/vicon/ViconTypes.h" +#include "utils/Print_Logger.h" +#include +#include +#include +#include + +using namespace std; +using namespace Eigen; +using namespace mins; +using namespace ov_type; +ROS2Publisher::ROS2Publisher(shared_ptr node, shared_ptr sys, shared_ptr op) : node(node), sys(sys), op(op) { + + // Setup our transform broadcaster + mTfBr = make_shared(node); + + // Basic IMU publish + pub_imu_pose = node->create_publisher("/mins/imu/pose", 2); + PRINT1("Publishing: %s\n", pub_imu_pose->get_topic_name()); + pub_imu_odom = node->create_publisher("/mins/imu/odom", 2); + PRINT1("Publishing: %s\n", pub_imu_odom->get_topic_name()); + pub_imu_path = node->create_publisher("/mins/imu/path", 2); + PRINT1("Publishing: %s\n", pub_imu_path->get_topic_name()); + + // CAM + if (sys->state->op->cam->enabled) { + pub_cam_msckf = node->create_publisher("/mins/cam/msckf", 2); + PRINT1("Publishing: %s\n", pub_cam_msckf->get_topic_name()); + pub_cam_slam = node->create_publisher("/mins/cam/slam", 2); + PRINT1("Publishing: %s\n", pub_cam_msckf->get_topic_name()); + for (int i = 0; i < op->est->cam->max_n; i++) { + image_transport::Publisher it = image_transport::create_publisher(node.get(), "/mins/cam" + to_string(i) + "/track_img"); // Our tracking image + pub_cam_image.push_back(it); + PRINT1("Publishing: %s\n", pub_cam_image.back().getTopic().c_str()); + } + } + + // GPS + if (sys->state->op->gps->enabled) { + for (int i = 0; i < sys->state->op->gps->max_n; i++) { + seq_gps.push_back(0); + pub_gps_pose.emplace_back(); + pub_gps_pose.back() = node->create_publisher("/mins/gps" + to_string(i) + "/pose", 2); + PRINT1("Publishing: %s\n", pub_gps_pose.back()->get_topic_name()); + pub_gps_path.emplace_back(); + pub_gps_path.back() = node->create_publisher("/mins/gps" + to_string(i) + "/path", 2); + PRINT1("Publishing: %s\n", pub_gps_path.back()->get_topic_name()); + } + path_gps = vector>(sys->state->op->gps->max_n); + } + + // VICON + if (sys->state->op->vicon->enabled) { + for (int i = 0; i < sys->state->op->vicon->max_n; i++) { + seq_vicon.push_back(0); + pub_vicon_pose.emplace_back(); + pub_vicon_pose.back() = node->create_publisher("/mins/vicon" + to_string(i) + "/pose", 2); + PRINT1("Publishing: %s\n", pub_vicon_pose.back()->get_topic_name()); + pub_vicon_path.emplace_back(); + pub_vicon_path.back() = node->create_publisher("/mins/vicon" + to_string(i) + "/path", 2); + PRINT1("Publishing: %s\n", pub_vicon_path.back()->get_topic_name()); + } + path_vicon = vector>(sys->state->op->vicon->max_n); + } + + // LIDAR + if (sys->state->op->lidar->enabled) { + for (int i = 0; i < sys->state->op->lidar->max_n; i++) { + pub_lidar_cloud.emplace_back(); + pub_lidar_cloud.back() = node->create_publisher("/mins/lidar" + to_string(i) + "/points", 2); + PRINT1("Publishing: %s\n", pub_lidar_cloud.back()->get_topic_name()); + pub_lidar_map.emplace_back(); + pub_lidar_map.back() = node->create_publisher("/mins/lidar" + to_string(i) + "/map", 2); + PRINT1("Publishing: %s\n", pub_lidar_map.back()->get_topic_name()); + } + } +} + +void ROS2Publisher::visualize() { + // Return if we have not inited + if (!sys->state->initialized) + return; + + // publish state + publish_state(); + + // publish camera + if (sys->state->op->cam->enabled) + publish_cam_features(); + + // publish lidar + if (sys->state->op->lidar->enabled) + publish_lidar_map(); +} + +void ROS2Publisher::publish_imu() { + + // Return if we have not initialized + if (!sys->state->initialized) + return; + + // Our odometry message + Matrix odom; + odom.block(0, 0, 4, 1) = sys->state->imu->quat(); + odom.block(4, 0, 3, 1) = sys->state->imu->pos(); + odom.block(7, 0, 3, 1) = sys->state->imu->vel(); + odom.block(10, 0, 3, 1) = sys->state->have_cpi(sys->state->time) ? sys->state->cpis.at(sys->state->time).w : Vector3d::Zero(); + nav_msgs::msg::Odometry odomIinG = ROS2Helper::ToOdometry(odom); + odomIinG.header.stamp = rclcpp::Time(sys->state->time); + odomIinG.header.frame_id = "global"; + odomIinG.child_frame_id = "imu"; + + // Finally set the covariance in the message (in the order position then orientation as per ros convention) + // TODO: this currently is an approximation since this should actually evolve over our propagation period + // TODO: but to save time we only propagate the mean and not the uncertainty, but maybe we should try to prop the covariance? + vector> var_pq, var_v; + var_pq.push_back(sys->state->imu->pose()->p()); + var_pq.push_back(sys->state->imu->pose()->q()); + var_v.push_back(sys->state->imu->v()); + Matrix covariance_posori = StateHelper::get_marginal_covariance(sys->state, var_pq); + Matrix covariance_linang = pow(op->est->imu->sigma_w, 2) * Matrix::Identity(); + covariance_linang.block(0, 0, 3, 3) = StateHelper::get_marginal_covariance(sys->state, var_v); + for (int r = 0; r < 6; r++) { + for (int c = 0; c < 6; c++) { + odomIinG.pose.covariance[6 * r + c] = covariance_posori(r, c); + odomIinG.twist.covariance[6 * r + c] = (isnan(covariance_linang(r, c))) ? 0 : covariance_linang(r, c); + } + } + + // Finally, publish the resulting odometry message + pub_imu_odom->publish(odomIinG); + + // Publish TF + publish_tf(); +} + +void ROS2Publisher::publish_tf() { + // Publish our transform on TF + // NOTE: since we use JPL we have an implicit conversion to Hamilton when we publish + // NOTE: a rotation from GtoI in JPL has the same xyzw as a ItoG Hamilton rotation + auto trans = ROS2Helper::Pose2TF(sys->state->imu->pose(), false); + trans.header.frame_id = "global"; + trans.child_frame_id = "imu"; + mTfBr->sendTransform(trans); + + // Loop through each sensor calibration and publish it + if (sys->state->op->cam->enabled) { + for (const auto &calib : sys->state->cam_extrinsic) { + geometry_msgs::msg::TransformStamped trans_calib = ROS2Helper::Pose2TF(calib.second, true); + trans_calib.header.frame_id = "imu"; + trans_calib.child_frame_id = "cam" + to_string(calib.first); + mTfBr->sendTransform(trans_calib); + } + } + + if (sys->state->op->vicon->enabled) { + for (const auto &calib : sys->state->vicon_extrinsic) { + geometry_msgs::msg::TransformStamped trans_calib = ROS2Helper::Pose2TF(calib.second, true); + trans_calib.header.frame_id = "imu"; + trans_calib.child_frame_id = "vicon" + to_string(calib.first); + mTfBr->sendTransform(trans_calib); + } + } + + if (sys->state->op->gps->enabled) { + for (const auto &calib : sys->state->gps_extrinsic) { + geometry_msgs::msg::TransformStamped trans_calib = ROS2Helper::Pos2TF(calib.second, true); + trans_calib.header.frame_id = "imu"; + trans_calib.child_frame_id = "gps" + to_string(calib.first); + mTfBr->sendTransform(trans_calib); + } + } + + if (sys->state->op->lidar->enabled) { + for (const auto &calib : sys->state->lidar_extrinsic) { + geometry_msgs::msg::TransformStamped trans_calib = ROS2Helper::Pose2TF(calib.second, true); + trans_calib.header.frame_id = "imu"; + trans_calib.child_frame_id = "lidar" + to_string(calib.first); + mTfBr->sendTransform(trans_calib); + } + } + + if (sys->state->op->wheel->enabled) { + geometry_msgs::msg::TransformStamped trans_calib = ROS2Helper::Pose2TF(sys->state->wheel_extrinsic, true); + trans_calib.header.frame_id = "imu"; + trans_calib.child_frame_id = "wheel"; + mTfBr->sendTransform(trans_calib); + } + + // Publish clone poses + int clone_count = 0; + for (const auto &C : sys->state->clones) { + geometry_msgs::msg::TransformStamped trans = ROS2Helper::Pose2TF(C.second, false); + trans.header.frame_id = "global"; + trans.child_frame_id = "c" + to_string(clone_count++); + mTfBr->sendTransform(trans); + } +} + +void ROS2Publisher::publish_state() { + // Transform the path history if GNSS is initialized + if (sys->state->op->gps->enabled && sys->up_gps->initialized && !traj_in_enu) { + // Transform individual pose stored in the path + for (auto &pose : path_imu) { + pose = ROS2Helper::ToENU(pose, sys->state->trans_WtoE->value()); + } + // We only transform the trajectory once. + traj_in_enu = true; + } + + // Create pose of IMU (note we use the bag time) + geometry_msgs::msg::PoseWithCovarianceStamped poseIinM = ROS2Helper::ToPoseCov(sys->state->imu->value().block(0, 0, 7, 1)); + poseIinM.header.stamp = rclcpp::Time(sys->state->time); + // poseIinM.header.seq = seq_imu; + poseIinM.header.frame_id = "global"; + + // Finally set the covariance in the message (in the order position then orientation as per ros convention) + vector> statevars; + statevars.push_back(sys->state->imu->pose()->p()); + statevars.push_back(sys->state->imu->pose()->q()); + Matrix covariance_posori = StateHelper::get_marginal_covariance(sys->state, statevars); + for (int r = 0; r < 6; r++) { + for (int c = 0; c < 6; c++) { + poseIinM.pose.covariance[6 * r + c] = covariance_posori(r, c); + } + } + pub_imu_pose->publish(poseIinM); + + //========================================================= + //========================================================= + + // Append to our pose vector + geometry_msgs::msg::PoseStamped posetemp; + posetemp.header = poseIinM.header; + posetemp.pose = poseIinM.pose.pose; + path_imu.push_back(posetemp); + + // Create our path (IMU) + // NOTE: We downsample the number of poses as needed to prevent rviz crashes + // NOTE: https://github.com/ros-visualization/rviz/issues/1107 + nav_msgs::msg::Path arrIMU; + arrIMU.header.stamp = rclcpp::Clock().now(); + // arrIMU.header.seq = seq_imu; + arrIMU.header.frame_id = "global"; + for (int i = 0; i < (int)path_imu.size(); i += floor((double)path_imu.size() / 16384.0) + 1) { + arrIMU.poses.push_back(path_imu.at(i)); + } + pub_imu_path->publish(arrIMU); + + // Move them forward in time + seq_imu++; + + // Publish TF + publish_tf(); +} + +void ROS2Publisher::publish_cam_images(vector cam_ids) { + for (auto cam_id : cam_ids) + publish_cam_images(cam_id); +} + +void ROS2Publisher::publish_cam_images(int cam_id) { + // Publish image at cam 0 rate and have all the images from each camera + if (cam_id != 0) + return; + + for (int i = 0; i < op->est->cam->max_n; i++) { + // skip if no subscriber + if (pub_cam_image.at(i).getNumSubscribers() == 0) + continue; + + // skip if this is larger id pair of stereo + if (op->est->cam->stereo_pairs.find(i) != op->est->cam->stereo_pairs.end() && i > op->est->cam->stereo_pairs.at(i)) + continue; + + // Create our message & Publish + std_msgs::msg::Header header; + header.stamp = rclcpp::Clock().now(); + pub_cam_image.at(i).publish(cv_bridge::CvImage(header, "bgr8", sys->up_cam->get_track_img(i)).toImageMsg()); + } +} + +void ROS2Publisher::publish_cam_features() { + + // Check if we have subscribers + if (pub_cam_msckf->get_subscription_count() == 0 && pub_cam_slam->get_subscription_count() == 0) + return; + + // Get our good MSCKF features + vector feats_msckf = sys->up_cam->get_used_msckf(); + sensor_msgs::msg::PointCloud2 cloud = ROS2Helper::ToPointcloud(feats_msckf, "global"); + pub_cam_msckf->publish(cloud); + + // Get our good SLAM features + vector feats_slam = sys->state->get_features_SLAM(); + sensor_msgs::msg::PointCloud2 cloud_SLAM = ROS2Helper::ToPointcloud(feats_slam, "global"); + pub_cam_slam->publish(cloud_SLAM); +} + +void ROS2Publisher::publish_gps(GPSData gps, bool isGeodetic) { + + // Visualize GPS measurements when we have datum + if (sys->gps_datum.hasNaN()) + return; + + // Convert from a geodetic WGS-84 coordinated to East-North-Up + if (isGeodetic) + gps.meas = MathGPS::GeodeticToEnu(gps.meas, sys->gps_datum); + + // Now put the measurement in publisher + geometry_msgs::msg::PoseStamped poseGPSinENU; + poseGPSinENU.header.stamp = rclcpp::Clock().now(); + // poseGPSinENU.header.seq = seq_gps[gps.id]; + poseGPSinENU.header.frame_id = "global"; + poseGPSinENU.pose.position.x = gps.meas(0); + poseGPSinENU.pose.position.y = gps.meas(1); + poseGPSinENU.pose.position.z = gps.meas(2); + poseGPSinENU.pose.orientation.x = 0.0; + poseGPSinENU.pose.orientation.y = 0.0; + poseGPSinENU.pose.orientation.z = 0.0; + poseGPSinENU.pose.orientation.w = 1.0; + pub_gps_pose[gps.id]->publish(poseGPSinENU); + + // Append to our poses vector and create GPS path + path_gps[gps.id].push_back(poseGPSinENU); + nav_msgs::msg::Path arrGPS; + arrGPS.header.stamp = rclcpp::Clock().now(); + // arrGPS.header.seq = seq_gps[gps.id]; + arrGPS.header.frame_id = "global"; + arrGPS.poses = path_gps[gps.id]; + pub_gps_path[gps.id]->publish(arrGPS); + + // move sequence forward + seq_gps[gps.id]++; +} + +void ROS2Publisher::publish_vicon(ViconData data) { + + // Now put the measurement in publisher + geometry_msgs::msg::PoseStamped poseVicon; + poseVicon.header.stamp = rclcpp::Clock().now(); + // poseVicon.header.seq = seq_vicon[data.id]; + poseVicon.header.frame_id = "global"; + poseVicon.pose.position.x = data.pose(3, 0); + poseVicon.pose.position.y = data.pose(4, 0); + poseVicon.pose.position.z = data.pose(5, 0); + Vector4d q = ov_core::rot_2_quat(ov_core::exp_so3(data.pose.block(0, 0, 3, 1))); + poseVicon.pose.orientation.x = q(0); + poseVicon.pose.orientation.y = q(1); + poseVicon.pose.orientation.z = q(2); + poseVicon.pose.orientation.w = q(3); + pub_vicon_pose[data.id]->publish(poseVicon); + + // Append to our poses vector and create vicon path + path_vicon[data.id].push_back(poseVicon); + nav_msgs::msg::Path arrVicon; + arrVicon.header.stamp = rclcpp::Clock().now(); + // arrVicon.header.seq = seq_vicon[data.id]; + arrVicon.header.frame_id = "global"; + arrVicon.poses = path_vicon[data.id]; + pub_vicon_path[data.id]->publish(arrVicon); + + // move sequence forward + seq_vicon[data.id]++; +} + +void ROS2Publisher::publish_lidar_cloud(std::shared_ptr> lidar) { + sensor_msgs::msg::PointCloud2 output; + pcl::toROSMsg(*lidar, output); + output.header.frame_id = "lidar" + lidar->header.frame_id; + output.header.stamp = rclcpp::Clock().now(); + pub_lidar_cloud.at(stoi(lidar->header.frame_id))->publish(output); +} + +void ROS2Publisher::reset_paths() { + if (sys->state->op->gps->enabled) { + path_gps.clear(); + } + + if (sys->state->op->vicon->enabled) { + path_vicon.clear(); + } + + path_imu.clear(); +} + +void ROS2Publisher::publish_lidar_map() { + // Publish LiDAR map + for (const auto &ikd : sys->up_ldr->ikd_data) { + // Return if the map is not initialized + if (!ikd->tree->initialized()) + return; + // Get sensor calibration parameters + double dt = sys->state->lidar_dt.at(ikd->id)->value()(0); + Matrix3d RItoL = sys->state->lidar_extrinsic.at(ikd->id)->Rot(); + Vector3d pIinL = sys->state->lidar_extrinsic.at(ikd->id)->pos(); + + // Get IMU pose at map time + Matrix3d RGtoI; + Vector3d pIinG; + if (!sys->state->get_interpolated_pose(ikd->time + dt, RGtoI, pIinG)) + continue; + + // map pose + Matrix3d RGtoM = RItoL * RGtoI; + Vector4d qGtoM = rot_2_quat(RGtoM); + Vector3d pMinG = pIinG + RGtoI.transpose() * (-RItoL.transpose() * pIinL); + + // Publish tf of map + geometry_msgs::msg::TransformStamped trans; + trans.header.stamp = rclcpp::Clock().now(); + geometry_msgs::msg::Quaternion quat; + + quat.x = qGtoM(0); + quat.y = qGtoM(1); + quat.z = qGtoM(2); + quat.w = qGtoM(3); + trans.transform.rotation = quat; + geometry_msgs::msg::Vector3 orig; + orig.x = pMinG(0); + orig.y = pMinG(1); + orig.z = pMinG(2); + trans.transform.translation = orig; + trans.header.frame_id = "global"; + trans.child_frame_id = "map" + to_string(ikd->id); + mTfBr->sendTransform(trans); + + if (pub_lidar_map.at(ikd->id)->get_subscription_count() == 0) + continue; + + // Publish pointcloud in the global frame + // This is slower because it requires pointcloud transform + POINTCLOUD_XYZI_PTR map_inL(new pcl::PointCloud); + ikd->tree->flatten(ikd->tree->Root_Node, map_inL->points, NOT_RECORD); + pair pose_LinG = sys->up_ldr->get_pose_LinG(ikd->id, ikd->time); + Matrix4d tr = Matrix4d::Identity(); + tr.block(0, 0, 3, 3) = pose_LinG.first.transpose(); + tr.block(0, 3, 3, 1) = pose_LinG.second; + POINTCLOUD_XYZI_PTR map_inG(new pcl::PointCloud); + map_inL->height = map_inL->points.size(); + map_inL->width = 1; + pcl::transformPointCloud(*map_inL, *map_inG, tr); + sensor_msgs::msg::PointCloud2 map_pointcloud; + pcl::toROSMsg(*map_inG, map_pointcloud); + map_pointcloud.header.frame_id = "global"; + map_pointcloud.header.stamp = rclcpp::Clock().now(); + pub_lidar_map.at(ikd->id)->publish(map_pointcloud); + } +} \ No newline at end of file diff --git a/mins/src/core/ROS2Publisher.h b/mins/src/core/ROS2Publisher.h new file mode 100644 index 0000000..4703bd4 --- /dev/null +++ b/mins/src/core/ROS2Publisher.h @@ -0,0 +1,131 @@ +/* + * 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 . + */ + +#ifndef MINS_ROSPUBLISHER_H +#define MINS_ROSPUBLISHER_H + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "nav_msgs/msg/path.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "tf2_ros/transform_broadcaster.h" +#include +#include +#include + +namespace pcl { +class PointXYZ; +template class PointCloud; +} // namespace pcl + +namespace mins { + +class SystemManager; +struct GPSData; +struct ViconData; +class State; +struct LiDARData; +struct Options; +class ROS2Publisher { +public: + /// ROS message publisher + ROS2Publisher(std::shared_ptr node, std::shared_ptr sys, std::shared_ptr op); + + ~ROS2Publisher(){}; + + /// Visualize state, camera & LiDAR features + void visualize(); + + /// Publish current IMU state (pose, ang/lin velocities) + void publish_imu(); + + /// Publish the gps measurements + void publish_gps(GPSData gps, bool isGeodetic); + + /// Publish the vicon measurements + void publish_vicon(ViconData data); + + /// Publish the active tracking image + void publish_cam_images(std::vector cam_ids); + void publish_cam_images(int cam_id); + + /// Publish lidar point cloud + void publish_lidar_cloud(std::shared_ptr> lidar); + + void reset_paths(); + +private: + /// Publish the current state + void publish_state(); + + /// Publish current camera features + void publish_cam_features(); + + /// Publish current lidar map + void publish_lidar_map(); + + /// Publish TFs + void publish_tf(); + + /// Global node handler + std::shared_ptr node; + + /// Core application of the filter system + std::shared_ptr sys; + + /// Options + std::shared_ptr op; + + // Our publishers + std::shared_ptr mTfBr; + std::vector pub_cam_image; + rclcpp::Publisher::SharedPtr pub_imu_pose; + rclcpp::Publisher::SharedPtr pub_imu_odom; + rclcpp::Publisher::SharedPtr pub_imu_path; + + rclcpp::Publisher::SharedPtr pub_cam_msckf, pub_cam_slam; + + std::vector::SharedPtr> pub_gps_pose; + std::vector::SharedPtr> pub_gps_path; + + std::vector::SharedPtr> pub_vicon_pose; + std::vector::SharedPtr> pub_vicon_path; + std::vector::SharedPtr> pub_lidar_cloud; + std::vector::SharedPtr> pub_lidar_map; + + // For path viz + unsigned int seq_imu = 0; + std::vector seq_gps, seq_vicon; + std::vector path_imu; + std::vector> path_gps, path_vicon; + bool traj_in_enu = false; +}; +} // namespace mins + +#endif // MINS_ROSPUBLISHER_H diff --git a/mins/src/core/ROS2Subscriber.cpp b/mins/src/core/ROS2Subscriber.cpp new file mode 100644 index 0000000..53fe6a3 --- /dev/null +++ b/mins/src/core/ROS2Subscriber.cpp @@ -0,0 +1,228 @@ +/* + * 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 . + */ + +#include "ROS2Subscriber.h" +#include "ROS2Helper.h" +#include "ROS2Publisher.h" +#include "SystemManager.h" +#include "options/OptionsCamera.h" +#include "options/OptionsEstimator.h" +#include "options/OptionsGPS.h" +#include "options/OptionsIMU.h" +#include "options/OptionsLidar.h" +#include "options/OptionsWheel.h" +#include "state/State.h" +#include "update/gps/GPSTypes.h" +#include "update/wheel/WheelTypes.h" +#include "utils/Print_Logger.h" + +using namespace std; +using namespace Eigen; +using namespace mins; + +ROS2Subscriber::ROS2Subscriber(std::shared_ptr node, std::shared_ptr sys, std::shared_ptr pub) : node(node), sys(sys), pub(pub) { + // Copy option for easier access + op = sys->state->op; + + // Create imu subscriber (handle legacy ros param info) + // subs.push_back(node->create_subscription(op->imu->topic, rclcpp::SensorDataQoS(), std::bind(&ROS2Subscriber::callback_inertial, this, std::placeholders::_1))); + + sub_imu = node->create_subscription(op->imu->topic, rclcpp::QoS(100), std::bind(&ROS2Subscriber::callback_inertial, this, std::placeholders::_1)); + PRINT1("subscribing to imu: %s\n", sub_imu->get_topic_name()); + + // Create camera subscriber + if (op->cam->enabled) { + for (int i = 0; i < op->cam->max_n; i++) { + // Check if this is stereo + if (op->cam->stereo_pairs.find(i) != op->cam->stereo_pairs.end()) { + if (i < op->cam->stereo_pairs.at(i)) { + // Logic for sync stereo subscriber + // https://answers.ros.org/question/96346/subscribe-to-two-image_raws-with-one-function/?answer=96491#post-id-96491 + int cam_id0 = i; + int cam_id1 = op->cam->stereo_pairs.at(i); + // Create sync filter (they have unique pointers internally, so we have to use move logic here...) + if (op->cam->compressed.at(cam_id0)) { + auto image_sub0 = std::make_shared>(node, op->cam->topic.at(cam_id0)); + auto image_sub1 = std::make_shared>(node, op->cam->topic.at(cam_id1)); + auto sync = std::make_shared>(csync_pol(10), *image_sub0, *image_sub1); + sync->registerCallback(std::bind(&ROS2Subscriber::callback_stereo_C, this, std::placeholders::_1, std::placeholders::_2, cam_id0, cam_id1)); + // Append to our vector of subscribers + csync_cam.push_back(sync); + cimage_subs.push_back(image_sub0); + cimage_subs.push_back(image_sub1); + PRINT2("subscribing to cam (stereo, compressed): %s\n", op->cam->topic.at(cam_id0).c_str()); + PRINT2("subscribing to cam (stereo, compressed): %s\n", op->cam->topic.at(cam_id1).c_str()); + + } else { + auto image_sub0 = std::make_shared>(node, op->cam->topic.at(cam_id0)); + auto image_sub1 = std::make_shared>(node, op->cam->topic.at(cam_id1)); + auto sync = std::make_shared>(sync_pol(10), *image_sub0, *image_sub1); + sync->registerCallback(std::bind(&ROS2Subscriber::callback_stereo_I, this, std::placeholders::_1, std::placeholders::_2, cam_id0, cam_id1)); + // Append to our vector of subscribers + sync_cam.push_back(sync); + image_subs.push_back(image_sub0); + image_subs.push_back(image_sub1); + PRINT2("subscribing to cam (stereo): %s\n", op->cam->topic.at(cam_id0).c_str()); + PRINT2("subscribing to cam (stereo): %s\n", op->cam->topic.at(cam_id1).c_str()); + } + } + } else { + // create MONO subscriber + if (op->cam->compressed.at(i)) { + auto image_sub = std::make_shared>(node, op->cam->topic.at(i)); + image_sub->registerCallback(std::bind(&ROS2Subscriber::callback_monocular_C, this, std::placeholders::_1, i)); + cimage_subs.push_back(image_sub); + PRINT2("subscribing to cam (mono, compressed): %s\n", op->cam->topic.at(i).c_str()); + } else { + auto image_sub = std::make_shared>(node, op->cam->topic.at(i)); + image_sub->registerCallback(std::bind(&ROS2Subscriber::callback_monocular_I, this, std::placeholders::_1, i)); + image_subs.push_back(image_sub); + PRINT2("subscribing to cam (mono): %s\n", op->cam->topic.at(i).c_str()); + } + } + } + } + + // Create wheel subscriber + if (op->wheel->enabled) { + subs.push_back(node->create_subscription(op->wheel->topic, rclcpp::SensorDataQoS(), std::bind(&ROS2Subscriber::callback_wheel, this, std::placeholders::_1))); + PRINT2("subscribing to wheel: %s\n", op->wheel->topic.c_str()); + } + + // Create gps subscriber + if (op->gps->enabled) { + for (int i = 0; i < op->gps->max_n; i++) { + subs.push_back(node->create_subscription(op->gps->topic.at(i), rclcpp::QoS(1000), [this, i](const NavSatFix::SharedPtr msg0) { this->callback_gnss(msg0, i); })); + PRINT2("subscribing to GNSS: %s\n", op->gps->topic.at(i).c_str()); + } + } + + // Create lidar subscriber + if (op->lidar->enabled) { + for (int i = 0; i < op->lidar->max_n; i++) { + subs.push_back(node->create_subscription(op->lidar->topic.at(i), 2, [this, i](const PointCloud2::SharedPtr msg) { this->callback_lidar(msg, i); })); + PRINT2("subscribing to LiDAR: %s\n", op->lidar->topic.at(i).c_str()); + } + } + + reset_srv = node->create_service("/mins/reset", std::bind(&ROS2Subscriber::reset_service, this, std::placeholders::_1, std::placeholders::_2)); +} + +void ROS2Subscriber::callback_inertial(const Imu::SharedPtr msg) { + // convert into correct format & send it to our system + ov_core::ImuData imu = ROS2Helper::Imu2Data(msg); + if (sys->feed_measurement_imu(imu)) { + pub->visualize(); + } + pub->publish_imu(); + PRINT1(YELLOW "[SUB] IMU measurement: %.3f" RESET, imu.timestamp); + PRINT1(YELLOW "|%.3f,%.3f,%.3f|%.3f,%.3f,%.3f\n" RESET, imu.wm(0), imu.wm(1), imu.wm(2), imu.am(0), imu.am(1), imu.am(2)); +} + +void ROS2Subscriber::callback_monocular_I(const Image::ConstSharedPtr msg, int cam_id) { + // convert into correct format & send it to our system + // + ov_core::CameraData cam; + if (ROS2Helper::Image2Data(msg, cam_id, cam, op->cam)) { + sys->feed_measurement_camera(cam); + pub->publish_cam_images(cam_id); + PRINT1(YELLOW "[SUB] MONO Cam measurement: %.3f|%d\n" RESET, cam.timestamp, cam_id); + } +} + +void ROS2Subscriber::callback_monocular_C(const CompressedImage::ConstSharedPtr msg, int cam_id) { + // convert into correct format & send it to our system + ov_core::CameraData cam; + if (ROS2Helper::Image2Data(msg, cam_id, cam, op->cam)) { + sys->feed_measurement_camera(cam); + pub->publish_cam_images(cam_id); + PRINT1(YELLOW "[SUB] MONO Cam measurement: %.3f|%d\n" RESET, cam.timestamp, cam_id); + } +} + +void ROS2Subscriber::callback_stereo_I(const Image::ConstSharedPtr msg0, const Image::ConstSharedPtr msg1, int cam_id0, int cam_id1) { + // convert into correct format & send it to our system + ov_core::CameraData cam; + bool success0 = ROS2Helper::Image2Data(msg0, cam_id0, cam, op->cam); + bool success1 = ROS2Helper::Image2Data(msg1, cam_id1, cam, op->cam); + if (success0 && success1) { + sys->feed_measurement_camera(cam); + pub->publish_cam_images({cam_id0, cam_id1}); + PRINT1(YELLOW "[SUB] STEREO Cam measurement: %.3f|%d|%d\n" RESET, cam.timestamp, cam_id0, cam_id1); + } else { + PRINT_ERROR("[SUB] Stereo Image Error!"); + } +} + + +void ROS2Subscriber::callback_stereo_C(const CompressedImage::ConstSharedPtr msg0, const CompressedImage::ConstSharedPtr msg1, int cam_id0, int cam_id1) { + // convert into correct format & send it to our system + ov_core::CameraData cam; + bool success0 = ROS2Helper::Image2Data(msg0, cam_id0, cam, op->cam); + bool success1 = ROS2Helper::Image2Data(msg1, cam_id1, cam, op->cam); + if (success0 && success1) { + sys->feed_measurement_camera(cam); + pub->publish_cam_images({cam_id0, cam_id1}); + PRINT1(YELLOW "[SUB] STEREO Cam measurement: %.3f|%d|%d\n" RESET, rclcpp::Time(msg0->header.stamp).seconds(), cam_id0, cam_id1); + } +} + +void ROS2Subscriber::callback_wheel(const JointState::SharedPtr msg) { + // Return if the message contains other than wheel measurement info + if (find(op->wheel->sub_topics.begin(), op->wheel->sub_topics.end(), msg->name.at(0)) == op->wheel->sub_topics.end()) + return; + + WheelData data = ROS2Helper::JointState2Data(msg); + sys->feed_measurement_wheel(data); + PRINT1(YELLOW "[SUB] Wheel measurement: %.3f|%.3f,%.3f\n" RESET, data.time, data.m1, data.m2); +} + +void ROS2Subscriber::callback_gnss(const NavSatFix::SharedPtr msg, int gps_id) { + // convert into correct format & send it to our system + GPSData data = ROS2Helper::NavSatFix2Data(msg, gps_id); + // In case GNSS message does not have GNSS noise value or we want to overwrite it, use preset values + data.noise(0) <= 0.0 || op->gps->overwrite_noise ? data.noise(0) = op->gps->noise : double(); + data.noise(1) <= 0.0 || op->gps->overwrite_noise ? data.noise(1) = op->gps->noise : double(); + data.noise(2) <= 0.0 || op->gps->overwrite_noise ? data.noise(2) = op->gps->noise * 2 : double(); + sys->feed_measurement_gps(data, true); + pub->publish_gps(data, true); + PRINT1(YELLOW "[SUB] GPS measurement: %.3f|%d|" RESET, data.time, data.id); + PRINT1(YELLOW "%.3f,%.3f,%.3f|%.3f,%.3f,%.3f\n" RESET, data.meas(0), data.meas(1), data.meas(2), data.noise(0), data.noise(1), data.noise(2)); +} + +void ROS2Subscriber::callback_lidar(const PointCloud2::SharedPtr msg, int lidar_id) { + // convert into correct format & send it to our system + std::shared_ptr> data = ROS2Helper::rosPC2pclPC(msg, lidar_id); + sys->feed_measurement_lidar(data); + pub->publish_lidar_cloud(data); + PRINT1(YELLOW "[SUB] LiDAR measurement: %.3f|%d\n" RESET, (double)msg->header.stamp.sec / 1000, lidar_id); +} + +void ROS2Subscriber::reset_service(std::shared_ptr request, std::shared_ptr response) { + sys->init(); + pub->reset_paths(); +} \ No newline at end of file diff --git a/mins/src/core/ROS2Subscriber.h b/mins/src/core/ROS2Subscriber.h new file mode 100644 index 0000000..298e76d --- /dev/null +++ b/mins/src/core/ROS2Subscriber.h @@ -0,0 +1,113 @@ +/* + * 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 . + */ + +#ifndef MINS_ROSSUBSCRIBER_H +#define MINS_ROSSUBSCRIBER_H + +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "nav_msgs/msg/path.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/compressed_image.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include "sensor_msgs/msg/joint_state.hpp" +#include "sensor_msgs/msg/nav_sat_fix.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "std_srvs/srv/empty.hpp" +#include +#include +#include + +using namespace std; +using namespace sensor_msgs::msg; +using namespace nav_msgs::msg; +namespace mins { + +class SystemManager; +struct OptionsEstimator; +class ROS2Publisher; +class ROS2Subscriber { + +public: + /// ROS message subscriber + ROS2Subscriber(shared_ptr node, shared_ptr sys, shared_ptr pub); + + /// Callback for IMU + void callback_inertial(const Imu::SharedPtr msg); + + /// Callback for Wheel + void callback_wheel(const JointState::SharedPtr msg); + + /// Callback for GNSS + void callback_gnss(const NavSatFix::SharedPtr msg, int gps_id); + + /// Callback for LiDAR + void callback_lidar(const PointCloud2::SharedPtr msg, int lidar_id); + + /// Callback for monocular camera (image, compressed image) + void callback_monocular_I(const Image::ConstSharedPtr msg, int cam_id); + void callback_monocular_C(const CompressedImage::ConstSharedPtr msg, int cam_id); + + /// Callback for synchronized stereo camera (image, compressed image) + void callback_stereo_I(const Image::ConstSharedPtr msg0, const Image::ConstSharedPtr msg1, int cam_id0, int cam_id1); + void callback_stereo_C(const CompressedImage::ConstSharedPtr msg0, const CompressedImage::ConstSharedPtr msg1, int cam_id0, int cam_id1); + + void reset_service(std::shared_ptr request, std::shared_ptr response); + +private: + /// Global node handler + shared_ptr node; + + /// MINS system + shared_ptr sys; + + /// ROS publisher + shared_ptr pub; + + /// Options + shared_ptr op; + + /// Our subscribers and camera synchronizers + rclcpp::Subscription::SharedPtr sub_imu; + rclcpp::Subscription::SharedPtr sub_wheel; + + vector subs; + vector>> image_subs; + vector>> cimage_subs; + + typedef message_filters::sync_policies::ApproximateTime sync_pol; + vector>> sync_cam; + + typedef message_filters::sync_policies::ApproximateTime csync_pol; + vector>> csync_cam; + + rclcpp::Service::SharedPtr reset_srv; +}; +} // namespace mins + +#endif // MINS_ROSSUBSCRIBER_H diff --git a/mins/src/core/SystemManager.cpp b/mins/src/core/SystemManager.cpp index 075a2a6..1a9cfd3 100644 --- a/mins/src/core/SystemManager.cpp +++ b/mins/src/core/SystemManager.cpp @@ -57,7 +57,11 @@ using namespace std; using namespace mins; -SystemManager::SystemManager(shared_ptr op, shared_ptr sim) { +SystemManager::SystemManager(shared_ptr op, shared_ptr sim): op(op), sim(sim) { + init(); +} + +void SystemManager::init() { // Create "THE MOST IMPORTANT" state state = std::make_shared(op, sim); tc_sensors = std::make_shared(); @@ -174,6 +178,7 @@ void SystemManager::feed_measurement_wheel(const WheelData &wheel) { state->initialized ? tc_sensors->dong("WHL") : void(); } + void SystemManager::feed_measurement_lidar(std::shared_ptr> lidar) { if (!state->op->lidar->enabled) return; @@ -481,8 +486,12 @@ void SystemManager::print_status() { // Wheel auto w_op = state->op->wheel; - if (w_op->enabled && up_whl->t_hist.size() > 2) - PRINT2(" WHL %.1f", (up_whl->t_hist.size() - 1) / (up_whl->t_hist.back() - up_whl->t_hist.front())); + if (w_op->enabled) + { + + if (w_op->type != "Rover" && up_whl->t_hist.size() > 2) + PRINT2(" WHL %.1f", (up_whl->t_hist.size() - 1) / (up_whl->t_hist.back() - up_whl->t_hist.front())); + } PRINT2("\n"); diff --git a/mins/src/core/SystemManager.h b/mins/src/core/SystemManager.h index 2adc738..bb93ce0 100644 --- a/mins/src/core/SystemManager.h +++ b/mins/src/core/SystemManager.h @@ -53,11 +53,15 @@ struct CamSimData; struct GPSData; struct STAT; struct WheelData; +struct RoverWheelData; +struct TLIOData; class UpdaterCamera; class UpdaterGPS; class UpdaterLidar; class UpdaterVicon; class UpdaterWheel; +class UpdaterRoverWheel; +class UpdaterTLIO; class TimeChecker; class SystemManager { @@ -68,6 +72,8 @@ class SystemManager { ~SystemManager(){}; + void init(); + /// IMU measurement feeder bool feed_measurement_imu(const ov_core::ImuData &imu); @@ -86,6 +92,10 @@ class SystemManager { /// Wheel measurement feeder void feed_measurement_wheel(const WheelData &wheel); + void feed_measurement_rover(const RoverWheelData &wheel); + + void feed_measurement_tlio(const TLIOData &tlio); + /// LiDAR measurement feeder void feed_measurement_lidar(std::shared_ptr> lidar); /** @@ -93,6 +103,9 @@ class SystemManager { */ void visualize_final(); + std::shared_ptr op; + std::shared_ptr sim; + /// Our master state object :D std::shared_ptr state; @@ -135,6 +148,8 @@ class SystemManager { /// Wheel updater std::shared_ptr up_whl; + std::shared_ptr up_whl_rover; + std::shared_ptr up_tlio; /// Average order and cloning frequency of the system std::shared_ptr avg_order, avg_freq; diff --git a/mins/src/options/OptionsCamera.cpp b/mins/src/options/OptionsCamera.cpp index ac61b76..f8c9dcb 100644 --- a/mins/src/options/OptionsCamera.cpp +++ b/mins/src/options/OptionsCamera.cpp @@ -181,6 +181,10 @@ void mins::OptionsCamera::load_i(const std::shared_ptr &par parser->parse_external(f, "cam" + std::to_string(i), "topic", cam_topic); topic.push_back(cam_topic); + bool cam_compressed; + parser->parse_external(f, "cam" + std::to_string(i), "compressed", cam_compressed, false); + compressed.insert({i, cam_compressed}); + if (use_mask_) { std::string mask_path; std::string mask_node = "mask" + std::to_string(i); @@ -249,5 +253,6 @@ void mins::OptionsCamera::print_i(int i) { PRINT1("\t\t\t- [%6.3f, %6.3f, %6.3f, %6.3f]\n", R(2), R(5), R(8), p(2)); PRINT1("\t\t\t- [ 0.000, 0.000, 0.000, 1.000]\n"); PRINT1("\t\t- use_mask: %s\n", use_mask.at(i) ? "true" : "false"); + PRINT1("\t\t- compressed: %s\n", compressed.at(i) ? "true" : "false"); PRINT1("\t\t- topic: %s\n", topic.at(i).c_str()); } diff --git a/mins/src/options/OptionsCamera.h b/mins/src/options/OptionsCamera.h index ff1c9ea..0c1031e 100644 --- a/mins/src/options/OptionsCamera.h +++ b/mins/src/options/OptionsCamera.h @@ -74,6 +74,8 @@ struct OptionsCamera { /// Map between camid and camera extrinsics (q_ItoC, p_IinC). std::map extrinsics; + std::map compressed; + /// initial covariance values for calibration parameters double init_cov_dt = 1e-4; double init_cov_ex_o = 1e-4; diff --git a/mins/src/options/OptionsSimulation.cpp b/mins/src/options/OptionsSimulation.cpp index e6f7835..80b9d43 100644 --- a/mins/src/options/OptionsSimulation.cpp +++ b/mins/src/options/OptionsSimulation.cpp @@ -29,7 +29,7 @@ #include "OptionsEstimator.h" #include "utils/Print_Logger.h" #include "utils/opencv_yaml_parse.h" -#include +#include mins::OptionsSimulation::OptionsSimulation() { est_true = std::make_shared(); } @@ -61,8 +61,10 @@ void mins::OptionsSimulation::load_print(const std::shared_ptrload(parser); diff --git a/mins/src/options/OptionsSystem.cpp b/mins/src/options/OptionsSystem.cpp index 6671785..ff90029 100644 --- a/mins/src/options/OptionsSystem.cpp +++ b/mins/src/options/OptionsSystem.cpp @@ -21,7 +21,7 @@ #include "OptionsSystem.h" #include "utils/Print_Logger.h" #include "utils/opencv_yaml_parse.h" -#include +#include void mins::OptionsSystem::load_print(const std::shared_ptr &parser) { if (parser != nullptr) { @@ -42,9 +42,10 @@ void mins::OptionsSystem::load_print(const std::shared_ptr parser->parse_external(f, "sys", "bag_durr", bag_durr); parser->parse_external(f, "sys", "path_gt", path_gt, false); // Replace MINS_DIR if we have it - path_timing.substr(0, 8) == "MINS_DIR" ? path_timing.replace(0, 8, ros::package::getPath("mins")) : std::string(); - path_state.substr(0, 8) == "MINS_DIR" ? path_state.replace(0, 8, ros::package::getPath("mins")) : std::string(); - path_trajectory.substr(0, 8) == "MINS_DIR" ? path_trajectory.replace(0, 8, ros::package::getPath("mins")) : std::string(); + auto dir = ament_index_cpp::get_package_share_directory("mins"); + path_timing.substr(0, 8) == "MINS_DIR" ? path_timing.replace(0, 8, dir) : std::string(); + path_state.substr(0, 8) == "MINS_DIR" ? path_state.replace(0, 8, dir) : std::string(); + path_trajectory.substr(0, 8) == "MINS_DIR" ? path_trajectory.replace(0, 8, dir) : std::string(); } PRINT1(BOLDBLUE "Options - System\n" RESET); PRINT1("\t- save_timing: %s\n", save_timing ? "true" : "false"); diff --git a/mins/src/run_simulation.cpp b/mins/src/run_simulation.cpp index 2bced23..4ae021c 100644 --- a/mins/src/run_simulation.cpp +++ b/mins/src/run_simulation.cpp @@ -27,7 +27,13 @@ #include +#if ROS_AVAILABLE == 2 +#include "core/ROS2Publisher.h" +#include "sim/Sim2Visualizer.h" +#include +#elif ROS_AVAILABLE == 1 #include "core/ROSPublisher.h" +#endif #include "core/SystemManager.h" #include "options/Options.h" #include "options/OptionsCamera.h" @@ -37,7 +43,6 @@ #include "options/OptionsSystem.h" #include "options/OptionsVicon.h" #include "options/OptionsWheel.h" -#include "sim/SimVisualizer.h" #include "sim/Simulator.h" #include "update/cam/CamTypes.h" #include "update/gps/GPSTypes.h" @@ -52,7 +57,6 @@ #include "utils/sensor_data.h" #include #include -#include using namespace mins; using namespace std; @@ -60,8 +64,10 @@ using namespace Eigen; shared_ptr sim; shared_ptr sys; -shared_ptr pub; -shared_ptr sim_viz; +#if ROS_AVAILABLE == 2 +shared_ptr pub; +shared_ptr sim_viz; +#endif shared_ptr op; shared_ptr save; @@ -72,7 +78,11 @@ int main(int argc, char **argv) { // Load parameters system_setup(argc, argv); // run simulation +#if ROS_AVAILABLE == 2 + while (sim->ok() && rclcpp::ok()) { +#else while (sim->ok() && ros::ok()) { +#endif // imu: get the next simulated imu measurement if we have it ov_core::ImuData imu; if (sim->get_next_imu(imu)) { @@ -129,7 +139,9 @@ int main(int argc, char **argv) { op->sys->save_timing ? save->save_timing_to_file(sys->tc_sensors->get_total_sum()) : void(); save->check_files(); - ros::shutdown(); +#if ROS_AVAILABLE == 2 + rclcpp::shutdown(); +#endif return EXIT_SUCCESS; } @@ -139,13 +151,19 @@ void system_setup(int argc, char **argv) { argc > 1 ? config_path = argv[1] : string(); // Launch our ros node - ros::init(argc, argv, "mins_simulation"); - auto nh = make_shared("~"); - nh->param("config_path", config_path, config_path); +#if ROS_AVAILABLE == 2 + rclcpp::init(argc, argv); + auto node = std::make_shared("mins_simulation"); + + node->get_parameter("config_path", config_path); +#endif // Load the config auto parser = make_shared(config_path); - parser->set_node_handler(nh); + +#if ROS_AVAILABLE == 2 + parser->set_node(node); +#endif op = make_shared(); op->load_print(parser); op->sys->save_prints ? mins::Print_Logger::open_file(op->sys->path_state, true) : void(); @@ -153,8 +171,11 @@ void system_setup(int argc, char **argv) { // Create our system sim = make_shared(op); sys = make_shared(op->est, sim); - pub = make_shared(nh, sys, op); - sim_viz = make_shared(nh, sys, sim); + +#if ROS_AVAILABLE == 2 + pub = make_shared(node, sys, op); + sim_viz = make_shared(node, sys, sim); +#endif // Ensure we read in all parameters required if (!parser->successful()) { diff --git a/mins/src/run_subscribe.cpp b/mins/src/run_subscribe.cpp index 9097d57..41b6ee5 100644 --- a/mins/src/run_subscribe.cpp +++ b/mins/src/run_subscribe.cpp @@ -27,8 +27,14 @@ #include +#if ROS_AVAILABLE == 2 +#include "core/ROS2Publisher.h" +#include "core/ROS2Subscriber.h" +#include "rclcpp/rclcpp.hpp" +#elif ROS_AVAILABLE == 1 #include "core/ROSPublisher.h" #include "core/ROSSubscriber.h" +#endif #include "core/SystemManager.h" #include "options/Options.h" #include "options/OptionsSystem.h" @@ -37,7 +43,6 @@ #include "utils/TimeChecker.h" #include "utils/colors.h" #include "utils/opencv_yaml_parse.h" -#include using namespace std; using namespace mins; @@ -50,21 +55,31 @@ int main(int argc, char **argv) { argc > 1 ? config_path = argv[1] : string(); // Launch our ros node - ros::init(argc, argv, "mins_subscribe"); - auto nh = make_shared("~"); - nh->param("config_path", config_path, config_path); +#if ROS_AVAILABLE == 2 + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + options.allow_undeclared_parameters(true); + options.automatically_declare_parameters_from_overrides(true); + auto node = std::make_shared("mins_subscribe", options); + node->get_parameter("config_path", config_path); +#endif + std::cout << "Loading config from path: " << config_path << std::endl; // Load the config auto parser = make_shared(config_path); - parser->set_node_handler(nh); +#if ROS_AVAILABLE == 2 + parser->set_node(node); +#endif shared_ptr op = make_shared(); op->load_print(parser); shared_ptr save = make_shared(op); // Create our system shared_ptr sys = make_shared(op->est); - shared_ptr pub = make_shared(nh, sys, op); - shared_ptr sub = make_shared(nh, sys, pub); +#if ROS_AVAILABLE == 2 + shared_ptr pub = make_shared(node, sys, op); + shared_ptr sub = make_shared(node, sys, pub); +#endif // Ensure we read in all parameters required if (!parser->successful()) { @@ -72,16 +87,22 @@ int main(int argc, char **argv) { exit(EXIT_FAILURE); } +#if ROS_AVAILABLE == 2 + // rclcpp::spin(node); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); // Spin off to ROS - ros::spin(); +#endif // Final visualization sys->visualize_final(); op->sys->save_timing ? save->save_timing_to_file(sys->tc_sensors->get_total_sum()) : void(); save->check_files(); - ros::shutdown(); - +#if ROS_AVAILABLE == 2 + rclcpp::shutdown(); +#endif // Done! return EXIT_SUCCESS; } diff --git a/mins/src/sim/Sim2Visualizer.cpp b/mins/src/sim/Sim2Visualizer.cpp new file mode 100644 index 0000000..8ab016a --- /dev/null +++ b/mins/src/sim/Sim2Visualizer.cpp @@ -0,0 +1,262 @@ +/* + * 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 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 . + */ + +#include "Sim2Visualizer.h" +#include "SimulationPlane.h" + +#include "core/ROS2Helper.h" + +#include "core/SystemManager.h" +#include "options/Options.h" +#include "options/OptionsCamera.h" +#include "options/OptionsEstimator.h" +#include "options/OptionsGPS.h" +#include "options/OptionsInit.h" +#include "options/OptionsLidar.h" +#include "options/OptionsSimulation.h" +#include "sim/Simulator.h" +#include "state/State.h" +#include "state/StateHelper.h" +#include "types/IMU.h" +#include "update/gps/PoseJPL_4DOF.h" +#include "update/gps/UpdaterGPS.h" +#include "utils/Print_Logger.h" +#include "utils/dataset_reader.h" +#include + +using namespace std; +using namespace ov_core; +using namespace ov_type; +using namespace mins; +using namespace visualization_msgs::msg; +Sim2Visualizer::Sim2Visualizer(shared_ptr node, shared_ptr sys, shared_ptr sim) : node(node), sys(sys), sim(sim) { + + // Publish simulated camera point cloud if simulation enabled + if (sim != nullptr && sys->state->op->cam->enabled) { + pub_sim_cam_points = node->create_publisher("/mins/cam/points_sim", 2); + PRINT1("Publishing: %s\n", pub_sim_cam_points->get_topic_name()); + } + + if (sim != nullptr && sys->state->op->lidar->enabled) { + pub_sim_lidar_map = node->create_publisher("/mins/sim_lidar_map", 2); + PRINT1("Publishing: %s\n", pub_sim_lidar_map->get_topic_name()); + } + + // Groundtruth publishers + mTfBr = make_shared(node); + pub_posegt = node->create_publisher("/mins/imu/pose_gt", 2); + PRINT1("Publishing: %s\n", pub_posegt->get_topic_name()); + pub_pathgt = node->create_publisher("/mins/imu/path_gt", 2); + PRINT1("Publishing: %s\n", pub_pathgt->get_topic_name()); +} + +void Sim2Visualizer::publish_sim_cam_features() { + // Check if we have subscribers + if (sim == nullptr || pub_sim_cam_points->get_subscription_count() == 0) + return; + + // Get our good SIMULATION features + vector feats_sim = sim->get_cam_map_vec(); + sensor_msgs::msg::PointCloud2 cloud_SIM = ROS2Helper::ToPointcloud(feats_sim, "global"); + pub_sim_cam_points->publish(cloud_SIM); +} + +void Sim2Visualizer::publish_groundtruth() { + // Transform the path history if GNSS is initialized + if (sys->state->op->gps->enabled && sys->up_gps->initialized && !traj_in_enu) { + // Transform individual pose stored in the path + for (auto &pose : poses_gt) { + pose = ROS2Helper::ToENU(pose, sys->state->trans_WtoE->value()); + } + // We only transform the trajectory once. + traj_in_enu = true; + } + + // Our groundtruth state + Eigen::Matrix state_gt; + if (sim != nullptr) { // Get ground truth from simulation if available + if (!sim->get_imu_state(sys->state->time, state_gt)) + return; + } else { // Otherwise, try to get it from the file + if (gt_states.empty()) { // Load gt file if didn't get yet + if (sys->state->op->init->path_gt.empty()) + return; + DatasetReader::load_gt_file(sys->state->op->init->path_gt, gt_states); + } + // Get gt pose + if (!DatasetReader::get_gt_state(sys->state->time, state_gt, gt_states)) + return; + } + + // Create pose of IMU + geometry_msgs::msg::PoseStamped poseIinM; + poseIinM.header.stamp = rclcpp::Time(sys->state->time); + // poseIinM.header.seq = poses_seq_gt; + poseIinM.header.frame_id = "global"; + poseIinM.pose.orientation.x = state_gt(1, 0); + poseIinM.pose.orientation.y = state_gt(2, 0); + poseIinM.pose.orientation.z = state_gt(3, 0); + poseIinM.pose.orientation.w = state_gt(4, 0); + poseIinM.pose.position.x = state_gt(5, 0); + poseIinM.pose.position.y = state_gt(6, 0); + poseIinM.pose.position.z = state_gt(7, 0); + pub_posegt->publish(poseIinM); + + // Append to our pose vector + poses_gt.push_back(poseIinM); + + // Create our path (IMU) + // NOTE: We downsample the number of poses as needed to prevent rviz crashes + // NOTE: https://github.com/ros-visualization/rviz/issues/1107 + nav_msgs::msg::Path arrIMU; + arrIMU.header.stamp = rclcpp::Clock().now(); + // arrIMU.header.seq = poses_seq_gt; + arrIMU.header.frame_id = "global"; + for (size_t i = 0; i < poses_gt.size(); i += floor((double)poses_gt.size() / 16384.0) + 1) { + arrIMU.poses.push_back(poses_gt[i]); + } + pub_pathgt->publish(arrIMU); + + // Move them forward in time + poses_seq_gt++; + + // Publish our transform on TF + geometry_msgs::msg::TransformStamped trans; + trans.header.stamp = rclcpp::Clock().now(); + trans.header.frame_id = "global"; + trans.child_frame_id = "truth"; + geometry_msgs::msg::Quaternion quat; + + quat.x = state_gt(1, 0); + quat.y = state_gt(2, 0); + quat.z = state_gt(3, 0); + quat.w = state_gt(4, 0); + trans.transform.rotation = quat; + geometry_msgs::msg::Vector3 orig; + + orig.x = state_gt(5, 0); + orig.y = state_gt(6, 0); + orig.z = state_gt(7, 0); + trans.transform.translation = orig; + mTfBr->sendTransform(trans); + + //========================================================================== + //========================================================================== + if (sim != nullptr) { + auto imu_pose = sys->state->imu->pose(); + Vector4d rmse_nees = sim->imu_rmse_nees(sys->state->time, imu_pose->value(), StateHelper::get_marginal_covariance(sys->state, {imu_pose})); + + // Update our average variables + if (!isnan(rmse_nees(2)) && !isnan(rmse_nees(3))) { + sum_rmse_ori += rmse_nees(0); + sum_rmse_pos += rmse_nees(1); + sum_nees_ori += rmse_nees(2); + sum_nees_pos += rmse_nees(3); + sum_cnt++; + } + + // Nice display for the user + printf(REDPURPLE); + PRINT2("\033[A%.2f | RMSE: %.3f, %.3f (deg,m) | ", sys->state->time, rmse_nees(0), rmse_nees(1)); + PRINT2("RMSE avg: %.3f, %.3f (deg,m) | ", sum_rmse_ori / sum_cnt, sum_rmse_pos / sum_cnt); + PRINT2("NEES: %.1f, %.1f | ", rmse_nees(2), rmse_nees(3)); + PRINT2("NEES avg: %.1f, %.1f\n\n", sum_nees_ori / sum_cnt, sum_nees_pos / sum_cnt); + printf(RESET); + } +} + +void Sim2Visualizer::publish_lidar_structure() { + // Skip the rest of we are not doing simulation + if (sim == nullptr || sim->get_lidar_planes().empty()) + return; + + // copy over the simulated planes + vector> planes = sim->get_lidar_planes(); + + // Transform the planes if GPS is enabled and initialized + Matrix3d RWtoE = quat_2_Rot(sim->op->sim->WtoE_trans.block(0, 0, 4, 1)); + Vector3d pWinE = sim->op->sim->WtoE_trans.block(4, 0, 3, 1); + for (auto &plane : planes) { + plane->pt_top_left = sim->trans_gt_to_ENU ? pWinE + RWtoE * plane->pt_top_left.eval() : plane->pt_top_left.eval(); + plane->pt_top_right = sim->trans_gt_to_ENU ? pWinE + RWtoE * plane->pt_top_right.eval() : plane->pt_top_right.eval(); + plane->pt_bottom_left = sim->trans_gt_to_ENU ? pWinE + RWtoE * plane->pt_bottom_left.eval() : plane->pt_bottom_left.eval(); + plane->pt_bottom_right = sim->trans_gt_to_ENU ? pWinE + RWtoE * plane->pt_bottom_right.eval() : plane->pt_bottom_right.eval(); + } + + // Our marker array + MarkerArray marker_arr; + // Else lets get all the planes + int ct = 0; + for (auto &plane : planes) { + // Our plane will be a line list + Marker marker_plane; + marker_plane.header.frame_id = "global"; + marker_plane.header.stamp = rclcpp::Clock().now(); + marker_plane.ns = "sim_lidar_map"; + marker_plane.id = ct; + marker_plane.type = Marker::LINE_LIST; + marker_plane.action = Marker::MODIFY; + marker_plane.scale.x = 0.03; + marker_plane.color.b = 1.0; + marker_plane.color.a = 1.0; + + // Convert our 4 points to the right format + geometry_msgs::msg::Point pt_tl, pt_tr, pt_bl, pt_br; + pt_tl.x = plane->pt_top_left(0); + pt_tl.y = plane->pt_top_left(1); + pt_tl.z = plane->pt_top_left(2); + pt_tr.x = plane->pt_top_right(0); + pt_tr.y = plane->pt_top_right(1); + pt_tr.z = plane->pt_top_right(2); + pt_bl.x = plane->pt_bottom_left(0); + pt_bl.y = plane->pt_bottom_left(1); + pt_bl.z = plane->pt_bottom_left(2); + pt_br.x = plane->pt_bottom_right(0); + pt_br.y = plane->pt_bottom_right(1); + pt_br.z = plane->pt_bottom_right(2); + + // Add the 4 bounding points + marker_plane.points.push_back(pt_tl); + marker_plane.points.push_back(pt_tr); + marker_plane.points.push_back(pt_tr); + marker_plane.points.push_back(pt_br); + marker_plane.points.push_back(pt_br); + marker_plane.points.push_back(pt_bl); + marker_plane.points.push_back(pt_bl); + marker_plane.points.push_back(pt_tl); + + // Add cross across middle of the plane + marker_plane.points.push_back(pt_tl); + marker_plane.points.push_back(pt_br); + marker_plane.points.push_back(pt_tr); + marker_plane.points.push_back(pt_bl); + + // Append and move plane count forward + marker_arr.markers.push_back(marker_plane); + ct++; + } + + pub_sim_lidar_map->publish(marker_arr); +} + +void Sim2Visualizer::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); +} \ No newline at end of file diff --git a/mins/src/sim/Sim2Visualizer.h b/mins/src/sim/Sim2Visualizer.h new file mode 100644 index 0000000..0aeca9c --- /dev/null +++ b/mins/src/sim/Sim2Visualizer.h @@ -0,0 +1,88 @@ +/* + * 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 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 . + */ + +#ifndef MINS_SIMVISUALIZER_H +#define MINS_SIMVISUALIZER_H + +#include "rclcpp/rclcpp.hpp" +#include +#include +#include + +#include "nav_msgs/msg/path.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "tf2_ros/transform_broadcaster.h" +#include + +namespace mins { +class Simulator; +class SystemManager; +class ROS2Publisher; +class Sim2Visualizer { +public: + Sim2Visualizer(std::shared_ptr node, std::shared_ptr sys, std::shared_ptr sim = nullptr); + + ~Sim2Visualizer(){}; + + /// Publish cam feature map + void publish_sim_cam_features(); + + /// Publish Lidar structure (eq simulated map) + void publish_lidar_structure(); + + /// Publish groundtruth (if we have it) + void publish_groundtruth(); + + /// Final visualization before shutdown + void visualize_final(); + +private: + /// Global node handler + std::shared_ptr node; + + /// Core application of the filter system + std::shared_ptr sys; + + /// Simulator (is nullptr if we are not sim'ing) + std::shared_ptr sim; + + std::map> gt_states; + + /// RMSE and NEES of the pose estimation + double sum_rmse_ori = 0.0; + double sum_rmse_pos = 0.0; + double sum_nees_ori = 0.0; + double sum_nees_pos = 0.0; + size_t sum_cnt = 0; + + /// For publish + rclcpp::Publisher::SharedPtr pub_sim_cam_points; + rclcpp::Publisher::SharedPtr pub_sim_lidar_map; + rclcpp::Publisher::SharedPtr pub_posegt; + rclcpp::Publisher::SharedPtr pub_pathgt; + + unsigned int poses_seq_gt = 0; + std::vector poses_gt; + std::shared_ptr mTfBr; + bool traj_in_enu = false; +}; +} // namespace mins + +#endif // MINS_SIMVISUALIZER_H diff --git a/mins/src/sim/Simulator.h b/mins/src/sim/Simulator.h index 8db6155..f9c167b 100644 --- a/mins/src/sim/Simulator.h +++ b/mins/src/sim/Simulator.h @@ -131,6 +131,7 @@ class Simulator { protected: friend class Initializer; friend class SimVisualizer; + friend class Sim2Visualizer; friend class State_Logger; /// Get LiDAR plane info diff --git a/mins_data/CMakeLists.txt b/mins_data/CMakeLists.txt index 4bf050a..9ee67ab 100644 --- a/mins_data/CMakeLists.txt +++ b/mins_data/CMakeLists.txt @@ -5,4 +5,13 @@ project(mins_data) # Find our ROS version! find_package(catkin QUIET COMPONENTS roscpp) -catkin_package() +find_package(ament_cmake QUIET) +if (catkin_FOUND) + message(STATUS "ROS *1* version found") + catkin_package() +elseif (ament_cmake_FOUND) + message(STATUS "ROS *2* version found") + ament_package() +else () + message(STATUS "No ROS versions found, doing nothing...") +endif () diff --git a/mins_data/package.xml b/mins_data/package.xml index b301532..e9f32b4 100644 --- a/mins_data/package.xml +++ b/mins_data/package.xml @@ -16,11 +16,13 @@ GNU General Public License v3.0 - catkin + catkin + ament_cmake - catkin + catkin + ament_cmake diff --git a/mins_eval/CMakeLists.txt b/mins_eval/CMakeLists.txt index 8d03f42..b65c944 100644 --- a/mins_eval/CMakeLists.txt +++ b/mins_eval/CMakeLists.txt @@ -36,70 +36,20 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-sign # Enable debug flags (use if you want to debug in gdb) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g3 -Wall -Wuninitialized -Wmaybe-uninitialized") -# Find ROS build system -find_package(catkin REQUIRED COMPONENTS roscpp rospy geometry_msgs nav_msgs sensor_msgs ov_core ov_eval) find_package(PCL REQUIRED QUIET) +find_package(libpointmatcher REQUIRED QUIET) +find_package(libnabo REQUIRED QUIET) +# Find ROS build system -# Describe ROS project -catkin_package( - CATKIN_DEPENDS roscpp rospy geometry_msgs nav_msgs sensor_msgs ov_core ov_eval - INCLUDE_DIRS src/ - LIBRARIES mins_eval_lib -) - -# Include our header files -include_directories( - src - ${EIGEN3_INCLUDE_DIR} - ${Boost_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} - ${PYTHON_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - -# Set link libraries used by all binaries -list(APPEND thirdparty_libraries - ${Boost_LIBRARIES} - ${catkin_LIBRARIES} - ) - -################################################## -# Make the shared library -################################################## - -add_library(mins_eval_lib SHARED - src/functions/ErrorPlot.cpp - src/functions/ResultTrajectory.cpp - ) -target_link_libraries(mins_eval_lib ${thirdparty_libraries}) -target_include_directories(mins_eval_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/) -install(TARGETS mins_eval_lib - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) - -install(DIRECTORY src/ - DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" -) - -################################################## -# Make binary files! -################################################## - -add_executable(plot_consistency src/plot_consistency.cpp) -target_link_libraries(plot_consistency mins_eval_lib ${thirdparty_libraries}) -install(TARGETS plot_consistency - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) - -add_executable(run_comparison src/run_comparison.cpp) -target_link_libraries(run_comparison mins_eval_lib ${thirdparty_libraries}) -install(TARGETS run_comparison - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) +find_package(catkin QUIET COMPONENTS roscpp) +find_package(ament_cmake QUIET) +if (catkin_FOUND) + message(STATUS "ROS *1* version found, building ROS1.cmake") + include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/ROS1.cmake) +elseif (ament_cmake_FOUND) + message(STATUS "ROS *2* version found, building ROS2.cmake") + include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/ROS2.cmake) +else () + message(STATUS "No ROS versions found, building ROS1.cmake") + include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/ROS1.cmake) +endif () \ No newline at end of file diff --git a/mins_eval/cmake/ROS1.cmake b/mins_eval/cmake/ROS1.cmake new file mode 100644 index 0000000..3c07536 --- /dev/null +++ b/mins_eval/cmake/ROS1.cmake @@ -0,0 +1,66 @@ +cmake_minimum_required(VERSION 3.3) + +find_package(catkin REQUIRED COMPONENTS roscpp rospy geometry_msgs nav_msgs sensor_msgs ov_core ov_eval) +# Describe ROS project +catkin_package( + CATKIN_DEPENDS roscpp rospy geometry_msgs nav_msgs sensor_msgs ov_core ov_eval + INCLUDE_DIRS src/ + LIBRARIES mins_eval_lib +) + +# Include our header files +include_directories( + src + ${EIGEN3_INCLUDE_DIR} + ${Boost_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} + ${PYTHON_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} +) + +# Set link libraries used by all binaries +list(APPEND thirdparty_libraries + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} + ) + +################################################## +# Make the shared library +################################################## + +add_library(mins_eval_lib SHARED + src/functions/ErrorPlot.cpp + src/functions/ResultTrajectory.cpp + ) +target_link_libraries(mins_eval_lib ${thirdparty_libraries}) +target_include_directories(mins_eval_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/) +install(TARGETS mins_eval_lib + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + +install(DIRECTORY src/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" +) + +################################################## +# Make binary files! +################################################## + +add_executable(plot_consistency src/plot_consistency.cpp) +target_link_libraries(plot_consistency mins_eval_lib ${thirdparty_libraries}) +install(TARGETS plot_consistency + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + +add_executable(run_comparison src/run_comparison.cpp) +target_link_libraries(run_comparison mins_eval_lib ${thirdparty_libraries}) +install(TARGETS run_comparison + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) diff --git a/mins_eval/cmake/ROS2.cmake b/mins_eval/cmake/ROS2.cmake new file mode 100644 index 0000000..ce0e2d1 --- /dev/null +++ b/mins_eval/cmake/ROS2.cmake @@ -0,0 +1,83 @@ +cmake_minimum_required(VERSION 3.3) + +find_package(ament_cmake REQUIRED) + +find_package(rclcpp REQUIRED) +find_package(rclpy REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) + +find_package(ov_core REQUIRED) # Might not be available as a ROS 2 package +find_package(ov_eval REQUIRED) # Might not be available as a ROS 2 package +find_package(mins REQUIRED) + +add_definitions(-DROS_AVAILABLE=2) +# Describe ROS project +ament_export_dependencies(roscpp rospy geometry_msgs nav_msgs sensor_msgs ov_core ov_eval mins) +ament_export_libraries(mins_eval_lib) + +list(APPEND ament_libraries + rclcpp + rosbag2 + std_msgs + geometry_msgs + sensor_msgs + nav_msgs + ov_core + ov_eval + mins +) + +# Include our header files +include_directories( + ${EIGEN3_INCLUDE_DIR} + ${Boost_INCLUDE_DIRS} + ${PYTHON_INCLUDE_DIRS} + ${libpointmatcher_INCLUDE_DIRS} +) + +# Set link libraries used by all binaries +list(APPEND thirdparty_libraries + ${Boost_LIBRARIES} + ) + +################################################## +# Make the shared library +################################################## + +add_library(mins_eval_lib SHARED + src/functions/ErrorPlot.cpp + src/functions/ResultTrajectory.cpp + ) +target_link_libraries(mins_eval_lib ${thirdparty_libraries}) +ament_target_dependencies(mins_eval_lib ${ament_libraries}) +#target_include_directories(mins_eval_lib PUBLIC ${thirdparty_libraries} ${rclcpp_LIBRARIES}) + +install(TARGETS mins_eval_lib + EXPORT mins_eval_targets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY src/ + DESTINATION include + FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" +) +ament_export_include_directories(include) +ament_export_targets(mins_eval_targets) + +################################################## +# Make binary files! +################################################## + +add_executable(plot_consistency src/plot_consistency.cpp) +target_link_libraries(plot_consistency mins_eval_lib) +install(TARGETS plot_consistency DESTINATION lib/${PROJECT_NAME}) + +add_executable(run_comparison src/run_comparison.cpp) +target_link_libraries(run_comparison mins_eval_lib) +install(TARGETS run_comparison DESTINATION lib/${PROJECT_NAME}) + +ament_package() \ No newline at end of file diff --git a/mins_eval/package.xml b/mins_eval/package.xml index 358ce3b..4d07d6b 100644 --- a/mins_eval/package.xml +++ b/mins_eval/package.xml @@ -16,10 +16,13 @@ GNU General Public License v3.0 - catkin + catkin + ament_cmake cmake_modules - roscpp - rospy + roscpp + rospy + rclpy + rclcpp geometry_msgs nav_msgs sensor_msgs @@ -28,7 +31,8 @@ - catkin + catkin + ament_cmake \ No newline at end of file diff --git a/mins_eval/src/plot_consistency.cpp b/mins_eval/src/plot_consistency.cpp index 2d7b4dc..90466ac 100644 --- a/mins_eval/src/plot_consistency.cpp +++ b/mins_eval/src/plot_consistency.cpp @@ -26,7 +26,12 @@ */ #include "functions/ErrorPlot.h" + +#if ROS_AVAILABLE == 2 +#include "rclcpp/rclcpp.hpp" +#else #include "ros/ros.h" +#endif #include "utils/colors.h" #include "utils/print.h" @@ -53,17 +58,26 @@ int main(int argc, char **argv) { } // overwrite options if ROS params exist +#if ROS_AVAILABLE == 2 + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + + auto node = std::make_shared("plot_consistency", options); + node->get_parameter("/plot_consistency/load_path", load_path); + node->get_parameter("/plot_consistency/save_path", save_path); + node->get_parameter("/plot_consistency/visualize", visualize); +#else ros::init(argc, argv, "plot_consistency"); ros::NodeHandle nh; ros::param::get("/plot_consistency/load_path", load_path); ros::param::get("/plot_consistency/save_path", save_path); ros::param::get("/plot_consistency/visualize", visualize); - +#endif // Create save directory save_path += "/Plot/"; if (argc > 2) save_path = argv[2]; - save_path += save_path.back() != '/' ? "/" : ""; + save_path += save_path.back() != '/' ? "/" : ""; boost::filesystem::create_directories(save_path.c_str()); // Create our trajectory object diff --git a/mins_eval/src/run_comparison.cpp b/mins_eval/src/run_comparison.cpp index 1c1cb14..19b4daa 100644 --- a/mins_eval/src/run_comparison.cpp +++ b/mins_eval/src/run_comparison.cpp @@ -26,7 +26,11 @@ */ #include "functions/ResultTrajectory.h" +#if ROS_AVAILABLE == 2 +#include "rclcpp/rclcpp.hpp" +#else #include "ros/ros.h" +#endif #include "utils/Loader.h" #include #include @@ -112,7 +116,11 @@ int main(int argc, char **argv) { // Loop through each algorithm type for (auto &path_al : path_als) { +#if ROS_AVAILABLE == 2 + if (!rclcpp::ok()) +#else if (!ros::ok()) +#endif break; // Debug print @@ -209,12 +217,26 @@ void basic_setup(int argc, char **argv) { folder_groundtruths = argc > 2 ? argv[2] : ""; folder_algorithms = argc > 3 ? argv[3] : ""; +#if ROS_AVAILABLE == 2 // overwrite options if ROS params exist + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + + options.allow_undeclared_parameters(true); + options.automatically_declare_parameters_from_overrides(true); + auto node = std::make_shared("run_comparison", options); + + node->get_parameter("align_mode", align_mode); + node->get_parameter("path_gts", folder_groundtruths); + node->get_parameter("path_alg", folder_algorithms); + node->get_parameter("viz_type", viz_type); +#else ros::init(argc, argv, "run_comparison"); ros::param::get("/run_comparison/align_mode", align_mode); ros::param::get("/run_comparison/path_gts", folder_groundtruths); ros::param::get("/run_comparison/path_alg", folder_algorithms); ros::param::get("/run_comparison/viz_type", viz_type); +#endif } /// Get paths of the ground truths @@ -499,7 +521,6 @@ void print_latex_ate_rmse() { for (int i = 0; i < (int)path_gts.size(); i++) vv_gt.at(i / max_col).push_back(path_gts.at(i)); - for (const auto &v_gt : vv_gt) { if (v_gt.empty()) break; @@ -527,8 +548,7 @@ void print_latex_ate_rmse() { auto rmse = alg.second.at(gt); if (rmse.first.values.empty() || rmse.second.values.empty()) { printf(" & - / -"); - } - else { + } else { printf(" & %.3f", rmse.first.mean); printf(" / %.3f", rmse.second.mean); } @@ -539,7 +559,6 @@ void print_latex_ate_rmse() { } } - /// Print ATE RMSE divided by the length of the trajectory (unit 1km) in Latex table format void print_latex_ate_1km() { printf("\n\n\n"); @@ -564,11 +583,10 @@ void print_latex_ate_1km() { auto rmse = alg.second.at(path_gt); if (rmse.second.values.empty()) { printf(" & -"); - } - else { + } else { assert(GT_DIST.at(path_gt) != 0); printf(" & %.2f", rmse.second.mean / GT_DIST.at(path_gt) * 1000); // per 1km -// printf(" & %.2f", rmse.second.mean ); + // printf(" & %.2f", rmse.second.mean ); } } printf(" \\\\\n"); @@ -576,7 +594,6 @@ void print_latex_ate_1km() { printf(" \\end{tabular}\n \\end{adjustbox}\n\\end{table*}\n\n\n"); } - /// Print RPE RMSE in Latex table format void print_latex_rpe_rmse_std() { assert(segments.size() == 1); @@ -593,7 +610,6 @@ void print_latex_rpe_rmse_std() { for (int i = 0; i < (int)path_gts.size(); i++) vv_gt.at(i / max_col).push_back(path_gts.at(i)); - for (const auto &v_gt : vv_gt) { if (v_gt.empty()) break; @@ -601,8 +617,7 @@ void print_latex_rpe_rmse_std() { string ls = "c"; for (auto gt : v_gt) ls += "c"; - printf("\\begin{table*}[t]\n \\begin{adjustbox}{width=%.1f\\textwidth,center}\n \\begin{tabular}{%s}\n \\toprule\n ", - (double)v_gt.size() / max_col, ls.c_str()); + printf("\\begin{table*}[t]\n \\begin{adjustbox}{width=%.1f\\textwidth,center}\n \\begin{tabular}{%s}\n \\toprule\n ", (double)v_gt.size() / max_col, ls.c_str()); // Name of the dataset with multi-column for (const auto &g : v_gt) @@ -610,9 +625,9 @@ void print_latex_rpe_rmse_std() { printf("\\\\\n "); // RMSE, NEES, and/or Time header -// for (int i = 0; i < (int)v_gt.size(); i++) -// printf(" & \\textbf{RMSE (deg / m)}"); -// printf("\\\\ \\midrule\n"); + // for (int i = 0; i < (int)v_gt.size(); i++) + // printf(" & \\textbf{RMSE (deg / m)}"); + // printf("\\\\ \\midrule\n"); // Contents for (const auto &alg : RPE) { @@ -624,8 +639,7 @@ void print_latex_rpe_rmse_std() { rmse.second.calculate(); if (rmse.first.values.empty() || rmse.second.values.empty()) { printf(" & - / -"); - } - else { + } else { printf(" & %.3f", rmse.first.mean); printf(" $\\pm$ %.3f", rmse.first.std); printf(" / %.3f", rmse.second.mean); @@ -638,7 +652,6 @@ void print_latex_rpe_rmse_std() { } } - /// Print RPE median error in Latex table format void print_latex_rpe_median() { assert(segments.size() == 1); @@ -655,7 +668,6 @@ void print_latex_rpe_median() { for (int i = 0; i < (int)path_gts.size(); i++) vv_gt.at(i / max_col).push_back(path_gts.at(i)); - for (const auto &v_gt : vv_gt) { if (v_gt.empty()) break; @@ -663,8 +675,7 @@ void print_latex_rpe_median() { string ls = "c"; for (auto gt : v_gt) ls += "c"; - printf("\\begin{table*}[t]\n \\begin{adjustbox}{width=%.1f\\textwidth,center}\n \\begin{tabular}{%s}\n \\toprule\n ", - (double)v_gt.size() / max_col, ls.c_str()); + printf("\\begin{table*}[t]\n \\begin{adjustbox}{width=%.1f\\textwidth,center}\n \\begin{tabular}{%s}\n \\toprule\n ", (double)v_gt.size() / max_col, ls.c_str()); // Name of the dataset with multi-column for (const auto &g : v_gt) @@ -672,9 +683,9 @@ void print_latex_rpe_median() { printf("\\\\\n "); // RMSE, NEES, and/or Time header -// for (int i = 0; i < (int)v_gt.size(); i++) -// printf(" & \\textbf{RMSE (deg / m)}"); -// printf("\\\\ \\midrule\n"); + // for (int i = 0; i < (int)v_gt.size(); i++) + // printf(" & \\textbf{RMSE (deg / m)}"); + // printf("\\\\ \\midrule\n"); // Contents for (const auto &alg : RPE) { @@ -686,8 +697,7 @@ void print_latex_rpe_median() { rmse.second.calculate(); if (rmse.first.values.empty() || rmse.second.values.empty()) { printf(" & - / -"); - } - else { + } else { printf(" & %.3f", rmse.first.median); printf(" / %.3f", rmse.second.median); } @@ -754,7 +764,7 @@ void print_matlab_ate_time_mean_std() { break; } } - if(!MATLAB_find) + if (!MATLAB_find) break; string name = name_full.substr(0, id); string number = name_full.substr(id); @@ -775,92 +785,92 @@ void print_matlab_ate_time_mean_std() { assert(MATLAB_find); for (auto name : name_alg_stat) { printf("%s_rmse_ori_mean = {", name.first.c_str()); - for (const auto& alg : name.second) { + for (const auto &alg : name.second) { printf("'%s', ", replace_all_copy(alg.first, "_", "\\_").c_str()); } printf("\b\b; "); - for (const auto& alg : name.second) { + for (const auto &alg : name.second) { printf("%.4f, ", alg.second.rmse_ori.mean); } printf("\b\b};\n"); printf("%s_rmse_ori_std = {", name.first.c_str()); - for (const auto& alg : name.second) { + for (const auto &alg : name.second) { printf("'%s', ", replace_all_copy(alg.first, "_", "\\_").c_str()); } printf("\b\b; "); - for (const auto& alg : name.second) { + for (const auto &alg : name.second) { printf("%.4f, ", alg.second.rmse_ori.std); } printf("\b\b};\n"); printf("%s_rmse_pos_mean = {", name.first.c_str()); - for (const auto& alg : name.second) { + for (const auto &alg : name.second) { printf("'%s', ", replace_all_copy(alg.first, "_", "\\_").c_str()); } printf("\b\b; "); - for (const auto& alg : name.second) { + for (const auto &alg : name.second) { printf("%.4f, ", alg.second.rmse_pos.mean); } printf("\b\b};\n"); printf("%s_rmse_pos_std = {", name.first.c_str()); - for (const auto& alg : name.second) { + for (const auto &alg : name.second) { printf("'%s', ", replace_all_copy(alg.first, "_", "\\_").c_str()); } printf("\b\b; "); - for (const auto& alg : name.second) { + for (const auto &alg : name.second) { printf("%.4f, ", alg.second.rmse_pos.std); } printf("\b\b};\n"); printf("%s_nees_ori_mean = {", name.first.c_str()); - for (const auto& alg : name.second) { + for (const auto &alg : name.second) { printf("'%s', ", replace_all_copy(alg.first, "_", "\\_").c_str()); } printf("\b\b; "); - for (const auto& alg : name.second) { + for (const auto &alg : name.second) { printf("%.4f, ", alg.second.nees_ori.mean); } printf("\b\b};\n"); printf("%s_nees_ori_std = {", name.first.c_str()); - for (const auto& alg : name.second) { + for (const auto &alg : name.second) { printf("'%s', ", replace_all_copy(alg.first, "_", "\\_").c_str()); } printf("\b\b; "); - for (const auto& alg : name.second) { + for (const auto &alg : name.second) { printf("%.4f, ", alg.second.nees_ori.std); } printf("\b\b};\n"); printf("%s_nees_pos_mean = {", name.first.c_str()); - for (const auto& alg : name.second) { + for (const auto &alg : name.second) { printf("'%s', ", replace_all_copy(alg.first, "_", "\\_").c_str()); } printf("\b\b; "); - for (const auto& alg : name.second) { + for (const auto &alg : name.second) { printf("%.4f, ", alg.second.nees_pos.mean); } printf("\b\b};\n"); printf("%s_nees_pos_std = {", name.first.c_str()); - for (const auto& alg : name.second) { + for (const auto &alg : name.second) { printf("'%s', ", replace_all_copy(alg.first, "_", "\\_").c_str()); } printf("\b\b; "); - for (const auto& alg : name.second) { + for (const auto &alg : name.second) { printf("%.4f, ", alg.second.nees_pos.std); } printf("\b\b};\n"); printf("%s_time_mean = {", name.first.c_str()); - for (const auto& alg : name.second) { + for (const auto &alg : name.second) { printf("'%s', ", replace_all_copy(alg.first, "_", "\\_").c_str()); } printf("\b\b; "); - for (const auto& alg : name.second) { + for (const auto &alg : name.second) { printf("%.4f, ", alg.second.time.mean); } printf("\b\b};\n"); printf("%s_time_std = {", name.first.c_str()); - for (const auto& alg : name.second) { + for (const auto &alg : name.second) { printf("'%s', ", replace_all_copy(alg.first, "_", "\\_").c_str()); } printf("\b\b; "); - for (const auto& alg : name.second) { + for (const auto &alg : name.second) { printf("%.4f, ", alg.second.time.std); } printf("\b\b};\n"); @@ -895,7 +905,6 @@ void print_markdown_ate_rmse_nees_time() { for (int i = 0; i < (int)path_gts.size(); i++) vv_gt.at(i / max_col).push_back(path_gts.at(i)); - printf("\n\n\n"); printf("============================================\n"); printf("ATE MARKDOWN RENDERING\n"); @@ -949,5 +958,3 @@ void print_markdown_ate_rmse_nees_time() { printf("\n"); } } - -