diff --git a/ov_eval/cmake/ROS2.cmake b/ov_eval/cmake/ROS2.cmake index 59590eed3..6825331ad 100644 --- a/ov_eval/cmake/ROS2.cmake +++ b/ov_eval/cmake/ROS2.cmake @@ -3,6 +3,8 @@ cmake_minimum_required(VERSION 3.3) # Find ROS build system find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) find_package(ov_core REQUIRED) # Describe ROS project @@ -58,13 +60,10 @@ ament_export_libraries(ov_eval_lib) # Make binary files! ################################################## -# TODO: UPGRADE THIS TO ROS2 AS ANOTHER FILE!! -#if (catkin_FOUND AND ENABLE_ROS) -# add_executable(pose_to_file src/pose_to_file.cpp) -# target_link_libraries(pose_to_file ov_eval_lib ${thirdparty_libraries}) -# add_executable(live_align_trajectory src/live_align_trajectory.cpp) -# target_link_libraries(live_align_trajectory ov_eval_lib ${thirdparty_libraries}) -#endif () +add_executable(pose_to_file src/pose_to_file_ros2.cpp) +ament_target_dependencies(pose_to_file rclcpp geometry_msgs nav_msgs ov_core) +target_link_libraries(pose_to_file ov_eval_lib ${thirdparty_libraries}) +install(TARGETS pose_to_file DESTINATION lib/${PROJECT_NAME}) add_executable(format_converter src/format_converter.cpp) ament_target_dependencies(format_converter rclcpp ov_core) diff --git a/ov_eval/package.xml b/ov_eval/package.xml index f63af370a..c08e6d452 100644 --- a/ov_eval/package.xml +++ b/ov_eval/package.xml @@ -33,6 +33,8 @@ ament_cmake rclcpp + geometry_msgs + nav_msgs ov_core diff --git a/ov_eval/src/pose_to_file_ros2.cpp b/ov_eval/src/pose_to_file_ros2.cpp new file mode 100644 index 000000000..8f1aa0765 --- /dev/null +++ b/ov_eval/src/pose_to_file_ros2.cpp @@ -0,0 +1,84 @@ +/* + * 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 "utils/Recorder.h" + +#include + +#include +#include +#include + +#include "utils/print.h" + +int main(int argc, char ** argv) { + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + options.allow_undeclared_parameters(true); + options.automatically_declare_parameters_from_overrides(true); + auto node = std::make_shared("pose_to_file", options); + + std::string verbosity; + node->get_parameter_or("verbosity", verbosity, "INFO"); + ov_core::Printer::setPrintLevel(verbosity); + + std::string topic; + node->get_parameter_or("topic", topic, ""); + std::string topic_type; + node->get_parameter_or("topic_type", topic_type, ""); + std::string fileoutput; + node->get_parameter_or("output", fileoutput, ""); + if (topic.empty() || topic_type.empty() || fileoutput.empty()) { + PRINT_ERROR("Missing parameters: topic, topic_type, and output are required"); + return EXIT_FAILURE; + } + + PRINT_DEBUG("Done reading config values"); + PRINT_DEBUG(" - topic = %s", topic.c_str()); + PRINT_DEBUG(" - topic_type = %s", topic_type.c_str()); + PRINT_DEBUG(" - file = %s", fileoutput.c_str()); + + ov_eval::Recorder recorder(fileoutput); + rclcpp::SubscriptionBase::SharedPtr sub; + + if (topic_type == "PoseWithCovarianceStamped") { + sub = node->create_subscription( + topic, 9999, std::bind(&ov_eval::Recorder::callback_posecovariance, &recorder, std::placeholders::_1)); + } else if (topic_type == "PoseStamped") { + sub = node->create_subscription( + topic, 9999, std::bind(&ov_eval::Recorder::callback_pose, &recorder, std::placeholders::_1)); + } else if (topic_type == "TransformStamped") { + sub = node->create_subscription( + topic, 9999, std::bind(&ov_eval::Recorder::callback_transform, &recorder, std::placeholders::_1)); + } else if (topic_type == "Odometry") { + sub = node->create_subscription( + topic, 9999, std::bind(&ov_eval::Recorder::callback_odometry, &recorder, std::placeholders::_1)); + } else { + PRINT_ERROR("The specified topic type is not supported"); + PRINT_ERROR("topic_type = %s", topic_type.c_str()); + PRINT_ERROR("please select from: PoseWithCovarianceStamped, PoseStamped, TransformStamped, Odometry"); + return EXIT_FAILURE; + } + + rclcpp::spin(node); + rclcpp::shutdown(); + return EXIT_SUCCESS; +} diff --git a/ov_eval/src/utils/Recorder.h b/ov_eval/src/utils/Recorder.h index 26301fb60..53e3ac371 100644 --- a/ov_eval/src/utils/Recorder.h +++ b/ov_eval/src/utils/Recorder.h @@ -29,11 +29,19 @@ #include #include +#if ROS_AVAILABLE == 1 #include #include #include #include #include +#elif ROS_AVAILABLE == 2 +#include +#include +#include +#include +#include "utils/print.h" +#endif namespace ov_eval { @@ -56,19 +64,31 @@ class Recorder { // Create folder path to this location if not exists boost::filesystem::path dir(filename.c_str()); if (boost::filesystem::create_directories(dir.parent_path())) { +#if ROS_AVAILABLE == 1 ROS_INFO("Created folder path to output file."); ROS_INFO("Path: %s", dir.parent_path().c_str()); +#elif ROS_AVAILABLE == 2 + PRINT_DEBUG("Created folder path to output file: %s", dir.parent_path().c_str()); +#endif } // If it exists, then delete it if (boost::filesystem::exists(filename)) { +#if ROS_AVAILABLE == 1 ROS_WARN("Output file exists, deleting old file...."); +#elif ROS_AVAILABLE == 2 + PRINT_DEBUG("Output file exists, deleting old file: %s", filename.c_str()); +#endif boost::filesystem::remove(filename); } // Open this file we want to write to outfile.open(filename.c_str()); if (outfile.fail()) { +#if ROS_AVAILABLE == 1 ROS_ERROR("Unable to open output file!!"); ROS_ERROR("Path: %s", filename.c_str()); +#elif ROS_AVAILABLE == 2 + PRINT_ERROR("Unable to open output file: %s", filename.c_str()); +#endif std::exit(EXIT_FAILURE); } outfile << "# timestamp(s) tx ty tz qx qy qz qw Pr11 Pr12 Pr13 Pr22 Pr23 Pr33 Pt11 Pt12 Pt13 Pt22 Pt23 Pt33" << std::endl; @@ -81,6 +101,29 @@ class Recorder { has_covariance = false; } + /** + * @brief Record a pose and write to file (ROS-agnostic; use from ROS2 or any source). + * @param ts timestamp in seconds + * @param p position (tx ty tz) + * @param q quaternion (qx qy qz qw) + * @param has_cov true if covariance is valid + * @param c_rot 3x3 rotation covariance (upper triangle written) + * @param c_pos 3x3 position covariance (upper triangle written) + */ + void record_pose(double ts, const Eigen::Vector3d & p, const Eigen::Vector4d & q, bool has_cov, + const Eigen::Matrix & c_rot, const Eigen::Matrix & c_pos) { + timestamp = ts; + p_IinG = p; + q_ItoG = q; + has_covariance = has_cov; + if (has_cov) { + cov_rot = c_rot; + cov_pos = c_pos; + } + write(); + } + +#if ROS_AVAILABLE == 1 /** * @brief Callback for nav_msgs::Odometry message types. * @@ -146,6 +189,75 @@ class Recorder { p_IinG << msg->transform.translation.x, msg->transform.translation.y, msg->transform.translation.z; write(); } +#elif ROS_AVAILABLE == 2 + /** + * @brief Callback for nav_msgs::msg::Odometry message types. + * + * Note that covariance is in the order (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis). + * http://docs.ros.org/api/geometry_msgs/html/msg/PoseWithCovariance.html + * + * @param msg New message + */ + void callback_odometry(const nav_msgs::msg::Odometry::SharedPtr msg) { + timestamp = static_cast(msg->header.stamp.sec) + 1e-9 * static_cast(msg->header.stamp.nanosec); + q_ItoG << msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w; + p_IinG << msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z; + cov_pos << msg->pose.covariance.at(0), msg->pose.covariance.at(1), msg->pose.covariance.at(2), msg->pose.covariance.at(6), + msg->pose.covariance.at(7), msg->pose.covariance.at(8), msg->pose.covariance.at(12), msg->pose.covariance.at(13), + msg->pose.covariance.at(14); + cov_rot << msg->pose.covariance.at(21), msg->pose.covariance.at(22), msg->pose.covariance.at(23), msg->pose.covariance.at(27), + msg->pose.covariance.at(28), msg->pose.covariance.at(29), msg->pose.covariance.at(33), msg->pose.covariance.at(34), + msg->pose.covariance.at(35); + has_covariance = true; + write(); + } + + /** + * @brief Callback for geometry_msgs::msg::PoseStamped message types + * @param msg New message + */ + void callback_pose(const geometry_msgs::msg::PoseStamped::SharedPtr msg) { + timestamp = static_cast(msg->header.stamp.sec) + 1e-9 * static_cast(msg->header.stamp.nanosec); + q_ItoG << msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, msg->pose.orientation.w; + p_IinG << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z; + has_covariance = false; + write(); + } + + /** + * @brief Callback for geometry_msgs::msg::PoseWithCovarianceStamped message types. + * + * Note that covariance is in the order (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis). + * http://docs.ros.org/api/geometry_msgs/html/msg/PoseWithCovariance.html + * + * @param msg New message + */ + void callback_posecovariance(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { + timestamp = static_cast(msg->header.stamp.sec) + 1e-9 * static_cast(msg->header.stamp.nanosec); + q_ItoG << msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w; + p_IinG << msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z; + cov_pos << msg->pose.covariance.at(0), msg->pose.covariance.at(1), msg->pose.covariance.at(2), msg->pose.covariance.at(6), + msg->pose.covariance.at(7), msg->pose.covariance.at(8), msg->pose.covariance.at(12), msg->pose.covariance.at(13), + msg->pose.covariance.at(14); + cov_rot << msg->pose.covariance.at(21), msg->pose.covariance.at(22), msg->pose.covariance.at(23), msg->pose.covariance.at(27), + msg->pose.covariance.at(28), msg->pose.covariance.at(29), msg->pose.covariance.at(33), msg->pose.covariance.at(34), + msg->pose.covariance.at(35); + has_covariance = true; + write(); + } + + /** + * @brief Callback for geometry_msgs::msg::TransformStamped message types + * @param msg New message + */ + void callback_transform(const geometry_msgs::msg::TransformStamped::SharedPtr msg) { + timestamp = static_cast(msg->header.stamp.sec) + 1e-9 * static_cast(msg->header.stamp.nanosec); + q_ItoG << msg->transform.rotation.x, msg->transform.rotation.y, msg->transform.rotation.z, msg->transform.rotation.w; + p_IinG << msg->transform.translation.x, msg->transform.translation.y, msg->transform.translation.z; + has_covariance = false; + write(); + } +#endif protected: /** diff --git a/ov_msckf/cmake/ROS2.cmake b/ov_msckf/cmake/ROS2.cmake index eacffbb20..cb2870719 100644 --- a/ov_msckf/cmake/ROS2.cmake +++ b/ov_msckf/cmake/ROS2.cmake @@ -11,6 +11,8 @@ find_package(sensor_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(cv_bridge REQUIRED) find_package(image_transport REQUIRED) +find_package(rosbag2_cpp REQUIRED) +find_package(rosbag2_transport REQUIRED) find_package(ov_core REQUIRED) find_package(ov_init REQUIRED) @@ -37,6 +39,8 @@ list(APPEND thirdparty_libraries ) list(APPEND ament_libraries rclcpp + rosbag2_cpp + rosbag2_transport tf2_ros tf2_geometry_msgs std_msgs @@ -93,6 +97,11 @@ ament_target_dependencies(run_subscribe_msckf ${ament_libraries}) target_link_libraries(run_subscribe_msckf ov_msckf_lib ${thirdparty_libraries}) install(TARGETS run_subscribe_msckf DESTINATION lib/${PROJECT_NAME}) +add_executable(ros2_serial_msckf src/ros2_serial_msckf.cpp) +ament_target_dependencies(ros2_serial_msckf ${ament_libraries}) +target_link_libraries(ros2_serial_msckf ov_msckf_lib ${thirdparty_libraries}) +install(TARGETS ros2_serial_msckf DESTINATION lib/${PROJECT_NAME}) + add_executable(run_simulation src/run_simulation.cpp) ament_target_dependencies(run_simulation ${ament_libraries}) target_link_libraries(run_simulation ov_msckf_lib ${thirdparty_libraries}) diff --git a/ov_msckf/launch/__pycache__/serial.launch.cpython-310.pyc b/ov_msckf/launch/__pycache__/serial.launch.cpython-310.pyc new file mode 100644 index 000000000..da53c51aa Binary files /dev/null and b/ov_msckf/launch/__pycache__/serial.launch.cpython-310.pyc differ diff --git a/ov_msckf/launch/serial.launch.py b/ov_msckf/launch/serial.launch.py new file mode 100644 index 000000000..40c384506 --- /dev/null +++ b/ov_msckf/launch/serial.launch.py @@ -0,0 +1,197 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, EmitEvent, LogInfo, OpaqueFunction, RegisterEventHandler +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessExit +from launch.events import Shutdown +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + + +launch_args = [ + DeclareLaunchArgument( + name="verbosity", + default_value="INFO", + description="ALL, DEBUG, INFO, WARNING, ERROR, SILENT", + ), + DeclareLaunchArgument( + name="config", + default_value="tum_vi", + description="euroc_mav, tum_vi, rpng_aruco, kaist", + ), + DeclareLaunchArgument( + name="config_path", + default_value="", + description="path to estimator_config.yaml. If not given, determined from 'config'.", + ), + DeclareLaunchArgument( + name="max_cameras", + default_value="2", + description="number of cameras: 1 = mono, 2 = stereo, >2 = binocular", + ), + DeclareLaunchArgument( + name="use_stereo", + default_value="true", + description="if more than 1 camera, track stereo constraints between pairs", + ), + DeclareLaunchArgument( + name="bag_start", + default_value="0.0", + description="start time (seconds) into the bag; e.g. mh1: 40, mh2: 35", + ), + DeclareLaunchArgument( + name="dataset", + default_value="dataset-room1_512_16", + description="dataset name, e.g. V1_01_easy, V2_02_medium, dataset-room1_512_16", + ), + DeclareLaunchArgument( + name="bag", + default_value="", + description="path to the rosbag2 directory (or bag file). Overrides config+dataset if set.", + ), + DeclareLaunchArgument( + name="dosave", + default_value="false", + description="record estimated trajectory to path_est", + ), + DeclareLaunchArgument( + name="dotime", + default_value="false", + description="record timing statistics to path_time", + ), + DeclareLaunchArgument( + name="path_est", + default_value="/tmp/traj_estimate.txt", + description="output path for estimated trajectory when dosave is true", + ), + DeclareLaunchArgument( + name="path_time", + default_value="/tmp/traj_timing.txt", + description="output path for timing stats when dotime is true", + ), + DeclareLaunchArgument( + name="dolivetraj", + default_value="false", + description="visualize aligned groundtruth path", + ), + DeclareLaunchArgument( + name="path_gt", + default_value="", + description="path to groundtruth file (csv ASL format). If empty and dolivetraj, derived from config+dataset.", + ), + DeclareLaunchArgument( + name="debug", + default_value="false", + description="run with gdb debugger", + ), +] + + +def launch_setup(context): + config = LaunchConfiguration("config").perform(context) + dataset = LaunchConfiguration("dataset").perform(context) + bag_arg = LaunchConfiguration("bag").perform(context) + + # resolve config_path + config_path = LaunchConfiguration("config_path").perform(context) + if not config_path: + configs_dir = os.path.join(get_package_share_directory("ov_msckf"), "config") + if not os.path.isdir(configs_dir): + return [ + LogInfo(msg="ERROR: ov_msckf config directory not found: {} - not starting.".format(configs_dir)) + ] + available_configs = os.listdir(configs_dir) + if config not in available_configs: + return [ + LogInfo( + msg="ERROR: unknown config: '{}' - available: {} - not starting.".format( + config, ", ".join(available_configs) + ) + ) + ] + config_path = os.path.join(configs_dir, config, "estimator_config.yaml") + if not os.path.isfile(config_path): + return [ + LogInfo(msg="ERROR: config_path does not exist: {} - not starting.".format(config_path)) + ] + + # resolve bag path: explicit 'bag' arg, or construct from config + dataset (same as ROS1 default) + if bag_arg: + path_to_bag = bag_arg + else: + path_to_bag = os.path.join("/home/patrick/datasets", config, dataset) + + # resolve path_gt for dolivetraj (optional) + path_gt = LaunchConfiguration("path_gt").perform(context) + if not path_gt and LaunchConfiguration("dolivetraj").perform(context) == "true": + try: + ov_data_share = get_package_share_directory("ov_data") + path_gt = os.path.join(ov_data_share, config, dataset + ".txt") + except Exception: + path_gt = "" + + # serial node parameters (same as ROS1 serial.launch) + serial_params = [ + {"path_bag": path_to_bag}, + {"bag_start": LaunchConfiguration("bag_start")}, + {"bag_durr": -1.0}, + {"verbosity": LaunchConfiguration("verbosity")}, + {"config_path": config_path}, + {"use_stereo": LaunchConfiguration("use_stereo")}, + {"max_cameras": LaunchConfiguration("max_cameras")}, + {"record_timing_information": LaunchConfiguration("dotime")}, + {"record_timing_filepath": LaunchConfiguration("path_time")}, + ] + if path_gt: + serial_params.append({"path_gt": path_gt}) + + serial_node = Node( + package="ov_msckf", + executable="ros2_serial_msckf", + name="ov_msckf", + output="screen", + parameters=serial_params, + prefix=['gdb -ex run --args'] if LaunchConfiguration("debug") == "true" else [], + ) + + actions = [serial_node] + + # optional: record trajectory to file (equivalent to ROS1 group if="$(arg dosave)") + recorder_node = Node( + package="ov_eval", + executable="pose_to_file", + name="recorder_estimate", + output="screen", + condition=IfCondition(LaunchConfiguration("dosave")), + parameters=[ + {"topic": "/poseimu"}, + {"topic_type": "PoseWithCovarianceStamped"}, + {"output": LaunchConfiguration("path_est")}, + ], + ) + actions.append(recorder_node) + + # ensure batch runs continue: stop the launch once the serial player exits. + # without this, pose_to_file keeps spinning and ros2 launch never returns. + actions.append( + RegisterEventHandler( + OnProcessExit( + target_action=serial_node, + on_exit=[ + EmitEvent(event=Shutdown(reason="serial node finished")), + ], + ) + ) + ) + + #TODO(will): Add ROS2 support for live_align_trajectory, so dolivetraj + # activates visualization of the ground truth trajectory. + + return actions + + +def generate_launch_description(): + ld = LaunchDescription(launch_args) + ld.add_action(OpaqueFunction(function=launch_setup)) + return ld diff --git a/ov_msckf/package.xml b/ov_msckf/package.xml index 3386aba83..91d037852 100644 --- a/ov_msckf/package.xml +++ b/ov_msckf/package.xml @@ -47,6 +47,8 @@ nav_msgs cv_bridge image_transport + rosbag2_cpp + rosbag2_transport ov_core ov_init diff --git a/ov_msckf/scripts/run_ros2_eth.sh b/ov_msckf/scripts/run_ros2_eth.sh new file mode 100755 index 000000000..1bdd704ee --- /dev/null +++ b/ov_msckf/scripts/run_ros2_eth.sh @@ -0,0 +1,142 @@ +#!/usr/bin/env bash +# +# ROS2 serial batch runner for EuRoC MAV datasets. +# Reads bag path and output paths from environment (with fallbacks): +# OV_WORKSPACE_ROOT- colcon workspace root (used when no first argument is passed) +# Only used to produce default paths if the three environment +# variables below (OV_*) are not set. +# OV_SAVE_PATH - where to write estimate files (default: ./results) +# OV_BAG_PATH - directory containing dataset bags (default: ./datasets) +# OV_VER - version tag for output subdirs (default: 2.7) +# +# Usage: run_ros2_eth.sh [WS_ROOT] +# WS_ROOT - optional; colcon workspace root. +# If not passed: use OV_WORKSPACE_ROOT if set, else four levels up from this script. +# + +set -e + +SCRIPT_DIR="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" &> /dev/null && pwd)" +# workspace root: first argument, else OV_WORKSPACE_ROOT env, else four levels up (workspace/.../open_vins/ov_msckf/scripts) +if [ -n "${1:-}" ]; then + WS_ROOT="$(cd "$1" && pwd)" +elif [ -n "${OV_WORKSPACE_ROOT:-}" ]; then + WS_ROOT="$(cd "${OV_WORKSPACE_ROOT}" && pwd)" +else + WS_ROOT="$(cd "${SCRIPT_DIR}/../../../../" && pwd)" +fi + +# source colcon workspace so ros2/ov_msckf is available in this script +if [ -f "${WS_ROOT}/install/setup.bash" ]; then + source "${WS_ROOT}/install/setup.bash" +else + echo "BASH: install/setup.bash not found in ${WS_ROOT}. Set OV_WORKSPACE_ROOT or pass workspace root as first argument." >&2 + exit 1 +fi + +#----------------------------------------------------------------------------- +# Paths from ENV (fallbacks for local use) +#----------------------------------------------------------------------------- +save_path1="${OV_SAVE_PATH:-${WS_ROOT}/results}/euroc/algorithms" +save_path2="${OV_SAVE_PATH:-${WS_ROOT}/results}/euroc/timings" +bag_path="${OV_BAG_PATH:-${WS_ROOT}/datasets}/euroc" +ov_ver="${OV_VER:-2.7}" + +# print paths +echo "BASH: estimates path = ${save_path1}" +echo "BASH: timings path = ${save_path2}" +echo "BASH: bags path = ${bag_path}" +echo "BASH: OpenVINS version = ${ov_ver}" + +#----------------------------------------------------------------------------- +# Estimator modes and datasets +#----------------------------------------------------------------------------- +modes=( + "mono" + "binocular" + "stereo" +) + +bagnames=( + "V1_01_easy" + "V1_02_medium" + "V1_03_difficult" + "V2_01_easy" + "V2_02_medium" + "V2_03_difficult" + "MH_01_easy" + "MH_02_easy" + "MH_03_medium" + "MH_04_difficult" + "MH_05_difficult" +) + +bagstarttimes=( + "0.0" "0.0" "0.0" "0.0" "0.0" "0.0" + "40.0" "35.0" "5.0" "10.0" "5.0" +) + +#----------------------------------------------------------------------------- +# Run +#----------------------------------------------------------------------------- +big_start_time="$(date -u +%s)" + +for i in "${!bagnames[@]}"; do + for h in "${!modes[@]}"; do + for j in {00..00}; do + start_time="$(date -u +%s)" + filename_est="${save_path1}/ov_${ov_ver}_${modes[h]}/${bagnames[i]}/${j}_estimate.txt" + filename_time="${save_path2}/ov_${ov_ver}_${modes[h]}/${bagnames[i]}/${j}_timing.txt" + + case "${modes[h]}" in + mono) temp1="1"; temp2="true" ;; + binocular) temp1="2"; temp2="false" ;; + stereo) temp1="2"; temp2="true" ;; + *) echo "BASH: unknown mode ${modes[h]}"; exit 1 ;; + esac + + # rosbag2: bag path is typically a directory per dataset (e.g. V1_01_easy/) + bag_uri="${bag_path}/${bagnames[i]}" + metadata_file="${bag_uri}/metadata.yaml" + + # skip datasets that are not available in this local checkout + if [ ! -f "$metadata_file" ]; then + echo "BASH: skipping ${bagnames[i]} (${modes[h]}) - missing rosbag2 metadata at ${metadata_file}" + continue + fi + + mkdir -p "$(dirname "$filename_est")" "$(dirname "$filename_time")" + + # launch and capture output + launch_err="$(mktemp)" + if ! ros2 launch ov_msckf serial.launch.py \ + max_cameras:="$temp1" \ + use_stereo:="$temp2" \ + config:=euroc_mav \ + dataset:="${bagnames[i]}" \ + bag:="$bag_uri" \ + bag_start:="${bagstarttimes[i]}" \ + dosave:=true \ + path_est:="$filename_est" \ + dotime:=true \ + debug:=false \ + path_time:="$filename_time" \ + >"$launch_err" 2>&1; then + echo "BASH: ros2 launch failed for ${modes[h]} - ${bagnames[i]} (bag: $bag_uri)" >&2 + echo "BASH: captured output (stdout+stderr):" >&2 + cat "$launch_err" >&2 + rm -f "$launch_err" + exit 1 + fi + rm -f "$launch_err" + + end_time="$(date -u +%s)" + elapsed=$((end_time - start_time)) + echo "BASH: ${modes[h]} - ${bagnames[i]} - run $j took $elapsed seconds" + done + done +done + +big_end_time="$(date -u +%s)" +big_elapsed=$((big_end_time - big_start_time)) +echo "BASH: script took $big_elapsed seconds in total!!" diff --git a/ov_msckf/scripts/run_ros2_uzhfpv.sh b/ov_msckf/scripts/run_ros2_uzhfpv.sh new file mode 100755 index 000000000..bbbfd4bea --- /dev/null +++ b/ov_msckf/scripts/run_ros2_uzhfpv.sh @@ -0,0 +1,152 @@ +#!/usr/bin/env bash +# +# ROS2 serial batch runner for UZHFPV datasets. +# +# Reads bag path and output paths from environment (with fallbacks): +# OV_WORKSPACE_ROOT- colcon workspace root (used when no first argument is passed) +# Only used to produce default paths if the three environment +# variables below (OV_*) are not set. +# OV_SAVE_PATH - where to write estimate files (default: ./results) +# OV_BAG_PATH - directory containing dataset bags (default: ./datasets) +# OV_VER - version tag for output subdirs (default: 2.7) +# +# Usage: run_ros2_uzhfpv.sh [WS_ROOT] +# WS_ROOT - optional; colcon workspace root. +# If not passed: use OV_WORKSPACE_ROOT if set, else four levels up from this script. +# + +set -e + +SCRIPT_DIR="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" &> /dev/null && pwd)" +# workspace root: first argument, else OV_WORKSPACE_ROOT env, else four levels up (workspace/.../open_vins/ov_msckf/scripts) +if [ -n "${1:-}" ]; then + WS_ROOT="$(cd "$1" && pwd)" +elif [ -n "${OV_WORKSPACE_ROOT:-}" ]; then + WS_ROOT="$(cd "${OV_WORKSPACE_ROOT}" && pwd)" +else + WS_ROOT="$(cd "${SCRIPT_DIR}/../../../../" && pwd)" +fi + +# source colcon workspace so ros2/ov_msckf is available in this script +if [ -f "${WS_ROOT}/install/setup.bash" ]; then + source "${WS_ROOT}/install/setup.bash" +else + echo "BASH: install/setup.bash not found in ${WS_ROOT}. Set OV_WORKSPACE_ROOT or pass workspace root as first argument." >&2 + exit 1 +fi + +#----------------------------------------------------------------------------- +# Paths from ENV (fallbacks for local use) +#----------------------------------------------------------------------------- +save_path1="${OV_SAVE_PATH:-${WS_ROOT}/results}/uzhfpv/algorithms" +save_path2="${OV_SAVE_PATH:-${WS_ROOT}/results}/uzhfpv/timings" +bag_path="${OV_BAG_PATH:-${WS_ROOT}/datasets}/uzhfpv" +ov_ver="${OV_VER:-2.7}" + +#----------------------------------------------------------------------------- +# Datasets and per-dataset config +#----------------------------------------------------------------------------- +modes=( + "mono" + "binocular" + "stereo" +) + +bagnames=( + "indoor_forward_3_snapdragon_with_gt" + "indoor_forward_5_snapdragon_with_gt" + "indoor_forward_6_snapdragon_with_gt" + "indoor_forward_7_snapdragon_with_gt" + "indoor_forward_9_snapdragon_with_gt" + "indoor_forward_10_snapdragon_with_gt" + "indoor_45_2_snapdragon_with_gt" + "indoor_45_4_snapdragon_with_gt" + "indoor_45_12_snapdragon_with_gt" + "indoor_45_13_snapdragon_with_gt" + "indoor_45_14_snapdragon_with_gt" + "outdoor_forward_1_snapdragon_with_gt" + "outdoor_forward_3_snapdragon_with_gt" + "outdoor_forward_5_snapdragon_with_gt" + "outdoor_45_1_snapdragon_with_gt" +) + +config=( + "uzhfpv_indoor" + "uzhfpv_indoor" + "uzhfpv_indoor" + "uzhfpv_indoor" + "uzhfpv_indoor" + "uzhfpv_indoor" + "uzhfpv_indoor_45" + "uzhfpv_indoor_45" + "uzhfpv_indoor_45" + "uzhfpv_indoor_45" + "uzhfpv_indoor_45" + "uzhfpv_outdoor" + "uzhfpv_outdoor" + "uzhfpv_outdoor" + "uzhfpv_outdoor_45" +) + +bagstarttimes=( + "0" "0" "0" "0" "0" "0" + "0" "0" "0" "0" "0" + "0" "0" "0" "0" +) + +#----------------------------------------------------------------------------- +# Run +#----------------------------------------------------------------------------- +big_start_time="$(date -u +%s)" + +for i in "${!bagnames[@]}"; do + for h in "${!modes[@]}"; do + for j in {00..00}; do + start_time="$(date -u +%s)" + filename_est="${save_path1}/ov_${ov_ver}_${modes[h]}/${bagnames[i]}/${j}_estimate.txt" + filename_time="${save_path2}/ov_${ov_ver}_${modes[h]}/${bagnames[i]}/${j}_timing.txt" + + case "${modes[h]}" in + mono) temp1="1"; temp2="true" ;; + binocular) temp1="2"; temp2="false" ;; + stereo) temp1="2"; temp2="true" ;; + *) echo "BASH: unknown mode ${modes[h]}"; exit 1 ;; + esac + + # bag path: bag_path/config_name/bagname (rosbag2 directory) + bag_uri="${bag_path}/${config[i]}/${bagnames[i]}" + mkdir -p "$(dirname "$filename_est")" "$(dirname "$filename_time")" + + # launch and capture output + launch_err="$(mktemp)" + if ! ros2 launch ov_msckf serial.launch.py \ + max_cameras:="$temp1" \ + use_stereo:="$temp2" \ + config:="${config[i]}" \ + dataset:="${bagnames[i]}" \ + bag:="$bag_uri" \ + bag_start:="${bagstarttimes[i]}" \ + dosave:=true \ + path_est:="$filename_est" \ + dotime:=true \ + #dolivetraj:=true \ # no support for live aligned trajectory viz (yet) + path_time:="$filename_time" \ + >"$launch_err" 2>&1; then + echo "BASH: ros2 launch failed for ${modes[h]} - ${bagnames[i]} (bag: $bag_uri)" >&2 + echo "BASH: captured output (stdout+stderr):" >&2 + cat "$launch_err" >&2 + rm -f "$launch_err" + exit 1 + fi + rm -f "$launch_err" + + end_time="$(date -u +%s)" + elapsed=$((end_time - start_time)) + echo "BASH: ${modes[h]} - ${bagnames[i]} - run $j took $elapsed seconds" + done + done +done + +big_end_time="$(date -u +%s)" +big_elapsed=$((big_end_time - big_start_time)) +echo "BASH: script took $big_elapsed seconds in total!!" diff --git a/ov_msckf/src/ros/ROS2Visualizer.h b/ov_msckf/src/ros/ROS2Visualizer.h index a737f80aa..905a12a80 100644 --- a/ov_msckf/src/ros/ROS2Visualizer.h +++ b/ov_msckf/src/ros/ROS2Visualizer.h @@ -25,7 +25,7 @@ #include #include #include -#include +#include #include #include #include @@ -42,7 +42,7 @@ #include #include #include -#include +#include #include #include diff --git a/ov_msckf/src/ros/ROSVisualizerHelper.h b/ov_msckf/src/ros/ROSVisualizerHelper.h index 14c1514c6..4dff3043e 100644 --- a/ov_msckf/src/ros/ROSVisualizerHelper.h +++ b/ov_msckf/src/ros/ROSVisualizerHelper.h @@ -34,7 +34,7 @@ #include #include #include -#include +#include #endif namespace ov_type { diff --git a/ov_msckf/src/ros2_serial_msckf.cpp b/ov_msckf/src/ros2_serial_msckf.cpp new file mode 100644 index 000000000..608e4dec5 --- /dev/null +++ b/ov_msckf/src/ros2_serial_msckf.cpp @@ -0,0 +1,313 @@ +/* + * 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 +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "core/VioManager.h" +#include "core/VioManagerOptions.h" +#include "ros/ROS2Visualizer.h" +#include "utils/dataset_reader.h" + +/** one entry in the time-ordered bag message list (either IMU or image) */ +struct BagMessage { + std::string topic; + double time_sec; + sensor_msgs::msg::Imu::SharedPtr imu; + sensor_msgs::msg::Image::SharedPtr image; +}; + +using namespace ov_msckf; + +// Main function +int main(int argc, char **argv) { + + // Ensure we have a path, if the user passes it then we should use it + std::string config_path = "unset_path_to_config.yaml"; + if (argc > 1) { + config_path = argv[1]; + } + + // Launch our ros node + rclcpp::init(argc, argv); + + rclcpp::NodeOptions options; + options.allow_undeclared_parameters(true); + options.automatically_declare_parameters_from_overrides(true); + auto node = std::make_shared("ros2_serial_msckf", options); + node->get_parameter_or("config_path", config_path, config_path); + + // Load the config + auto parser = std::make_shared(config_path); + parser->set_node(node); + + // Verbosity + std::string verbosity = "INFO"; + parser->parse_config("verbosity", verbosity); + ov_core::Printer::setPrintLevel(verbosity); + + // Create our VIO system + VioManagerOptions params; + params.print_and_load(parser); + //params.num_opencv_threads = 0; // uncomment if you want repeatability + //params.use_multi_threading_pubs = 0; // uncomment if you want repeatability + params.use_multi_threading_subs = false; + auto sys = std::make_shared(params); + auto viz = std::make_shared(node, sys); + + // tear down ROS-owned objects before shutting down the ROS context. + auto shutdown_and_cleanup = [&]() { + viz.reset(); + sys.reset(); + node.reset(); + if (rclcpp::ok()) { + rclcpp::shutdown(); + } + }; + + // Ensure we read in all parameters required + if (!parser->successful()) { + PRINT_ERROR(RED "[SERIAL]: unable to parse all parameters, please fix\n" RESET); + shutdown_and_cleanup(); + return EXIT_FAILURE; + } + + if (params.state_options.num_cameras > 2) { + PRINT_ERROR(RED "[SERIAL]: We currently only support 1 or 2 camera serial input....\n" RESET); + shutdown_and_cleanup(); + return EXIT_FAILURE; + } + + //=================================================================================== + //=================================================================================== + //=================================================================================== + + // Our imu topic + std::string topic_imu; + node->get_parameter_or("topic_imu", topic_imu, "/imu0"); + parser->parse_external("relative_config_imu", "imu0", "rostopic", topic_imu); + PRINT_DEBUG("[SERIAL]: imu: %s\n", topic_imu.c_str()); + + // Our camera topics + std::vector topic_cameras; + for (int i = 0; i < params.state_options.num_cameras; i++) { + std::string cam_topic; + node->get_parameter_or( + "topic_camera" + std::to_string(i), cam_topic, "/cam" + std::to_string(i) + "/image_raw"); + parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "rostopic", cam_topic); + topic_cameras.emplace_back(cam_topic); + PRINT_DEBUG("[SERIAL]: cam: %s\n", cam_topic.c_str()); + } + + // Location of the ROS bag we want to read in + std::string path_to_bag; + node->get_parameter_or( + "path_bag", path_to_bag, "/home/patrick/datasets/eth/V1_01_easy.bag"); + PRINT_DEBUG("[SERIAL]: ros bag path is: %s\n", path_to_bag.c_str()); + + // Load groundtruth if we have it + // NOTE: needs to be a csv ASL format file + std::map> gt_states; + if (node->has_parameter("path_gt")) { + std::string path_to_gt; + node->get_parameter_or("path_gt", path_to_gt, ""); + if (!path_to_gt.empty()) { + ov_core::DatasetReader::load_gt_file(path_to_gt, gt_states); + PRINT_DEBUG("[SERIAL]: gt file path is: %s\n", path_to_gt.c_str()); + } + } + + // Get our start location and how much of the bag we want to play + // Make the bag duration < 0 to just process to the end of the bag + double bag_start; + node->get_parameter_or("bag_start", bag_start, 0.0); + double bag_durr; + node->get_parameter_or("bag_durr", bag_durr, -1.0); + PRINT_DEBUG("[SERIAL]: bag start: %.1f\n", bag_start); + PRINT_DEBUG("[SERIAL]: bag duration: %.1f\n", bag_durr); + + //=================================================================================== + //=================================================================================== + //=================================================================================== + + // open bag with rosbag2 (uri = directory or path to bag; factory picks backend) + rosbag2_storage::StorageOptions storage_options; + storage_options.uri = path_to_bag; + + auto reader = rosbag2_transport::ReaderWriterFactory::make_reader(storage_options); + reader->open(storage_options); + + rclcpp::Serialization imu_serialization; + rclcpp::Serialization image_serialization; + + // collect all messages on our topics and deserialize + std::vector msgs; + double min_time_sec = std::numeric_limits::max(); + double max_time_sec = -std::numeric_limits::max(); + double max_camera_time = -1; + + while (reader->has_next()) { + if (!rclcpp::ok()) { + break; + } + auto serialized = reader->read_next(); + const std::string & topic = serialized->topic_name; + const double time_sec = static_cast(serialized->time_stamp) / 1e9; + + if (topic == topic_imu) { + auto imu_msg = std::make_shared(); + rclcpp::SerializedMessage extracted(*serialized->serialized_data); + imu_serialization.deserialize_message(&extracted, imu_msg.get()); + msgs.push_back(BagMessage{topic, time_sec, imu_msg, nullptr}); + min_time_sec = std::min(min_time_sec, time_sec); + max_time_sec = std::max(max_time_sec, time_sec); + } else { + for (int i = 0; i < params.state_options.num_cameras; i++) { + if (topic == topic_cameras.at(i)) { + auto image_msg = std::make_shared(); + rclcpp::SerializedMessage extracted(*serialized->serialized_data); + image_serialization.deserialize_message(&extracted, image_msg.get()); + msgs.push_back(BagMessage{topic, time_sec, nullptr, image_msg}); + min_time_sec = std::min(min_time_sec, time_sec); + max_time_sec = std::max(max_time_sec, time_sec); + max_camera_time = std::max(max_camera_time, time_sec); + break; + } + } + } + } + + if (msgs.empty()) { + PRINT_ERROR(RED "[SERIAL]: No messages to play on specified topics. Exiting.\n" RESET); + shutdown_and_cleanup(); + return EXIT_FAILURE; + } + + // sort by time (same as ROS1 view order) + std::sort(msgs.begin(), msgs.end(), + [](const BagMessage & a, const BagMessage & b) { return a.time_sec < b.time_sec; }); + + const double time_init = min_time_sec + bag_start; + const double time_finish = + (bag_durr < 0) ? max_time_sec : time_init + bag_durr; + PRINT_DEBUG("time start = %.6f\n", time_init); + PRINT_DEBUG("time end = %.6f\n", time_finish); + PRINT_DEBUG("[SERIAL]: total of %zu messages!\n", msgs.size()); + + //=================================================================================== + //=================================================================================== + //=================================================================================== + + // loop through our message array and process them + std::set used_index; + for (int m = 0; m < (int)msgs.size(); m++) { + const BagMessage & bm = msgs.at(m); + + // end once we reach the last time, or skip if before beginning time + if (!rclcpp::ok() || bm.time_sec > time_finish || bm.time_sec > max_camera_time) { + break; + } + if (bm.time_sec < time_init) { + continue; + } + + // skip messages that we have already used + if (used_index.find(m) != used_index.end()) { + used_index.erase(m); + continue; + } + + // IMU processing + if (bm.topic == topic_imu) { + viz->callback_inertial(bm.imu); + continue; + } + + // camera processing + for (int cam_id = 0; cam_id < params.state_options.num_cameras; cam_id++) { + if (bm.topic != topic_cameras.at(cam_id)) { + continue; + } + + // find the other camera(s) for this time (within 0.02 s) + std::map camid_to_msg_index; + double meas_time = bm.time_sec; + for (int cam_idt = 0; cam_idt < params.state_options.num_cameras; cam_idt++) { + if (cam_idt == cam_id) { + camid_to_msg_index.insert({cam_id, m}); + continue; + } + int cam_idt_idx = -1; + for (int mt = m; mt < (int)msgs.size(); mt++) { + if (msgs.at(mt).topic != topic_cameras.at(cam_idt)) { + continue; + } + if (std::abs(msgs.at(mt).time_sec - meas_time) < 0.02) { + cam_idt_idx = mt; + } + break; + } + if (cam_idt_idx != -1) { + camid_to_msg_index.insert({cam_idt, cam_idt_idx}); + } + } + + if ((int)camid_to_msg_index.size() != params.state_options.num_cameras) { + PRINT_DEBUG(YELLOW "[SERIAL]: Unable to find stereo pair for message %d at %.2f into bag (will skip!)\n" RESET, + m, meas_time - time_init); + continue; + } + + Eigen::Matrix imustate; + if (!gt_states.empty() && !sys->initialized() && + ov_core::DatasetReader::get_gt_state(meas_time, imustate, gt_states)) { + sys->initialize_with_gt(imustate); + } + + if (params.state_options.num_cameras == 1) { + viz->callback_monocular(msgs.at(camid_to_msg_index.at(0)).image, 0); + } else if (params.state_options.num_cameras == 2) { + used_index.insert(camid_to_msg_index.at(0)); + used_index.insert(camid_to_msg_index.at(1)); + viz->callback_stereo( + msgs.at(camid_to_msg_index.at(0)).image, + msgs.at(camid_to_msg_index.at(1)).image, + 0, 1); + } + break; + } + } + + viz->visualize_final(); + shutdown_and_cleanup(); + + return EXIT_SUCCESS; +}